added : nϵ field in MHE

franckgaga · franckgaga · commit 6e2fc12cde02 · 2024-06-09T17:29:11.000-04:00
diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl
@@ -15,8 +15,7 @@ end
 function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n)
     nu, nd = estim.model.nu, estim.model.nd
     nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu
-    He = estim.He
-    nϵ = isinf(estim.C) ? 0 : 1
+    He, nϵ = estim.He, estim.nϵ
     println(io, "$(lpad(He, n)) estimation steps He")
     println(io, "$(lpad(nϵ, n)) slack variable ϵ (estimation constraints)")
     println(io, "$(lpad(nu, n)) manipulated inputs u ($(sum(estim.nint_u)) integrating states)")
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -60,6 +60,7 @@ struct MovingHorizonEstimator{
     f̂op::Vector{NT}
     x̂0 ::Vector{NT}
     He::Int
+    nϵ::Int
     i_ym::Vector{Int}
     nx̂ ::Int
     nym::Int
@@ -125,7 +126,9 @@ struct MovingHorizonEstimator{
         )
         # dummy values (updated just before optimization):
         F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He)
-        con, Ẽ, ẽx̄ = init_defaultcon_mhe(model, He, Cwt, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂)
+        con, nϵ, Ẽ, ẽx̄ = init_defaultcon_mhe(
+            model, He, Cwt, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂
+        )
         nZ̃ = size(Ẽ, 2)
         # dummy values, updated before optimization:
         H̃, q̃, p = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1)
@@ -140,7 +143,7 @@ struct MovingHorizonEstimator{
         estim = new{NT, SM, JM, CE}(
             model, optim, con, covestim,  
             Z̃, lastu0, x̂op, f̂op, x̂0, 
-            He,
+            He, nϵ,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
             Â, B̂u, Ĉ, B̂d, D̂d,
@@ -641,6 +644,7 @@ function init_defaultcon_mhe(
 ) where {NT<:Real}
     nŵ = nx̂
     nZ̃, nX̂, nŴ, nYm = nx̂+nŵ*He, nx̂*He, nŵ*He, nym*He
+    nϵ = isinf(C) ? 0 : 1
     x̂min, x̂max = fill(convert(NT,-Inf), nx̂),  fill(convert(NT,+Inf), nx̂)
     X̂min, X̂max = fill(convert(NT,-Inf), nX̂),  fill(convert(NT,+Inf), nX̂)
     Ŵmin, Ŵmax = fill(convert(NT,-Inf), nŴ),  fill(convert(NT,+Inf), nŴ)
@@ -649,10 +653,10 @@ function init_defaultcon_mhe(
     C_x̂min, C_x̂max = fill(0.0, nX̂),  fill(0.0, nX̂)
     C_ŵmin, C_ŵmax = fill(0.0, nŴ),  fill(0.0, nŴ)
     C_v̂min, C_v̂max = fill(0.0, nYm), fill(0.0, nYm)
-    A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄ = relaxarrival(model, C, c_x̂min, c_x̂max, x̂min, x̂max, ex̄)
-    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, C, C_x̂min, C_x̂max, Ex̂)
-    A_Ŵmin, A_Ŵmax = relaxŴ(model, C, C_ŵmin, C_ŵmax, nx̂)
-    A_V̂min, A_V̂max, Ẽ = relaxV̂(model, C, C_v̂min, C_v̂max, E)
+    A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄ = relaxarrival(model, nϵ, c_x̂min, c_x̂max, x̂min, x̂max, ex̄)
+    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nϵ, C_x̂min, C_x̂max, Ex̂)
+    A_Ŵmin, A_Ŵmax = relaxŴ(model, nϵ, C_ŵmin, C_ŵmax, nx̂)
+    A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nϵ, C_v̂min, C_v̂max, E)
     i_x̃min, i_x̃max = .!isinf.(x̃min), .!isinf.(x̃max)
     i_X̂min, i_X̂max = .!isinf.(X̂min), .!isinf.(X̂max)
     i_Ŵmin, i_Ŵmax = .!isinf.(Ŵmin), .!isinf.(Ŵmax)
@@ -670,12 +674,12 @@ function init_defaultcon_mhe(
         C_x̂min, C_x̂max, C_v̂min, C_v̂max,
         i_b, i_g
     )
