Soft constraints for MHE seems to work

Author: franckgaga

src/controller/construct.jl

@@ -235,25 +235,25 @@ function setconstraint!(
         size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))"))
         any(C_ymin .< 0) && error("C_ymin weights should be non-negative")
         con.C_ymin[:] = C_ymin
-        size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] = -C_ymin) # for LinModel
+        size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] = -con.C_ymin) # for LinModel
     end
     if !isnothing(C_ymax)
         size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))"))
         any(C_ymax .< 0) && error("C_ymax weights should be non-negative")
         con.C_ymax[:] = C_ymax
-        size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] = -C_ymax) # for LinModel
+        size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] = -con.C_ymax) # for LinModel
     end
     if !isnothing(c_x̂min)
         size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))"))
         any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative")
         con.c_x̂min[:] = c_x̂min
-        size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] = -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̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))"))
         any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative")
         con.c_x̂max[:] = c_x̂max
-        size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] = -c_x̂max) # for LinModel
+        size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] = -con.c_x̂max) # for LinModel
     end
     i_Umin,  i_Umax  = .!isinf.(con.Umin),  .!isinf.(con.Umax)
     i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmin)

src/estimator/mhe/construct.jl

@@ -440,17 +440,51 @@ function setconstraint!(
         size(V̂max) == (nym*He,) || throw(ArgumentError("V̂max size must be $((nym*He,))"))
         con.V̂max[:] = V̂max
     end
+    if !isnothing(C_x̂min)
+        size(C_x̂min) == (nX̂con,) || throw(ArgumentError("C_x̂min size must be $((nX̂con,))"))
+        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
+    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
+    end
+    if !isnothing(C_ŵmin)
+        size(C_ŵmin) == (nŵ*He,) || throw(ArgumentError("C_ŵmin size must be $((nŵ*He,))"))
+        any(C_ŵmin .< 0) && error("C_ŵmin weights should be non-negative")
+        con.A_Ŵmin[:, end] = -C_ŵmin
+    end
+    if !isnothing(C_ŵmax)
+        size(C_ŵmax) == (nŵ*He,) || throw(ArgumentError("C_ŵmax size must be $((nŵ*He,))"))
+        any(C_ŵmax .< 0) && error("C_ŵmax weights should be non-negative")
+        con.A_Ŵmax[:, end] = -C_ŵmax
+    end
+    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
+    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
+    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)
     i_Ŵmin, i_Ŵmax  = .!isinf.(con.Ŵmin)  , .!isinf.(con.Ŵmax)
     i_V̂min, i_V̂max  = .!isinf.(con.V̂min)  , .!isinf.(con.V̂max)
     if notSolvedYet
         con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mhe(model, 
             i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max,
-            con.A_x̃min, con.A_x̃max, 
-            con.A_X̂min, con.A_X̂max, 
-            con.A_Ŵmin, con.A_Ŵmax, 
-            con.A_V̂min, con.A_V̂max
+            con.A_x̃min, con.A_x̃max, con.A_X̂min, con.A_X̂max, 
+            con.A_Ŵmin, con.A_Ŵmax, con.A_V̂min, con.A_V̂max
         )
         A = con.A[con.i_b, :]
         b = con.b[con.i_b]

src/estimator/mhe/execute.jl

@@ -95,6 +95,7 @@ dictionary `info` with the following fields:
 
 - `:Ŵ`   : optimal estimated process noise over ``N_k``, ``\mathbf{Ŵ}``.
 - `:x̂arr`: optimal estimated state at arrival, ``\mathbf{x̂}_k(k-N_k+1)``.
+- `:ϵ`   : optimal slack variable, ``ϵ``.
 - `:J`   : objective value optimum, ``J``.
 - `:X̂`   : optimal estimated states over ``N_k+1``, ``\mathbf{X̂}``.
 - `:x̂`   : optimal estimated state for the next time step, ``\mathbf{x̂}_k(k+1)``.
@@ -123,11 +124,12 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     model, Nk = estim.model, estim.Nk[]
     nu, ny, nd = model.nu, model.ny, model.nd
     nx̂, nym, nŵ = estim.nx̂, estim.nym, estim.nx̂
+    nx̃ = !isinf(estim.C) + nx̂
     MyTypes = Union{JuMP._SolutionSummary, Hermitian{NT, Matrix{NT}}, Vector{NT}, NT}
     info = Dict{Symbol, MyTypes}()
     V̂, X̂ = similar(estim.Ym[1:nym*Nk]), similar(estim.X̂[1:nx̂*Nk])
     V̂, X̂ = predict!(V̂, X̂, estim, model, estim.Z̃)
-    x̂arr = estim.Z̃[1:nx̂]
+    x̂arr = estim.Z̃[nx̃-nx̂+1:nx̃]
     X̂ = [x̂arr; X̂]
     Ym, U, D = estim.Ym[1:nym*Nk], estim.U[1:nu*Nk], estim.D[1:nd*Nk]
     Ŷ = Vector{NT}(undef, ny*Nk)
@@ -139,6 +141,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     Ŷm = Ym - V̂
     info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
     info[:x̂arr] = x̂arr
+    info[:ϵ] = isinf(estim.C) ? NaN : estim.Z̃[begin]
     info[:J] = obj_nonlinprog(estim, estim.model, V̂, estim.Z̃)
     info[:X̂] = X̂
     info[:x̂] = estim.x̂

test/test_state_estim.jl

@@ -603,7 +603,8 @@ end
 
     I_6 = Matrix{Float64}(I, 6, 6)
     I_2 = Matrix{Float64}(I, 2, 2)
-    mhe9 = MovingHorizonEstimator(nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Model(Ipopt.Optimizer))
+    optim = Model(Ipopt.Optimizer)
+    mhe9 = MovingHorizonEstimator(nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, 1e5 ,optim)
     @test mhe9.P̂0 ≈ I(6)
     @test mhe9.Q̂ ≈ I(6)
     @test mhe9.R̂ ≈ I(2)