Merge pull request #20 from franckgaga/mhe_linmodel

Added: MHE with quadratic optim + process and sensor noises bounds

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -16,7 +16,7 @@ ModelPredictiveControl.init_predmat
 ModelPredictiveControl.init_ΔUtoU
 ModelPredictiveControl.init_quadprog
 ModelPredictiveControl.init_stochpred
-ModelPredictiveControl.init_matconstraint
+ModelPredictiveControl.init_matconstraint_mpc
 ```
 
 ## Constraint Relaxation
@@ -28,12 +28,6 @@ ModelPredictiveControl.relaxŶ
 ModelPredictiveControl.relaxterminal
 ```
 
-## Predictions
-
-```@docs
-ModelPredictiveControl.initpred!
-```
-
 ## Constraints
 
 ```@docs

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/ModelPredictiveControl.jl

@@ -27,6 +27,9 @@ const FATAL_STATUSES = [
     OTHER_ERROR
 ]
 
+"Evaluate the quadratic programming objective function `0.5x'*H*x + q'*x` at `x`."
+obj_quadprog(x, H, q) = 0.5*dot(x, H, x) + q'*x  # dot(x, H, x) is faster than x'*H*x
+
 "Generate a block diagonal matrix repeating `n` times the matrix `A`."
 repeatdiag(A, n::Int) = kron(I(n), A)
 

src/controller/explicitmpc.jl

@@ -20,10 +20,10 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     J::Matrix{NT}
     K::Matrix{NT}
     V::Matrix{NT}
-    P̃::Hermitian{NT, Matrix{NT}}
+    H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
     p::Vector{NT}
-    P̃_chol::Cholesky{NT, Matrix{NT}}
+    H̃_chol::Cholesky{NT, Matrix{NT}}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
     d0::Vector{NT}
@@ -47,8 +47,8 @@ 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
-        P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
-        P̃_chol = cholesky(P̃)
+        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):
         d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
@@ -62,8 +62,8 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, 
             R̂u, R̂y, noR̂u,
             S̃, T, 
-            Ẽ, F, G, J, K, V, P̃, q̃, p,
-            P̃_chol,
+            Ẽ, F, G, J, K, V, H̃, q̃, p,
+            H̃_chol,
             Ks, Ps,
             d0, D̂0, D̂E,
             Ŷop, Dop,
@@ -195,10 +195,10 @@ linconstraint!(::ExplicitMPC, ::LinModel) = nothing
 @doc raw"""
 Analytically solve the optimization problem for [`ExplicitMPC`](@ref).
 
-The solution is ``\mathbf{ΔŨ = - P̃^{-1} q̃}``, see [`init_quadprog`](@ref).
+The solution is ``\mathbf{ΔŨ = - H̃^{-1} q̃}``, see [`init_quadprog`](@ref).
 """
 function optim_objective!(mpc::ExplicitMPC)
-    return lmul!(-1, ldiv!(mpc.ΔŨ, mpc.P̃_chol, mpc.q̃))
+    return lmul!(-1, ldiv!(mpc.ΔŨ, mpc.H̃_chol, mpc.q̃))
 end
 
 "Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)."

src/controller/linmpc.jl

@@ -30,7 +30,7 @@ struct LinMPC{
     J::Matrix{NT}
     K::Matrix{NT}
     V::Matrix{NT}
-    P̃::Hermitian{NT, Matrix{NT}}
+    H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
     p::Vector{NT}
     Ks::Matrix{NT}
@@ -54,8 +54,8 @@ struct LinMPC{
         noR̂u = iszero(L_Hp)
         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(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
-        P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        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(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)
@@ -69,7 +69,7 @@ struct LinMPC{
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, 
             R̂u, R̂y, noR̂u,
             S̃, T,
-            Ẽ, F, G, J, K, V, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, H̃, q̃, p,
             Ks, Ps,
             d0, D̂0, D̂E,
             Ŷop, Dop,
@@ -121,7 +121,7 @@ arguments.
 - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
 - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in
   the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
-  (default to [`OSQP.jl`](https://osqp.org/docs/parsers/jump.html) optimizer).
+  (default to [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer).
 - `M_Hp` / `N_Hc` / `L_Hp` : diagonal matrices ``\mathbf{M}_{H_p}, \mathbf{N}_{H_c},
   \mathbf{L}_{H_p}``, for time-varying weights (generated from `Mwt/Nwt/Lwt` args if omitted).
 - additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor.
@@ -241,7 +241,7 @@ function init_optimization!(mpc::LinMPC, optim::JuMP.GenericModel)
     b = con.b[con.i_b]
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
     # --- quadratic optimization init ---
-    @objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.P̃, mpc.q̃))
+    @objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.H̃, mpc.q̃))
     return nothing
 end
 

src/controller/nonlinmpc.jl

@@ -32,7 +32,7 @@ struct NonLinMPC{
     J::Matrix{NT}
     K::Matrix{NT}
     V::Matrix{NT}
-    P̃::Hermitian{NT, Matrix{NT}}
+    H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
     p::Vector{NT}
     Ks::Matrix{NT}
@@ -55,8 +55,8 @@ struct NonLinMPC{
         noR̂u = iszero(L_Hp)
         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(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
-        P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
+        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(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)
@@ -70,7 +70,7 @@ struct NonLinMPC{
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, 
             R̂u, R̂y, noR̂u,
             S̃, T,  
-            Ẽ, F, G, J, K, V, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, H̃, q̃, p,
             Ks, Ps,
             d0, D̂0, D̂E,
             Ŷop, Dop,
@@ -136,7 +136,7 @@ This method uses the default state estimator :
   \mathbf{L}_{H_p}``, for time-varying weights (generated from `Mwt/Nwt/Lwt` args if omitted).
 - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive
    controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
-   (default to [`Ipopt.jl`](https://github.com/jump-dev/Ipopt.jl) optimizer).
+   (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer).
 - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor 
   (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)).
 
@@ -344,19 +344,21 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where
     register(optim, :Jfunc, nvar, Jfunc, autodiff=true)
     @NLobjective(optim, Min, Jfunc(ΔŨvar...))
     if ng ≠ 0
-        i_end_Ymin, i_end_Ymax, i_end_x̂min = 1Hp*ny, 2Hp*ny, 2Hp*ny + nx̂
         for i in eachindex(con.Ymin)
             sym = Symbol("g_Ymin_$i")
             register(optim, sym, nvar, gfunc[i], autodiff=true)
         end
+        i_end_Ymin = 1Hp*ny
         for i in eachindex(con.Ymax)
             sym = Symbol("g_Ymax_$i")
             register(optim, sym, nvar, gfunc[i_end_Ymin+i], autodiff=true)
         end
+        i_end_Ymax = 2Hp*ny
         for i in eachindex(con.x̂min)
             sym = Symbol("g_x̂min_$i")
             register(optim, sym, nvar, gfunc[i_end_Ymax+i], autodiff=true)
         end
+        i_end_x̂min = 2Hp*ny + nx̂
         for i in eachindex(con.x̂max)
             sym = Symbol("g_x̂max_$i")
             register(optim, sym, nvar, gfunc[i_end_x̂min+i], autodiff=true)