added : ExplicitMPC

Author: franckgaga

README.md

@@ -84,6 +84,7 @@ for more detailed examples.
   - [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

example/juMPC.jl

@@ -57,9 +57,10 @@ updatestate!(luenberger, [0,0], [0,0])
 
 mpcluen = LinMPC(luenberger)
 
+mpcexplicit = ExplicitMPC(LinModel(append(tf(3,[2, 1]), tf(2, [6, 1])), 0.1), Hp=10000, Hc=1)
+moveinput!(mpcexplicit, [10, 10])
 
-
-
+#=
 kalmanFilter1 = KalmanFilter(linModel1)
 kalmanFilter2 = KalmanFilter(linModel1,nint_ym=0)
 
@@ -211,3 +212,4 @@ display(pu)
 display(py)
 
 =#
+=#

src/ModelPredictiveControl.jl

@@ -15,7 +15,8 @@ export SimModel, LinModel, NonLinModel, setop!, setstate!, updatestate!, evalout
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export initstate!
-export PredictiveController, LinMPC, NonLinMPC, setconstraint!, moveinput!, getinfo, sim!
+export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput!
+export getinfo, sim!
 
 "Generate a block diagonal matrix repeating `n` times the matrix `A`."
 repeatdiag(A, n::Int) = kron(I(n), A)

src/controller/explicitmpc.jl

@@ -0,0 +1,176 @@
+struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
+    estim::S
+    ΔŨ::Vector{Float64}
+    x̂d::Vector{Float64}
+    x̂s::Vector{Float64}
+    ŷ ::Vector{Float64}
+    Ŷs::Vector{Float64}
+    Hp::Int
+    Hc::Int
+    M_Hp::Diagonal{Float64, Vector{Float64}}
+    Ñ_Hc::Diagonal{Float64, Vector{Float64}}
+    L_Hp::Diagonal{Float64, Vector{Float64}}
+    R̂u::Vector{Float64}
+    R̂y::Vector{Float64}
+    S̃_Hp::Matrix{Bool}
+    T_Hp::Matrix{Bool}
+    T_Hc::Matrix{Bool}
+    Ẽ ::Matrix{Float64}
+    F ::Vector{Float64}
+    G ::Matrix{Float64}
+    J ::Matrix{Float64}
+    Kd::Matrix{Float64}
+    Q ::Matrix{Float64}
+    P̃ ::Hermitian{Float64, Matrix{Float64}}
+    q̃ ::Vector{Float64}
+    p ::Vector{Float64}
+    Ks::Matrix{Float64}
+    Ps::Matrix{Float64}
+    d::Vector{Float64}
+    D̂::Vector{Float64}
+    Yop::Vector{Float64}
+    Dop::Vector{Float64}
+    function ExplicitMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, ru) where {S<:StateEstimator}
+        model = estim.model
+        nu, nxd, nxs, ny, nd = model.nu, model.nx, estim.nxs, model.ny, model.nd
+        x̂d, x̂s, ŷ, Ŷs = zeros(nxd), zeros(nxs), zeros(ny), zeros(ny*Hp)
+        validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Inf, ru)
+        M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
+        N_Hc = Diagonal{Float64}(repeat(Nwt, Hc)) 
+        L_Hp = Diagonal{Float64}(repeat(Lwt, Hp))
+        # manipulated input setpoint predictions are constant over Hp :
+        R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[]
+        R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
+        S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
+        E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
+        _ , S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, Inf, S_Hp, S_Hc, N_Hc, E)
+        P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
+        Ks, Ps = init_stochpred(estim, Hp)
+        d, D̂ = zeros(nd), zeros(nd*Hp)
+        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
+        nvar = size(Ẽ, 2)
+        ΔŨ = zeros(nvar)
+        mpc = new(
+            estim,
+            ΔŨ, x̂d, x̂s, ŷ, Ŷs,
+            Hp, Hc, 
+            M_Hp, Ñ_Hc, L_Hp, R̂u, R̂y,
+            S̃_Hp, T_Hp, T_Hc, 
+            Ẽ, F, G, J, Kd, Q, P̃, q̃, p,
+            Ks, Ps,
+            d, D̂,
+            Yop, Dop,
+        )
+        return mpc
+    end
+end
+
+@doc raw"""
+    ExplicitMPC(model::LinModel; <keyword arguments>)
+
+Construct an explicit linear predictive controller based on [`LinModel`](@ref) `model`.
+
+The controller minimizes the following objective function at each discrete time ``k``:
+```math
+\min_{\mathbf{ΔU}}       \mathbf{(R̂_y - Ŷ)}' \mathbf{M}_{H_p} \mathbf{(R̂_y - Ŷ)}   
+                       + \mathbf{(ΔU)}'      \mathbf{N}_{H_c} \mathbf{(ΔU)}  
+                       + \mathbf{(R̂_u - U)}' \mathbf{L}_{H_p} \mathbf{(R̂_u - U)} 
+```
+
+This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) with default
+arguments.
+
+# Arguments
+- `model::LinModel` : model used for controller predictions and state estimations.
+- `Hp=10+nk`: prediction horizon ``H_p``, `nk` is the number of delays in `model`.
+- `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).
+- `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
+- `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
+- `ru=model.uop` : manipulated input setpoints ``\mathbf{r_u}`` (vector).
+
+# Examples
+```jldoctest
+julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 4);
+
+julia> mpc = ExplicitMPC(model, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
+ExplicitMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFilter estimator and:
+ 30 prediction steps Hp
+  1 control steps Hc
+  1 manipulated inputs u
+  4 states x̂
+  2 measured outputs ym
+  0 unmeasured outputs yu
+  0 measured disturbances d
+```
+
+"""
+ExplicitMPC(model::LinModel; kwargs...) = ExplicitMPC(SteadyKalmanFilter(model); kwargs...)
+
+"""
+    ExplicitMPC(estim::StateEstimator; <keyword arguments>)
+
+Use custom state estimator `estim` to construct `ExplicitMPC`.
+
+`estim.model` must be a [`LinModel`](@ref). Else, a [`NonLinMPC`](@ref) is required. 
+
+# Examples
+```jldoctest
+julia> estim = KalmanFilter(LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 4), i_ym=[2]);
+
+julia> mpc = ExplicitMPC(estim, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
+ExplicitMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, KalmanFilter estimator and:
+ 30 prediction steps Hp
+  1 control steps Hc
+  1 manipulated inputs u
+  3 states x̂
+  1 measured outputs ym
+  1 unmeasured outputs yu
+  0 measured disturbances d
+```
+"""
+function ExplicitMPC(
+    estim::S;
+    Hp::Union{Int, Nothing} = nothing,
+    Hc::Int = 2,
+    Mwt = fill(1.0, estim.model.ny),
+    Nwt = fill(0.1, estim.model.nu),
+    Lwt = fill(0.0, estim.model.nu),
+    ru  = estim.model.uop,
+) where {S<: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 = 10 + 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
+    return ExplicitMPC{S}(estim, Hp, Hc, Mwt, Nwt, Lwt, ru)
+end
+
+function Base.show(io::IO, mpc::ExplicitMPC)
+    Hp, Hc = mpc.Hp, mpc.Hc
+    nu, nd = mpc.estim.model.nu, mpc.estim.model.nd
+    nx̂, nym, nyu = mpc.estim.nx̂, mpc.estim.nym, mpc.estim.nyu
+    n = maximum(ndigits.((Hp, Hc, nu, nx̂, nym, nyu, nd))) + 1
+    println(io, "$(typeof(mpc).name.name) controller with a sample time Ts = "*
+                "$(mpc.estim.model.Ts) s, "*
+                "$(typeof(mpc.estim).name.name) estimator and:")
+    println(io, "$(lpad(Hp, n)) prediction steps Hp")
+    println(io, "$(lpad(Hc, n)) control steps Hc")
+    print_estim_dim(io, mpc.estim, n)
+end
+
+linconstraint!(::ExplicitMPC, ::LinModel) = nothing
+
+"""
+Analytically solve the optimization problem for [`ExplicitMPC`](@ref).
+"""
+function optim_objective!(mpc::ExplicitMPC)
+    mpc.ΔŨ[:] = -mpc.P̃\mpc.q̃
+    return mpc.ΔŨ
+end

src/estimator/kalman.jl

@@ -95,8 +95,10 @@ ones, for ``\mathbf{Ĉ^u, D̂_d^u}``).
     ``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
 - `σR=fill(1,length(i_ym))` : main diagonal of the sensor noise covariance ``\mathbf{R}``
     of `model` measured outputs, specified as a standard deviation vector.
