Merge pull request #201 from JuliaControl/move_block

added: move blocking feature in `LinMPC`, `ExplicitMPC` and `NonLinMPC`

Author: franckgaga

Project.toml

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

README.md

@@ -82,6 +82,7 @@ for more detailed examples.
   - input setpoint tracking
   - terminal costs
   - custom economic costs (economic model predictive control)
+- control horizon distinct from prediction horizon and custom move blocking
 - adaptive linear model predictive controller
   - manual model modification
   - automatic successive linearization of a nonlinear model

docs/src/internals/predictive_control.md

@@ -12,6 +12,7 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 ## Controller Construction
 
 ```@docs
+ModelPredictiveControl.move_blocking
 ModelPredictiveControl.init_ZtoΔU   
 ModelPredictiveControl.init_ZtoU
 ModelPredictiveControl.init_predmat

docs/src/public/predictive_control.md

@@ -48,7 +48,8 @@ are shifted by one time step:
 in which ``\mathbf{U}`` and ``\mathbf{R̂_u}`` are both vectors of `nu*Hp` elements. Defining
 the manipulated input increment as ``\mathbf{Δu}(k+j) = \mathbf{u}(k+j) - \mathbf{u}(k+j-1)``,
 the control horizon ``H_c`` enforces that ``\mathbf{Δu}(k+j) = \mathbf{0}`` for ``j ≥ H_c``.
-For this reason, the vector that collects them is truncated up to ``k+H_c-1``:
+For this reason, the vector that collects them is truncated up to ``k+H_c-1`` (without
+any custom move blocking):
 
 ```math
     \mathbf{ΔU} =

src/controller/construct.jl

@@ -456,6 +456,72 @@ end
 "Return `0` when model is not a [`LinModel`](@ref)."
 estimate_delays(::SimModel) = 0
 
+
+@doc raw"""
+    move_blocking(Hp::Int, Hc::AbstractVector{Int}) -> nb
+
+Get the move blocking vector `nb` from the `Hc` argument, and modify it to match `Hp`.
+
+This feature is also known as manipulated variable blocking. The argument `Hc` is
+interpreted as the move blocking vector `nb`. It specifies the length of each step (or
+"block") in the ``\mathbf{ΔU}`` vector, to customize the pattern (in time steps, thus
+strictly positive integers):
+```math
+    \mathbf{n_b} = \begin{bmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{bmatrix}'
+```
+The vector that includes all the manipulated input increments ``\mathbf{Δu}`` is then
+defined as:
+```math
+\mathbf{ΔU} = \begin{bmatrix}
+    \mathbf{Δu}(k + 0)                                  \\[0.1em]
+    \mathbf{Δu}(k + ∑_{i=1}^1 n_i)                      \\[0.1em]
+    \mathbf{Δu}(k + ∑_{i=1}^2 n_i)                      \\[0.1em]
+    \vdots                                              \\[0.1em]
+    \mathbf{Δu}(k + ∑_{i=1}^{H_c-1} n_i)   
+\end{bmatrix}
+```
+The provided `nb` vector is modified to ensure `sum(nb) == Hp`:
+- If `sum(nb) < Hp`, a new element is pushed to `nb` with the value `Hp - sum(nb)`.
+- If `sum(nb) > Hp`, the intervals are truncated until `sum(nb) == Hp`. For example, if
+  `Hp = 10` and `nb = [1, 2, 3, 6, 7]`, then `nb` is truncated to `[1, 2, 3, 4]`.
+""" 
+function move_blocking(Hp_arg::Int, Hc_arg::AbstractVector{Int})
+    Hp = Hp_arg
+    nb = Hc_arg
+    all(>(0), nb) || throw(ArgumentError("Move blocking vector must be strictly positive integers."))
+    if sum(nb) < Hp
+        newblock = [Hp - sum(nb)]
+        nb = [nb; newblock]
+    elseif sum(nb) > Hp
+        nb = nb[begin:findfirst(≥(Hp), cumsum(nb))]
+        if sum(nb) > Hp
+            # if the last block is too large, it is truncated to fit Hp:
+            nb[end] = Hp - @views sum(nb[begin:end-1])
+        end
+    end
+    return nb
+end
+
+"""
+    move_blocking(Hp::Int, Hc::Int) -> nb
+
+Construct a move blocking vector `nb` that match the provided `Hp` and `Hc` integers.
+
+The vector is filled with `1`s, except for the last element which is `Hp - Hc + 1`.
+"""
+function move_blocking(Hp_arg::Int, Hc_arg::Int)
+    Hp, Hc = Hp_arg, Hc_arg
+    nb = fill(1, Hc)
+    if Hc > 0 # if Hc < 1, it will crash later with a clear error message
+        nb[end] = Hp - Hc + 1
+    end
+    return nb
+end
+
+"Get the actual control Horizon `Hc` (integer) from the move blocking vector `nb`."
+get_Hc(nb::AbstractVector{Int}) = length(nb)
+
+
 """
     validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u)
 

src/controller/explicitmpc.jl

@@ -10,6 +10,7 @@ struct ExplicitMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
+    nb::Vector{Int}
     weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
@@ -39,7 +40,7 @@ struct ExplicitMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function ExplicitMPC{NT}(
-        estim::SE, Hp, Hc, weights::CW
+        estim::SE, Hp, Hc, nb, weights::CW
     ) where {NT<:Real, SE<:StateEstimator, CW<:ControllerWeights}
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
@@ -50,7 +51,7 @@ struct ExplicitMPC{
         lastu0 = zeros(NT, nu)
         transcription = SingleShooting() # explicit MPC only supports SingleShooting
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
-        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
+        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
         # dummy val (updated just before optimization):
         F = zeros(NT, ny*Hp)
@@ -70,7 +71,7 @@ struct ExplicitMPC{
             estim,
             transcription,
             Z̃, ŷ,
-            Hp, Hc, nϵ,
+            Hp, Hc, nϵ, nb,
             weights,
             R̂u, R̂y,
             lastu0,
@@ -129,12 +130,12 @@ ExplicitMPC controller with a sample time Ts = 4.0 s, SteadyKalmanFilter estimat
 function ExplicitMPC(
     model::LinModel; 
     Hp::Int = default_Hp(model),
-    Hc::Int = DEFAULT_HC,
+    Hc::IntVectorOrInt = DEFAULT_HC,
     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)),
+    N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
     kwargs...
 ) 
@@ -152,12 +153,12 @@ Use custom state estimator `estim` to construct `ExplicitMPC`.
 function ExplicitMPC(
     estim::SE;
     Hp::Int = default_Hp(estim.model),
-    Hc::Int = DEFAULT_HC,
+    Hc::IntVectorOrInt = DEFAULT_HC,
     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)),
+    N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
 ) where {NT<:Real, SE<:StateEstimator{NT}}
     isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) 
@@ -166,8 +167,10 @@ function ExplicitMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
+    nb = move_blocking(Hp, Hc)
+    Hc = get_Hc(nb)
     weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp)
-    return ExplicitMPC{NT}(estim, Hp, Hc, weights)
+    return ExplicitMPC{NT}(estim, Hp, Hc, nb, weights)
 end
 
 setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.")

src/controller/linmpc.jl

@@ -19,6 +19,7 @@ struct LinMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
+    nb::Vector{Int}
     weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
@@ -47,7 +48,7 @@ struct LinMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function LinMPC{NT}(
-        estim::SE, Hp, Hc, weights::CW, 
+        estim::SE, Hp, Hc, nb, weights::CW, 
         transcription::TM, optim::JM
     ) where {
             NT<:Real, 
@@ -63,7 +64,7 @@ struct LinMPC{
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
-        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
+        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
             model, estim, transcription, Hp, Hc
         )
@@ -90,7 +91,7 @@ struct LinMPC{
         mpc = new{NT, SE, CW, TM, JM}(
             estim, transcription, optim, con,
             Z̃, ŷ,
-            Hp, Hc, nϵ,
+            Hp, Hc, nϵ, nb,
             weights,
             R̂u, R̂y,
             lastu0,
@@ -145,8 +146,9 @@ arguments. This controller allocates memory at each time step for the optimizati
 
 # 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``.
+- `Hp::Int=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays in `model`.
+- `Hc::Union{Int, Vector{Int}}=2` : control horizon ``H_c``, custom move blocking pattern is 
+   specified with a vector of integers (see [`move_blocking`](@ref) for details).
 - `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).
@@ -205,12 +207,12 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil
 function LinMPC(
     model::LinModel;
     Hp::Int = default_Hp(model),
-    Hc::Int = DEFAULT_HC,
+    Hc::IntVectorOrInt = DEFAULT_HC,
     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)),
+    N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt = DEFAULT_CWT,
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
@@ -227,7 +229,8 @@ end
 
 Use custom state estimator `estim` to construct `LinMPC`.
 
-`estim.model` must be a [`LinModel`](@ref). Else, a [`NonLinMPC`](@ref) is required. 
+`estim.model` must be a [`LinModel`](@ref). Else, a [`NonLinMPC`](@ref) is required. See
+[`ManualEstimator`](@ref) for linear controllers with nonlinear state estimation.
 
 # Examples
 ```jldoctest
@@ -248,12 +251,12 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, KalmanFilter es
 function LinMPC(
     estim::SE;
     Hp::Int = default_Hp(estim.model),
-    Hc::Int = DEFAULT_HC,
+    Hc::IntVectorOrInt = DEFAULT_HC,
     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)),
+    N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
@@ -265,8 +268,10 @@ function LinMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
+    nb = move_blocking(Hp, Hc)
+    Hc = get_Hc(nb)
     weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
-    return LinMPC{NT}(estim, Hp, Hc, weights, transcription, optim)
+    return LinMPC{NT}(estim, Hp, Hc, nb, weights, transcription, optim)
 end
 
 """

src/controller/nonlinmpc.jl

@@ -33,6 +33,7 @@ struct NonLinMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
+    nb::Vector{Int}
     weights::CW
     JE::JEfunc
     p::PT
@@ -63,7 +64,7 @@ struct NonLinMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function NonLinMPC{NT}(
-        estim::SE, Hp, Hc, weights::CW,
+        estim::SE, Hp, Hc, nb, weights::CW,
         JE::JEfunc, gc!::GCfunc, nc, p::PT, 
         transcription::TM, optim::JM, 
         gradient::GB, jacobian::JB
@@ -86,7 +87,7 @@ struct NonLinMPC{
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         lastu0 = zeros(NT, nu)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
-        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc)
+        Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
             model, estim, transcription, Hp, Hc
         )
@@ -116,7 +117,7 @@ struct NonLinMPC{
             estim, transcription, optim, con,
             gradient, jacobian,
             Z̃, ŷ,
-            Hp, Hc, nϵ,
+            Hp, Hc, nϵ, nb,
             weights,
             JE, p,
             R̂u, R̂y,
@@ -188,8 +189,10 @@ This controller allocates memory at each time step for the optimization.
 
 # Arguments
 - `model::SimModel` : model used for controller predictions and state estimations.
-- `Hp=nothing`: prediction horizon ``H_p``, must be specified for [`NonLinModel`](@ref).
-- `Hc=2` : control horizon ``H_c``.
+- `Hp::Int=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays if `model` is a
+   [`LinModel`](@ref) (must be specified otherwise).
+- `Hc::Union{Int, Vector{Int}}=2` : control horizon ``H_c``, custom move blocking pattern is 
+   specified with a vector of integers (see [`move_blocking`](@ref) for details).
 - `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).
@@ -281,12 +284,12 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
 function NonLinMPC(
     model::SimModel;
     Hp::Int = default_Hp(model),
-    Hc::Int = DEFAULT_HC,
+    Hc::IntVectorOrInt = DEFAULT_HC,
     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)),
+    N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
@@ -338,12 +341,12 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
 function NonLinMPC(
     estim::SE;
     Hp::Int = default_Hp(estim.model),
-    Hc::Int = DEFAULT_HC,
+    Hc::IntVectorOrInt = DEFAULT_HC,
     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)),
+    N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
     L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
@@ -365,11 +368,13 @@ function NonLinMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
+    nb = move_blocking(Hp, Hc)
+    Hc = get_Hc(nb)
     validate_JE(NT, JE)
     gc! = get_mutating_gc(NT, gc)
     weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
     return NonLinMPC{NT}(
-        estim, Hp, Hc, weights, JE, gc!, nc, p, transcription, optim, gradient, jacobian
+        estim, Hp, Hc, nb, weights, JE, gc!, nc, p, transcription, optim, gradient, jacobian
     )
 end