debug: setting softness parameters for MHE now works

Author: franckgaga

src/estimator/mhe/construct.jl

@@ -197,7 +197,7 @@ the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. The
 # Arguments
 - `model::SimModel` : (deterministic) model for the estimations.
 - `He=nothing` : estimation horizon ``H_e``, must be specified.
-- `Cwt=Inf` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
+- `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only.
 - `optim=default_optim_mhe(model)` : a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
    with a quadratic/nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl),
    or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)).
@@ -337,10 +337,10 @@ Extended Help for details on model augmentation and time-varying constraints.
 - `v̂max = fill(+Inf,nym)` : sensor noise upper bounds ``\mathbf{v̂_{max}}``.
 - `c_x̂min = fill(0.0,nx̂)`  : `x̂min` softness weights ``\mathbf{c_{x̂_{min}}}``.
 - `c_x̂max = fill(0.0,nx̂)`  : `x̂max` softness weights ``\mathbf{c_{x̂_{max}}}``.
-- `c_ŵmin = fill(1.0,nx̂)`  : `ŵmin` softness weights ``\mathbf{c_{ŵ_{min}}}``.
-- `c_ŵmax = fill(1.0,nx̂)`  : `ŵmax` softness weights ``\mathbf{c_{ŵ_{max}}}``.
-- `c_v̂min = fill(1.0,nym)` : `v̂min` softness weights ``\mathbf{c_{v̂_{min}}}``.
-- `c_v̂max = fill(1.0,nym)` : `v̂max` softness weights ``\mathbf{c_{v̂_{max}}}``.
+- `c_ŵmin = fill(0.0,nx̂)`  : `ŵmin` softness weights ``\mathbf{c_{ŵ_{min}}}``.
+- `c_ŵmax = fill(0.0,nx̂)`  : `ŵmax` softness weights ``\mathbf{c_{ŵ_{max}}}``.
+- `c_v̂min = fill(0.0,nym)` : `v̂min` softness weights ``\mathbf{c_{v̂_{min}}}``.
+- `c_v̂max = fill(0.0,nym)` : `v̂max` softness weights ``\mathbf{c_{v̂_{max}}}``.
 - all the keyword arguments above but with a first capital letter, e.g. `X̂max` or `C_ŵmax`:
   for time-varying constraints (see Extended Help).
 
@@ -412,7 +412,7 @@ function setconstraint!(
     isnothing(C_v̂max) && !isnothing(c_v̂max) && (C_v̂max = repeat(c_v̂max, He))
     if !all(isnothing.((C_x̂min, C_x̂max, C_ŵmin, C_ŵmax, C_v̂min, C_v̂max)))
         !isinf(C) || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters"))
-        notSolvedYet || throw(ArgumentError("Cannot set softness parameters after calling updatestate!"))
+        notSolvedYet || error("Cannot set softness parameters after calling updatestate!")
     end
     if !isnothing(X̂min)
         size(X̂min) == (nX̂con,) || throw(ArgumentError("X̂min size must be $((nX̂con,))"))
@@ -445,14 +445,14 @@ function setconstraint!(
         any(C_x̂min .< 0) && error("C_x̂min weights should be non-negative")
         con.A_x̃min[end-nx̂+1:end, end] = -C_x̂min[1:nx̂] # if C is finite : x̃ = [ϵ; x̂]
         con.C_x̂min[:] = C_x̂min[nx̂+1:end]
-        size(con.A_X̂min) ≠ 0 && (con.A_X̂min[:, end] = -con.C_x̂min) # for LinModel
+        size(con.A_X̂min, 1) ≠ 0 && (con.A_X̂min[:, end] = -con.C_x̂min) # for LinModel
     end
     if !isnothing(C_x̂max)
         size(C_x̂max) == (nX̂con,) || throw(ArgumentError("C_x̂max size must be $((nX̂con,))"))
         any(C_x̂max .< 0) && error("C_x̂max weights should be non-negative")
         con.A_x̃max[end-nx̂+1:end, end] = -C_x̂max[1:nx̂] # if C is finite : x̃ = [ϵ; x̂]
         con.C_x̂max[:] = C_x̂max[nx̂+1:end]
-        size(con.A_X̂max) ≠ 0 && (con.A_X̂max[:, end] = -con.C_x̂max) # for LinModel
+        size(con.A_X̂max, 1) ≠ 0 && (con.A_X̂max[:, end] = -con.C_x̂max) # for LinModel
     end
     if !isnothing(C_ŵmin)
         size(C_ŵmin) == (nŵ*He,) || throw(ArgumentError("C_ŵmin size must be $((nŵ*He,))"))
@@ -467,14 +467,14 @@ function setconstraint!(
     if !isnothing(C_v̂min)
         size(C_v̂min) == (nym*He,) || throw(ArgumentError("C_v̂min size must be $((nym*He,))"))
         any(C_v̂min .< 0) && error("C_v̂min weights should be non-negative")
-        con.C_V̂min[:] = C_v̂min
-        size(con.A_V̂min) ≠ 0 && (con.A_V̂min[:, end] = -con.C_V̂min) # for LinModel
+        con.C_v̂min[:] = C_v̂min
+        size(con.A_V̂min, 1) ≠ 0 && (con.A_V̂min[:, end] = -con.C_v̂min) # for LinModel
     end
     if !isnothing(C_v̂max)
         size(C_v̂max) == (nym*He,) || throw(ArgumentError("C_v̂max size must be $((nym*He,))"))
         any(C_v̂max .< 0) && error("C_v̂max weights should be non-negative")
-        con.C_V̂max[:] = C_v̂max
-        size(con.A_V̂max) ≠ 0 && (con.A_V̂max[:, end] = -con.C_V̂max) # for LinModel
+        con.C_v̂max[:] = C_v̂max
+        size(con.A_V̂max, 1) ≠ 0 && (con.A_V̂max[:, end] = -con.C_v̂max) # for LinModel
     end
     i_x̃min, i_x̃max  = .!isinf.(con.x̃min)  , .!isinf.(con.x̃max)
     i_X̂min, i_X̂max  = .!isinf.(con.X̂min)  , .!isinf.(con.X̂max)
@@ -598,8 +598,8 @@ function init_defaultcon_mhe(
     V̂min, V̂max = fill(convert(NT,-Inf), nYm), fill(convert(NT,+Inf), nYm)
     c_x̂min, c_x̂max = fill(0.0, nx̂),  fill(0.0, nx̂)
     C_x̂min, C_x̂max = fill(0.0, nX̂),  fill(0.0, nX̂)
-    C_ŵmin, C_ŵmax = fill(1.0, nŴ),  fill(1.0, nŴ)
-    C_v̂min, C_v̂max = fill(1.0, nYm), fill(1.0, nYm)
+    C_ŵmin, C_ŵmax = fill(0.0, nŴ),  fill(0.0, nŴ)
+    C_v̂min, C_v̂max = fill(0.0, nYm), fill(0.0, nYm)
     A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄ = relaxarrival(model, C, c_x̂min, c_x̂max, x̂min, x̂max, ex̄)
     A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, C, C_x̂min, C_x̂max, Ex̂)
     A_Ŵmin, A_Ŵmax = relaxŴ(model, C, C_ŵmin, C_ŵmax, nx̂)

test/test_state_estim.jl

@@ -561,12 +561,14 @@ end
     @test mhe1.nyu == 0
     @test mhe1.nxs == 2
     @test mhe1.nx̂ == 6
+    @test size(mhe1.Ẽ, 2) == 6*mhe1.nx̂
 
     mhe2 = MovingHorizonEstimator(nonlinmodel, He=5)
     @test mhe2.nym == 2
     @test mhe2.nyu == 0
     @test mhe2.nxs == 2
     @test mhe2.nx̂ == 6
+    @test size(mhe1.Ẽ, 2) == 6*mhe1.nx̂
 
     mhe3 = MovingHorizonEstimator(nonlinmodel, He=5, i_ym=[2])
     @test mhe3.nym == 1
@@ -612,12 +614,17 @@ end
     mhe10 = MovingHorizonEstimator(nonlinmodel, He=5, optim=Model(OSQP.Optimizer))
     @test solver_name(mhe10.optim) == "OSQP"
 
+    mhe11 = MovingHorizonEstimator(nonlinmodel, He=5, Cwt=1e3)
+    @test size(mhe11.Ẽ, 2) == 6*mhe11.nx̂ + 1
+    @test mhe11.C == 1e3
+
     linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mhe11 = MovingHorizonEstimator(linmodel2, He=5)
     @test isa(mhe11, MovingHorizonEstimator{Float32})
 
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel1, He=0)
+    @test_throws ArgumentError MovingHorizonEstimator(linmodel1, Cwt=-1)
 end
 
 @testset "MovingHorizonEstimator estimation and getinfo" begin
@@ -677,30 +684,60 @@ end
 
 @testset "MovingHorizonEstimator set constraints" begin
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
-    mhe1 = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0)
+    mhe1 = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0, Cwt=1e3)
     setconstraint!(mhe1, x̂min=[-51,-52], x̂max=[53,54])
     @test all((mhe1.con.X̂min, mhe1.con.X̂max) .≈ ([-51,-52], [53,54]))
-    @test all((mhe1.con.x̃min, mhe1.con.x̃max) .≈ ([-51,-52], [53,54]))
+    @test all((mhe1.con.x̃min[2:end], mhe1.con.x̃max[2:end]) .≈ ([-51,-52], [53,54]))
     setconstraint!(mhe1, ŵmin=[-55,-56], ŵmax=[57,58])
     @test all((mhe1.con.Ŵmin, mhe1.con.Ŵmax) .≈ ([-55,-56], [57,58]))
     setconstraint!(mhe1, v̂min=[-59,-60], v̂max=[61,62])
     @test all((mhe1.con.V̂min, mhe1.con.V̂max) .≈ ([-59,-60], [61,62]))
-
-    mhe2 = MovingHorizonEstimator(linmodel1, He=4, nint_ym=0)
+    setconstraint!(mhe1, c_x̂min=[0.01,0.02], c_x̂max=[0.03,0.04])
+    @test all((-mhe1.con.A_X̂min[:, end], -mhe1.con.A_X̂max[:, end]) .≈ ([0.01, 0.02], [0.03,0.04]))
+    @test all((-mhe1.con.A_x̃min[2:end, end], -mhe1.con.A_x̃max[2:end, end]) .≈ ([0.01,0.02], [0.03,0.04]))
+    setconstraint!(mhe1, c_ŵmin=[0.05,0.06], c_ŵmax=[0.07,0.08])
+    @test all((-mhe1.con.A_Ŵmin[:, end], -mhe1.con.A_Ŵmax[:, end]) .≈ ([0.05, 0.06], [0.07,0.08]))
+    setconstraint!(mhe1, c_v̂min=[0.09,0.10], c_v̂max=[0.11,0.12])
+    @test all((-mhe1.con.A_V̂min[:, end], -mhe1.con.A_V̂max[:, end]) .≈ ([0.09, 0.10], [0.11,0.12]))
+
+    mhe2 = MovingHorizonEstimator(linmodel1, He=4, nint_ym=0, Cwt=1e3)
     setconstraint!(mhe2, X̂min=-1(1:10), X̂max=1(1:10))
     @test all((mhe2.con.X̂min, mhe2.con.X̂max) .≈ (-1(3:10), 1(3:10)))
-    @test all((mhe2.con.x̃min, mhe2.con.x̃max) .≈ (-1(1:2),  1(1:2)))
+    @test all((mhe2.con.x̃min[2:end], mhe2.con.x̃max[2:end]) .≈ (-1(1:2),  1(1:2)))
     setconstraint!(mhe2, Ŵmin=-1(11:18), Ŵmax=1(11:18))
     @test all((mhe2.con.Ŵmin, mhe2.con.Ŵmax) .≈ (-1(11:18), 1(11:18)))
     setconstraint!(mhe2, V̂min=-1(31:38), V̂max=1(31:38))
     @test all((mhe2.con.V̂min, mhe2.con.V̂max) .≈ (-1(31:38), 1(31:38)))
+    setconstraint!(mhe2, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10))
+    @test all((-mhe2.con.A_X̂min[:, end], -mhe2.con.A_X̂max[:, end]) .≈ (0.01(3:10), 0.02(3:10)))
+    @test all((-mhe2.con.A_x̃min[2:end, end], -mhe2.con.A_x̃max[2:end, end]) .≈ (0.01(1:2), 0.02(1:2)))
+    setconstraint!(mhe2, C_ŵmin=0.03(11:18), C_ŵmax=0.04(11:18))
+    @test all((-mhe2.con.A_Ŵmin[:, end], -mhe2.con.A_Ŵmax[:, end]) .≈ (0.03(11:18), 0.04(11:18)))
+    setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38))
+    @test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38)))
+
+    f(x,u,d) = linmodel1.A*x + linmodel1.Bu*u
+    h(x,d)   = linmodel1.C*x 
+    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2), uop=[10,50], yop=[50,30])
+
+    mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3)
+    setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10))
+    @test all((mhe3.con.C_x̂min, mhe3.con.C_x̂max) .≈ (0.01(3:10), 0.02(3:10)))
+    setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18))
+    @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18)))
 
     @test_throws ArgumentError setconstraint!(mhe2, x̂min=[-1])
     @test_throws ArgumentError setconstraint!(mhe2, x̂max=[+1])
     @test_throws ArgumentError setconstraint!(mhe2, ŵmin=[-1])
     @test_throws ArgumentError setconstraint!(mhe2, ŵmax=[+1])
     @test_throws ArgumentError setconstraint!(mhe2, v̂min=[-1])
     @test_throws ArgumentError setconstraint!(mhe2, v̂max=[+1])
+    @test_throws ArgumentError setconstraint!(mhe2, c_x̂min=[-1])
+    @test_throws ArgumentError setconstraint!(mhe2, c_x̂max=[+1])
+    @test_throws ArgumentError setconstraint!(mhe2, c_ŵmin=[-1])
+    @test_throws ArgumentError setconstraint!(mhe2, c_ŵmax=[+1])
+    @test_throws ArgumentError setconstraint!(mhe2, c_v̂min=[-1])
+    @test_throws ArgumentError setconstraint!(mhe2, c_v̂max=[+1])
 
     updatestate!(mhe1, [10, 50], [50, 30])
     @test_throws ErrorException setconstraint!(mhe1, x̂min=[-Inf,-Inf])
@@ -709,6 +746,20 @@ end
     @test_throws ErrorException setconstraint!(mhe1, ŵmax=[+Inf,+Inf])
     @test_throws ErrorException setconstraint!(mhe1, v̂min=[-Inf,-Inf])
     @test_throws ErrorException setconstraint!(mhe1, v̂max=[+Inf,+Inf])
+    @test_throws ErrorException setconstraint!(mhe1, c_x̂min=[100,100])
+    @test_throws ErrorException setconstraint!(mhe1, c_x̂max=[200,200])
+    @test_throws ErrorException setconstraint!(mhe1, c_ŵmin=[300,300])
+    @test_throws ErrorException setconstraint!(mhe1, c_ŵmax=[400,400])
+    @test_throws ErrorException setconstraint!(mhe1, c_v̂min=[500,500])
+    @test_throws ErrorException setconstraint!(mhe1, c_v̂max=[600,600])
+
+    mhe4 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0, Cwt=Inf)
+    @test_throws ArgumentError setconstraint!(mhe4, c_x̂min=[1,1])
+    @test_throws ArgumentError setconstraint!(mhe4, c_x̂max=[1,1])
+    @test_throws ArgumentError setconstraint!(mhe4, c_ŵmin=[1,1])
+    @test_throws ArgumentError setconstraint!(mhe4, c_ŵmax=[1,1])
+    @test_throws ArgumentError setconstraint!(mhe4, c_v̂min=[1,1])
+    @test_throws ArgumentError setconstraint!(mhe4, c_v̂max=[1,1])
 end
 
 @testset "MovingHorizonEstimator constraint violation" begin