Merge pull request #25 from JuliaControl/terminal_cost

Added: terminal cost support

Author: franckgaga

Project.toml

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

src/controller/construct.jl

@@ -672,18 +672,18 @@ returns the augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{
 ```
 """
 function relaxΔU(::SimModel{NT}, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) where {NT<:Real}
-    diag_N_Hc = diag(N_Hc)
+    nΔU = size(N_Hc, 1)
     if !isinf(C) # ΔŨ = [ΔU; ϵ]
         # 0 ≤ ϵ ≤ ∞  
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]]
         A_ϵ = [zeros(NT, 1, length(ΔUmin)) NT[1.0]]
         A_ΔŨmin, A_ΔŨmax = -[I  C_Δumin; A_ϵ], [I -C_Δumax; A_ϵ]
-        Ñ_Hc = Diagonal{NT}([diag_N_Hc; C])
+        Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1);zeros(NT, 1, nΔU) C])
     else # ΔŨ = ΔU (only hard constraints)
         ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
-        I_Hc = Matrix{NT}(I, size(N_Hc))
+        I_Hc = Matrix{NT}(I, nΔU, nΔU)
         A_ΔŨmin, A_ΔŨmax = -I_Hc,  I_Hc
-        Ñ_Hc = Diagonal{NT}(diag_N_Hc)
+        Ñ_Hc = N_Hc
     end
     return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc
 end
@@ -817,9 +817,12 @@ function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C, E=nothing)
     size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)"))
     size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)"))
     size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)"))
-    (!isdiag(M_Hp) || any(diag(M_Hp).<0)) && throw(ArgumentError("M_Hp should be a positive semidefinite diagonal matrix"))
-    (!isdiag(N_Hc) || any(diag(N_Hc).<0)) && throw(ArgumentError("N_Hc should be a positive semidefinite diagonal matrix"))
-    (!isdiag(L_Hp) || any(diag(L_Hp).<0)) && throw(ArgumentError("L_Hp should be a positive semidefinite diagonal matrix"))
+    (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative"))
+    (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative"))
+    (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative"))
+    !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian"))
+    !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian"))
+    !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian"))
     size(C) ≠ ()    && throw(ArgumentError("Cwt should be a real scalar"))
     C < 0           && throw(ArgumentError("Cwt weight should be ≥ 0"))
     !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar"))

src/controller/execute.jl

@@ -180,7 +180,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
         mpc.R̂u .= R̂u
         C_u = mpc.T_lastu .- mpc.R̂u
         mpc.q̃ .+= lmul!(2, (mpc.L_Hp*mpc.S̃)'*C_u)
-        mpc.p .+= dot(C_y, mpc.L_Hp, C_u)
+        mpc.p .+= dot(C_u, mpc.L_Hp, C_u)
     end
     return nothing
 end

src/controller/explicitmpc.jl

@@ -4,9 +4,9 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     ŷ ::Vector{NT}
     Hp::Int
     Hc::Int
-    M_Hp::Diagonal{NT, Vector{NT}}
-    Ñ_Hc::Diagonal{NT, Vector{NT}}
-    L_Hp::Diagonal{NT, Vector{NT}}
+    M_Hp::Hermitian{NT, Matrix{NT}}
+    Ñ_Hc::Hermitian{NT, Matrix{NT}}
+    L_Hp::Hermitian{NT, Matrix{NT}}
     C::NT
     E::NT
     R̂u::Vector{NT}
@@ -41,7 +41,8 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         Cwt = Inf # no slack variable ϵ for ExplicitMPC
         Ewt = 0   # economic costs not supported for ExplicitMPC
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
-        M_Hp, N_Hc, L_Hp = Diagonal{NT}(M_Hp), Diagonal{NT}(N_Hc), Diagonal{NT}(L_Hp) # debug julia 1.6
+        # Matrix() call is needed to convert `Diagonal` to normal `Matrix`
+        M_Hp, N_Hc, L_Hp = Hermitian(Matrix(M_Hp)), Hermitian(Matrix(N_Hc)), Hermitian(Matrix(L_Hp))
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)
@@ -118,9 +119,9 @@ function ExplicitMPC(
     Mwt = fill(DEFAULT_MWT, model.ny),
     Nwt = fill(DEFAULT_NWT, model.nu),
     Lwt = fill(DEFAULT_LWT, model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
     kwargs...
 ) 
     estim = SteadyKalmanFilter(model; kwargs...)
@@ -156,9 +157,9 @@ function ExplicitMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
 ) where {NT<:Real, SE<:StateEstimator{NT}}
     isa(estim.model, LinModel) || error("estim.model type must be a LinModel") 
     nk = estimate_delays(estim.model)

src/controller/linmpc.jl

@@ -14,9 +14,9 @@ struct LinMPC{
     ŷ ::Vector{NT}
     Hp::Int
     Hc::Int
-    M_Hp::Diagonal{NT, Vector{NT}}
-    Ñ_Hc::Diagonal{NT, Vector{NT}}
-    L_Hp::Diagonal{NT, Vector{NT}}
+    M_Hp::Hermitian{NT, Matrix{NT}}
+    Ñ_Hc::Hermitian{NT, Matrix{NT}}
+    L_Hp::Hermitian{NT, Matrix{NT}}
     C::NT
     E::NT
     R̂u::Vector{NT}
@@ -49,7 +49,8 @@ struct LinMPC{
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         Ewt = 0   # economic costs not supported for LinMPC
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
-        M_Hp, N_Hc, L_Hp = Diagonal{NT}(M_Hp), Diagonal{NT}(N_Hc), Diagonal{NT}(L_Hp) # debug julia 1.6
+        # Matrix() call is needed to convert `Diagonal` to normal `Matrix`
+        M_Hp, N_Hc, L_Hp = Hermitian(Matrix(M_Hp)), Hermitian(Matrix(N_Hc)), Hermitian(Matrix(L_Hp))
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)
@@ -102,8 +103,9 @@ in which the weight matrices are repeated ``H_p`` or ``H_c`` times by default:
     \mathbf{L}_{H_p} &= \text{diag}\mathbf{(L,L,...,L)}     
 \end{aligned}
 ```
-Time-varying weights over the horizons are also supported. The ``\mathbf{ΔU}`` includes the 
-input increments ``\mathbf{Δu}(k+j) = \mathbf{u}(k+j) - \mathbf{u}(k+j-1)`` from ``j=0`` to
+Time-varying and non-diagonal weights are also supported. Modify the last block in 
+``\mathbf{M}_{H_p}`` to specify a terminal weight. The ``\mathbf{ΔU}`` includes the input 
+increments ``\mathbf{Δu}(k+j) = \mathbf{u}(k+j) - \mathbf{u}(k+j-1)`` from ``j=0`` to
 ``H_c-1``, the ``\mathbf{Ŷ}`` vector, the output predictions ``\mathbf{ŷ}(k+j)`` from
 ``j=1`` to ``H_p``, and the ``\mathbf{U}`` vector, the manipulated inputs ``\mathbf{u}(k+j)``
 from ``j=0`` to ``H_p-1``. The slack variable ``ϵ`` relaxes the constraints, as described
@@ -119,9 +121,9 @@ arguments.
 - `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).
 - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
-- `M_Hp=Diagonal(repeat(Mwt),Hp)` : diagonal weight matrix ``\mathbf{M}_{H_p}``.
-- `N_Hc=Diagonal(repeat(Nwt),Hc)` : diagonal weight matrix ``\mathbf{N}_{H_c}``.
-- `L_Hp=Diagonal(repeat(Lwt),Hp)` : diagonal weight matrix ``\mathbf{L}_{H_p}``.
+- `M_Hp=diagm(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
+- `N_Hc=diagm(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
+- `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``.
 - `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)
@@ -174,9 +176,9 @@ function LinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt = DEFAULT_CWT,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
@@ -215,9 +217,9 @@ function LinMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
 ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel}

src/controller/nonlinmpc.jl

@@ -15,9 +15,9 @@ struct NonLinMPC{
     ŷ ::Vector{NT}
     Hp::Int
     Hc::Int
-    M_Hp::Diagonal{NT, Vector{NT}}
-    Ñ_Hc::Diagonal{NT, Vector{NT}}
-    L_Hp::Diagonal{NT, Vector{NT}}
+    M_Hp::Hermitian{NT, Matrix{NT}}
+    Ñ_Hc::Hermitian{NT, Matrix{NT}}
+    L_Hp::Hermitian{NT, Matrix{NT}}
     C::NT
     E::NT
     JE::JEfunc
@@ -50,7 +50,8 @@ struct NonLinMPC{
         nu, ny, nd = model.nu, model.ny, model.nd
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
-        M_Hp, N_Hc, L_Hp = Diagonal{NT}(M_Hp), Diagonal{NT}(N_Hc), Diagonal{NT}(L_Hp) # debug julia 1.6
+        # Matrix() call is needed to convert `Diagonal` to normal `Matrix`
+        M_Hp, N_Hc, L_Hp = Hermitian(Matrix(M_Hp)), Hermitian(Matrix(N_Hc)), Hermitian(Matrix(L_Hp))
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)
@@ -130,9 +131,9 @@ This method uses the default state estimator :
 - `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).
 - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
-- `M_Hp=Diagonal(repeat(Mwt),Hp)` : diagonal weight matrix ``\mathbf{M}_{H_p}``.
-- `N_Hc=Diagonal(repeat(Nwt),Hc)` : diagonal weight matrix ``\mathbf{N}_{H_c}``.
-- `L_Hp=Diagonal(repeat(Lwt),Hp)` : diagonal weight matrix ``\mathbf{L}_{H_p}``.
+- `M_Hp=diagm(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
+- `N_Hc=diagm(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
+- `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``.
 - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
 - `Ewt=0.0` : economic costs weight ``E`` (scalar). 
 - `JE=(_,_,_)->0.0` : economic function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E)``.
@@ -179,9 +180,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE::Function = (_,_,_) -> 0.0,
@@ -199,9 +200,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE::Function = (_,_,_) -> 0.0,
@@ -242,9 +243,9 @@ function NonLinMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = Diagonal(repeat(Mwt, Hp)),
-    N_Hc = Diagonal(repeat(Nwt, Hc)),
-    L_Hp = Diagonal(repeat(Lwt, Hp)),
+    M_Hp = diagm(repeat(Mwt, Hp)),
+    N_Hc = diagm(repeat(Nwt, Hc)),
+    L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     JE::JEFunc = (_,_,_) -> 0.0,

test/test_predictive_control.jl

@@ -232,6 +232,32 @@ end
     @test info[:x̂end][1] ≈ 0 atol=1e-1
 end
 
+@testset "LinMPC terminal cost" begin
+    model = LinModel(ss([0.5 -0.4;0.6 0.5], [1 0;0 1], [1 0; 0 1], 0, 1))
+    K = lqr(Discrete, model.A, model.Bu, I, 0.5I)
+    M_end, _ = ControlSystemsBase.ared(model.A, model.Bu, I, 0.5I)
+    M_Hp = [I(4) zeros(4,2); zeros(2,4) M_end]
+    mpc = LinMPC(model; Hp=3, Hc=3, M_Hp, Nwt=[0; 0], Lwt=[0.5, 0.5], nint_ym=0)
+    X_mpc = zeros(2,20)
+    setstate!(mpc,[1,1])
+    setstate!(model, [1,1])
+    for i=1:20
+        y = model()
+        u = moveinput!(mpc, [0, 0])
+        X_mpc[:,i] = model.x
+        updatestate!(mpc, u, y)
+        updatestate!(model, u)
+    end
+    X_lqr = zeros(2,20)
+    x=[1,1]
+    for i=1:20
+        u = -K*x
+        X_lqr[:,i] = x
+        x = model.A*x + model.Bu*u
+    end
+    @test all(isapprox.(X_mpc, X_lqr, atol=1e-3))
+end
+
 @testset "ExplicitMPC construction" begin
     model = LinModel(sys, Ts, i_d=[3])
     mpc1 = ExplicitMPC(model, Hp=15)