-- `nint_ym=`[`default_nint`](@ref)`(model,i_ym)` : integrator quantity per measured outputs (vector)
-    for the stochastic model, use `nint_ym=0` for no integrator at all (see Extended Help).
+- `nint_u=0`: integrator quantity per manipulated inputs (vector) for the
+    stochastic model, use `nint_u=0` for no integrator.
+- `nint_ym=`[`default_nint`](@ref)`(model,i_ym)` : integrator quantity per measured outputs 
+    (vector) for the stochastic model, use `nint_ym=0` for no integrator (see Extended Help).
 - `σQ_int=fill(1,sum(nint_ym))` : same than `σQ` but for the stochastic model covariance
     ``\mathbf{Q_{int}}`` (composed of output integrators).
 

src/predictive_control.jl

@@ -512,7 +512,7 @@ function linconstraint!(mpc::PredictiveController, model::SimModel)
 end
 
 """
-    optim_objective!(mpc::PredictiveController, b, p)
+    optim_objective!(mpc::PredictiveController)
 
 Optimize the objective function ``J`` of `mpc` controller and return the solution `ΔŨ`.
 """
@@ -1011,11 +1011,7 @@ function Base.show(io::IO, mpc::PredictiveController)
                 "$(typeof(mpc.estim).name.name) estimator and:")
     println(io, "$(lpad(Hp, n)) prediction steps Hp")
     println(io, "$(lpad(Hc, n)) control steps Hc")
