doc: internals, quadprog update equations (MHE and MPC)

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -12,24 +12,20 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 ## Controller Construction
 
 ```@docs
-ModelPredictiveControl.init_predmat
 ModelPredictiveControl.init_ΔUtoU
-ModelPredictiveControl.init_quadprog
-ModelPredictiveControl.init_stochpred
-ModelPredictiveControl.init_matconstraint_mpc
-```
-
-## Constraint Relaxation
-
-```@docs
+ModelPredictiveControl.init_predmat
 ModelPredictiveControl.relaxU
 ModelPredictiveControl.relaxΔU
 ModelPredictiveControl.relaxŶ
 ModelPredictiveControl.relaxterminal
+ModelPredictiveControl.init_quadprog
+ModelPredictiveControl.init_stochpred
+ModelPredictiveControl.init_matconstraint_mpc
 ```
 
-## Constraints
+## Update Quadratic Optimization
 
 ```@docs
+ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any)
 ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel)
 ```

docs/src/internals/state_estim.md

@@ -11,6 +11,12 @@ ModelPredictiveControl.f̂
 ModelPredictiveControl.ĥ
 ```
 
+## Constraint Relaxation
+
+```@docs
+
+```
+
 ## Estimator Construction
 
 ```@docs
@@ -20,21 +26,17 @@ ModelPredictiveControl.augment_model
 ModelPredictiveControl.init_ukf
 ModelPredictiveControl.init_internalmodel
 ModelPredictiveControl.init_predmat_mhe
-ModelPredictiveControl.init_matconstraint_mhe
-```
-
-## Constraint Relaxation
-
-```@docs
 ModelPredictiveControl.relaxarrival
 ModelPredictiveControl.relaxX̂
 ModelPredictiveControl.relaxŴ
 ModelPredictiveControl.relaxV̂
+ModelPredictiveControl.init_matconstraint_mhe
 ```
 
-## Constraints
+## Update Quadratic Optimization
 
 ```@docs
+ModelPredictiveControl.initpred!(::MovingHorizonEstimator, ::LinModel)
 ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 ```
 

src/controller/construct.jl

@@ -523,16 +523,21 @@ end
 @doc raw"""
     init_quadprog(model::LinModel, Ẽ, S, M_Hp, N_Hc, L_Hp) -> H̃, q̃, p
 
-Init the quadratic programming optimization matrix `H̃` and `q̃` for MPC.
+Init the quadratic programming optimization matrix `H̃` and `q̃` and scalar `p` for MPC.
 
 The matrices appear in the quadratic general form :
 ```math
     J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + p 
 ```
-``\mathbf{H̃}`` is constant if the model and weights are linear and time invariant (LTI). The 
-vector ``\mathbf{q̃}`` and scalar ``p`` need recalculation each control period ``k``. ``p``
-does not impact the minima position. It is thus useless at optimization but required to 
-evaluate the minimal ``J`` value.
+The Hessian matrix is constant if the model and weights are linear and time invariant (LTI): 
+```math
+    \mathbf{H̃} = 2 (  \mathbf{Ẽ}'\mathbf{M}_{H_p}\mathbf{Ẽ} + \mathbf{Ñ}_{H_c} 
+                    + \mathbf{S̃}'\mathbf{L}_{H_p}\mathbf{S̃} )
+```
+The vector ``\mathbf{q̃}`` and scalar ``p`` need recalculation each control period ``k`` (init
+with zeros, the method [`initpred!`](@ref) compute the real values). ``p`` does not impact
+the minima position. It is thus useless at optimization but required to evaluate the minimal
+``J`` value.
 """
 function init_quadprog(::LinModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real}
     H̃ = Hermitian(convert(Matrix{NT}, 2*(Ẽ'*M_Hp*Ẽ + Ñ_Hc + S̃'*L_Hp*S̃)), :L)

src/controller/execute.jl

@@ -135,11 +135,21 @@ end
 
 
 @doc raw"""
-    initpred!(mpc, model::LinModel, d, ym, D̂, R̂y, R̂u)
+    initpred!(mpc, model::LinModel, d, ym, D̂, R̂y, R̂u) -> nothing
 
 Init linear model prediction matrices `F, q̃, p` and current estimated output `ŷ`.
 
 See [`init_predmat`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
+They are computed with:
+```math
+\begin{aligned}
+    \mathbf{F}   &= \mathbf{G d}(k) + \mathbf{J D̂} + \mathbf{K x̂}_{k-1}(k) + \mathbf{V u}(k-1) \\
+    \mathbf{C_y} &= \mathbf{F}                 - \mathbf{R̂_y} \\
+    \mathbf{C_u} &= \mathbf{T} \mathbf{u}(k-1) - \mathbf{R̂_u} \\
+    \mathbf{q̃}   &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\
+            p    &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u}
+\end{aligned}
+```
 """
 function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y, R̂u)
     mpc.ŷ[:] = evalŷ(mpc.estim, ym, d)
@@ -151,15 +161,15 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
         mpc.F[:]  = mpc.F  + mpc.G  * mpc.d0 + mpc.J  * mpc.D̂0
     end
     mpc.R̂y[:] = R̂y
-    Ẑ = mpc.F - mpc.R̂y
-    mpc.q̃[:] = 2(mpc.M_Hp*mpc.Ẽ)'*Ẑ
-    mpc.p[]  = dot(Ẑ, mpc.M_Hp, Ẑ)
+    C_y = mpc.F - mpc.R̂y
+    mpc.q̃[:] = 2(mpc.M_Hp*mpc.Ẽ)'*C_y
+    mpc.p[]  = dot(C_y, mpc.M_Hp, C_y)
     if ~mpc.noR̂u
         mpc.R̂u[:] = R̂u
         lastu = mpc.estim.lastu0 + model.uop
-        Ẑ = mpc.T*lastu - mpc.R̂u
-        mpc.q̃[:] = mpc.q̃ + 2(mpc.L_Hp*mpc.S̃)'*Ẑ
-        mpc.p[]  = mpc.p[] + dot(Ẑ, mpc.L_Hp, Ẑ)
+        C_u = mpc.T*lastu - mpc.R̂u
+        mpc.q̃[:] = mpc.q̃ + 2(mpc.L_Hp*mpc.S̃)'*C_u
+        mpc.p[]  = mpc.p[] + dot(C_y, mpc.L_Hp, C_u)
     end
     return nothing
 end
@@ -216,7 +226,8 @@ predictstoch!(::PredictiveController, ::StateEstimator, _ , _ ) = nothing
 
 Set `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
 
