Merge pull request #335 from JuliaControl/colloc_lin_defect

added: stochastic state defects as linear equality constraints for all `CollocationMethod`s

Author: franckgaga

docs/src/internals/state_estim.md

@@ -27,7 +27,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre
 ```@docs
 ModelPredictiveControl.f̂!
 ModelPredictiveControl.ĥ!
-ModelPredictiveControl.f̂_input!
 ```
 
 ## Update Quadratic Optimization

src/controller/construct.jl

@@ -133,13 +133,13 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     vx̂      ::Matrix{NT}
     bx̂      ::Vector{NT}
     # matrices for the zero defect constraints (N/A for single shooting transcriptions):
-    Ẽŝ      ::Matrix{NT}
-    Fŝ      ::Vector{NT}
-    Gŝ      ::Matrix{NT}
-    Jŝ      ::Matrix{NT}
-    Kŝ      ::Matrix{NT}
-    Vŝ      ::Matrix{NT}
-    Bŝ      ::Vector{NT}
+    ẼS      ::Matrix{NT}
+    FS      ::Vector{NT}
+    GS      ::Matrix{NT}
+    JS      ::Matrix{NT}
+    KS      ::Matrix{NT}
+    VS      ::Matrix{NT}
+    BS      ::Vector{NT}
     # custom linear equality constraints:
     Ẽw      ::Matrix{NT}
     Fw      ::Vector{NT}
@@ -176,7 +176,6 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     # indices of finite numbers in the b vector (linear inequality constraints):
     i_b     ::BitVector
     # A matrices for the linear equality constraints:
-    A_ŝ     ::Matrix{NT}
     Aeq     ::Matrix{NT}
     # b vector for the linear equality constraints:
     beq     ::Vector{NT}
@@ -518,7 +517,7 @@ function setconstraint!(
             con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, 
             con.A_Ymin, con.A_Ymax, con.A_Wmin, con.A_Wmax,
             con.A_x̂min, con.A_x̂max,
-            con.A_ŝ
+            con.Aeq
         )
         A = con.A[con.i_b, :]
         b = con.b[con.i_b]
@@ -858,7 +857,7 @@ verify_cond(::TranscriptionMethod,_,_) = nothing
         Hp, Hc, 
         PΔu, Pu, E, 
         ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
-        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
+        ES, GS, JS, KS, VS, BS,
         Wy, Wu, Wd, Wr,
         gc!=nothing, nc=0
     ) -> con, nϵ, P̃Δu, P̃u, Ẽ
@@ -874,14 +873,15 @@ function init_defaultcon_mpc(
     Hp,  Hc, 
     PΔu, Pu, E, 
     ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
-    Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
+    ES, GS, JS, KS, VS, BS,
     Wy, Wu, Wd, Wr,
     gc!::GCfunc = nothing, nc = 0
 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}}
     model = estim.model
     nu, ny, nx̂ = model.nu, model.ny, estim.nx̂
     nw = size(Wy, 1)
     nW = nw*(Hp+1)
+    nS = size(ES, 1)
     nϵ = weights.isinf_C ? 0 : 1
     u0min,      u0max   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     Δumin,      Δumax   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
@@ -910,7 +910,7 @@ function init_defaultcon_mpc(
     A_Ymin,  A_Ymax, Ẽ  = relaxŶ(E, C_ymin, C_ymax, nϵ)
     A_Wmin,  A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ)
     A_x̂min,  A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ)
-    A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ)
+    Aeq, ẼS = augmentdefect(ES, nϵ)
     i_Umin,  i_Umax  = .!isinf.(U0min), .!isinf.(U0max)
     i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(Y0min), .!isinf.(Y0max)
@@ -920,22 +920,21 @@ function init_defaultcon_mpc(
         model, transcription, nc,
         i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max,
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min,
-        A_ŝ
+        Aeq
     )