-    println(io, "$(lpad(nu, n)) manipulated inputs u")
-    println(io, "$(lpad(nx̂, n)) states x̂")
-    println(io, "$(lpad(nym, n)) measured outputs ym")
-    println(io, "$(lpad(nyu, n)) unmeasured outputs yu")
-    print(io,   "$(lpad(nd, n)) measured disturbances d")
+    print_estim_dim(io, mpc.estim, n)
 end
 
 "Verify that the solver termination status means 'no solution available'."
@@ -1037,5 +1033,6 @@ function (mpc::PredictiveController)(
     return moveinput!(mpc, ry, d; kwargs...)
 end
 
+include("controller/explicitmpc.jl")
 include("controller/linmpc.jl")
 include("controller/nonlinmpc.jl")

src/state_estim.jl

@@ -39,6 +39,13 @@ function Base.show(io::IO, estim::StateEstimator)
     n = maximum(ndigits.((nu, nx̂, nym, nyu, nd))) + 1
     println(io, "$(typeof(estim).name.name) estimator with a sample time "*
                 "Ts = $(estim.model.Ts) s, $(typeof(estim.model).name.name) and:")
+    print_estim_dim(io, estim, n)
+end
+
+"Print the overall dimensions of the state estimator `estim` with left padding `n`."
+function print_estim_dim(io::IO, estim::StateEstimator, n)
+    nu, nd = estim.model.nu, estim.model.nd
+    nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu
     println(io, "$(lpad(nu, n)) manipulated inputs u")
     println(io, "$(lpad(nx̂, n)) states x̂")
     println(io, "$(lpad(nym, n)) measured outputs ym")
@@ -127,6 +134,33 @@ function init_estimstoch(i_ym, nint_ym)
     return Asm, Csm, nint_ym
 end
 
+function init_estimstoch_u(nu, nint_u)
+    if nint_u == 0 # alias for no output integrator at all
+        nint_u = fill(0, length(nu))
+    end
+    #nym = length(i_ym);
+    if length(nint_u) ≠ nu
+        error("nint_u size ($(length(nint_u))) ≠ manipulated input quantity nu ($nu)")
+    end
+    any(nint_u .< 0) && error("nint_u values should be ≥ 0")
+    nxs = sum(nint_u)
+    Asm, Csm = zeros(nxs, nxs), zeros(nym, nxs)
+    if nxs ≠ 0 # construct stochastic model state-space matrices (integrators) :
+        i_Asm, i_Csm = 1, 1
+        for iym = 1:nym
+            nint = nint_u[iym]
+            if nint ≠ 0
+                rows_Asm = (i_Asm):(i_Asm + nint - 1)
+                Asm[rows_Asm, rows_Asm] = Bidiagonal(ones(nint), ones(nint-1), :L)
+                Csm[iym, i_Csm+nint-1] = 1
+                i_Asm += nint
+                i_Csm += nint
+            end
+        end
+    end
+    return Asm, Csm, nint_u
+end
+
 @doc raw"""
     augment_model(model::LinModel, As, Cs; verify_obsv=true) -> Â, B̂u, Ĉ, B̂d, D̂d