-Also init ``\mathbf{f_x̂}`` vector for the terminal constraints, see [`init_predmat`](@ref).
+Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d}(k) + \mathbf{j_x̂ D̂} + \mathbf{k_x̂ x̂}_{k-1}(k) + \mathbf{v_x̂ u}(k-1)``
+vector for the terminal constraints, see [`init_predmat`](@ref).
 """
 function linconstraint!(mpc::PredictiveController, model::LinModel)
     mpc.con.fx̂[:] = mpc.con.kx̂ * mpc.estim.x̂  + mpc.con.vx̂ * mpc.estim.lastu0

src/estimator/mhe/construct.jl

@@ -777,30 +777,29 @@ end
 
 Construct the MHE prediction matrices for [`LinModel`](@ref) `model`.
 
-Introducing the vector ``\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂}_k(k-H_e+1) 
+Introducing the vector ``\mathbf{Z} = [\begin{smallmatrix} \mathbf{x̂}_k(k-N_k+1) 
 \\ \mathbf{Ŵ} \end{smallmatrix}]`` with the decision variables, the estimated sensor
-noises from time ``k-H_e+1`` to ``k`` are computed by:
+noises from time ``k-N_k+1`` to ``k`` are computed by:
 ```math
 \begin{aligned}
 \mathbf{V̂} = \mathbf{Y^m - Ŷ^m} &= \mathbf{E Z + G U + J D + Y^m}     \\
                                 &= \mathbf{E Z + F}
 \end{aligned}
 ```
 in which ``\mathbf{U, D}`` and ``\mathbf{Y^m}`` contains respectively the manipulated
-inputs, measured disturbances and measured outputs from time ``k-H_e+1`` to ``k``. The
+inputs, measured disturbances and measured outputs from time ``k-N_k+1`` to ``k``. The
 method also returns similar matrices but for the estimation error at arrival:
 ```math
-\mathbf{x̄} = \mathbf{x̂}_{k-H_e}(k-H_e+1) - \mathbf{x̂}_{k}(k-H_e+1) = \mathbf{e_x̄ Z + f_x̄}
+\mathbf{x̄} = \mathbf{x̂}_{k-N_k}(k-N_k+1) - \mathbf{x̂}_{k}(k-N_k+1) = \mathbf{e_x̄ Z + f_x̄}
 ```
-Lastly, the estimated states from time ``k-H_e+2`` to ``k+1`` are given by the equation:
+Lastly, the estimated states from time ``k-N_k+2`` to ``k+1`` are given by the equation:
 ```math
 \begin{aligned}
 \mathbf{X̂}  &= \mathbf{E_x̂ Z + G_x̂ U + J_x̂ D} \\
             &= \mathbf{E_x̂ Z + F_x̂}
 \end{aligned}
 ```
-All these equations omit the operating points ``\mathbf{u_{op}, y_{op}, d_{op}}``. These
-matrices are truncated when ``N_k < H_e`` (at the beginning).
+All these equations omit the operating points ``\mathbf{u_{op}, y_{op}, d_{op}}``.
 
 # Extended Help
 !!! details "Extended Help"
@@ -850,6 +849,7 @@ matrices are truncated when ``N_k < H_e`` (at the beginning).
         \mathbf{Â}^{H_e-1}\mathbf{B̂_d}      & \mathbf{Â}^{H_e-2}\mathbf{B̂_d}    & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d}   \end{bmatrix}
     \end{aligned}
     ```
+    All these matrices are truncated when ``N_k < H_e`` (at the beginning).
 """
 function init_predmat_mhe(model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d) where {NT<:Real}
     nu, nd = model.nu, model.nd

src/estimator/mhe/execute.jl

@@ -183,14 +183,30 @@ function add_data_windows!(estim::MovingHorizonEstimator, u, d, ym)
 end
 
 @doc raw"""
-    initpred!(estim::MovingHorizonEstimator, model::LinModel)
+    initpred!(estim::MovingHorizonEstimator, model::LinModel) -> nothing
 
-Init linear model prediction matrices `F, fx̄, H̃, q̃, p` for [`MovingHorizonEstimator`](@ref).
+Init quadratic optimization matrices `F, fx̄, H̃, q̃, p` for [`MovingHorizonEstimator`](@ref).
 
-Also init `estim.optim` objective function. See [`init_predmat_mhe`](@ref) for the 
-definition of the matrices. The Hessian ``H̃`` matrix of the quadratic general form is not
-constant here because of the time-varying ``\mathbf{P̄}`` weight (the estimation error 
-covariance at arrival).
+See [`init_predmat_mhe`](@ref) for the definition of the vectors ``\mathbf{F, f_x̄}``. It
+also inits `estim.optim` objective function, expressed as the quadratic general form:
+```math
+    J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + p 
+```
+The Hessian ``\mathbf{H̃}`` matrix of the quadratic general form is not constant here because
+of the time-varying ``\mathbf{P̄}`` covariance . The matrices are computed with:
+```math
+\begin{aligned}
+    \mathbf{F}       &= \mathbf{G U} + \mathbf{J D} + \mathbf{Y^m} \\
+    \mathbf{f_x̄}     &= \mathbf{x̂}_{k-N_k}(k-N_k+1) \\
+    \mathbf{F_Z̃}     &= [\begin{smallmatrix}\mathbf{f_x̄} \\ \mathbf{F} \end{smallmatrix}] \\
+    \mathbf{Ẽ_Z̃}     &= [\begin{smallmatrix}\mathbf{ẽ_x̄} \\ \mathbf{Ẽ} \end{smallmatrix}] \\
+    \mathbf{M}_{N_k} &= \mathrm{diag}(\mathbf{P̄}^{-1}, \mathbf{R̂}_{N_k}^{-1}) \\
+    \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C,  \mathbf{0},  \mathbf{Q̂}_{N_k}^{-1}) \\
+    \mathbf{H̃}       &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k}) \\
+    \mathbf{q̃}       &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\
+            p        &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃}
+\end{aligned}
+```
 """
 function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     C, optim = estim.C, estim.optim
@@ -205,14 +221,14 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     end
     estim.fx̄[:] = estim.x̂arr_old
     # --- update H̃, q̃ and p vectors for quadratic optimization ---
-    Ẽ = @views [estim.ẽx̄[:, 1:nZ̃]; estim.Ẽ[1:nYm, 1:nZ̃]]
-    F = @views [estim.fx̄; estim.F[1:nYm]]
+    ẼZ̃ = @views [estim.ẽx̄[:, 1:nZ̃]; estim.Ẽ[1:nYm, 1:nZ̃]]
+    FZ̃ = @views [estim.fx̄; estim.F[1:nYm]]
     invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm]
-    M = [estim.invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk]
-    Ñ = [fill(C, nϵ, nϵ) zeros(nϵ, nx̂+nŴ); zeros(nx̂, nϵ+nx̂+nŴ); zeros(nŴ, nϵ+nx̂) invQ̂_Nk]
-    estim.q̃[1:nZ̃] = 2(M*Ẽ)'*F
-    estim.p[] = dot(F, M, F)
-    estim.H̃.data[1:nZ̃, 1:nZ̃] = 2*(Ẽ'*M*Ẽ + Ñ)
+    M_Nk = [estim.invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk]
+    Ñ_Nk = [fill(C, nϵ, nϵ) zeros(nϵ, nx̂+nŴ); zeros(nx̂, nϵ+nx̂+nŴ); zeros(nŴ, nϵ+nx̂) invQ̂_Nk]
+    estim.q̃[1:nZ̃] = 2(M_Nk*ẼZ̃)'*FZ̃
+    estim.p[] = dot(FZ̃, M_Nk, FZ̃)
+    estim.H̃.data[1:nZ̃, 1:nZ̃] = 2*(ẼZ̃'*M_Nk*ẼZ̃ + Ñ_Nk)
     Z̃var_Nk::Vector{VariableRef} = @views optim[:Z̃var][1:nZ̃]
     H̃_Nk = @views estim.H̃[1:nZ̃,1:nZ̃]
     q̃_Nk = @views estim.q̃[1:nZ̃]
@@ -227,7 +243,8 @@ initpred!(::MovingHorizonEstimator, ::SimModel) = nothing
 
 Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MHE.
 
-Also init ``\mathbf{F_x̂}`` vector for the state constraints, see [`init_predmat_mhe`](@ref).
+Also init ``\mathbf{F_x̂ = G_x̂ U + J_x̂ D}`` vector for the state constraints, see 
+[`init_predmat_mhe`](@ref).
 """
 function linconstraint!(estim::MovingHorizonEstimator, model::LinModel)
     estim.con.Fx̂[:] = estim.con.Gx̂*estim.U