-    return con, Ẽ, ẽx̄
+    return con, nϵ, Ẽ, ẽx̄
 end
 
 @doc raw"""
     relaxarrival(
-        model::SimModel, C, c_x̂min, c_x̂max, x̂min, x̂max, ex̄
+        model::SimModel, nϵ, c_x̂min, c_x̂max, x̂min, x̂max, ex̄
     ) -> A_x̃min, A_x̃max, x̃min, x̃max, ẽx̄
 
 Augment arrival state constraints with slack variable ϵ for softening the MHE.
@@ -700,9 +704,9 @@ in which
 ``\mathbf{x̃_{max}} = [\begin{smallmatrix} ∞ \\ \mathbf{x̂_{max}} \end{smallmatrix}]`` and
 ``\mathbf{x̃_{op}}  = [\begin{smallmatrix} 0 \\ \mathbf{x̂_{op}}  \end{smallmatrix}]``
 """
-function relaxarrival(::SimModel{NT}, C, c_x̂min, c_x̂max, x̂min, x̂max, ex̄) where {NT<:Real}
+function relaxarrival(::SimModel{NT}, nϵ, c_x̂min, c_x̂max, x̂min, x̂max, ex̄) where {NT<:Real}
     ex̂ = -ex̄
-    if !isinf(C) # Z̃ = [ϵ; Z]
+    if nϵ ≠ 0 # Z̃ = [ϵ; Z]
         x̃min, x̃max = [NT[0.0]; x̂min], [NT[Inf]; x̂max]
         A_ϵ = [NT[1.0] zeros(NT, 1, size(ex̂, 2))]
         # ϵ impacts arrival state constraint calculations:
