doc: MHE constraint details

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -14,9 +14,9 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 ```@docs
 ModelPredictiveControl.init_predmat
 ModelPredictiveControl.init_ΔUtoU
-ModelPredictiveControl.init_quadprog_mpc
+ModelPredictiveControl.init_quadprog
 ModelPredictiveControl.init_stochpred
-ModelPredictiveControl.init_matconstraint
+ModelPredictiveControl.init_matconstraint_mpc
 ```
 
 ## Constraint Relaxation

docs/src/internals/sim_model.md

@@ -4,15 +4,15 @@
 Pages = ["sim_model.md"]
 ```
 
-## Steady-State Calculation
+## State-Space Functions
 
 ```@docs
-ModelPredictiveControl.steadystate!
+ModelPredictiveControl.f
+ModelPredictiveControl.h
 ```
 
-## State-Space Functions
+## Steady-State Calculation
 
 ```@docs
-ModelPredictiveControl.f
-ModelPredictiveControl.h
+ModelPredictiveControl.steadystate!
 ```

docs/src/internals/state_estim.md

@@ -4,6 +4,13 @@
 Pages = ["state_estim.md"]
 ```
 
+## Augmented Model
+
+```@docs
+ModelPredictiveControl.f̂
+ModelPredictiveControl.ĥ
+```
+
 ## Estimator Construction
 
 ```@docs
@@ -12,13 +19,8 @@ ModelPredictiveControl.init_integrators
 ModelPredictiveControl.augment_model
 ModelPredictiveControl.init_ukf
 ModelPredictiveControl.init_internalmodel
-```
-
-## Augmented Model
-
-```@docs
-ModelPredictiveControl.f̂
-ModelPredictiveControl.ĥ
+ModelPredictiveControl.init_predmat_mhe
+ModelPredictiveControl.init_matconstraint_mhe
 ```
 
 ## Evaluate Estimated Output

src/controller/explicitmpc.jl

@@ -47,7 +47,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, F, G, J, K, V = init_predmat(estim, model, Hp, Hc)
         S̃, Ñ_Hc, Ẽ  = S, N_Hc, E # no slack variable ϵ for ExplicitMPC
-        H̃, q̃, p = init_quadprog_mpc(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        H̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         H̃_chol = cholesky(H̃)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):

src/controller/linmpc.jl

