added: stochastic defects treated as linear equality constraints in MovingHorizonEstimator

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.9.3"
+version = "2.10.0"
 authors = ["Francis Gagnon"]
 
 [deps]

src/controller/execute.jl

@@ -326,38 +326,6 @@ end
 "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)."
 predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
 
-"""
-    disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) -> nothing
-
-Fill disturbed inputs of the augmented model `Û0` in-place with stochastic states in `X̂0`
-
-Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+H_p-1``. The
-predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+H_p-1`` (the current one
-is stored in `estim.x̂0`).
-
-This function is needed for the collocation methods that directly call the state derivative 
-function `estim.model.f!` with the manipulated inputs augmented with the estimated 
-disturbances at model input (see [`init_estimstoch`](@ref)). This is also needed for
-[`MultipleShooting`](@ref) since it calls the discrete-time update function of the
-deterministic model [`f!`](@ref) to treat the stochastic defects as linear equality
-constraints. Lastly, it's also necessary to prefill the `Û0` vector before anything else
-since both `û0` and `û0next` are needed at each stage with hold order `h>0`, thus potential
-race conditions with multi-threading.
-"""
-function disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0)
-    nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂
-    Cs_u = estim.Cs_u
-    Û0 .= U0                            
-    for j=0:mpc.Hp-1
-        xs = @views j < 1 ? estim.x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] 
-        û0 = @views Û0[(1+nu*j):(nu*(j+1))]
-        mul!(û0, Cs_u, xs, 1, 1)                    # û0 = u0 + Cs_u*xs               
-    end
-    return nothing
-end
-"No input disturbances for [`InternalModel`](@ref), hence do `Û0 .= U0`."
-disturbedinput!(Û0, ::PredictiveController, ::InternalModel, U0, _) = (Û0 .= U0; nothing)
-
 @doc raw"""
     linconstraint_custom!(mpc::PredictiveController, model::SimModel)
 

src/controller/transcription.jl

@@ -1347,7 +1347,7 @@ function con_nonlinprogeq!(
     f_threads = transcription.f_threads
     D̂0 = mpc.D̂0
     X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)]
-    disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃)
+    Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0)
     @threadsif f_threads for j=1:Hp
         if j < 2
             x̂d_Z̃ = @views mpc.estim.x̂0[1:nx]
@@ -1406,7 +1406,7 @@ function con_nonlinprogeq!(
     nk = get_nk_mpc(model, transcription)
     D̂0 = mpc.D̂0
     X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)]
-    disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃)
+    Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0)
     @threadsif f_threads for j=1:Hp
         if j < 2
             x̂d_Z̃ = @views mpc.estim.x̂0[1:nx]
@@ -1502,7 +1502,7 @@ function con_nonlinprogeq!(
     D̂0 = mpc.D̂0
     X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)]
     D̂temp = mpc.buffer.D̂
-    disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃)
+    Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0)
     @threadsif f_threads for j=1:Hp
         if j < 2
             x̂d_Z̃ = @views mpc.estim.x̂0[1:nx]

src/estimator/execute.jl

@@ -141,6 +141,37 @@ function ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0)
     return nothing
 end
 
+"""
+    disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0) -> Û0
+
+Fill disturbed inputs of the augmented model `Û0` in-place with the stochastic states in `X̂0`.
+
+Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+N-1``. The
+predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+N``, and `x̂0` is the
+value at ``k``.
+
+This function is needed for the collocation methods that directly call the state derivative 
+function `estim.model.f!` with the manipulated inputs augmented with the estimated 
+disturbances at model input (see [`init_estimstoch`](@ref)). This is also needed for
+[`MultipleShooting`](@ref) since it calls the discrete-time update function of the
+deterministic model [`f!`](@ref) to treat the stochastic defects as linear equality
+constraints. Lastly, it's also necessary to prefill the `Û0` vector before anything else
+since both `û0` and `û0next` are needed at each stage with hold order `h>0`, thus potential
+race conditions with multi-threading.
+"""
+function disturbedinput!(Û0, estim::StateEstimator, x̂0, X̂0, U0)
+    nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂
+    N = length(Û0) ÷ nu
+    Cs_u = estim.Cs_u
+    Û0 .= U0                            
+    for j=0:N-1
+        xs = @views j < 1 ? x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] 
+        û0 = @views Û0[(1+nu*j):(nu*(j+1))]
+        mul!(û0, Cs_u, xs, 1, 1)                    # û0 = u0 + Cs_u*xs               
+    end
+    return Û0
+end
+
 @doc raw"""
     initstate!(estim::StateEstimator, u, ym, d=[]) -> x̂
 

src/estimator/internal_model.jl

@@ -191,6 +191,8 @@ function ĥ!(ŷ0, ::InternalModel, model::NonLinModel, x̂0, d0)
     return h!(ŷ0, model, x̂0, d0, model.p)
 end
 