@@ -718,7 +722,7 @@ function relaxarrival(::SimModel{NT}, C, c_x̂min, c_x̂max, x̂min, x̂max, ex
 end
 
 @doc raw"""
-    relaxX̂(model::SimModel, C, C_x̂min, C_x̂max, Ex̂) -> A_X̂min, A_X̂max, Ẽx̂
+    relaxX̂(model::SimModel, nϵ, C_x̂min, C_x̂max, Ex̂) -> A_X̂min, A_X̂max, Ẽx̂
 
 Augment estimated state constraints with slack variable ϵ for softening the MHE.
 
@@ -739,8 +743,8 @@ also returns the ``\mathbf{A}`` matrices for the inequality constraints:
 in which ``\mathbf{X̂_{min}, X̂_{max}}`` and ``\mathbf{X̂_{op}}`` vectors respectively contains
 ``\mathbf{x̂_{min}, x̂_{max}}`` and ``\mathbf{x̂_{op}}`` repeated ``H_e`` times.
 """
-function relaxX̂(::LinModel{NT}, C, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
-    if !isinf(C) # Z̃ = [ϵ; Z]
+function relaxX̂(::LinModel{NT}, nϵ, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
+    if nϵ ≠ 0 # Z̃ = [ϵ; Z]
         # ϵ impacts estimated process noise constraint calculations:
         A_X̂min, A_X̂max = -[C_x̂min Ex̂], [-C_x̂max Ex̂]
         # ϵ has no impact on estimated process noises:
@@ -753,14 +757,14 @@ function relaxX̂(::LinModel{NT}, C, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
 end
 
 "Return empty matrices if model is not a [`LinModel`](@ref)"
-function relaxX̂(::SimModel{NT}, C, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
-    Ẽx̂ = !isinf(C) ? [zeros(NT, 0, 1) Ex̂] : Ex̂
+function relaxX̂(::SimModel{NT}, nϵ, C_x̂min, C_x̂max, Ex̂) where {NT<:Real}
+    Ẽx̂ = [zeros(NT, 0, nϵ) Ex̂]
     A_X̂min, A_X̂max = -Ẽx̂,  Ẽx̂
     return A_X̂min, A_X̂max, Ẽx̂
 end
 
 @doc raw"""
-    relaxŴ(model::SimModel, C, C_ŵmin, C_ŵmax, nx̂) -> A_Ŵmin, A_Ŵmax
+    relaxŴ(model::SimModel, nϵ, C_ŵmin, C_ŵmax, nx̂) -> A_Ŵmin, A_Ŵmax
 
 Augment estimated process noise constraints with slack variable ϵ for softening the MHE.
 
@@ -778,9 +782,9 @@ matrices for the inequality constraints:
 \end{bmatrix}
 ```
 """
-function relaxŴ(::SimModel{NT}, C, C_ŵmin, C_ŵmax, nx̂) where {NT<:Real}
+function relaxŴ(::SimModel{NT}, nϵ, C_ŵmin, C_ŵmax, nx̂) where {NT<:Real}
     A = [zeros(NT, length(C_ŵmin), nx̂) I]
-    if !isinf(C) # Z̃ = [ϵ; Z]
+    if nϵ ≠ 0 # Z̃ = [ϵ; Z]
         A_Ŵmin, A_Ŵmax = -[C_ŵmin A], [-C_ŵmax A]
     else # Z̃ = Z (only hard constraints)
         A_Ŵmin, A_Ŵmax = -A, A
@@ -789,7 +793,7 @@ function relaxŴ(::SimModel{NT}, C, C_ŵmin, C_ŵmax, nx̂) where {NT<:Real}
 end
 
 @doc raw"""
-    relaxV̂(model::SimModel, C, C_v̂min, C_v̂max, E) -> A_V̂min, A_V̂max, Ẽ
+    relaxV̂(model::SimModel, nϵ, C_v̂min, C_v̂max, E) -> A_V̂min, A_V̂max, Ẽ
 
 Augment estimated sensor noise constraints with slack variable ϵ for softening the MHE.
 
@@ -808,8 +812,8 @@ also returns the ``\mathbf{A}`` matrices for the inequality constraints:
 \end{bmatrix}
 ```
 """
-function relaxV̂(::LinModel{NT}, C, C_v̂min, C_v̂max, E) where {NT<:Real}
-    if !isinf(C) # Z̃ = [ϵ; Z]
+function relaxV̂(::LinModel{NT}, nϵ, C_v̂min, C_v̂max, E) where {NT<:Real}
+    if nϵ ≠ 0 # Z̃ = [ϵ; Z]
         # ϵ impacts estimated sensor noise constraint calculations:
         A_V̂min, A_V̂max = -[C_v̂min E], [-C_v̂max E]
         # ϵ has no impact on estimated sensor noises:
@@ -822,8 +826,8 @@ function relaxV̂(::LinModel{NT}, C, C_v̂min, C_v̂max, E) where {NT<:Real}
 end
 
 "Return empty matrices if model is not a [`LinModel`](@ref)"
-function relaxV̂(::SimModel{NT}, C, C_v̂min, C_v̂max, E) where {NT<:Real}
-    Ẽ = !isinf(C) ? [zeros(NT, 0, 1) E] : E
+function relaxV̂(::SimModel{NT}, nϵ, C_v̂min, C_v̂max, E) where {NT<:Real}
+    Ẽ = [zeros(NT, 0, nϵ) E]
     A_V̂min, A_V̂max = -Ẽ, Ẽ
     return A_V̂min, A_V̂max, Ẽ
 end
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
@@ -33,15 +33,16 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u0, y0m, d0) where
     add_data_windows!(estim::MovingHorizonEstimator, u0, d0, y0m)
     initpred!(estim, model)
     linconstraint!(estim, model)
-    nu, ny, nx̂, nym, nŵ, Nk = model.nu, model.ny, estim.nx̂, estim.nym, estim.nx̂, estim.Nk[]
-    nx̃ = !isinf(estim.C) + nx̂
+    nu, ny = model.nu, model.ny
+    nx̂, nym, nŵ, nϵ, Nk = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ, estim.Nk[]
+    nx̃ = nϵ + nx̂
     Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
     V̂  = Vector{NT}(undef, nym*Nk)
     X̂0 = Vector{NT}(undef, nx̂*Nk)
     û0 = Vector{NT}(undef, nu)
     ŷ0 = Vector{NT}(undef, ny)
     x̄  = Vector{NT}(undef, nx̂)
-    ϵ_0 = isinf(estim.C) ? empty(estim.Z̃) : estim.Z̃[begin]
+    ϵ_0 = estim.nϵ ≠ 0 ? estim.Z̃[begin] : empty(estim.Z̃)
     Z̃_0 = [ϵ_0; estim.x̂0arr_old; estim.Ŵ]
     V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃_0)
     J_0 = obj_nonlinprog!(x̄, estim, model, V̂, Z̃_0)
@@ -128,8 +129,8 @@ julia> round.(getinfo(estim)[:Ŷ], digits=3)
 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, nŵ = estim.nx̂, estim.nym, estim.nx̂
-    nx̃ = !isinf(estim.C) + nx̂
+    nx̂, nym, nŵ, nϵ = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ
+    nx̃ = nϵ + nx̂
     MyTypes = Union{JuMP._SolutionSummary, Hermitian{NT, Matrix{NT}}, Vector{NT}, NT}
     info = Dict{Symbol, MyTypes}()
     V̂,  X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk])
@@ -156,7 +157,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     end
     info[:Ŵ] = estim.Ŵ[1:Nk*nŵ]
     info[:x̂arr] = x̂0arr + estim.x̂op
-    info[:ϵ]  = isinf(estim.C) ? NaN : estim.Z̃[begin]
+    info[:ϵ]  = nϵ ≠ 0 ? estim.Z̃[begin] : NaN
     info[:J]  = obj_nonlinprog!(x̄, estim, estim.model, V̂, estim.Z̃)
     info[:X̂]  = X̂0       .+ @views [estim.x̂op; estim.X̂op[1:nx̂*Nk]]
     info[:x̂]  = estim.x̂0 .+ estim.x̂op
@@ -243,8 +244,7 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are:
 """
 function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     F, C, optim = estim.F, estim.C, estim.optim
-    nϵ = isinf(C) ? 0 : 1
-    nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[]
+    nx̂, nŵ, nym, nϵ, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nϵ, estim.Nk[]
     nYm, nŴ = nym*Nk, nŵ*Nk
     nZ̃ = nϵ + nx̂ + nŴ
     # --- update F and fx̄ vectors for MHE predictions ---
@@ -388,13 +388,13 @@ The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This me
 `x̄` vector arguments.
 """
 function obj_nonlinprog!(x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃) 
-    Nk, nϵ = estim.Nk[], !isinf(estim.C)
+    nϵ, Nk = estim.nϵ, estim.Nk[] 
     nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄
     nx̃ = nϵ + nx̂
     invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm]
     x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm]
     x̄ .= estim.x̂0arr_old .- x̂0arr