@@ -55,7 +55,7 @@ struct LinMPC{
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
         con, S̃, Ñ_Hc, Ẽ = init_defaultcon_mpc(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
-        H̃, q̃, p = init_quadprog_mpc(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        H̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)

src/controller/nonlinmpc.jl

@@ -56,7 +56,7 @@ struct NonLinMPC{
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
         con, S̃, Ñ_Hc, Ẽ = init_defaultcon_mpc(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
-        H̃, q̃, p = init_quadprog_mpc(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        H̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)

src/estimator/mhe.jl

@@ -618,16 +618,20 @@ end
 
 Set the constraint parameters of `estim` [`MovingHorizonEstimator`](@ref).
    
-The constraints of the moving horizon estimator are defined as:
+The moving horizon estimator supports constraints on the estimated states ``\mathbf{x̂}``,
+process noise ``\mathbf{ŵ}`` and sensor noise ``\mathbf{v̂}``:
 ```math 
 \begin{alignat*}{3}
     \mathbf{x̂_{min}} ≤&&\   \mathbf{x̂}_k(k-j+1) &≤ \mathbf{x̂_{max}}  &&\qquad  j = N_k, N_k - 1, ... , 0    \\
     \mathbf{ŵ_{min}} ≤&&\     \mathbf{ŵ}(k-j+1) &≤ \mathbf{ŵ_{max}}  &&\qquad  j = N_k, N_k - 1, ... , 1    \\
     \mathbf{v̂_{min}} ≤&&\     \mathbf{v̂}(k-j+1) &≤ \mathbf{v̂_{max}}  &&\qquad  j = N_k, N_k - 1, ... , 1
 \end{alignat*}
 ```
-Note that state constraints are applied on the augmented state vector ``\mathbf{x̂}`` (see
-the extended help of [`SteadyKalmanFilter`](@ref) for details on augmentation).
+Note that state and process noise constraints are applied on augmented model vectors (see
+the extended help of [`SteadyKalmanFilter`](@ref) for details on augmentation). Also, 
+constraining the estimated sensor noises is equivalent to constraining the innovation term
+since ``\mathbf{v̂}(k) = \mathbf{y^m}(k) - \mathbf{ŷ^m}(k)`` in the MHE. See Extended Help
+for time-varying constraints.
 
 # Arguments
 !!! info
@@ -637,7 +641,11 @@ the extended help of [`SteadyKalmanFilter`](@ref) for details on augmentation).
 - `estim::MovingHorizonEstimator` : moving horizon estimator to set constraints.
 - `x̂min = fill(-Inf,nx̂)` : augmented state vector lower bounds ``\mathbf{x̂_{min}}``.
 - `x̂max = fill(+Inf,nx̂)` : augmented state vector upper bounds ``\mathbf{x̂_{max}}``.
-- all the keyword arguments above but with a capital letter, e.g. `X̂max` or `X̂min` : for
+- `ŵmin = fill(-Inf,nx̂)` : augmented process noise vector lower bounds ``\mathbf{ŵ_{min}}``.
+- `ŵmax = fill(+Inf,nx̂)` : augmented process noise vector upper bounds ``\mathbf{ŵ_{max}}``.
+- `v̂min = fill(-Inf,nym)` : sensor noise vector lower bounds ``\mathbf{v̂_{min}}``.
+- `v̂max = fill(+Inf,nym)` : sensor noise vector upper bounds ``\mathbf{v̂_{max}}``.
+- all the keyword arguments above but with a capital letter, e.g. `X̂max` or `V̂max` : for
   time-varying constraints (see Extended Help).
 
 # Examples
@@ -653,6 +661,23 @@ MovingHorizonEstimator estimator with a sample time Ts = 1.0 s, LinModel and:
  0 unmeasured outputs yu
  0 measured disturbances d
 ```
+
+# Extended Help
+
+For variable constraints, the bounds can be modified after calling [`updatestate!`](@ref),
+that is, at runtime, except for `±Inf` bounds. It is also possible to specify time-varying
+constraints over the horizon. In such a case, they are defined by:
+```math 
+\begin{alignat*}{3}
+    \mathbf{X̂_{min}} ≤&&\ \mathbf{X̂}  &≤ \mathbf{X̂_{max}} \\
+    \mathbf{Ŵ_{min}} ≤&&\ \mathbf{Ŵ}  &≤ \mathbf{Ŵ_{max}} \\
+    \mathbf{V̂_{min}} ≤&&\ \mathbf{V̂}  &≤ \mathbf{V̂_{max}}
+\end{alignat*}
+```
+For this, use the same keyword arguments as above but with a capital letter:
+- `X̂min` / `X̂max` : ``\mathbf{X̂}`` constraints `(nx̂*(He+1),)`.
+- `Ŵmin` / `Ŵmax` : ``\mathbf{Ŵ}`` constraints `(nx̂*He,)`.
+- `V̂min` / `V̂max` : ``\mathbf{V̂}`` constraints `(nym*He,)`.
 """
 function setconstraint!(
     estim::MovingHorizonEstimator; 

src/predictive_control.jl

@@ -299,7 +299,7 @@ function setconstraint!(
     i_Ymin,  i_Ymax  = .!isinf.(con.Ymin),  .!isinf.(con.Ymax)
     i_x̂min,  i_x̂max  = .!isinf.(con.x̂min),  .!isinf.(con.x̂max)
     if notSolvedYet
-        con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint(model,
+        con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mpc(model,
             i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, 
             i_Ymin, i_Ymax, i_x̂min, i_x̂max,
             con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, 
@@ -313,7 +313,7 @@ function setconstraint!(
         @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
         setnonlincon!(mpc, model)
     else
-        i_b, i_g = init_matconstraint(model, 
+        i_b, i_g = init_matconstraint_mpc(model, 
             i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, 
             i_Ymin, i_Ymax, i_x̂min, i_x̂max
         )
@@ -545,7 +545,7 @@ end
 
 Init linear model prediction matrices `F, q̃, p` and current estimated output `ŷ`.
 
-See [`init_predmat`](@ref) and [`init_quadprog_mpc`](@ref) for the definition of the matrices.
+See [`init_predmat`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
 """
 function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y, R̂u)
     mpc.ŷ[:] = evalŷ(mpc.estim, ym, d)
@@ -908,7 +908,7 @@ function init_predmat(estim::StateEstimator{NT}, model::SimModel, Hp, Hc) where
 end
 
 @doc raw"""
-    init_quadprog_mpc(model::LinModel, Ẽ, S, M_Hp, N_Hc, L_Hp) -> H̃, q̃, p
+    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.
 
@@ -921,14 +921,14 @@ vector ``\mathbf{q̃}`` and scalar ``p`` need recalculation each control period
 [`initpred!`](@ref) method). ``p`` does not impact the minima position. It is thus 
 useless at optimization but required to evaluate the minimal ``J`` value.
 """
-function init_quadprog_mpc(::LinModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real}
+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)
     q̃ = zeros(NT, size(H̃, 1))   # dummy value (updated just before optimization)
     p = zeros(NT, 1)            # dummy value (updated just before optimization)
     return H̃, q̃, p
 end
 "Return empty matrices if `model` is not a [`LinModel`](@ref)."
-function init_quadprog_mpc(::SimModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real}
+function init_quadprog(::SimModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real}
     H̃ = Hermitian(zeros(NT, 0, 0))
     q̃ = zeros(NT, 0)
     p = zeros(NT, 1)            # dummy value (updated just before optimization)
@@ -1026,7 +1026,7 @@ function init_defaultcon_mpc(
     i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(Ymin),  .!isinf.(Ymax)
     i_x̂min,  i_x̂max  = .!isinf.(x̂min),  .!isinf.(x̂max)
-    i_b, i_g, A = init_matconstraint(
+    i_b, i_g, A = init_matconstraint_mpc(
         model, 
         i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max,
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min
@@ -1246,7 +1246,7 @@ function init_stochpred(estim::StateEstimator{NT}, _ ) where NT<:Real
 end
 
 @doc raw"""
-    init_matconstraint(model::LinModel,
+    init_matconstraint_mpc(model::LinModel,
         i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args...
     ) -> i_b, i_g, A
 
@@ -1265,7 +1265,7 @@ The linear and nonlinear inequality constraints are respectively defined as:
 provided. In such a case, `args`  needs to contain all the inequality constraint matrices: 
 `A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max`.
 """
-function init_matconstraint(::LinModel{NT}, 
+function init_matconstraint_mpc(::LinModel{NT}, 
     i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args...
 ) where {NT<:Real}
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max]
@@ -1280,7 +1280,7 @@ function init_matconstraint(::LinModel{NT},
 end
 
 "Init `i_b, A` without outputs and terminal constraints if `model` is not a [`LinModel`](@ref)."
-function init_matconstraint(::SimModel{NT},
+function init_matconstraint_mpc(::SimModel{NT},
     i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args...
 ) where {NT<:Real}
     i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax]