diff --git a/Project.toml b/Project.toml index 1312d184d..fa8630df1 100644 --- a/Project.toml +++ b/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" diff --git a/README.md b/README.md index 72962eb5c..680d47754 100644 --- a/README.md +++ b/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 diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 415927e83..18e960663 100644 --- a/docs/src/internals/predictive_control.md +++ b/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 diff --git a/docs/src/public/predictive_control.md b/docs/src/public/predictive_control.md index 068a1ab2a..439b585ef 100644 --- a/docs/src/public/predictive_control.md +++ b/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} = diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 294b32d50..b41224bca 100644 --- a/src/controller/construct.jl +++ b/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) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index cfb4dac7c..e22dd79d7 100644 --- a/src/controller/explicitmpc.jl +++ b/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.") diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 3d54586d2..74aa10178 100644 --- a/src/controller/linmpc.jl +++ b/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 """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e39f8d8d8..29d18a278 100644 --- a/src/controller/nonlinmpc.jl +++ b/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 diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index c81e8b3fe..d94105d90 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -10,7 +10,8 @@ abstract type TranscriptionMethod end Construct a direct single shooting [`TranscriptionMethod`](@ref). -The decision variable in the optimization problem is (excluding the slack ``ϵ``): +The decision variable in the optimization problem is (excluding the slack ``ϵ`` and without +any custom move blocking): ```math \mathbf{Z} = \mathbf{ΔU} = \begin{bmatrix} \mathbf{Δu}(k+0) \\ @@ -53,10 +54,10 @@ for this transcription method. struct MultipleShooting <: TranscriptionMethod end "Get the number of elements in the optimization decision vector `Z`." -function get_nZ(estim::StateEstimator, transcription::SingleShooting, Hp, Hc) +function get_nZ(estim::StateEstimator, ::SingleShooting, Hp, Hc) return estim.model.nu*Hc end -function get_nZ(estim::StateEstimator, transcription::MultipleShooting, Hp, Hc) +function get_nZ(estim::StateEstimator, ::MultipleShooting, Hp, Hc) return estim.model.nu*Hc + estim.nx̂*Hp end @@ -70,6 +71,7 @@ increments over ``H_c``, is computed by: ```math \mathbf{ΔU} = \mathbf{P_{Δu}} \mathbf{Z} ``` + in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section. # Extended Help @@ -83,14 +85,14 @@ in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section. function init_ZtoΔU end function init_ZtoΔU( - estim::StateEstimator{NT}, transcription::SingleShooting, _ , Hc + estim::StateEstimator{NT}, ::SingleShooting, _ , Hc ) where {NT<:Real} PΔu = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc) return PΔu end function init_ZtoΔU( - estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc + estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc ) where {NT<:Real} I_nu_Hc = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc) PΔu = [I_nu_Hc zeros(NT, estim.model.nu*Hc, estim.nx̂*Hp)] @@ -98,7 +100,7 @@ function init_ZtoΔU( end @doc raw""" - init_ZtoU(estim, transcription, Hp, Hc) -> Pu, Tu + init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu Init decision variables to inputs over ``H_p`` conversion matrices. @@ -111,29 +113,32 @@ The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended H # Extended Help !!! details "Extended Help" + With ``n_i``, the ``i``th element of the ``\mathbf{n_b}`` vector defined in [`move_blocking`](@ref) + documentation, we introduce the ``\mathbf{Q}(i)`` matrix of size `(nu*ni, nu)`: + ```math + \mathbf{Q}(i) = \begin{bmatrix} + \mathbf{I} \\ + \mathbf{I} \\ + \vdots \\ + \mathbf{I} \end{bmatrix} + ``` The ``\mathbf{U}`` vector and the conversion matrices are defined as: ```math \mathbf{U} = \begin{bmatrix} - \mathbf{u}(k + 0) \\ - \mathbf{u}(k + 1) \\ - \vdots \\ - \mathbf{u}(k + H_c - 1) \\ - \vdots \\ - \mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad + \mathbf{u}(k + 0) \\ + \mathbf{u}(k + 1) \\ + \vdots \\ + \mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad \mathbf{P_u^†} = \begin{bmatrix} - \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{I} & \mathbf{I} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \end{bmatrix} , \quad + \mathbf{Q}(n_1) & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Q}(n_2) & \mathbf{Q}(n_2) & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} , \quad \mathbf{T_u} = \begin{bmatrix} - \mathbf{I} \\ - \mathbf{I} \\ - \vdots \\ - \mathbf{I} \\ - \vdots \\ - \mathbf{I} \end{bmatrix} + \mathbf{I} \\ + \mathbf{I} \\ + \vdots \\ + \mathbf{I} \end{bmatrix} ``` and, depending on the transcription method, we have: - ``\mathbf{P_u} = \mathbf{P_u^†}`` if `transcription` is a [`SingleShooting`](@ref) @@ -141,21 +146,28 @@ The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended H if `transcription` is a [`MultipleShooting`](@ref) """ function init_ZtoU( - estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc + estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb ) where {NT<:Real} - model = estim.model + nu = estim.model.nu # Pu and Tu are `Matrix{NT}`, conversion is faster than `Matrix{Bool}` or `BitMatrix` - I_nu = Matrix{NT}(I, model.nu, model.nu) - PU_Hc = LowerTriangular(repeat(I_nu, Hc, Hc)) - PUdagger = [PU_Hc; repeat(I_nu, Hp - Hc, Hc)] - Pu = init_PUmat(estim, transcription, Hp, Hc, PUdagger) + I_nu = Matrix{NT}(I, nu, nu) + PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc) + for i=1:Hc + ni = nb[i] + Qi = repeat(I_nu, ni, 1) + iRows = (1:nu*ni) .+ @views nu*sum(nb[1:i-1]) + PuDagger[iRows, :] = [repeat(Qi, 1, i) zeros(nu*ni, nu*(Hc-i))] + end + Pu = init_PUmat(estim, transcription, Hp, Hc, PuDagger) Tu = repeat(I_nu, Hp) return Pu, Tu end -init_PUmat( _ , transcription::SingleShooting, _ , _ , PUdagger) = PUdagger -function init_PUmat(estim, transcription::MultipleShooting, Hp, _ , PUdagger) - return [PUdagger zeros(eltype(PUdagger), estim.model.nu*Hp, estim.nx̂*Hp)] + + +init_PUmat( _ , ::SingleShooting, _ , _ , PuDagger) = PuDagger +function init_PUmat(estim, ::MultipleShooting, Hp, _ , PuDagger) + return [PuDagger zeros(eltype(PuDagger), estim.model.nu*Hp, estim.nx̂*Hp)] end @doc raw""" @@ -349,7 +361,7 @@ They are defined in the Extended Help section. ``` """ function init_predmat( - model::LinModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc + model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc ) where {NT<:Real} Ĉ, D̂d = estim.Ĉ, estim.D̂d nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd @@ -471,7 +483,7 @@ matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in th ``` """ function init_defectmat( - model::LinModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc + model::LinModel, estim::StateEstimator{NT}, ::MultipleShooting, Hp, Hc ) where {NT<:Real} nu, nx̂, nd = model.nu, estim.nx̂, model.nd Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d @@ -881,7 +893,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript end "Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`." -function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod) +function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod) nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ # here, updating fx̂ is not necessary since fx̂ = 0 @@ -971,7 +983,7 @@ If supported by `mpc.optim`, it warm-starts the solver at: where ``\mathbf{Δu}(k+j|k-1)`` is the input increment for time ``k+j`` computed at the last control period ``k-1``, and ``ϵ(k-1)``, the slack variable of the last control period. """ -function set_warmstart!(mpc::PredictiveController, transcription::SingleShooting, Z̃var) +function set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) nu, Hc, Z̃s = mpc.estim.model.nu, mpc.Hc, mpc.buffer.Z̃ # --- input increments ΔU --- Z̃s[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] @@ -1008,7 +1020,7 @@ where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` comput last control period ``k-1``, expressed as a deviation from the operating point ``\mathbf{x̂_{op}}``. """ -function set_warmstart!(mpc::PredictiveController, transcription::MultipleShooting, Z̃var) +function set_warmstart!(mpc::PredictiveController, ::MultipleShooting, Z̃var) nu, nx̂, Hp, Hc, Z̃s = mpc.estim.model.nu, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.buffer.Z̃ # --- input increments ΔU --- Z̃s[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] @@ -1022,7 +1034,7 @@ function set_warmstart!(mpc::PredictiveController, transcription::MultipleShooti return Z̃s end -getΔŨ!(ΔŨ, mpc::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃) +getΔŨ!(ΔŨ, ::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃) function getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃) # avoid explicit matrix multiplication with mpc.P̃Δu for performance: nΔU = mpc.Hc*mpc.estim.model.nu @@ -1136,7 +1148,7 @@ The method mutates the `g` vectors in argument and returns it. Only the custom c are include in the `g` vector. """ function con_nonlinprog!( - g, mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ + g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ ) for i in eachindex(g) g[i] = gc[i] diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 247f53a6b..59dd0cedb 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -116,7 +116,7 @@ function init_integrators(nint::IntVectorOrInt, ny, varname::String) nint = fill(0, ny) end if length(nint) ≠ ny - error("nint_$(varname) size ($(length(nint))) ≠ n$(varname) ($ny)") + error("nint_$(varname) length ($(length(nint))) ≠ n$(varname) ($ny)") end any(nint .< 0) && error("nint_$(varname) values should be ≥ 0") nx = sum(nint) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index f7b255b67..7c138cd59 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -102,7 +102,7 @@ InternalModel estimator with a sample time Ts = 0.5 s, LinModel and: """ function InternalModel( model::SM; - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, stoch_ym::LTISystem = (In = I(length(i_ym)); ss(In, In, In, In, model.Ts)) ) where {NT<:Real, SM<:SimModel{NT}} stoch_ym = minreal(ss(stoch_ym)) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3986b3ce7..397d8014d 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -164,7 +164,7 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: """ function SteadyKalmanFilter( model::SM; - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), nint_u ::IntVectorOrInt = 0, @@ -393,7 +393,7 @@ KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: """ function KalmanFilter( model::SM; - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, sigmaP_0 = fill(1/model.nx, model.nx), sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), @@ -648,7 +648,7 @@ UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and: """ function UnscentedKalmanFilter( model::SM; - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, sigmaP_0 = fill(1/model.nx, model.nx), sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), @@ -1001,7 +1001,7 @@ ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and: """ function ExtendedKalmanFilter( model::SM; - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, sigmaP_0 = fill(1/model.nx, model.nx), sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 8c574350f..c186bf157 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -93,7 +93,7 @@ Luenberger estimator with a sample time Ts = 0.5 s, LinModel and: """ function Luenberger( model::SM; - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, nint_u ::IntVectorOrInt = 0, nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5, diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 19897c98d..4958256f0 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -376,7 +376,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer function MovingHorizonEstimator( model::SM; He::Union{Int, Nothing} = nothing, - i_ym::IntRangeOrVector = 1:model.ny, + i_ym::AbstractVector{Int} = 1:model.ny, sigmaP_0 = fill(1/model.nx, model.nx), sigmaQ = fill(1/model.nx, model.nx), sigmaR = fill(1, length(i_ym)), diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index ab779713f..a6fe34c39 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -136,8 +136,8 @@ LinModel with a sample time Ts = 0.1 s and: function LinModel( sys::StateSpace{E, NT}, Ts::Union{Real,Nothing} = nothing; - i_u::IntRangeOrVector = 1:size(sys,2), - i_d::IntRangeOrVector = Int[] + i_u::AbstractVector{Int} = 1:size(sys,2), + i_d::AbstractVector{Int} = Int[] ) where {E, NT<:Real} if !isempty(i_d) # common indexes in i_u and i_d are interpreted as measured disturbances d : diff --git a/src/plot_sim.jl b/src/plot_sim.jl index 36538f298..7261e636d 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -319,7 +319,7 @@ end "Keep manipulated input `u` unchanged for state estimator simulation." sim_getu!(::StateEstimator, u, _ , _ ) = u -function get_indices(arg::IntRangeOrVector, n) +function get_indices(arg::AbstractVector{Int}, n) if length(unique(arg)) ≠ length(arg) || maximum(arg) > n error("Plot keyword argument arguments should contains valid and unique indices") end diff --git a/src/sim_model.jl b/src/sim_model.jl index c9ee82bb2..d79063e16 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -1,5 +1,3 @@ -const IntRangeOrVector = Union{UnitRange{Int}, Vector{Int}} - @doc raw""" Abstract supertype of [`LinModel`](@ref) and [`NonLinModel`](@ref) types. diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index d9b91cf92..4a98ddd23 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -47,6 +47,12 @@ @test mpc14.transcription == MultipleShooting() @test length(mpc14.Z̃) == model2.nu*mpc14.Hc + mpc14.estim.nx̂*mpc14.Hp + mpc14.nϵ @test size(mpc14.con.Aeq, 1) == mpc14.estim.nx̂*mpc14.Hp + mpc15 = LinMPC(model, Hc=[1,2,3], Hp=10, Cwt=Inf) + @test mpc15.Hc == 4 # the constructor will push an element to nb + @test size(mpc15.P̃u) == (10*mpc1.estim.model.nu, 4*mpc1.estim.model.nu) + mpc16 = LinMPC(model, Hc=[1,2,3,6,6,6], Hp=10, Cwt=Inf) + @test mpc16.Hc == 4 # the last 2 elements of Hc are ignored + @test size(mpc16.P̃u) == (10*mpc1.estim.model.nu, 4*mpc1.estim.model.nu) @test_logs( (:warn, @@ -108,6 +114,13 @@ end d = [0.1] u = moveinput!(mpc6, 7d, d) @test u ≈ [0] atol=1e-2 + mpc7 = LinMPC(linmodel, Hp=10, Hc=[1, 2, 3, 4], Nwt=[10]) + preparestate!(mpc7, [10]) + r = [15] + moveinput!(mpc7, r) + ΔU_diff = diff(getinfo(mpc7)[:U]) + @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9 + @test_throws DimensionMismatch moveinput!(mpc1, [0,0,0]) @test_throws DimensionMismatch moveinput!(mpc1, [0], [0,0]) @test_throws DimensionMismatch moveinput!(mpc1; D̂ = fill(0, mpc1.Hp+1)) @@ -447,7 +460,8 @@ end @testitem "ExplicitMPC moves and getinfo" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - mpc1 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1) + model = LinModel(tf(5, [2, 1]), 3) + mpc1 = ExplicitMPC(model, Nwt=[0], Hp=1000, Hc=1) r, y = [5], [0] preparestate!(mpc1, y) u = moveinput!(mpc1, r) @@ -458,18 +472,24 @@ end info = getinfo(mpc1) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ r[1] atol=1e-2 - mpc2 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1) - preparestate!(mpc2, [0]) + mpc2 = ExplicitMPC(model, Nwt=[0], Hp=1000, Hc=1) + preparestate!(mpc2, y) u = moveinput!(mpc2, [5]) @test u ≈ [1] atol=1e-2 - mpc3 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1]) - preparestate!(mpc3, [0]) + mpc3 = ExplicitMPC(model, Mwt=[0], Nwt=[0], Lwt=[1]) + preparestate!(mpc3, y) u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp)) @test u ≈ [12] atol=1e-2 model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mpc4 = ExplicitMPC(model2) - preparestate!(mpc4, [0]) + preparestate!(mpc4, y) moveinput!(mpc4, [0]) ≈ [0.0] + mpc5 = ExplicitMPC(model, Hp=10, Hc=[1, 2, 3, 4], Nwt=[10]) + preparestate!(mpc5, y) + moveinput!(mpc5, r) + ΔU_diff = diff(getinfo(mpc5)[:U]) + @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9 + @test_nowarn ModelPredictiveControl.info2debugstr(info) end @@ -770,6 +790,12 @@ end info = getinfo(nmpc10) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 + nmpc11 = NonLinMPC(nonlinmodel, Hp=10, Hc=[1, 2, 3, 4], Nwt=[10]) + preparestate!(nmpc11, y, [0]) + moveinput!(nmpc11, [10], [0]) + ΔU_diff = diff(getinfo(nmpc11)[:U]) + println(ΔU_diff) + @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) end