-    # dummy fx̂, Fw and Fŝ vectors (updated just before optimization)
-    fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp)
+    # dummy fx̂, Fw and FS vectors (updated just before optimization)
+    fx̂, Fw, FS = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nS)
     # dummy b and beq vectors (updated just before optimization)
     b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1))
     con = ControllerConstraint{NT, GCfunc}(
         ẽx̂      , fx̂     , gx̂      , jx̂       , kx̂     , vx̂     , bx̂     ,
-        Ẽŝ      , Fŝ     , Gŝ      , Jŝ       , Kŝ     , Vŝ     , Bŝ     ,
+        ẼS      , FS     , GS      , JS       , KS     , VS     , BS     ,
         Ẽw      , Fw     , W̄y      , W̄u       , W̄d     , W̄r     , nw     ,
         U0min   , U0max  , ΔŨmin   , ΔŨmax    , 
         Y0min   , Y0max  , Wmin    , Wmax     , x̂0min  , x̂0max  , 
         A_Umin  , A_Umax , A_ΔŨmin , A_ΔŨmax  , 
         A_Ymin  , A_Ymax , A_Wmin  , A_Wmax   , A_x̂min , A_x̂max , 
         A       , b      , i_b     , 
-        A_ŝ     ,
         Aeq     , beq    ,
         neq     ,
         C_ymin  , C_ymax , C_wmin  , C_wmax   , c_x̂min , c_x̂max , 
@@ -1194,24 +1193,24 @@ function relaxterminal(ex̂::AbstractMatrix{NT}, c_x̂min, c_x̂max, nϵ) where
 end
 
 @doc raw"""
-    augmentdefect(Eŝ, nϵ) -> A_ŝ, Ẽŝ
+    augmentdefect(ES, nϵ) -> Aeq, ẼS
 
 Augment defect equality constraints with slack variable ϵ if `nϵ == 1`.
 
-It returns the ``\mathbf{Ẽ_ŝ}`` matrix that appears in the defect equation 
-``\mathbf{Ŝ = Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints:
+It returns the ``\mathbf{Ẽ_S}`` matrix that appears in the linear defect equation 
+``\mathbf{Ẽ_S Z̃ + F_S}`` and the ``\mathbf{A}`` matrix for the equality constraints:
 ```math
-\mathbf{A_ŝ Z̃} = - \mathbf{F_ŝ}
+\mathbf{A_{eq} Z̃} = \mathbf{b_{eq}} = - \mathbf{F_S}
 ```
 """
-function augmentdefect(Eŝ::AbstractMatrix{NT}, nϵ) where NT<:Real
+function augmentdefect(ES::AbstractMatrix{NT}, nϵ) where NT<:Real
     if nϵ == 1 # Z̃ = [Z; ϵ]
-        Ẽŝ = [Eŝ zeros(NT, size(Eŝ, 1), 1)]
+        ẼS = [ES zeros(NT, size(ES, 1), 1)]
     else # Z̃ = Z (only hard constraints)
-        Ẽŝ = Eŝ
+        ẼS = ES
     end
-    A_ŝ = Ẽŝ
-    return A_ŝ, Ẽŝ
+    Aeq = ẼS
+    return Aeq, ẼS
 end
 
 @doc raw"""

src/controller/execute.jl

@@ -74,7 +74,7 @@ function moveinput!(
     validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u)
     initpred!(mpc, mpc.estim.model, ry, d, lastu, D̂, R̂y, R̂u)
     linconstraint!(mpc, mpc.estim.model, mpc.transcription)
-    linconstrainteq!(mpc, mpc.estim.model, mpc.transcription)
+    linconstrainteq!(mpc, mpc.estim.model, mpc.estim, mpc.transcription)
     Z̃ = optim_objective!(mpc)
     return getinput!(mpc, Z̃)
 end
@@ -312,7 +312,36 @@ function predictstoch!(Ŷs, mpc::PredictiveController, estim::InternalModel)
     return nothing
 end
 "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)."
-predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
+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 used 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)). It's also necessary to prefill
+the `Û0` vector before anything 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)
@@ -690,14 +719,14 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     con.vx̂ .= vx̂
     con.bx̂ .= bx̂
     # --- defect matrices ---
-    Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb)
-    A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ)
-    con.Ẽŝ .= Ẽŝ
-    con.Gŝ .= Gŝ
-    con.Jŝ .= Jŝ
-    con.Kŝ .= Kŝ
-    con.Vŝ .= Vŝ
-    con.Bŝ .= Bŝ
+    ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb)
+    Aeq, ẼS = augmentdefect(ES, mpc.nϵ)
+    con.ẼS .= ẼS
+    con.GS .= GS
+    con.JS .= JS
+    con.KS .= KS
+    con.VS .= VS
+    con.BS .= BS
     # --- custom linear constraints ---
     con.Ẽw .= Ẽw
     # --- linear inequality constraints ---
@@ -718,8 +747,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
         con.A_x̂max
     ]
     # --- linear equality constraints ---
-    con.A_ŝ .= A_ŝ
-    con.Aeq .= A_ŝ
+    con.Aeq .= Aeq
     # --- operating points ---
     con.U0min .+= mpc.Uop # convert U0 to U with the old operating point
     con.U0max .+= mpc.Uop # convert U0 to U with the old operating point

src/controller/explicitmpc.jl

@@ -191,7 +191,9 @@ function Base.show(io::IO, mpc::ExplicitMPC)
     print_estim_dim(io, mpc.estim, n)
 end
 
-linconstraint!(::ExplicitMPC, ::LinModel, ::TranscriptionMethod) = nothing
+linconstraint!(::ExplicitMPC, ::LinModel, ::SingleShooting) = nothing
+linconstrainteq!(::ExplicitMPC, ::LinModel, ::StateEstimator, ::SingleShooting) = nothing
+linconstrainteq!(::ExplicitMPC, ::LinModel, ::InternalModel,  ::SingleShooting) = nothing
 
 @doc raw"""
     optim_objective!(mpc::ExplicitMPC) -> Z̃

src/controller/linmpc.jl

@@ -73,13 +73,13 @@ struct LinMPC{
             model, estim, transcription, Hp, Hc, nb
         )
         F = zeros(NT, ny*Hp) # dummy value (updated just before optimization)
-        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb)
+        ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb)
         con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc(
             estim, weights, transcription,
             Hp, Hc, 
             PΔu, Pu, E, 
             ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
-            Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
+            ES, GS, JS, KS, VS, BS,
             Wy, Wu, Wd, Wr
         )
         H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u)

src/controller/nonlinmpc.jl

@@ -103,13 +103,13 @@ struct NonLinMPC{
             model, estim, transcription, Hp, Hc, nb
         )
         F = zeros(NT, ny*Hp) # dummy value (updated just before optimization)
-        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb)
+        ES, GS, JS, KS, VS, BS = init_defectmat(model, estim, transcription, Hp, Hc, nb)
         con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc(
             estim, weights, transcription,
             Hp, Hc, 
             PΔu, Pu, E, 
             ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
-            Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
+            ES, GS, JS, KS, VS, BS,
             Wy, Wu, Wd, Wr,
             gc!, nc
         )