Merge pull request #16 from franckgaga/terminal

Added : terminal constraints

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.9.1"
+version = "0.10.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -83,12 +83,12 @@ for more detailed examples.
   - [x] move suppression
   - [x] input setpoint tracking
   - [x] economic costs (economic model predictive control)
-  - [ ] terminal cost to ensure nominal stability
 - [x] explicit predictive controller for problems without constraint
 - [x] soft and hard constraints on:
   - [x] output predictions
   - [x] manipulated inputs
   - [x] manipulated inputs increments
+  - [x] terminal states to ensure nominal stability
 - [ ] custom manipulated input constraints that are a function of the predictions
 - [x] supported feedback strategy:
   - [x] state estimator (see State Estimation features)

docs/src/manual/linmpc.md

@@ -84,8 +84,8 @@ These are the integrating states for the unmeasured plant disturbances, and they
 automatically added to the model outputs by default if feasible (see [`SteadyKalmanFilter`](@ref)
 for details).
 
-[^1]: To avoid observer design, we could have use an [`InternalModel`](@ref) structure with
-    `mpc = LinMPC(InternalModel(model), Hp=15, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])` . It was
+[^1]: As an alternative to state observer, we could have use an [`InternalModel`](@ref)
+    structure with `mpc = LinMPC(InternalModel(model), Hp=15, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])` . It was
     tested on the example of this page and it gives similar results.
 
 Before closing the loop, we call [`initstate!`](@ref) with the actual plant inputs and

src/controller/explicitmpc.jl

@@ -19,7 +19,13 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
     G::Matrix{Float64}
     J::Matrix{Float64}
     K::Matrix{Float64}
-    Q::Matrix{Float64}
+    V::Matrix{Float64}
+    ẽx̂::Matrix{Float64}
+    fx̂::Vector{Float64}
+    gx̂::Matrix{Float64}
+    jx̂::Matrix{Float64}
+    kx̂::Matrix{Float64}
+    vx̂::Matrix{Float64}
     P̃::Hermitian{Float64, Matrix{Float64}}
     q̃::Vector{Float64}
     p::Vector{Float64}
@@ -44,8 +50,8 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
         R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(nu, Hp, Hc)
-        E, F, G, J, K, Q = init_predmat(estim, model, Hp, Hc)
-        _ , S̃, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S, N_Hc, E)
+        E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
+        S̃, Ñ_Hc, Ẽ, ẽx̂  = S, N_Hc, E, ex̂ # no slack variable ϵ for ExplicitMPC
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         P̃_chol = cholesky(P̃)
         Ks, Ps = init_stochpred(estim, Hp)
@@ -59,7 +65,7 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y, noR̂u,
             S̃, T, 
-            Ẽ, F, G, J, K, Q, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
             P̃_chol,
             Ks, Ps,
             d0, D̂0,
@@ -155,15 +161,7 @@ function ExplicitMPC(
     Lwt = fill(DEFAULT_LWT, estim.model.nu)
 ) where {SE<:StateEstimator}
     isa(estim.model, LinModel) || error("estim.model type must be LinModel") 
-    poles = eigvals(estim.model.A)
-    nk = sum(poles .≈ 0)
-    if isnothing(Hp)
-        Hp = DEFAULT_HP + nk
-    end
-    if Hp ≤ nk
-        @warn("prediction horizon Hp ($Hp) ≤ number of delays in model "*
-              "($nk), the closed-loop system may be zero-gain (unresponsive) or unstable")
-    end
+    Hp = default_Hp(estim.model, Hp)
     return ExplicitMPC{SE}(estim, Hp, Hc, Mwt, Nwt, Lwt)
 end
 

src/controller/linmpc.jl

@@ -21,7 +21,13 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
     G::Matrix{Float64}
     J::Matrix{Float64}
     K::Matrix{Float64}