-    Jϵ = nϵ ? estim.C*Z̃[begin]^2 : 0
+    Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : 0
     return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ
 end
 
@@ -408,7 +408,7 @@ estimated sensor noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimate
 ``k-N_k+2`` to ``k+1``.
 """
 function predict!(V̂, X̂0, _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) 
-    Nk, nϵ = estim.Nk[], !isinf(estim.C)
+    nϵ, Nk = estim.nϵ, estim.Nk[]
     nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk
     nZ̃ = nϵ + estim.nx̂ + nŴ
     V̂[1:nYm] .= @views estim.Ẽ[1:nYm, 1:nZ̃]*Z̃[1:nZ̃]     + estim.F[1:nYm]
@@ -418,9 +418,9 @@ end
 
 "Compute the two vectors when `model` is not a `LinModel`."
 function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃)
-    Nk = estim.Nk[]
+    nϵ, Nk = estim.nϵ, estim.Nk[]
     nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym
-    nx̃ = !isinf(estim.C) + nx̂
+    nx̃ = nϵ + nx̂
     x̂0 = @views Z̃[nx̃-nx̂+1:nx̃]
     for j=1:Nk
         u0  = @views estim.U0[ (1 + nu  * (j-1)):(nu*j)]
@@ -446,7 +446,7 @@ Nonlinear constrains for [`MovingHorizonEstimator`](@ref).
 function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, Z̃)
     nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[]
     nV̂con, nV̂ = length(estim.con.V̂min),  estim.nym*estim.Nk[]
-    ϵ = isinf(estim.C) ? 0 : Z̃[begin] # ϵ = 0 if Cwt=Inf (meaning: no relaxation)
+    ϵ = estim.nϵ ≠ 0 ? Z̃[begin] : 0 # ϵ = 0 if Cwt=Inf (meaning: no relaxation)
     for i in eachindex(g)
         estim.con.i_g[i] || continue
         if i ≤ nX̂con
@@ -476,8 +476,7 @@ function setmodel_estimator!(
     estim::MovingHorizonEstimator, model, uop_old, yop_old, dop_old, Q̂, R̂
 )
     con = estim.con
-    nx̂, nym, nu, nd, He = estim.nx̂, estim.nym, model.nu, model.nd, estim.He
-    nϵ = isinf(estim.C) ? 0 : 1
+    nx̂, nym, nu, nd, He, nϵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.He, estim.nϵ
     As, Cs_u, Cs_y = estim.As, estim.Cs_u, estim.Cs_y
     Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y, verify_obsv=false)
     # --- update augmented state-space matrices ---
@@ -496,8 +495,8 @@ function setmodel_estimator!(
     E, G, J, B, _ , Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe(
         model, He, estim.i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
     )
-    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, estim.C, con.C_x̂min, con.C_x̂max, Ex̂)   
-    A_V̂min, A_V̂max, Ẽ  = relaxV̂(model, estim.C, con.C_v̂min, con.C_v̂max, E) 
+    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nϵ, con.C_x̂min, con.C_x̂max, Ex̂)   
+    A_V̂min, A_V̂max, Ẽ  = relaxV̂(model, nϵ, con.C_v̂min, con.C_v̂max, E) 
     estim.Ẽ .= Ẽ
     estim.G .= G
     estim.J .= J