+"No input disturbances for [`InternalModel`](@ref), return `Û0 .= U0`."
+disturbedinput!(Û0, ::InternalModel, _ , _ , U0) = (Û0 .= U0)
 
 @doc raw"""
     init_internalmodel(As, Bs, Cs, Ds) -> Âs, B̂s

src/estimator/mhe/construct.jl

@@ -183,7 +183,7 @@ struct MovingHorizonEstimator{
             model, transcription, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
         )
         ES, GS, JS, BS = init_defectmat_mhe(
-            model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, direct
+            model, transcription, He, Â, B̂u, B̂d, x̂op, f̂op, As, direct
         ) 
         # dummy values (updated just before optimization):
         F, fx̄ = zeros(NT, nym*He), zeros(NT, nx̂)

src/estimator/mhe/execute.jl

@@ -17,6 +17,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0)
     estim.q̃         .= 0
     estim.r         .= 0
     estim.con.FX̂    .= 0
+    estim.con.FS    .= 0
     if estim.direct
         # add y0m(-1) to the extended data window (custom NL constraints):
         estim.Yem[1:ny] .= y0m .+ @views yop[estim.i_ym]
@@ -422,7 +423,7 @@ function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT<
     nYm, nZ = estim.nym*Nk, get_nZ_mhe(estim.transcription, Nk, nx̂, nŵ)
     # --- truncate vectors and matrices if Nk < He ---
     U0, D0, Y0m = trunc_windows(estim)
-    Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var = trunc_predmat(estim, estim.transcription)
+    Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var = trunc_predmat(estim)
     invQ̂_Nk = trunc_cov(invQ̂_He, nx̂, Nk, estim.He)
     invR̂_Nk = trunc_cov(invR̂_He, nym, Nk, estim.He)
     # --- update F and fx̄ vectors for MHE predictions ---
@@ -652,6 +653,55 @@ function trunc_windows(estim::MovingHorizonEstimator)
     return U0, D0, Y0m
 end
 
+"Truncate or extract subparts of the prediction matrices if `Nk < He`"
+function trunc_predmat(estim::MovingHorizonEstimator)
+    model, transcription = estim.model, estim.transcription
+    nŵ, nym, nε, Nk = estim.nx̂, estim.nym, estim.nε, estim.Nk[]
+    nU, nYm, nŴ, nD = model.nu*Nk, nym*Nk, nŵ*Nk, model.nd*(Nk+1)
+    if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul!
+        i_Z̃_Nk = get_i_Z̃_Nk(estim, transcription)
+        i_Z_Nk  = nε < 1 ? i_Z̃_Nk : i_Z̃_Nk[begin+1:end] .- 1
+        Ẽ       = estim.Ẽ[1:nYm, i_Z̃_Nk]
+        G, J, B = estim.G[1:nYm, 1:nU], estim.J[1:nYm, 1:nD], estim.B[1:nYm]
+        ẽx̄      = estim.ẽx̄[:, i_Z̃_Nk]
+        Tŵ      = estim.Tŵ[1:nŴ, i_Z_Nk]
+        F       = @views estim.F[1:nYm] # views here since they will store results
+        H̃_data  = @views estim.H̃.data[i_Z̃_Nk, i_Z̃_Nk]
+        H̃       = @views estim.H̃[i_Z̃_Nk, i_Z̃_Nk]
+        q̃       = @views estim.q̃[i_Z̃_Nk]
+        Z̃var    = @views estim.optim[:Z̃var][i_Z̃_Nk]
+    else
+        Ẽ, F, G, J, B = estim.Ẽ, estim.F, estim.G, estim.J, estim.B
+        ẽx̄, Tŵ        = estim.ẽx̄, estim.Tŵ
+        H̃, H̃_data, q̃  = estim.H̃, estim.H̃.data, estim.q̃
+        Z̃var          = estim.optim[:Z̃var]
+    end
+    return Ẽ, F, G, J, B, ẽx̄, Tŵ, H̃, H̃_data, q̃, Z̃var
+end
+
+"Extract subparts of the defect matrices if `Nk < He`"
+function trunc_defectmat(estim::MovingHorizonEstimator)
+    model, transcription, con = estim.model, estim.transcription, estim.con
+    FS = con.FS
+    nx̂, Nk = estim.nx̂, estim.Nk[]
+    nU, nX̂, nD = model.nu*Nk, nx̂*Nk, model.nd*(Nk+1)
+    if Nk < estim.He # avoid views since allocations only when Nk < He and we want fast mul!
+        i_Z̃_Nk     = get_i_Z̃_Nk(estim, transcription)
+        ẼS         = con.ẼS[1:nX̂, i_Z̃_Nk]
+        GS, JS, BS = con.GS[1:nX̂, 1:nU], con.JS[1:nX̂, 1:nD], con.BS[1:nX̂]
+        FS         = @views con.FS[1:nX̂] # views here since they will store results
+        Aeq        = @views con.Aeq[1:nX̂, i_Z̃_Nk]
+        beq        = @views con.beq[1:nX̂]
+        Z̃var       = @views estim.optim[:Z̃var][i_Z̃_Nk]
+    else
+        ẼS, FS, GS, JS, BS = con.ẼS, con.FS, con.GS, con.JS, con.BS
+        Aeq  = con.Aeq
+        beq  = con.beq
+        Z̃var = estim.optim[:Z̃var]
+    end
+    return ẼS, FS, GS, JS, BS, Aeq, beq, Z̃var
+end
+
 "Truncate the inverse covariance `invA_He` to the window size `Nk` if `Nk < He`."
 function trunc_cov(invA_He::Hermitian{<:Real, <:AbstractMatrix}, n, Nk, He) 
     if Nk < He
@@ -931,8 +981,8 @@ function setmodel_estimator!(
     con.BX̂ .= BX̂
     # --- defect matrices ---
     ES, GS, JS, BS = init_defectmat_mhe(
-        model, transcription, He, estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, 
-        estim.direct
+        model, transcription, He, 
+        estim.Â, estim.B̂u, estim.B̂d, estim.x̂op, estim.f̂op, estim.As, estim.direct
     )
     Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true)
     con.ẼS .= ẼS