-    Q::Matrix{Float64}
+    V::Matrix{Float64}
+    ẽx̂::Matrix{Float64}
+    fx̂::Vector{Float64}
+    gx̂::Matrix{Float64}
+    jx̂::Matrix{Float64}
+    kx̂::Matrix{Float64}
+    vx̂::Matrix{Float64}
     P̃::Hermitian{Float64, Matrix{Float64}}
     q̃::Vector{Float64}
     p::Vector{Float64}
@@ -44,8 +50,8 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
         R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(nu, Hp, Hc)
-        E, F, G, J, K, Q = init_predmat(estim, model, Hp, Hc)
-        con, S̃, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S, N_Hc, E)
+        E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
+        con, S̃, Ñ_Hc, Ẽ, ẽx̂ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d0, D̂0 = zeros(nd), zeros(nd*Hp)
@@ -58,7 +64,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y, noR̂u,
             S̃, T,
-            Ẽ, F, G, J, K, Q, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
             Ks, Ps,
             d0, D̂0,
             Ŷop, Dop,
@@ -187,7 +193,7 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, KalmanFilter es
 """
 function LinMPC(
     estim::SE;
-    Hp::Union{Int, Nothing} = DEFAULT_HP,
+    Hp::Union{Int, Nothing} = nothing,
     Hc::Int = DEFAULT_HC,
     Mwt = fill(DEFAULT_MWT, estim.model.ny),
     Nwt = fill(DEFAULT_NWT, estim.model.nu),
@@ -196,15 +202,7 @@ function LinMPC(
     optim::JuMP.Model = JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)
 ) where {SE<:StateEstimator}
     isa(estim.model, LinModel) || error("estim.model type must be LinModel") 
-    poles = eigvals(estim.model.A)
-    nk = sum(poles .≈ 0)
-    if isnothing(Hp)
-        Hp = DEFAULT_HP + nk
-    end
-    if Hp ≤ nk
-        @warn("prediction horizon Hp ($Hp) ≤ number of delays in model "*
-              "($nk), the closed-loop system may be zero-gain (unresponsive) or unstable")
-    end
+    Hp = default_Hp(estim.model, Hp)
     return LinMPC{SE}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, optim)
 end
 

src/controller/nonlinmpc.jl

@@ -24,7 +24,13 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
     G::Matrix{Float64}
     J::Matrix{Float64}
     K::Matrix{Float64}
-    Q::Matrix{Float64}
+    V::Matrix{Float64}
+    ẽx̂::Matrix{Float64}
+    fx̂::Vector{Float64}
+    gx̂::Matrix{Float64}
+    jx̂::Matrix{Float64}
+    kx̂::Matrix{Float64}
+    vx̂::Matrix{Float64}
     P̃::Hermitian{Float64, Matrix{Float64}}
     q̃::Vector{Float64}
     p::Vector{Float64}
@@ -48,8 +54,8 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
         R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(nu, Hp, Hc)
-        E, F, G, J, K, Q = init_predmat(estim, model, Hp, Hc)
-        con, S̃, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S, N_Hc, E)
+        E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
+        con, S̃, Ñ_Hc, Ẽ, ẽx̂ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d0, D̂0 = zeros(nd), zeros(nd*Hp)
@@ -62,7 +68,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y, noR̂u,
             S̃, T,  
-            Ẽ, F, G, J, K, Q, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
             Ks, Ps,
             d0, D̂0,
             Ŷop, Dop,
@@ -113,7 +119,7 @@ This method uses the default state estimator :
 
 # Arguments
 - `model::SimModel` : model used for controller predictions and state estimations.
-- `Hp=10`: prediction horizon ``H_p``.
+- `Hp=nothing`: prediction horizon ``H_p``, must be specified for [`NonLinModel`](@ref).
 - `Hc=2` : control horizon ``H_c``.
 - `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
 - `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
@@ -155,7 +161,7 @@ for common mistakes when writing these functions.
 """
 function NonLinMPC(
     model::SimModel;
-    Hp::Int = DEFAULT_HP,
+    Hp::Union{Int, Nothing} = nothing,
     Hc::Int = DEFAULT_HC,
     Mwt = fill(DEFAULT_MWT, model.ny),
     Nwt = fill(DEFAULT_NWT, model.nu),
@@ -169,9 +175,10 @@ function NonLinMPC(
     estim = UnscentedKalmanFilter(model; kwargs...)
     NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
 end
+
 function NonLinMPC(
     model::LinModel;
-    Hp::Int = DEFAULT_HP,
+    Hp::Union{Int, Nothing} = nothing,
     Hc::Int = DEFAULT_HC,
     Mwt = fill(DEFAULT_MWT, model.ny),
     Nwt = fill(DEFAULT_NWT, model.nu),
@@ -211,7 +218,7 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
 """
 function NonLinMPC(
     estim::SE;
-    Hp::Int = DEFAULT_HP,
+    Hp::Union{Int, Nothing} = nothing,
     Hc::Int = DEFAULT_HC,
     Mwt = fill(DEFAULT_MWT, estim.model.ny),
     Nwt = fill(DEFAULT_NWT, estim.model.nu),
@@ -221,6 +228,7 @@ function NonLinMPC(
     JE::JEFunc = (_,_,_) -> 0.0,
     optim::JuMP.Model = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"))
 ) where {SE<:StateEstimator, JEFunc<:Function}
+    Hp = default_Hp(estim.model, Hp)
     return NonLinMPC{SE, JEFunc}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, optim)
 end
 
@@ -256,20 +264,22 @@ function init_optimization!(mpc::NonLinMPC)
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
     # --- nonlinear optimization init ---
     model = mpc.estim.model
-    ny, nu, Hp, Hc = model.ny, model.nu, mpc.Hp, mpc.Hc
-    nC = (2*Hp*nu + 2*nvar + 2*Hp*ny) - length(mpc.con.b)
+    ny, nu, nx̂, Hp = model.ny, model.nu, mpc.estim.nx̂, mpc.Hp
+    nC = (2*Hp*nu + 2*nvar + 2*Hp*ny + 2*nx̂) - length(mpc.con.b)
     # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs
-    Jfunc, Cfunc = let mpc=mpc, model=model, nC=nC, nvar=nvar , nŶ=Hp*ny
+    Jfunc, Cfunc = let mpc=mpc, model=model, nC=nC, nvar=nvar , nŶ=Hp*ny, nx̂=nx̂
         last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing
         Ŷ_cache::DiffCacheType = DiffCache(zeros(nŶ), nvar + 3)
         C_cache::DiffCacheType = DiffCache(zeros(nC), nvar + 3)
+        x̂_cache::DiffCacheType = DiffCache(zeros(nx̂), nvar + 3)
         function Jfunc(ΔŨtup::Float64...)
             Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
             ΔŨ = collect(ΔŨtup)
             if ΔŨtup != last_ΔŨtup_float
+                x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 C = get_tmp(C_cache, ΔŨtup[1])
-                predict!(Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_float = ΔŨtup
             end
             return obj_nonlinprog(mpc, model, Ŷ, ΔŨ)
@@ -278,31 +288,34 @@ function init_optimization!(mpc::NonLinMPC)
             Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
             ΔŨ = collect(ΔŨtup)
             if ΔŨtup != last_ΔŨtup_dual
+                x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 C = get_tmp(C_cache, ΔŨtup[1])
-                predict!(Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_dual = ΔŨtup
             end
             return obj_nonlinprog(mpc, model, Ŷ, ΔŨ)
         end
         function con_nonlinprog_i(i, ΔŨtup::NTuple{N, Float64}) where {N}
             C = get_tmp(C_cache, ΔŨtup[1])
             if ΔŨtup != last_ΔŨtup_float
+                x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
                 ΔŨ = collect(ΔŨtup)
-                predict!(Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                C = con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_float = ΔŨtup
             end
             return C[i]
         end
         function con_nonlinprog_i(i, ΔŨtup::NTuple{N, Real}) where {N}
             C = get_tmp(C_cache, ΔŨtup[1])
             if ΔŨtup != last_ΔŨtup_dual
+                x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
                 ΔŨ = collect(ΔŨtup)
-                predict!(Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                C = con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_dual = ΔŨtup
             end
             return C[i]
@@ -313,15 +326,22 @@ function init_optimization!(mpc::NonLinMPC)
     register(optim, :Jfunc, nvar, Jfunc, autodiff=true)
     @NLobjective(optim, Min, Jfunc(ΔŨvar...))
     if nC ≠ 0
-        n = 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("C_Ymin_$i")
-            register(optim, sym, nvar, Cfunc[n + i], autodiff=true)
+            register(optim, sym, nvar, Cfunc[i], autodiff=true)
         end
-        n = lastindex(con.Ymin)
         for i in eachindex(con.Ymax)
             sym = Symbol("C_Ymax_$i")
-            register(optim, sym, nvar, Cfunc[n + i], autodiff=true)
+            register(optim, sym, nvar, Cfunc[i_end_Ymin+i], autodiff=true)
+        end
+        for i in eachindex(con.x̂min)
+            sym = Symbol("C_x̂min_$i")
+            register(optim, sym, nvar, Cfunc[i_end_Ymax+i], autodiff=true)
+        end
+        for i in eachindex(con.x̂max)
+            sym = Symbol("C_x̂max_$i")
+            register(optim, sym, nvar, Cfunc[i_end_x̂min+i], autodiff=true)
         end
     end
     return nothing
@@ -345,27 +365,41 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
         f_sym = Symbol("C_Ymax_$(i)")
         add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
     end
+    for i in findall(.!isinf.(con.x̂min))
+        f_sym = Symbol("C_x̂min_$(i)")
+        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
+    end
+    for i in findall(.!isinf.(con.x̂max))
+        f_sym = Symbol("C_x̂max_$(i)")
+        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
+    end
     return nothing
 end
 
 """
-    con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, ΔŨ)
+    con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
 
 Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
 """
-function con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, Ŷ, ΔŨ)
-    ny, Hp = model.ny, mpc.Hp
+function con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
+    ny, nx̂, Hp = model.ny, mpc.estim.nx̂, mpc.Hp
+    i_end_Ymin, i_end_Ymax = 1Hp*ny      , 2Hp*ny
+    i_end_x̂min, i_end_x̂max = 2Hp*ny + 1nx̂, 2Hp*ny + 2nx̂
     if !isinf(mpc.C) # constraint softening activated :
         ϵ = ΔŨ[end]
-        C[begin:(Hp*ny)] = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
-        C[(Hp*ny+1):end] = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
+        C[           1:i_end_Ymin] = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
+        C[i_end_Ymin+1:i_end_Ymax] = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
+        C[i_end_Ymax+1:i_end_x̂min] = (mpc.con.x̂min - x̂end) - ϵ*mpc.con.c_x̂min
+        C[i_end_x̂min+1:i_end_x̂max] = (x̂end - mpc.con.x̂max) - ϵ*mpc.con.c_x̂max
     else # no constraint softening :
-        C[begin:(Hp*ny)] = (mpc.con.Ymin - Ŷ)
-        C[(Hp*ny+1):end] = (Ŷ - mpc.con.Ymax)
+        C[           1:i_end_Ymin] = mpc.con.Ymin - Ŷ
+        C[i_end_Ymin+1:i_end_Ymax] = Ŷ - mpc.con.Ymax
+        C[i_end_Ymax+1:i_end_x̂min] = mpc.con.x̂min - x̂end
+        C[i_end_x̂min+1:i_end_x̂max] = x̂end - mpc.con.x̂max
     end
     C[isinf.(C)] .= 0 # replace ±Inf with 0 to avoid INVALID_MODEL error
     return C
 end
 
 "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `C` unchanged."
-con_nonlinprog!(C, ::NonLinMPC, ::LinModel, _ , _ ) = C
+con_nonlinprog!(C, ::NonLinMPC, ::LinModel, _ , _ , _ ) = C