diff --git a/Project.toml b/Project.toml index b81bc3d0c..14e9d5006 100644 --- a/Project.toml +++ b/Project.toml @@ -1,35 +1,37 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.3.5" +version = "1.4.0" [deps] +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" +Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" PreallocationTools = "d236fae5-4411-538c-8e31-a6e3d9e00b46" PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" ProgressLogging = "33c8b6b6-d38a-422a-b730-caa89a2f386c" -Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" [compat] +julia = "1.10" +LinearAlgebra = "1.10" +Logging = "1.10" +Random = "1.10" ControlSystemsBase = "1.9" ForwardDiff = "0.10" Ipopt = "1" JuMP = "1.21" -LinearAlgebra = "1.6" OSQP = "0.8" PreallocationTools = "0.4.14" PrecompileTools = "1" ProgressLogging = "0.1" -Random = "1.6" RecipesBase = "1" TestItemRunner = "1.1" -julia = "1.10" [extras] DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" diff --git a/docs/Project.toml b/docs/Project.toml index 5382034d5..cd71da677 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -1,19 +1,19 @@ [deps] +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" DAQP = "c47d62df-3981-49c8-9651-128b1cd08617" Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" -LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" -Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" [compat] +LinearAlgebra = "1.10" +Logging = "1.10" ControlSystemsBase = "1" -DAQP = "0.5" Documenter = "1" JuMP = "1" -LinearAlgebra = "1.6" -Logging = "1.6" +DAQP = "0.6" Plots = "1" ModelingToolkit = "9.50" diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 2b94ce2e6..eb49a9546 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -12,8 +12,10 @@ The prediction methodology of this module is mainly based on Maciejowski textboo ## Controller Construction ```@docs -ModelPredictiveControl.init_ΔUtoU +ModelPredictiveControl.init_ZtoΔU +ModelPredictiveControl.init_ZtoU ModelPredictiveControl.init_predmat +ModelPredictiveControl.init_defectmat ModelPredictiveControl.relaxU ModelPredictiveControl.relaxΔU ModelPredictiveControl.relaxŶ @@ -21,6 +23,7 @@ ModelPredictiveControl.relaxterminal ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc +ModelPredictiveControl.init_nonlincon! ``` ## Update Quadratic Optimization @@ -28,11 +31,13 @@ ModelPredictiveControl.init_matconstraint_mpc ```@docs ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any) ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel) +ModelPredictiveControl.linconstrainteq! ``` ## Solve Optimization Problem ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) +ModelPredictiveControl.set_warmstart! ModelPredictiveControl.getinput ``` diff --git a/docs/src/public/predictive_control.md b/docs/src/public/predictive_control.md index 729d00dde..068a1ab2a 100644 --- a/docs/src/public/predictive_control.md +++ b/docs/src/public/predictive_control.md @@ -88,3 +88,23 @@ NonLinMPC ```@docs moveinput! ``` + +## Direct Transcription Methods + +### TranscriptionMethod + +```@docs +ModelPredictiveControl.TranscriptionMethod +``` + +### SingleShooting + +```@docs +SingleShooting +``` + +### MultipleShooting + +```@docs +MultipleShooting +``` diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index da4ab2ebe..95d8acf25 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -32,6 +32,7 @@ export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFi export MovingHorizonEstimator export default_nint, initstate! export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput! +export SingleShooting, MultipleShooting export SimResult, getinfo, sim! include("general.jl") diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 0b00c6ceb..77f3431b6 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -1,3 +1,37 @@ +struct PredictiveControllerBuffer{NT<:Real} + u ::Vector{NT} + Z̃ ::Vector{NT} + D̂ ::Vector{NT} + Ŷ ::Vector{NT} + U ::Vector{NT} + Ẽ ::Matrix{NT} + P̃u::Matrix{NT} + empty::Vector{NT} +end + +@doc raw""" + PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) + +Create a buffer for `PredictiveController` objects. + +The buffer is used to store intermediate results during computation without allocating. +""" +function PredictiveControllerBuffer( + estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp::Int, Hc::Int, nϵ::Int +) where NT <: Real + nu, ny, nd, nx̂ = estim.model.nu, estim.model.ny, estim.model.nd, estim.nx̂ + nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + u = Vector{NT}(undef, nu) + Z̃ = Vector{NT}(undef, nZ̃) + D̂ = Vector{NT}(undef, nd*Hp) + Ŷ = Vector{NT}(undef, ny*Hp) + U = Vector{NT}(undef, nu*Hp) + Ẽ = Matrix{NT}(undef, ny*Hp, nZ̃) + P̃u = Matrix{NT}(undef, nu*Hp, nZ̃) + empty = Vector{NT}(undef, 0) + return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, Ẽ, P̃u, empty) +end + "Include all the objective function weights of [`PredictiveController`](@ref)" struct ControllerWeights{NT<:Real} M_Hp::Hermitian{NT, Matrix{NT}} @@ -36,6 +70,7 @@ end "Include all the data for the constraints of [`PredictiveController`](@ref)" struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} + # matrices for the terminal constraints: ẽx̂ ::Matrix{NT} fx̂ ::Vector{NT} gx̂ ::Matrix{NT} @@ -43,6 +78,15 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} kx̂ ::Matrix{NT} vx̂ ::Matrix{NT} bx̂ ::Vector{NT} + # matrices for the zero defect constraints (N/A for single shooting transcriptions): + Ẽŝ ::Matrix{NT} + Fŝ ::Vector{NT} + Gŝ ::Matrix{NT} + Jŝ ::Matrix{NT} + Kŝ ::Matrix{NT} + Vŝ ::Matrix{NT} + Bŝ ::Vector{NT} + # bounds over the prediction horizon (deviation vectors from operating points): U0min ::Vector{NT} U0max ::Vector{NT} ΔŨmin ::Vector{NT} @@ -51,6 +95,7 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Y0max ::Vector{NT} x̂0min ::Vector{NT} x̂0max ::Vector{NT} + # A matrices for the linear inequality constraints: A_Umin ::Matrix{NT} A_Umax ::Matrix{NT} A_ΔŨmin ::Matrix{NT} @@ -60,13 +105,25 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} A_x̂min ::Matrix{NT} A_x̂max ::Matrix{NT} A ::Matrix{NT} + # b vector for the linear inequality constraints: b ::Vector{NT} + # indices of finite numbers in the b vector (linear inequality constraints): i_b ::BitVector + # A matrices for the linear equality constraints: + A_ŝ ::Matrix{NT} + Aeq ::Matrix{NT} + # b vector for the linear equality constraints: + beq ::Vector{NT} + # nonlinear equality constraints: + neq ::Int + # constraint softness parameter vectors for the nonlinear inequality constraints: C_ymin ::Vector{NT} C_ymax ::Vector{NT} c_x̂min ::Vector{NT} c_x̂max ::Vector{NT} + # indices of finite numbers in the g vector (nonlinear inequality constraints): i_g ::BitVector + # custom nonlinear inequality constraints: gc! ::GCfunc nc ::Int end @@ -183,7 +240,8 @@ function setconstraint!( ΔUmin = DeltaUmin, ΔUmax = DeltaUmax, C_Δumin = C_Deltaumin, C_Δumax = C_Deltaumax, ) - model, con, optim = mpc.estim.model, mpc.con, mpc.optim + model, con = mpc.estim.model, mpc.con + transcription, optim = mpc.transcription, mpc.optim nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc nϵ, nc = mpc.nϵ, con.nc notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) @@ -315,22 +373,23 @@ function setconstraint!( i_x̂min, i_x̂max = .!isinf.(con.x̂0min), .!isinf.(con.x̂0max) if notSolvedYet con.i_b[:], con.i_g[:], con.A[:] = init_matconstraint_mpc( - model, nc, + model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, con.A_Umin, con.A_Umax, con.A_ΔŨmin, con.A_ΔŨmax, - con.A_Ymin, con.A_Ymax, con.A_x̂min, con.A_x̂max + con.A_Ymin, con.A_Ymax, con.A_x̂min, con.A_x̂max, + con.A_ŝ ) A = con.A[con.i_b, :] b = con.b[con.i_b] - ΔŨvar::Vector{JuMP.VariableRef} = optim[:ΔŨvar] + Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) - @constraint(optim, linconstraint, A*ΔŨvar .≤ b) + @constraint(optim, linconstraint, A*Z̃var .≤ b) set_nonlincon!(mpc, model, optim) else i_b, i_g = init_matconstraint_mpc( - model, nc, + model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max ) @@ -344,60 +403,88 @@ end @doc raw""" init_matconstraint_mpc( - model::LinModel, nc::Int, + model::LinModel, transcription::TranscriptionMethod, nc::Int, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args... - ) -> i_b, i_g, A + ) -> i_b, i_g, A, Aeq, neq -Init `i_b`, `i_g` and `A` matrices for the linear and nonlinear inequality constraints. +Init `i_b`, `i_g`, `neq`, and `A` and `Aeq` matrices for the all the MPC constraints. -The linear and nonlinear inequality constraints are respectively defined as: +The linear and nonlinear constraints are respectively defined as: ```math \begin{aligned} - \mathbf{A ΔŨ } &≤ \mathbf{b} \\ - \mathbf{g(ΔŨ)} &≤ \mathbf{0} + \mathbf{A Z̃ } &≤ \mathbf{b} \\ + \mathbf{A_{eq} Z̃} &= \mathbf{b_{eq}} \\ + \mathbf{g(Z̃)} &≤ \mathbf{0} \\ + \mathbf{g_{eq}(Z̃)} &= \mathbf{0} \\ \end{aligned} ``` -The argument `nc` is the number of custom nonlinear constraints in ``\mathbf{g_c}``. `i_b` -is a `BitVector` including the indices of ``\mathbf{b}`` that are finite numbers. `i_g` is a -similar vector but for the indices of ``\mathbf{g}``. The method also returns the -``\mathbf{A}`` matrix if `args` is provided. In such a case, `args` needs to contain all -the inequality constraint matrices: -`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max`. +The argument `nc` is the number of custom nonlinear inequality constraints in +``\mathbf{g_c}``. `i_b` is a `BitVector` including the indices of ``\mathbf{b}`` that are +finite numbers. `i_g` is a similar vector but for the indices of ``\mathbf{g}``. The method +also returns the ``\mathbf{A, A_{eq}}`` matrices and `neq` if `args` is provided. In such a +case, `args` needs to contain all the inequality and equality constraint matrices: +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ`. The integer `neq` +is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mpc( - ::LinModel{NT}, nc::Int, + ::LinModel{NT}, ::TranscriptionMethod, nc::Int, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args... ) where {NT<:Real} + if isempty(args) + A, Aeq, neq = nothing, nothing, nothing + else + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max] + Aeq = A_ŝ + neq = 0 + end i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_x̂min; i_x̂max] i_g = trues(nc) + return i_b, i_g, A, Aeq, neq +end + +"Init `i_b` without output constraints if [`NonLinModel`](@ref) & [`MultipleShooting`](@ref)." +function init_matconstraint_mpc( + ::NonLinModel{NT}, ::MultipleShooting, nc::Int, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + args... +) where {NT<:Real} if isempty(args) - A = nothing + A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max] + Aeq = A_ŝ + nΔŨ, nZ̃ = size(A_ΔŨmin) + neq = nZ̃ - nΔŨ end - return i_b, i_g, A + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_x̂min; i_x̂max] + i_g = [i_Ymin; i_Ymax; trues(nc)] + return i_b, i_g, A, Aeq, neq end -"Init `i_b, A` without outputs and terminal constraints if `model` is not a [`LinModel`](@ref)." +"Init `i_b` without output & terminal constraints if [`NonLinModel`](@ref) & [`SingleShooting`](@ref)." function init_matconstraint_mpc( - ::SimModel{NT}, nc::Int, + ::NonLinModel{NT}, ::SingleShooting, nc::Int, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, args... ) where {NT<:Real} - i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax] - i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] if isempty(args) - A = nothing + A, Aeq, neq = nothing, nothing, nothing else - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , _ , _ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂min, A_x̂max, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ymin; A_Ymax; A_x̂min; A_x̂max] + Aeq = A_ŝ + neq = 0 end - return i_b, i_g, A + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax] + i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] + return i_b, i_g, A, Aeq, neq end + "By default, there is no nonlinear constraint, thus do nothing." set_nonlincon!(::PredictiveController, ::SimModel, ::JuMP.GenericModel) = nothing @@ -445,273 +532,59 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u) size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) end - @doc raw""" - init_ΔUtoU(model, Hp, Hc) -> S, T - -Init manipulated input increments to inputs conversion matrices. - -The conversion from the input increments ``\mathbf{ΔU}`` to manipulated inputs over ``H_p`` -are calculated by: -```math -\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1) -``` -The ``\mathbf{S}`` and ``\mathbf{T}`` matrices are defined in the Extended Help section. - -# Extended Help -!!! details "Extended Help" - The ``\mathbf{U}`` vector and the two 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{S} = \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{T} = \begin{bmatrix} - \mathbf{I} \\ - \mathbf{I} \\ - \vdots \\ - \mathbf{I} \\ - \vdots \\ - \mathbf{I} \end{bmatrix} - ``` -""" -function init_ΔUtoU(model::SimModel{NT}, Hp, Hc) where {NT<:Real} - # S and T are `Matrix{NT}` since conversion is faster than `Matrix{Bool}` or `BitMatrix` - I_nu = Matrix{NT}(I, model.nu, model.nu) - S_Hc = LowerTriangular(repeat(I_nu, Hc, Hc)) - S = [S_Hc; repeat(I_nu, Hp - Hc, Hc)] - T = repeat(I_nu, Hp) - return S, T -end - - -@doc raw""" - init_predmat(estim, model::LinModel, Hp, Hc) -> E, G, J, K, V, ex̂, gx̂, jx̂, kx̂, vx̂ - -Construct the prediction matrices for [`LinModel`](@ref) `model`. - -The model predictions are evaluated from the deviation vectors (see [`setop!`](@ref)) and: -```math -\begin{aligned} - \mathbf{Ŷ_0} &= \mathbf{E ΔU} + \mathbf{G d_0}(k) + \mathbf{J D̂_0} - + \mathbf{K x̂_0}(k) + \mathbf{V u_0}(k-1) - + \mathbf{B} + \mathbf{Ŷ_s} \\ - &= \mathbf{E ΔU} + \mathbf{F} -\end{aligned} -``` -in which ``\mathbf{x̂_0}(k) = \mathbf{x̂}_i(k) - \mathbf{x̂_{op}}``, with ``i = k`` if -`estim.direct==true`, otherwise ``i = k - 1``. The predicted outputs ``\mathbf{Ŷ_0}`` and -measured disturbances ``\mathbf{D̂_0}`` respectively include ``\mathbf{ŷ_0}(k+j)`` and -``\mathbf{d̂_0}(k+j)`` values with ``j=1`` to ``H_p``, and input increments ``\mathbf{ΔU}``, -``\mathbf{Δu}(k+j)`` from ``j=0`` to ``H_c-1``. The vector ``\mathbf{B}`` contains the -contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` -operating points (for linearization at non-equilibrium point, see [`linearize`](@ref)). The -stochastic predictions ``\mathbf{Ŷ_s=0}`` if `estim` is not a [`InternalModel`](@ref), see -[`init_stochpred`](@ref). The method also computes similar matrices for the predicted -terminal states at ``k+H_p``: -```math -\begin{aligned} - \mathbf{x̂_0}(k+H_p) &= \mathbf{e_x̂ ΔU} + \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} - + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u_0}(k-1) - + \mathbf{b_x̂} \\ - &= \mathbf{e_x̂ ΔU} + \mathbf{f_x̂} -\end{aligned} -``` -The matrices ``\mathbf{E, G, J, K, V, B, e_x̂, g_x̂, j_x̂, k_x̂, v_x̂, b_x̂}`` are defined in the -Extended Help section. The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at -each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). - -# Extended Help -!!! details "Extended Help" - Using the augmented matrices ``\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}`` in `estim` (see - [`augment_model`](@ref)), and the function ``\mathbf{W}(j) = ∑_{i=0}^j \mathbf{Â}^i``, - the prediction matrices are computed by : - ```math - \begin{aligned} - \mathbf{E} &= \begin{bmatrix} - \mathbf{Ĉ W}(0)\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ W}(1)\mathbf{B̂_u} & \mathbf{Ĉ W}(0)\mathbf{B̂_u} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ W}(H_p-1)\mathbf{B̂_u} & \mathbf{Ĉ W}(H_p-2)\mathbf{B̂_u} & \cdots & \mathbf{Ĉ W}(H_p-H_c+1)\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{G} &= \begin{bmatrix} - \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} \\ - \mathbf{Ĉ}\mathbf{Â}^{1} \mathbf{B̂_d} \\ - \vdots \\ - \mathbf{Ĉ}\mathbf{Â}^{H_p-1} \mathbf{B̂_d} \end{bmatrix} \\ - \mathbf{J} &= \begin{bmatrix} - \mathbf{D̂_d} & \mathbf{0} & \cdots & \mathbf{0} \\ - \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} & \mathbf{D̂_d} & \cdots & \mathbf{0} \\ - \vdots & \vdots & \ddots & \vdots \\ - \mathbf{Ĉ}\mathbf{Â}^{H_p-2} \mathbf{B̂_d} & \mathbf{Ĉ}\mathbf{Â}^{H_p-3} \mathbf{B̂_d} & \cdots & \mathbf{D̂_d} \end{bmatrix} \\ - \mathbf{K} &= \begin{bmatrix} - \mathbf{Ĉ}\mathbf{Â}^{1} \\ - \mathbf{Ĉ}\mathbf{Â}^{2} \\ - \vdots \\ - \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ - \mathbf{V} &= \begin{bmatrix} - \mathbf{Ĉ W}(0)\mathbf{B̂_u} \\ - \mathbf{Ĉ W}(1)\mathbf{B̂_u} \\ - \vdots \\ - \mathbf{Ĉ W}(H_p-1)\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{B} &= \begin{bmatrix} - \mathbf{Ĉ W}(0) \\ - \mathbf{Ĉ W}(1) \\ - \vdots \\ - \mathbf{Ĉ W}(H_p-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} - \end{aligned} - ``` - For the terminal constraints, the matrices are computed with: - ```math - \begin{aligned} - \mathbf{e_x̂} &= \begin{bmatrix} - \mathbf{W}(H_p-1)\mathbf{B̂_u} & - \mathbf{W}(H_p-2)\mathbf{B̂_u} & - \cdots & - \mathbf{W}(H_p-H_c+1)\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ - \mathbf{j_x̂} &= \begin{bmatrix} - \mathbf{Â}^{H_p-2} \mathbf{B̂_d} & - \mathbf{Â}^{H_p-3} \mathbf{B̂_d} & - \cdots & - \mathbf{0} - \end{bmatrix} \\ - \mathbf{k_x̂} &= \mathbf{Â}^{H_p} \\ - \mathbf{v_x̂} &= \mathbf{W}(H_p-1)\mathbf{B̂_u} \\ - \mathbf{b_x̂} &= \mathbf{W}(H_p-1) \mathbf{\big(f̂_{op} - x̂_{op}\big)} - \end{aligned} - ``` -""" -function init_predmat(estim::StateEstimator{NT}, model::LinModel, Hp, Hc) where {NT<:Real} - Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d - nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd - # --- pre-compute matrix powers --- - # Apow 3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,Hp+1] = A^Hp - Âpow = Array{NT}(undef, nx̂, nx̂, Hp+1) - Âpow[:,:,1] = I(nx̂) - for j=2:Hp+1 - Âpow[:,:,j] = @views Âpow[:,:,j-1]*Â - end - # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... - Âpow_csum = cumsum(Âpow, dims=3) - # helper function to improve code clarity and be similar to eqs. in docstring: - getpower(array3D, power) = @views array3D[:,:, power+1] - # --- state estimates x̂ --- - kx̂ = getpower(Âpow, Hp) - K = Matrix{NT}(undef, Hp*ny, nx̂) - for j=1:Hp - iRow = (1:ny) .+ ny*(j-1) - K[iRow,:] = Ĉ*getpower(Âpow, j) - end - # --- manipulated inputs u --- - vx̂ = getpower(Âpow_csum, Hp-1)*B̂u - V = Matrix{NT}(undef, Hp*ny, nu) - for j=1:Hp - iRow = (1:ny) .+ ny*(j-1) - V[iRow,:] = Ĉ*getpower(Âpow_csum, j-1)*B̂u - end - ex̂ = Matrix{NT}(undef, nx̂, Hc*nu) - E = zeros(NT, Hp*ny, Hc*nu) - for j=1:Hc # truncated with control horizon - iRow = (ny*(j-1)+1):(ny*Hp) - iCol = (1:nu) .+ nu*(j-1) - E[iRow, iCol] = V[iRow .- ny*(j-1),:] - ex̂[: , iCol] = getpower(Âpow_csum, Hp-j)*B̂u - end - # --- measured disturbances d --- - gx̂ = getpower(Âpow, Hp-1)*B̂d - G = Matrix{NT}(undef, Hp*ny, nd) - jx̂ = Matrix{NT}(undef, nx̂, Hp*nd) - J = repeatdiag(D̂d, Hp) - if nd ≠ 0 - for j=1:Hp - iRow = (1:ny) .+ ny*(j-1) - G[iRow,:] = Ĉ*getpower(Âpow, j-1)*B̂d - end - for j=1:Hp - iRow = (ny*j+1):(ny*Hp) - iCol = (1:nd) .+ nd*(j-1) - J[iRow, iCol] = G[iRow .- ny*j,:] - jx̂[: , iCol] = j < Hp ? getpower(Âpow, Hp-j-1)*B̂d : zeros(NT, nx̂, nd) - end - end - # --- state x̂op and state update f̂op operating points --- - coef_bx̂ = getpower(Âpow_csum, Hp-1) - coef_B = Matrix{NT}(undef, ny*Hp, nx̂) - for j=1:Hp - iRow = (1:ny) .+ ny*(j-1) - coef_B[iRow,:] = Ĉ*getpower(Âpow_csum, j-1) - end - f̂op_n_x̂op = estim.f̂op - estim.x̂op - bx̂ = coef_bx̂ * f̂op_n_x̂op - B = coef_B * f̂op_n_x̂op - return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ -end - -"Return empty matrices if `model` is not a [`LinModel`](@ref)" -function init_predmat(estim::StateEstimator{NT}, model::SimModel, Hp, Hc) where {NT<:Real} - nu, nx̂, nd = model.nu, estim.nx̂, model.nd - E = zeros(NT, 0, nu*Hc) - G = zeros(NT, 0, nd) - J = zeros(NT, 0, nd*Hp) - K = zeros(NT, 0, nx̂) - V = zeros(NT, 0, nu) - B = zeros(NT, 0) - ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = E, G, J, K, V, B - return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ -end - -@doc raw""" - init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, S) -> H̃ + init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, P̃Δu, P̃u) -> H̃ Init the quadratic programming Hessian `H̃` for MPC. The matrix appear in the quadratic general form: ```math - J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + r + J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r ``` The Hessian matrix is constant if the model and weights are linear and time invariant (LTI): ```math - \mathbf{H̃} = 2 ( \mathbf{Ẽ}'\mathbf{M}_{H_p}\mathbf{Ẽ} + \mathbf{Ñ}_{H_c} - + \mathbf{S̃}'\mathbf{L}_{H_p}\mathbf{S̃} ) + \mathbf{H̃} = 2 ( \mathbf{Ẽ'} \mathbf{M}_{H_p} \mathbf{Ẽ} + + \mathbf{P̃_{Δu}'} \mathbf{Ñ}_{H_c} \mathbf{P̃_{Δu}} + + \mathbf{P̃_{u}'} \mathbf{L}_{H_p} \mathbf{P̃_{u}} ) ``` -The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see +in which ``\mathbf{Ẽ}``, ``\mathbf{P̃_{Δu}}`` and ``\mathbf{P̃_{u}}`` matrices are defined +at [`relaxŶ`](@ref), [`relaxΔU`](@ref) and [`relaxU`](@ref) documentation, respectively. The +vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see [`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at optimization but required to evaluate the minimal ``J`` value. """ -function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, S̃) +function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, P̃Δu, P̃u) M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp - H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + Ñ_Hc + S̃'*L_Hp*S̃), :L) + H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + P̃Δu'*Ñ_Hc*P̃Δu + P̃u'*L_Hp*P̃u), :L) return H̃ end "Return empty matrix if `model` is not a [`LinModel`](@ref)." -function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _) where {NT<:Real} +function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _, _) where {NT<:Real} H̃ = Hermitian(zeros(NT, 0, 0), :L) return H̃ end """ init_defaultcon_mpc( - estim, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, + estim::StateEstimator, transcription::TranscriptionMethod, + Hp, Hc, C, + PΔu, Pu, E, + ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, gc!=nothing, nc=0 - ) -> con, S̃, Ẽ + ) -> con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ Init `ControllerConstraint` struct with default parameters based on estimator `estim`. -Also return `S̃` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`. +Also return `P̃Δu`, `P̃u`, `Ẽ` and `Ẽŝ` matrices for the the augmented decision vector `Z̃`. """ function init_defaultcon_mpc( - estim::StateEstimator{NT}, Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - gc!::GCfunc=nothing, nc=0 + estim::StateEstimator{NT}, transcription::TranscriptionMethod, + Hp, Hc, C, + PΔu, Pu, E, + ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ @@ -728,28 +601,36 @@ function init_defaultcon_mpc( repeat_constraints(Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max) C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax) - A_Umin, A_Umax, S̃ = relaxU(model, nϵ, C_umin, C_umax, S) - A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax = relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) - A_Ymin, A_Ymax, Ẽ = relaxŶ(model, nϵ, C_ymin, C_ymax, E) - A_x̂min, A_x̂max, ẽx̂ = relaxterminal(model, nϵ, c_x̂min, c_x̂max, ex̂) - i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) + A_Umin, A_Umax, P̃u = relaxU(Pu, C_umin, C_umax, nϵ) + A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu = relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) + A_Ymin, A_Ymax, Ẽ = relaxŶ(E, C_ymin, C_ymax, nϵ) + A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) + A_ŝ, Ẽŝ = augmentdefect(Eŝ, nϵ) + i_Umin, i_Umax = .!isinf.(U0min), .!isinf.(U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax) - i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) - i_x̂min, i_x̂max = .!isinf.(x̂0min), .!isinf.(x̂0max) - i_b, i_g, A = init_matconstraint_mpc( - model, nc, + i_Ymin, i_Ymax = .!isinf.(Y0min), .!isinf.(Y0max) + i_x̂min, i_x̂max = .!isinf.(x̂0min), .!isinf.(x̂0max) + i_b, i_g, A, Aeq, neq = init_matconstraint_mpc( + model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, - A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min, + A_ŝ ) - b = zeros(NT, size(A, 1)) # dummy b vector (updated just before optimization) + # dummy b and beq vectors (updated just before optimization) + b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1)) con = ControllerConstraint{NT, GCfunc}( - ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , - U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max, - A_Umin , A_Umax, A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max, - A , b , i_b , C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, + ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , + Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , + U0min , U0max , ΔŨmin , ΔŨmax , Y0min , Y0max , x̂0min , x̂0max, + A_Umin , A_Umax , A_ΔŨmin, A_ΔŨmax , A_Ymin , A_Ymax , A_x̂min , A_x̂max, + A , b , i_b , + A_ŝ , + Aeq , beq , + neq , + C_ymin , C_ymax , c_x̂min , c_x̂max , i_g, gc! , nc ) - return con, nϵ, S̃, Ẽ + return con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ end "Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons." @@ -763,93 +644,100 @@ function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax) return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax end - - @doc raw""" - relaxU(model, nϵ, C_umin, C_umax, S) -> A_Umin, A_Umax, S̃ + relaxU(Pu, C_umin, C_umax, nϵ) -> A_Umin, A_Umax, P̃u Augment manipulated inputs constraints with slack variable ϵ for softening. -Denoting the input increments augmented with the slack variable -``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the -augmented conversion matrix ``\mathbf{S̃}``, similar to the one described at -[`init_ΔUtoU`](@ref). It also returns the ``\mathbf{A}`` matrices for the inequality +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the +augmented conversion matrix ``\mathbf{P̃_u}``, similar to the one described at +[`init_ZtoU`](@ref). It also returns the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} \mathbf{A_{U_{min}}} \\ \mathbf{A_{U_{max}}} -\end{bmatrix} \mathbf{ΔŨ} ≤ +\end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - - \mathbf{U_{min} + T} \mathbf{u}(k-1) \\ - + \mathbf{U_{max} - T} \mathbf{u}(k-1) + - \mathbf{U_{min} + T_u u}(k-1) \\ + + \mathbf{U_{max} - T_u u}(k-1) \end{bmatrix} ``` in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively contains ``\mathbf{u_{min}}`` and ``\mathbf{u_{max}}`` repeated ``H_p`` times. """ -function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real - if nϵ == 1 # ΔŨ = [ΔU; ϵ] - # ϵ impacts ΔU → U conversion for constraint calculations: - A_Umin, A_Umax = -[S C_umin], [S -C_umax] - # ϵ has no impact on ΔU → U conversion for prediction calculations: - S̃ = [S zeros(NT, size(S, 1))] - else # ΔŨ = ΔU (only hard constraints) - A_Umin, A_Umax = -S, S - S̃ = S +function relaxU(Pu::Matrix{NT}, C_umin, C_umax, nϵ) where NT<:Real + if nϵ == 1 # Z̃ = [Z; ϵ] + # ϵ impacts Z → U conversion for constraint calculations: + A_Umin, A_Umax = -[Pu C_umin], [Pu -C_umax] + # ϵ has no impact on Z → U conversion for prediction calculations: + P̃u = [Pu zeros(NT, size(Pu, 1))] + else # Z̃ = Z (only hard constraints) + A_Umin, A_Umax = -Pu, Pu + P̃u = Pu end - return A_Umin, A_Umax, S̃ + return A_Umin, A_Umax, P̃u end @doc raw""" - relaxΔU(model, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax + relaxΔU(PΔu, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu Augment input increments constraints with slack variable ϵ for softening. -Denoting the input increments augmented with the slack variable -``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the -augmented constraints ``\mathbf{ΔŨ_{min}}`` and ``\mathbf{ΔŨ_{max}}`` and the ``\mathbf{A}`` -matrices for the inequality constraints: +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the +augmented conversion matrix ``\mathbf{P̃_{Δu}}``, similar to the one described at +[`init_ZtoΔU`](@ref), but extracting the input increments augmented with the slack variable +``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}] = \mathbf{P̃_{Δu} Z̃}``. +Also, knowing that ``0 ≤ ϵ ≤ ∞``, it also returns the augmented bounds +``\mathbf{ΔŨ_{min}} = [\begin{smallmatrix} \mathbf{ΔU_{min}} \\ 0 \end{smallmatrix}]`` and +``\mathbf{ΔŨ_{max}} = [\begin{smallmatrix} \mathbf{ΔU_{min}} \\ ∞ \end{smallmatrix}]``, +and the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} \mathbf{A_{ΔŨ_{min}}} \\ \mathbf{A_{ΔŨ_{max}}} -\end{bmatrix} \mathbf{ΔŨ} ≤ +\end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{ΔŨ_{min}} \\ + \mathbf{ΔŨ_{max}} \end{bmatrix} ``` +Note that strictly speaking, the lower bound on the slack variable ϵ is a decision variable +bound, which is more precise than a linear inequality constraint. However, it is more +convenient to treat it as a linear inequality constraint since the optimizer `OSQP.jl` does +not support pure bounds on the decision variables. """ -function relaxΔU(::SimModel{NT}, nϵ, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax) where NT<:Real - nΔU = length(ΔUmin) - if nϵ == 1 # ΔŨ = [ΔU; ϵ] - # 0 ≤ ϵ ≤ ∞ - ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]] - A_ϵ = [zeros(NT, 1, nΔU) NT[1.0]] - A_ΔŨmin, A_ΔŨmax = -[I C_Δumin; A_ϵ], [I -C_Δumax; A_ϵ] - else # ΔŨ = ΔU (only hard constraints) +function relaxΔU(PΔu::Matrix{NT}, C_Δumin, C_Δumax, ΔUmin, ΔUmax, nϵ) where NT<:Real + nZ = size(PΔu, 2) + if nϵ == 1 # Z̃ = [Z; ϵ] + ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]] # 0 ≤ ϵ ≤ ∞ + A_ϵ = [zeros(NT, 1, nZ) NT[1.0]] + A_ΔŨmin, A_ΔŨmax = -[PΔu C_Δumin; A_ϵ], [PΔu -C_Δumax; A_ϵ] + P̃Δu = [PΔu zeros(NT, size(PΔu, 1), 1); zeros(NT, 1, size(PΔu, 2)) NT[1.0]] + else # Z̃ = Z (only hard constraints) ΔŨmin, ΔŨmax = ΔUmin, ΔUmax - I_Hc = Matrix{NT}(I, nΔU, nΔU) - A_ΔŨmin, A_ΔŨmax = -I_Hc, I_Hc + A_ΔŨmin, A_ΔŨmax = -PΔu, PΔu + P̃Δu = PΔu end - return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax + return A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃Δu end @doc raw""" - relaxŶ(::LinModel, nϵ, C_ymin, C_ymax, E) -> A_Ymin, A_Ymax, Ẽ + relaxŶ(E, C_ymin, C_ymax, nϵ) -> A_Ymin, A_Ymax, Ẽ Augment linear output prediction constraints with slack variable ϵ for softening. -Denoting the input increments augmented with the slack variable -``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ}`` matrix that appears in the linear model prediction equation -``\mathbf{Ŷ_0 = Ẽ ΔŨ + F}``, and the ``\mathbf{A}`` matrices for the inequality constraints: +``\mathbf{Ŷ_0 = Ẽ Z̃ + F}``, and the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} \mathbf{A_{Y_{min}}} \\ \mathbf{A_{Y_{max}}} -\end{bmatrix} \mathbf{ΔŨ} ≤ +\end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{(Y_{min} - Y_{op}) + F} \\ + \mathbf{(Y_{max} - Y_{op}) - F} @@ -858,65 +746,81 @@ Denoting the input increments augmented with the slack variable in which ``\mathbf{Y_{min}, Y_{max}}`` and ``\mathbf{Y_{op}}`` vectors respectively contains ``\mathbf{y_{min}, y_{max}}`` and ``\mathbf{y_{op}}`` repeated ``H_p`` times. """ -function relaxŶ(::LinModel{NT}, nϵ, C_ymin, C_ymax, E) where NT<:Real - if nϵ == 1 # ΔŨ = [ΔU; ϵ] +function relaxŶ(E::Matrix{NT}, C_ymin, C_ymax, nϵ) where NT<:Real + if nϵ == 1 # Z̃ = [Z; ϵ] + if iszero(size(E, 1)) + # model is not a LinModel, thus Ŷ constraints are not linear: + C_ymin = C_ymax = zeros(NT, 0, 1) + end # ϵ impacts predicted output constraint calculations: A_Ymin, A_Ymax = -[E C_ymin], [E -C_ymax] # ϵ has no impact on output predictions: Ẽ = [E zeros(NT, size(E, 1), 1)] - else # ΔŨ = ΔU (only hard constraints) + else # Z̃ = Z (only hard constraints) Ẽ = E A_Ymin, A_Ymax = -E, E end return A_Ymin, A_Ymax, Ẽ end -"Return empty matrices if model is not a [`LinModel`](@ref)" -function relaxŶ(::SimModel{NT}, nϵ, C_ymin, C_ymax, E) where NT<:Real - Ẽ = [E zeros(NT, 0, nϵ)] - A_Ymin, A_Ymax = -Ẽ, Ẽ - return A_Ymin, A_Ymax, Ẽ -end - @doc raw""" - relaxterminal(::LinModel, nϵ, c_x̂min, c_x̂max, ex̂) -> A_x̂min, A_x̂max, ẽx̂ + relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) -> A_x̂min, A_x̂max, ẽx̂ Augment terminal state constraints with slack variable ϵ for softening. -Denoting the input increments augmented with the slack variable -``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}]``, it returns the +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the ``\mathbf{ẽ_{x̂}}`` matrix that appears in the terminal state equation -``\mathbf{x̂_0}(k + H_p) = \mathbf{ẽ_x̂ ΔŨ + f_x̂}``, and the ``\mathbf{A}`` matrices for +``\mathbf{x̂_0}(k + H_p) = \mathbf{ẽ_x̂ Z̃ + f_x̂}``, and the ``\mathbf{A}`` matrices for the inequality constraints: ```math \begin{bmatrix} \mathbf{A_{x̂_{min}}} \\ \mathbf{A_{x̂_{max}}} -\end{bmatrix} \mathbf{ΔŨ} ≤ +\end{bmatrix} \mathbf{Z̃} ≤ \begin{bmatrix} - \mathbf{(x̂_{min} - x̂_{op}) + f_x̂} \\ + \mathbf{(x̂_{max} - x̂_{op}) - f_x̂} \end{bmatrix} ``` """ -function relaxterminal(::LinModel{NT}, nϵ, c_x̂min, c_x̂max, ex̂) where {NT<:Real} - if nϵ == 1 # ΔŨ = [ΔU; ϵ] +function relaxterminal(ex̂::Matrix{NT}, c_x̂min, c_x̂max, nϵ) where {NT<:Real} + if nϵ == 1 # Z̃ = [Z; ϵ] + if iszero(size(ex̂, 1)) + # model is not a LinModel and transcription is a SingleShooting, thus terminal + # state constraints are not linear: + c_x̂min = c_x̂max = zeros(NT, 0, 1) + end # ϵ impacts terminal state constraint calculations: A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max] # ϵ has no impact on terminal state predictions: ẽx̂ = [ex̂ zeros(NT, size(ex̂, 1), 1)] - else # ΔŨ = ΔU (only hard constraints) + else # Z̃ = Z (only hard constraints) ẽx̂ = ex̂ A_x̂min, A_x̂max = -ex̂, ex̂ end return A_x̂min, A_x̂max, ẽx̂ end -"Return empty matrices if model is not a [`LinModel`](@ref)" -function relaxterminal(::SimModel{NT}, nϵ, c_x̂min, c_x̂max, ex̂) where {NT<:Real} - ẽx̂ = [ex̂ zeros(NT, 0, nϵ)] - A_x̂min, A_x̂max = -ẽx̂, ẽx̂ - return A_x̂min, A_x̂max, ẽx̂ +@doc raw""" + augmentdefect(Eŝ, nϵ) -> A_ŝ, Ẽŝ + +Augment defect equality constraints with slack variable ϵ if `nϵ == 1`. + +It returns the ``\mathbf{Ẽŝ}`` matrix that appears in the defect equation +``\mathbf{Ŝ = Ẽ_ŝ Z̃ + F_ŝ}`` and the ``\mathbf{A}`` matrix for the equality constraints: +```math +\mathbf{A_ŝ Z̃} = - \mathbf{F_ŝ} +``` +""" +function augmentdefect(Eŝ::Matrix{NT}, nϵ) where NT<:Real + if nϵ == 1 # Z̃ = [Z; ϵ] + Ẽŝ = [Eŝ zeros(NT, size(Eŝ, 1), 1)] + else # Z̃ = Z (only hard constraints) + Ẽŝ = Eŝ + end + A_ŝ = Ẽŝ + return A_ŝ, Ẽŝ end @doc raw""" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 205376030..807586ff5 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -1,10 +1,10 @@ @doc raw""" initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂ -Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero. +Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.Z̃` at zero. """ function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) - mpc.ΔŨ .= 0 + mpc.Z̃ .= 0 return initstate!(mpc.estim, u, ym, d) end @@ -67,8 +67,9 @@ function moveinput!( validate_args(mpc, ry, d, D̂, R̂y, R̂u) initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model) - ΔŨ = optim_objective!(mpc) - return getinput(mpc, ΔŨ) + linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) + Z̃ = optim_objective!(mpc) + return getinput(mpc, Z̃) end @doc raw""" @@ -112,23 +113,29 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) ``` """ function getinfo(mpc::PredictiveController{NT}) where NT<:Real - model = mpc.estim.model - nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu + model, transcription = mpc.estim.model, mpc.transcription + nΔŨ = mpc.Hc*model.nu + mpc.nϵ + nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu + nX̂0, nÛ0 = mpc.estim.nx̂*mpc.Hp, model.nu*mpc.Hp + Z̃ = mpc.Z̃ info = Dict{Symbol, Any}() - Ŷ0, u0, û0 = similar(mpc.Yop), similar(model.uop), similar(model.uop) - Ŷs = similar(mpc.Yop) - x̂0, x̂0next = similar(mpc.estim.x̂0), similar(mpc.estim.x̂0) - Ȳ, Ū = similar(mpc.Yop), similar(mpc.Uop) - Ŷe, Ue = Vector{NT}(undef, nŶe), Vector{NT}(undef, nUe) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, mpc.ΔŨ) - Ŷe, Ue = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, mpc.ΔŨ) - J = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, mpc.ΔŨ) - U, Ŷ = Ū, Ȳ - U .= mul!(U, mpc.S̃, mpc.ΔŨ) .+ mpc.T_lastu - Ŷ .= Ŷ0 .+ mpc.Yop + ΔŨ = Vector{NT}(undef, nΔŨ) + x̂0end = similar(mpc.estim.x̂0) + Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe) + U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop) + X̂0, Û0 = Vector{NT}(undef, nX̂0), Vector{NT}(undef, nÛ0) + U, Ŷ = similar(mpc.Uop), similar(mpc.Yop) + Ŷs = similar(mpc.Yop) + U0 = getU0!(U0, mpc, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, mpc.transcription, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) + Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) + U .= U0 .+ mpc.Uop + Ŷ .= Ŷ0 .+ mpc.Yop + J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) predictstoch!(Ŷs, mpc, mpc.estim) - info[:ΔU] = mpc.ΔŨ[1:mpc.Hc*model.nu] - info[:ϵ] = mpc.nϵ == 1 ? mpc.ΔŨ[end] : zero(NT) + info[:ΔU] = Z̃[1:mpc.Hc*model.nu] + info[:ϵ] = mpc.nϵ == 1 ? mpc.Z̃[end] : zero(NT) info[:J] = J info[:U] = U info[:u] = info[:U][1:model.nu] @@ -164,7 +171,6 @@ function addinfo!(info, mpc::PredictiveController) return info end - @doc raw""" initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) -> nothing @@ -177,11 +183,11 @@ They are computed with these equations using in-place operations: \mathbf{F} &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k) + \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s} \\ \mathbf{C_y} &= \mathbf{F} + \mathbf{Y_{op}} - \mathbf{R̂_y} \\ - \mathbf{C_u} &= \mathbf{T}\mathbf{u}(k-1) - \mathbf{R̂_u} \\ - \mathbf{q̃} &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} - + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\ - r &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} - + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u} + \mathbf{C_u} &= \mathbf{T_u}\mathbf{u}(k-1) - \mathbf{R̂_u} \\ + \mathbf{q̃} &= 2[ (\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} + + (\mathbf{L}_{H_p} \mathbf{P̃_U})' \mathbf{C_u} ] \\ + r &= \mathbf{C_y'} \mathbf{M}_{H_p} \mathbf{C_y} + + \mathbf{C_u'} \mathbf{L}_{H_p} \mathbf{C_u} \end{aligned} ``` """ @@ -194,7 +200,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ mul!(F, mpc.G, mpc.d0, 1, 1) # F = F + G*d0 mul!(F, mpc.J, mpc.D̂0, 1, 1) # F = F + J*D̂0 end - Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃ + Cy, Cu, M_Hp_Ẽ, L_Hp_P̃U = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.P̃u q̃, r = mpc.q̃, mpc.r q̃ .= 0 r .= 0 @@ -207,9 +213,9 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ end # --- input setpoint tracking term --- if !mpc.weights.iszero_L_Hp[] - Cu .= mpc.T_lastu .- R̂u - mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃) - mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu + Cu .= mpc.Tu_lastu0 .+ mpc.Uop .- R̂u + mul!(L_Hp_P̃U, mpc.weights.L_Hp, mpc.P̃u) + mul!(q̃, L_Hp_P̃U', Cu, 1, 1) # q̃ = q̃ + L_Hp*P̃u'*Cu r .+= dot(Cu, mpc.weights.L_Hp, Cu) # r = r + Cu'*L_Hp*Cu end # --- finalize --- @@ -232,13 +238,11 @@ end Common computations of `initpred!` for all types of [`SimModel`](@ref). -Will init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` if `mpc.estim` is -an [`InternalModel`](@ref). The function returns `mpc.F`. +Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` if `mpc.estim` +is an [`InternalModel`](@ref). The function returns `mpc.F`. """ function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) - lastu = mpc.buffer.u - lastu .= mpc.estim.lastu0 .+ model.uop - mul!(mpc.T_lastu, mpc.T, lastu) + mul!(mpc.Tu_lastu0, mpc.Tu, mpc.estim.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) if model.nd ≠ 0 mpc.d0 .= d .- model.dop @@ -268,10 +272,11 @@ predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; n @doc raw""" linconstraint!(mpc::PredictiveController, model::LinModel) -Set `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``). +Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``). -Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d}(k) + \mathbf{j_x̂ D̂} + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u}(k-1) + \mathbf{b_x̂}`` -vector for the terminal constraints, see [`init_predmat`](@ref). +Also init ``\mathbf{f_x̂} = \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + \mathbf{k_x̂ x̂_0}(k) + +\mathbf{v_x̂ u_0}(k-1) + \mathbf{b_x̂}`` vector for the terminal constraints, see +[`init_predmat`](@ref). """ function linconstraint!(mpc::PredictiveController, model::LinModel) nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) @@ -284,9 +289,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) end n = 0 - mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu + mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU - mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu + mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0 n += nU mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin n += nΔŨ @@ -310,9 +315,9 @@ end function linconstraint!(mpc::PredictiveController, ::SimModel) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) n = 0 - mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu + mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU - mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu + mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.Tu_lastu0 n += nU mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin n += nΔŨ @@ -324,101 +329,55 @@ function linconstraint!(mpc::PredictiveController, ::SimModel) return nothing end -@doc raw""" - predict!(Ŷ0, x̂0, _, _, _, mpc::PredictiveController, model::LinModel, ΔŨ) -> Ŷ0, x̂0end - -Compute the predictions `Ŷ0` and terminal states `x̂0end` if model is a [`LinModel`](@ref). - -The method mutates `Ŷ0` and `x̂0` vector arguments. The `x̂end` vector is used for -the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. """ -function predict!(Ŷ0, x̂0, _ , _ , _ , mpc::PredictiveController, ::LinModel, ΔŨ) - # in-place operations to reduce allocations : - Ŷ0 .= mul!(Ŷ0, mpc.Ẽ, ΔŨ) .+ mpc.F - x̂0 .= mul!(x̂0, mpc.con.ẽx̂, ΔŨ) .+ mpc.con.fx̂ - x̂0end = x̂0 - return Ŷ0, x̂0end -end - -@doc raw""" - predict!( - Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, model::SimModel, ΔŨ - ) -> Ŷ0, x̂end - -Compute both vectors if `model` is not a [`LinModel`](@ref). - -The method mutates `Ŷ0`, `x̂0`, `x̂0next`, `u0` and `û0` arguments. -""" -function predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, model::SimModel, ΔŨ) - nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc - x̂0 .= mpc.estim.x̂0 - u0 .= mpc.estim.lastu0 - d0 = @views mpc.d0[1:end] - for j=1:Hp - if j ≤ Hc - u0 .+= @views ΔŨ[(1 + nu*(j-1)):(nu*j)] - end - f̂!(x̂0next, û0, mpc.estim, model, x̂0, u0, d0) - x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op - x̂0 .= x̂0next - d0 = @views mpc.D̂0[(1 + nd*(j-1)):(nd*j)] - ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] - ĥ!(ŷ0, mpc.estim, model, x̂0, d0) - end - Ŷ0 .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0. - x̂end = x̂0 - return Ŷ0, x̂end -end + extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe -""" - extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ) -> Ŷe, Ue +Compute the extended `Ue` and `Ŷe` vectors for nonlinear programming using `U0` and `Ŷ0`. -Compute the extended vectors `Ŷe` and `Ue` for the nonlinear optimization. +See [`NonLinMPC`](@ref) for the definition of the vectors. The function mutates `Ue` and +and `Ŷe` in arguments, without assuming any initial values for them. Using +`nocustomfcts = mpc.weights.iszero_E && mpc.con.nc == 0`, there are three special cases in +which `Ue` and `Ŷe` are not mutated: -The function mutates `Ŷe`, `Ue` and `Ū` in arguments, without assuming any initial values -for them. Using `nocustomfcts = mpc.weights.iszero_E && mpc.con.nc == 0`, there is two -special cases in which `Ŷe`, `Ue` and `Ū` are not mutated: - -- If `mpc.weights.iszero_M_Hp[] && nocustomfcts`, the `Ŷe` vector is not computed to reduce - the burden in the optimization problem. +- If `mpc.weights.iszero_M_Hp[] && nocustomfcts`, the `Ŷe` vector is not computed for the + same reason as above. - If `mpc.weights.iszero_L_Hp[] && nocustomfcts`, the `Ue` vector is not computed for the same reason as above. """ -function extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ) +function extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) + model = mpc.estim.model ny, nu = model.ny, model.nu nocustomfcts = (mpc.weights.iszero_E && iszero_nc(mpc)) + # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] --- + if !(mpc.weights.iszero_L_Hp[] && nocustomfcts) + Ue[1:end-nu] .= U0 .+ mpc.Uop + # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp): + Ue[end-nu+1:end] .= @views Ue[end-2*nu+1:end-nu] + end # --- extended output predictions Ŷe = [ŷ(k); Ŷ] --- if !(mpc.weights.iszero_M_Hp[] && nocustomfcts) Ŷe[1:ny] .= mpc.ŷ Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop end - # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] --- - if !(mpc.weights.iszero_L_Hp[] && nocustomfcts) - U = Ū - U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu - Ue[1:end-nu] .= U - # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp): - Ue[end-nu+1:end] .= @views U[end-nu+1:end] - end - return Ŷe, Ue + return Ue, Ŷe end "Verify if the custom nonlinear constraint has zero elements." iszero_nc(mpc::PredictiveController) = (mpc.con.nc == 0) """ - obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ) + obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃) Nonlinear programming objective function when `model` is a [`LinModel`](@ref). The method is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J` -at specific `Ue`, `Ŷe` and `ΔŨ`, values. It does not mutate any argument. +at specific `Ue`, `Ŷe` and `Z̃`, values. It does not mutate any argument. """ function obj_nonlinprog!( - _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ::AbstractVector{NT} + _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃::AbstractVector{NT} ) where NT <: Real - JQP = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[] + JQP = obj_quadprog(Z̃, mpc.H̃, mpc.q̃) + mpc.r[] E_JE = obj_econ(mpc, model, Ue, Ŷe) return JQP + E_JE end @@ -440,7 +399,7 @@ function obj_nonlinprog!( JR̂y = zero(NT) else Ȳ .= @views Ŷe[ny+1:end] - Ȳ .= mpc.R̂y .- Ȳ + Ȳ .= Ȳ .- mpc.R̂y JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ) end # --- move suppression and slack variable term --- @@ -454,7 +413,7 @@ function obj_nonlinprog!( JR̂u = zero(NT) else Ū .= @views Ue[1:end-nu] - Ū .= mpc.R̂u .- Ū + Ū .= Ū .- mpc.R̂u JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) end # --- economic term --- @@ -468,39 +427,21 @@ function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) end @doc raw""" - optim_objective!(mpc::PredictiveController) -> ΔŨ + optim_objective!(mpc::PredictiveController) -> Z̃ -Optimize the objective function of `mpc` [`PredictiveController`](@ref) and return the solution `ΔŨ`. +Optimize the objective function of `mpc` [`PredictiveController`](@ref) and return the solution `Z̃`. -If supported by `mpc.optim`, it warm-starts the solver at: -```math -\mathbf{ΔŨ} = -\begin{bmatrix} - \mathbf{Δu}_{k-1}(k+0) \\ - \mathbf{Δu}_{k-1}(k+1) \\ - \vdots \\ - \mathbf{Δu}_{k-1}(k+H_c-2) \\ - \mathbf{0} \\ - ϵ_{k-1} -\end{bmatrix} -``` -where ``\mathbf{Δu}_{k-1}(k+j)`` is the input increment for time ``k+j`` computed at the -last control period ``k-1``. It then calls `JuMP.optimize!(mpc.optim)` and extract the -solution. A failed optimization prints an `@error` log in the REPL and returns the -warm-start value. A failed optimization also prints [`getinfo`](@ref) results in -the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). +If first warm-starts the solver with [`set_warmstart!`](@ref). It then calls +`JuMP.optimize!(mpc.optim)` and extract the solution. A failed optimization prints an +`@error` log in the REPL and returns the warm-start value. A failed optimization also prints +[`getinfo`](@ref) results in the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). """ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} model, optim = mpc.estim.model, mpc.optim - nu, Hc = model.nu, mpc.Hc - ΔŨvar::Vector{JuMP.VariableRef} = optim[:ΔŨvar] - # initial ΔŨ (warm-start): [Δu_{k-1}(k); Δu_{k-1}(k+1); ... ; 0_{nu × 1}; ϵ_{k-1}] - ΔŨ0 = mpc.buffer.ΔŨ - ΔŨ0[1:(Hc*nu-nu)] .= @views mpc.ΔŨ[nu+1:Hc*nu] - ΔŨ0[(Hc*nu-nu+1):(Hc*nu)] .= 0 - mpc.nϵ == 1 && (ΔŨ0[end] = mpc.ΔŨ[end]) - JuMP.set_start_value.(ΔŨvar, ΔŨ0) - set_objective_linear_coef!(mpc, ΔŨvar) + nu, Hc = model.nu, mpc.Hc + Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] + Z̃0 = set_warmstart!(mpc, mpc.transcription, Z̃var) + set_objective_linear_coef!(mpc, Z̃var) try JuMP.optimize!(optim) catch err @@ -530,11 +471,11 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} @debug info2debugstr(getinfo(mpc)) end if iserror(optim) - mpc.ΔŨ .= ΔŨ0 + mpc.Z̃ .= Z̃0 else - mpc.ΔŨ .= JuMP.value.(ΔŨvar) + mpc.Z̃ .= JuMP.value.(Z̃var) end - return mpc.ΔŨ + return mpc.Z̃ end "By default, no need to modify the objective function." @@ -550,17 +491,17 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty) end @doc raw""" - getinput(mpc::PredictiveController, ΔŨ) -> u + getinput(mpc::PredictiveController, Z̃) -> u -Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `ΔŨ`. +Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `Z̃`. -The first manipulated input ``\mathbf{u}(k)`` is extracted from the input increments vector -``\mathbf{ΔŨ}`` and applied on the plant (from the receding horizon principle). +The first manipulated input ``\mathbf{u}(k)`` is extracted from the decision vector +``\mathbf{Z̃}`` and applied on the plant (from the receding horizon principle). """ -function getinput(mpc, ΔŨ) +function getinput(mpc, Z̃) Δu = mpc.buffer.u for i in 1:mpc.estim.model.nu - Δu[i] = ΔŨ[i] + Δu[i] = Z̃[i] end u = Δu u .+= mpc.estim.lastu0 .+ mpc.estim.model.uop @@ -704,13 +645,15 @@ end "Update the prediction matrices, linear constraints and JuMP optimization." function setmodel_controller!(mpc::PredictiveController, x̂op_old) - estim, model = mpc.estim, mpc.estim.model + model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc optim, con = mpc.optim, mpc.con # --- predictions matrices --- - E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) - A_Ymin, A_Ymax, Ẽ = relaxŶ(model, mpc.nϵ, con.C_ymin, con.C_ymax, E) - A_x̂min, A_x̂max, ẽx̂ = relaxterminal(model, mpc.nϵ, con.c_x̂min, con.c_x̂max, ex̂) + E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( + model, estim, transcription, Hp, Hc + ) + A_Ymin, A_Ymax, Ẽ = relaxŶ(E, con.C_ymin, con.C_ymax, mpc.nϵ) + A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, con.c_x̂min, con.c_x̂max, mpc.nϵ) mpc.Ẽ .= Ẽ mpc.G .= G mpc.J .= J @@ -756,17 +699,22 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old) con.A_x̂min con.A_x̂max ] + Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var] A = con.A[con.i_b, :] b = con.b[con.i_b] - ΔŨvar::Vector{JuMP.VariableRef} = optim[:ΔŨvar] # deletion is required for sparse solvers like OSQP, when the sparsity pattern changes JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) - @constraint(optim, linconstraint, A*ΔŨvar .≤ b) + @constraint(optim, linconstraint, A*Z̃var .≤ b) + Aeq = con.Aeq + beq = con.beq + JuMP.delete(optim, optim[:linconstrainteq]) + JuMP.unregister(optim, :linconstrainteq) + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) # --- quadratic programming Hessian matrix --- - H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃) + H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u) mpc.H̃ .= H̃ - set_objective_hessian!(mpc, ΔŨvar) + set_objective_hessian!(mpc, Z̃var) return nothing end diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 108a33b50..ffbd9107c 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -1,16 +1,18 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} estim::SE - ΔŨ::Vector{NT} - ŷ ::Vector{NT} + transcription::SingleShooting + Z̃::Vector{NT} + ŷ::Vector{NT} Hp::Int Hc::Int nϵ::Int weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} - S̃::Matrix{NT} - T::Matrix{NT} - T_lastu::Vector{NT} + P̃Δu::Matrix{NT} + P̃u ::Matrix{NT} + Tu ::Matrix{NT} + Tu_lastu0::Vector{NT} Ẽ::Matrix{NT} F::Vector{NT} G::Matrix{NT} @@ -44,13 +46,15 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L) L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) # dummy vals (updated just before optimization): - R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - S, T = init_ΔUtoU(model, Hp, Hc) - E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc) + R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + transcription = SingleShooting() # explicit MPC only supports SingleShooting + PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) + Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc) + E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc) # dummy val (updated just before optimization): F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) - S̃, Ñ_Hc, Ẽ = S, N_Hc, E # no slack variable ϵ for ExplicitMPC - H̃ = init_quadprog(model, weights ,Ẽ, S̃) + P̃Δu, P̃u, Ñ_Hc, Ẽ = PΔu, Pu, N_Hc, E # no slack variable ϵ for ExplicitMPC + H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) H̃_chol = cholesky(H̃) @@ -58,16 +62,17 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} # dummy vals (updated just before optimization): d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) - nΔŨ = size(Ẽ, 2) - ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) + nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + Z̃ = zeros(NT, nZ̃) + buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE}( estim, - ΔŨ, ŷ, + transcription, + Z̃, ŷ, Hp, Hc, nϵ, weights, R̂u, R̂y, - S̃, T, T_lastu, + P̃Δu, P̃u, Tu, Tu_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, H̃_chol, @@ -97,7 +102,8 @@ The controller minimizes the following objective function at each discrete time See [`LinMPC`](@ref) for the variable definitions. This controller does not support constraints but the computational costs are extremely low (array division), therefore suitable for applications that require small sample times. The keyword arguments are -identical to [`LinMPC`](@ref), except for `Cwt` and `optim` which are not supported. +identical to [`LinMPC`](@ref), except for `Cwt` and `optim` which are not supported. This +controller uses a [`SingleShooting`](@ref) transcription method. This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) with default arguments. This controller is allocation-free. @@ -179,20 +185,22 @@ end linconstraint!(::ExplicitMPC, ::LinModel) = nothing @doc raw""" - optim_objective!(mpc::ExplicitMPC) -> ΔŨ + optim_objective!(mpc::ExplicitMPC) -> Z̃ Analytically solve the optimization problem for [`ExplicitMPC`](@ref). -The solution is ``\mathbf{ΔŨ = - H̃^{-1} q̃}``, see [`init_quadprog`](@ref). +The solution is ``\mathbf{Z̃ = - H̃^{-1} q̃}``, see [`init_quadprog`](@ref). """ -optim_objective!(mpc::ExplicitMPC) = lmul!(-1, ldiv!(mpc.ΔŨ, mpc.H̃_chol, mpc.q̃)) +optim_objective!(mpc::ExplicitMPC) = lmul!(-1, ldiv!(mpc.Z̃, mpc.H̃_chol, mpc.q̃)) "Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)." -function predict!(Ŷ, x̂, _ , _ , _ , mpc::ExplicitMPC, ::LinModel, ΔŨ) +function predict!( + Ŷ0, x̂0end, _ , _ , mpc::ExplicitMPC, ::LinModel, ::TranscriptionMethod, _ , Z̃ +) # in-place operations to reduce allocations : - Ŷ .= mul!(Ŷ, mpc.Ẽ, ΔŨ) .+ mpc.F - x̂ .= NaN - return Ŷ, x̂ + Ŷ0 .= mul!(Ŷ0, mpc.Ẽ, Z̃) .+ mpc.F + x̂0end .= NaN + return Ŷ0, x̂0end end "`ExplicitMPC` does not support custom nonlinear constraint, return `true`." @@ -208,10 +216,10 @@ addinfo!(info, mpc::ExplicitMPC) = info "Update the prediction matrices and Cholesky factorization." function setmodel_controller!(mpc::ExplicitMPC, _ ) - estim, model = mpc.estim, mpc.estim.model + model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc # --- predictions matrices --- - E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc) + E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc) Ẽ = E # no slack variable ϵ for ExplicitMPC mpc.Ẽ .= Ẽ mpc.G .= G @@ -220,7 +228,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ ) mpc.V .= V mpc.B .= B # --- quadratic programming Hessian matrix --- - H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃) + H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃Δu, mpc.P̃u) mpc.H̃ .= H̃ set_objective_hessian!(mpc) # --- operating points --- diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 0112d3e26..224d0fdf7 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -1,26 +1,30 @@ const DEFAULT_LINMPC_OPTIMIZER = OSQP.MathOptInterfaceOSQP.Optimizer +const DEFAULT_LINMPC_TRANSCRIPTION = SingleShooting() struct LinMPC{ NT<:Real, SE<:StateEstimator, + TM<:TranscriptionMethod, JM<:JuMP.GenericModel } <: PredictiveController{NT} estim::SE + transcription::TM # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM con::ControllerConstraint{NT, Nothing} - ΔŨ::Vector{NT} - ŷ ::Vector{NT} + Z̃::Vector{NT} + ŷ::Vector{NT} Hp::Int Hc::Int nϵ::Int weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} - S̃::Matrix{NT} - T::Matrix{NT} - T_lastu::Vector{NT} + P̃Δu::Matrix{NT} + P̃u ::Matrix{NT} + Tu ::Matrix{NT} + Tu_lastu0::Vector{NT} Ẽ::Matrix{NT} F::Vector{NT} G::Matrix{NT} @@ -41,38 +45,46 @@ struct LinMPC{ Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} function LinMPC{NT}( - estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM - ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel} + estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, + transcription::TM, optim::JM + ) where {NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, JM<:JuMP.GenericModel} model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) # dummy vals (updated just before optimization): - R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - S, T = init_ΔUtoU(model, Hp, Hc) - E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) + R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) + Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc) + E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( + model, estim, transcription, Hp, Hc + ) + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) # dummy vals (updated just before optimization): - F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) - con, nϵ, S̃, Ẽ = init_defaultcon_mpc( - estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂ + F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) + con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ = init_defaultcon_mpc( + estim, transcription, + Hp, Hc, Cwt, PΔu, Pu, E, + ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ ) - H̃ = init_quadprog(model, weights, Ẽ, S̃) + H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) Ks, Ps = init_stochpred(estim, Hp) # dummy vals (updated just before optimization): d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) - nΔŨ = size(Ẽ, 2) - ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) - mpc = new{NT, SE, JM}( - estim, optim, con, - ΔŨ, ŷ, + nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + Z̃ = zeros(NT, nZ̃) + buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) + mpc = new{NT, SE, TM, JM}( + estim, transcription, optim, con, + Z̃, ŷ, Hp, Hc, nϵ, weights, R̂u, R̂y, - S̃, T, T_lastu, + P̃Δu, P̃u, Tu, Tu_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, Ks, Ps, @@ -93,7 +105,7 @@ Construct a linear predictive controller based on [`LinModel`](@ref) `model`. The controller minimizes the following objective function at each discrete time ``k``: ```math \begin{aligned} -\min_{\mathbf{ΔU}, ϵ} \mathbf{(R̂_y - Ŷ)}' \mathbf{M}_{H_p} \mathbf{(R̂_y - Ŷ)} +\min_{\mathbf{Z}, ϵ} \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)} + C ϵ^2 @@ -109,7 +121,9 @@ subject to [`setconstraint!`](@ref) bounds, and in which the weight matrices are \end{aligned} ``` 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 +``\mathbf{M}_{H_p}`` to specify a terminal weight. The content of the decision vector +``\mathbf{Z}`` depends on the chosen [`TranscriptionMethod`](@ref) (default to +[`SingleShooting`](@ref), hence ``\mathbf{Z = ΔU}``). 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)`` @@ -130,10 +144,10 @@ arguments. This controller allocates memory at each time step for the optimizati - `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. +- `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `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) (default to [`OSQP`](https://osqp.org/docs/parsers/jump.html) optimizer). - - additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor. # Examples @@ -164,9 +178,11 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil | :------------------- | :------------------------------------------------------- | :--------------- | | ``H_p`` | prediction horizon (integer) | `()` | | ``H_c`` | control horizon (integer) | `()` | + | ``\mathbf{Z}`` | decision variable vector (excluding ``ϵ``) | var. | | ``\mathbf{ΔU}`` | manipulated input increments over ``H_c`` | `(nu*Hc,)` | | ``\mathbf{D̂}`` | predicted measured disturbances over ``H_p`` | `(nd*Hp,)` | | ``\mathbf{Ŷ}`` | predicted outputs over ``H_p`` | `(ny*Hp,)` | + | ``\mathbf{X̂}`` | predicted states over ``H_p`` | `(nx̂*Hp,)` | | ``\mathbf{U}`` | manipulated inputs over ``H_p`` | `(nu*Hp,)` | | ``\mathbf{R̂_y}`` | predicted output setpoints over ``H_p`` | `(ny*Hp,)` | | ``\mathbf{R̂_u}`` | predicted manipulated input setpoints over ``H_p`` | `(nu*Hp,)` | @@ -187,11 +203,12 @@ function LinMPC( N_Hc = diagm(repeat(Nwt, Hc)), L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, + transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), kwargs... ) estim = SteadyKalmanFilter(model; kwargs...) - return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, optim) + return LinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, transcription, optim) end @@ -229,6 +246,7 @@ function LinMPC( N_Hc = diagm(repeat(Nwt, Hc)), L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, + transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} isa(estim.model, LinModel) || error("estim.model type must be a LinModel") @@ -237,7 +255,7 @@ 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 - return LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim) + return LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, transcription, optim) end """ @@ -248,26 +266,29 @@ Init the quadratic optimization for [`LinMPC`](@ref) controllers. function init_optimization!(mpc::LinMPC, model::LinModel, optim) # --- variables and linear constraints --- con = mpc.con - nΔŨ = length(mpc.ΔŨ) + nZ̃ = length(mpc.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) limit_solve_time(mpc.optim, model.Ts) - @variable(optim, ΔŨvar[1:nΔŨ]) + @variable(optim, Z̃var[1:nZ̃]) A = con.A[con.i_b, :] b = con.b[con.i_b] - @constraint(optim, linconstraint, A*ΔŨvar .≤ b) - set_objective_hessian!(mpc, ΔŨvar) + @constraint(optim, linconstraint, A*Z̃var .≤ b) + Aeq = con.Aeq + beq = con.beq + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) + set_objective_hessian!(mpc, Z̃var) return nothing end "For [`LinMPC`](@ref), set the QP linear coefficient `q̃` just before optimization." -function set_objective_linear_coef!(mpc::LinMPC, ΔŨvar) - JuMP.set_objective_coefficient(mpc.optim, ΔŨvar, mpc.q̃) +function set_objective_linear_coef!(mpc::LinMPC, Z̃var) + JuMP.set_objective_coefficient(mpc.optim, Z̃var, mpc.q̃) return nothing end "Update the quadratic objective function for [`LinMPC`](@ref) controllers." -function set_objective_hessian!(mpc::LinMPC, ΔŨvar) - @objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.H̃, mpc.q̃)) +function set_objective_hessian!(mpc::LinMPC, Z̃var) + @objective(mpc.optim, Min, obj_quadprog(Z̃var, mpc.H̃, mpc.q̃)) return nothing end \ No newline at end of file diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index a73df83bc..1b791cd42 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1,31 +1,35 @@ const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") +const DEFAULT_NONLINMPC_TRANSCRIPTION = SingleShooting() struct NonLinMPC{ NT<:Real, SE<:StateEstimator, + TM<:TranscriptionMethod, JM<:JuMP.GenericModel, - P<:Any, + PT<:Any, JEfunc<:Function, GCfunc<:Function } <: PredictiveController{NT} estim::SE + transcription::TM # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM con::ControllerConstraint{NT, GCfunc} - ΔŨ::Vector{NT} - ŷ ::Vector{NT} + Z̃::Vector{NT} + ŷ::Vector{NT} Hp::Int Hc::Int nϵ::Int weights::ControllerWeights{NT} JE::JEfunc - p::P + p::PT R̂u::Vector{NT} R̂y::Vector{NT} - S̃::Matrix{NT} - T::Matrix{NT} - T_lastu::Vector{NT} + P̃Δu::Matrix{NT} + P̃u ::Matrix{NT} + Tu ::Matrix{NT} + Tu_lastu0::Vector{NT} Ẽ::Matrix{NT} F::Vector{NT} G::Matrix{NT} @@ -47,12 +51,14 @@ struct NonLinMPC{ buffer::PredictiveControllerBuffer{NT} function NonLinMPC{NT}( estim::SE, - Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::P, optim::JM + Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::PT, + transcription::TM, optim::JM ) where { NT<:Real, SE<:StateEstimator, + TM<:TranscriptionMethod, JM<:JuMP.GenericModel, - P<:Any, + PT<:Any, JEfunc<:Function, GCfunc<:Function, } @@ -61,15 +67,23 @@ struct NonLinMPC{ ŷ = copy(model.yop) # dummy vals (updated just before optimization) weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # dummy vals (updated just before optimization): - R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - S, T = init_ΔUtoU(model, Hp, Hc) - E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) + R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) + Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc) + E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( + model, estim, transcription, Hp, Hc + ) + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc) # dummy vals (updated just before optimization): - F, fx̂ = zeros(NT, ny*Hp), zeros(NT, nx̂) - con, nϵ, S̃, Ẽ = init_defaultcon_mpc( - estim, Hp, Hc, Cwt, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gc!, nc + F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) + con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ = init_defaultcon_mpc( + estim, transcription, + Hp, Hc, Cwt, PΔu, Pu, E, + ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + gc!, nc ) - H̃ = init_quadprog(model, weights, Ẽ, S̃) + H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) Ks, Ps = init_stochpred(estim, Hp) @@ -77,17 +91,17 @@ struct NonLinMPC{ d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) - nΔŨ = size(Ẽ, 2) - ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) - mpc = new{NT, SE, JM, P, JEfunc, GCfunc}( - estim, optim, con, - ΔŨ, ŷ, + nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ + Z̃ = zeros(NT, nZ̃) + buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) + mpc = new{NT, SE, TM, JM, PT, JEfunc, GCfunc}( + estim, transcription, optim, con, + Z̃, ŷ, Hp, Hc, nϵ, weights, JE, p, R̂u, R̂y, - S̃, T, T_lastu, + P̃Δu, P̃u, Tu, Tu_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, Ks, Ps, @@ -109,7 +123,7 @@ Both [`NonLinModel`](@ref) and [`LinModel`](@ref) are supported (see Extended He controller minimizes the following objective function at each discrete time ``k``: ```math \begin{aligned} -\min_{\mathbf{ΔU}, ϵ}\ & \mathbf{(R̂_y - Ŷ)}' \mathbf{M}_{H_p} \mathbf{(R̂_y - Ŷ)} +\min_{\mathbf{Z}, ϵ}\ & \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)} + C ϵ^2 @@ -120,12 +134,14 @@ subject to [`setconstraint!`](@ref) bounds, and the custom inequality constraint ```math \mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0} ``` -The economic function ``J_E`` can penalizes solutions with high economic costs. Setting all -the weights to 0 except ``E`` creates a pure economic model predictive controller (EMPC). -As a matter of fact, ``J_E`` can be any nonlinear function to customize the objective, even -if there is no economic interpretation to it. The arguments of ``J_E`` and ``\mathbf{g_c}`` -include the manipulated inputs, predicted outputs and measured disturbances, extended from -``k`` to ``k + H_p`` (inclusively, see Extended Help for more details): +with the decision variables ``\mathbf{Z}`` and slack ``ϵ``. By default, a [`SingleShooting`](@ref) +transcription method is used, hence ``\mathbf{Z=ΔU}``. The economic function ``J_E`` can +penalizes solutions with high economic costs. Setting all the weights to 0 except ``E`` +creates a pure economic model predictive controller (EMPC). As a matter of fact, ``J_E`` can +be any nonlinear function as a custom objective, even if there is no economic interpretation +to it. The arguments of ``J_E`` and ``\mathbf{g_c}`` include the manipulated inputs, +predicted outputs and measured disturbances, extended from ``k`` to ``k+H_p`` (inclusively, +see Extended Help for more details): ```math \mathbf{U_e} = \begin{bmatrix} \mathbf{U} \\ \mathbf{u}(k+H_p-1) \end{bmatrix} , \quad \mathbf{Ŷ_e} = \begin{bmatrix} \mathbf{ŷ}(k) \\ \mathbf{Ŷ} \end{bmatrix} , \quad @@ -169,6 +185,7 @@ This controller allocates memory at each time step for the optimization. not (details in Extended Help). - `nc=0` : number of custom inequality constraints. - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type). +- `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization. - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). @@ -246,13 +263,15 @@ function NonLinMPC( gc ::Function = gc!, nc::Int = 0, p = model.p, + transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), kwargs... ) estim = UnscentedKalmanFilter(model; kwargs...) return NonLinMPC( estim; - Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, optim + Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, + transcription, optim ) end @@ -273,13 +292,15 @@ function NonLinMPC( gc ::Function = gc!, nc::Int = 0, p = model.p, + transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), kwargs... ) estim = SteadyKalmanFilter(model; kwargs...) return NonLinMPC( estim; - Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, optim + Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, + transcription, optim ) end @@ -323,13 +344,12 @@ function NonLinMPC( gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc = 0, - p::P = estim.model.p, - optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), + p = estim.model.p, + transcription::TranscriptionMethod = DEFAULT_NONLINMPC_TRANSCRIPTION, + optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), ) where { NT<:Real, - SE<:StateEstimator{NT}, - JM<:JuMP.GenericModel, - P<:Any + SE<:StateEstimator{NT} } nk = estimate_delays(estim.model) if Hp ≤ nk @@ -339,7 +359,8 @@ function NonLinMPC( validate_JE(NT, JE) gc! = get_mutating_gc(NT, gc) return NonLinMPC{NT}( - estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, optim + estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, + transcription, optim ) end @@ -462,15 +483,18 @@ Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) # --- variables and linear constraints --- - con = mpc.con - nΔŨ = length(mpc.ΔŨ) + con, transcription = mpc.con, mpc.transcription + nZ̃ = length(mpc.Z̃) JuMP.num_variables(optim) == 0 || JuMP.empty!(optim) JuMP.set_silent(optim) limit_solve_time(mpc.optim, mpc.estim.model.Ts) - @variable(optim, ΔŨvar[1:nΔŨ]) + @variable(optim, Z̃var[1:nZ̃]) A = con.A[con.i_b, :] b = con.b[con.i_b] - @constraint(optim, linconstraint, A*ΔŨvar .≤ b) + @constraint(optim, linconstraint, A*Z̃var .≤ b) + Aeq = con.Aeq + beq = con.beq + @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq) # --- nonlinear optimization init --- if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt" C = mpc.weights.Ñ_Hc[end] @@ -481,124 +505,217 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, gfuncs = get_optim_functions(mpc, mpc.optim) - @operator(optim, J, nΔŨ, Jfunc) - @objective(optim, Min, J(ΔŨvar...)) - init_nonlincon!(mpc, model, gfuncs) - set_nonlincon!(mpc, model, mpc.optim) + Jfunc, gfuncs, geqfuncs = get_optim_functions(mpc, optim) + @operator(optim, J, nZ̃, Jfunc) + @objective(optim, Min, J(Z̃var...)) + init_nonlincon!(mpc, model, transcription, gfuncs, geqfuncs) + set_nonlincon!(mpc, model, optim) return nothing end """ - get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel) -> Jfunc, gfuncs + get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel) -> Jfunc, gfuncs, geqfuncs -Get the objective `Jfunc` function and constraint `gfuncs` function vector for -[`NonLinMPC`](@ref). +Get the objective `Jfunc` function, and constraint `gfuncs` and `geqfuncs` function vectors +for [`NonLinMPC`](@ref). Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) """ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real - model = mpc.estim.model - nu, ny, nx̂, nϵ, Hp = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp - ng, nc, nΔŨ, nU, nŶ = length(mpc.con.i_g), mpc.con.nc, length(mpc.ΔŨ), Hp*nu, Hp*ny - nUe, nŶe = nU + nu, nŶ + ny - Ncache = nΔŨ + 3 + model, transcription = mpc.estim.model, mpc.transcription + nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc + ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq + nZ̃, nU, nŶ, nX̂ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂ + nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny + Ncache = nZ̃ + 3 + myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: + Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nΔŨ), Ncache) + x̂0end_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) Ŷe_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶe), Ncache) Ue_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nUe), Ncache) - Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Ncache) - Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) - x̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) - x̂0next_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) - u0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Ncache) - û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Ncache) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) + Ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), Ncache) + U0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) + Û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), Ncache) + X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Ncache) gc_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nc), Ncache) - function update_simulations!(ΔŨ, ΔŨtup::NTuple{N, T}) where {N, T<:Real} - if any(new !== old for (new, old) in zip(ΔŨtup, ΔŨ)) # new ΔŨtup, update predictions: - ΔŨ1 = ΔŨtup[begin] - for i in eachindex(ΔŨtup) - ΔŨ[i] = ΔŨtup[i] # ΔŨ .= ΔŨtup seems to produce a type instability + g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) + geq_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, neq), Ncache) + function update_simulations!(Z̃, Z̃tup::NTuple{N, T}) where {N, T<:Real} + if any(new !== old for (new, old) in zip(Z̃tup, Z̃)) # new Z̃tup, update predictions: + Z̃1 = Z̃tup[begin] + for i in eachindex(Z̃tup) + Z̃[i] = Z̃tup[i] # Z̃ .= Z̃tup seems to produce a type instability end - Ŷe, Ue = get_tmp(Ŷe_cache, ΔŨ1), get_tmp(Ue_cache, ΔŨ1) - Ȳ, Ū = get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1) - x̂0, x̂0next = get_tmp(x̂0_cache, ΔŨ1), get_tmp(x̂0next_cache, ΔŨ1) - u0, û0 = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1) - gc, g = get_tmp(gc_cache, ΔŨ1), get_tmp(g_cache, ΔŨ1) - Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) - Ŷe, Ue = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) - gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) - g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) + ϵ = (nϵ ≠ 0) ? Z̃[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ΔŨ = get_tmp(ΔŨ_cache, Z̃1) + x̂0end = get_tmp(x̂0end_cache, Z̃1) + Ue, Ŷe = get_tmp(Ue_cache, Z̃1), get_tmp(Ŷe_cache, Z̃1) + U0, Ŷ0 = get_tmp(U0_cache, Z̃1), get_tmp(Ŷ0_cache, Z̃1) + X̂0, Û0 = get_tmp(X̂0_cache, Z̃1), get_tmp(Û0_cache, Z̃1) + gc, g = get_tmp(gc_cache, Z̃1), get_tmp(g_cache, Z̃1) + geq = get_tmp(geq_cache, Z̃1) + U0 = getU0!(U0, mpc, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, mpc.transcription, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) + Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) + gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) + g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) + geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃) end return nothing end - function Jfunc(ΔŨtup::Vararg{T, N}) where {N, T<:Real} - ΔŨ1 = ΔŨtup[begin] - ΔŨ = get_tmp(ΔŨ_cache, ΔŨ1) - update_simulations!(ΔŨ, ΔŨtup) - Ȳ, Ū = get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1) - Ŷe, Ue = get_tmp(Ŷe_cache, ΔŨ1), get_tmp(Ue_cache, ΔŨ1) - return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ)::T + function Jfunc(Z̃tup::Vararg{T, N}) where {N, T<:Real} + Z̃1 = Z̃tup[begin] + Z̃ = get_tmp(Z̃_cache, Z̃1) + update_simulations!(Z̃, Z̃tup) + ΔŨ = get_tmp(ΔŨ_cache, Z̃1) + Ue, Ŷe = get_tmp(Ue_cache, Z̃1), get_tmp(Ŷe_cache, Z̃1) + U0, Ŷ0 = get_tmp(U0_cache, Z̃1), get_tmp(Ŷ0_cache, Z̃1) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - function gfunc_i(i, ΔŨtup::NTuple{N, T}) where {N, T<:Real} - ΔŨ1 = ΔŨtup[begin] - ΔŨ = get_tmp(ΔŨ_cache, ΔŨ1) - update_simulations!(ΔŨ, ΔŨtup) - g = get_tmp(g_cache, ΔŨ1) + function gfunc_i(i, Z̃tup::NTuple{N, T}) where {N, T<:Real} + Z̃1 = Z̃tup[begin] + Z̃ = get_tmp(Z̃_cache, Z̃1) + update_simulations!(Z̃, Z̃tup) + g = get_tmp(g_cache, Z̃1) return g[i]::T end gfuncs = Vector{Function}(undef, ng) for i in 1:ng # this is another syntax for anonymous function, allowing parameters T and N: - gfuncs[i] = function (ΔŨtup::Vararg{T, N}) where {N, T<:Real} - return gfunc_i(i, ΔŨtup) + gfuncs[i] = function (Z̃tup::Vararg{T, N}) where {N, T<:Real} + return gfunc_i(i, Z̃tup) end end - return Jfunc, gfuncs + function gfunceq_i(i, Z̃tup::NTuple{N, T}) where {N, T<:Real} + Z̃1 = Z̃tup[begin] + Z̃ = get_tmp(Z̃_cache, Z̃1) + update_simulations!(Z̃, Z̃tup) + geq = get_tmp(geq_cache, Z̃1) + return geq[i]::T + end + geqfuncs = Vector{Function}(undef, neq) + for i in 1:neq + geqfuncs[i] = function (Z̃tup::Vararg{T, N}) where {N, T<:Real} + return gfunceq_i(i, Z̃tup) + end + end + return Jfunc, gfuncs, geqfuncs end -function init_nonlincon!(mpc::NonLinMPC, ::LinModel, gfuncs::Vector{<:Function}) +""" + init_nonlincon!( + mpc::NonLinMPC, model::LinModel, ::TranscriptionMethod, gfuncs, geqfuncs + ) + +Init nonlinear constraints for [`LinModel`](@ref). + +The only nonlinear constraints are the custom inequality constraints `gc`. +""" +function init_nonlincon!( + mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, + gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function} +) optim, con = mpc.optim, mpc.con - nΔŨ = length(mpc.ΔŨ) + nZ̃ = length(mpc.Z̃) if length(con.i_g) ≠ 0 i_base = 0 for i in 1:con.nc name = Symbol("g_c_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nΔŨ, gfuncs[i_base+i]; name) + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) end end return nothing end -function init_nonlincon!(mpc::NonLinMPC, ::NonLinModel, gfuncs::Vector{<:Function}) +""" + init_nonlincon!( + mpc::NonLinMPC, model::NonLinModel, ::MultipleShooting, gfuncs, geqfuncs + ) + +Init nonlinear constraints for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). + +The nonlinear constraints are the output prediction `Ŷ` bounds, the custom inequality +constraints `gc` and all the nonlinear equality constraints `geq`. +""" +function init_nonlincon!( + mpc::NonLinMPC, ::NonLinModel, ::MultipleShooting, + gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function} +) optim, con = mpc.optim, mpc.con - ny, nx̂, Hp, nΔŨ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.ΔŨ) + ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) + # --- nonlinear inequality constraints --- if length(con.i_g) ≠ 0 i_base = 0 for i in eachindex(con.Y0min) name = Symbol("g_Y0min_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nΔŨ, gfuncs[i_base+i]; name) + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) end i_base = 1Hp*ny for i in eachindex(con.Y0max) name = Symbol("g_Y0max_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nΔŨ, gfuncs[i_base+i]; name) + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) + end + i_base = 2Hp*ny + for i in 1:con.nc + name = Symbol("g_c_$i") + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) + end + end + # --- nonlinear equality constraints --- + Z̃var = optim[:Z̃var] + for i in 1:con.neq + name = Symbol("geq_$i") + geqfunc_i = JuMP.add_nonlinear_operator(optim, nZ̃, geqfuncs[i]; name) + # set with @constrains here instead of set_nonlincon!, since the number of nonlinear + # equality constraints is known and constant (±Inf are impossible): + @constraint(optim, geqfunc_i(Z̃var...) == 0) + end + return nothing +end + +""" + init_nonlincon!( + mpc::NonLinMPC, model::NonLinModel, ::SingleShooting, gfuncs, geqfuncs + ) + +Init nonlinear constraints for [`NonLinModel`](@ref) and [`SingleShooting`](@ref). + +The nonlinear constraints are the custom inequality constraints `gc`, the output +prediction `Ŷ` bounds and the terminal state `x̂end` bounds. +""" +function init_nonlincon!( + mpc::NonLinMPC, ::NonLinModel, ::SingleShooting, + gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function} +) + optim, con = mpc.optim, mpc.con + ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) + if length(con.i_g) ≠ 0 + i_base = 0 + for i in eachindex(con.Y0min) + name = Symbol("g_Y0min_$i") + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) + end + i_base = 1Hp*ny + for i in eachindex(con.Y0max) + name = Symbol("g_Y0max_$i") + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) end i_base = 2Hp*ny for i in eachindex(con.x̂0min) name = Symbol("g_x̂0min_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nΔŨ, gfuncs[i_base+i]; name) + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) end i_base = 2Hp*ny + nx̂ for i in eachindex(con.x̂0max) name = Symbol("g_x̂0max_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nΔŨ, gfuncs[i_base+i]; name) + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) end i_base = 2Hp*ny + 2nx̂ for i in 1:con.nc name = Symbol("g_c_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nΔŨ, gfuncs[i_base+i]; name) + optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) end end return nothing @@ -612,13 +729,13 @@ Set the custom nonlinear inequality constraints for `LinModel`. function set_nonlincon!( mpc::NonLinMPC, ::LinModel, optim::JuMP.GenericModel{JNT} ) where JNT<:Real - ΔŨvar = optim[:ΔŨvar] + Z̃var = optim[:Z̃var] con = mpc.con nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) for i in 1:con.nc gfunc_i = optim[Symbol("g_c_$i")] - @constraint(optim, gfunc_i(ΔŨvar...) <= 0) + @constraint(optim, gfunc_i(Z̃var...) <= 0) end return nothing end @@ -631,29 +748,29 @@ Also set output prediction `Ŷ` and terminal state `x̂end` constraints when no function set_nonlincon!( mpc::NonLinMPC, ::SimModel, optim::JuMP.GenericModel{JNT} ) where JNT<:Real - ΔŨvar = optim[:ΔŨvar] + Z̃var = optim[:Z̃var] con = mpc.con nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) for i in findall(.!isinf.(con.Y0min)) gfunc_i = optim[Symbol("g_Y0min_$(i)")] - @constraint(optim, gfunc_i(ΔŨvar...) <= 0) + @constraint(optim, gfunc_i(Z̃var...) <= 0) end for i in findall(.!isinf.(con.Y0max)) gfunc_i = optim[Symbol("g_Y0max_$(i)")] - @constraint(optim, gfunc_i(ΔŨvar...) <= 0) + @constraint(optim, gfunc_i(Z̃var...) <= 0) end for i in findall(.!isinf.(con.x̂0min)) gfunc_i = optim[Symbol("g_x̂0min_$(i)")] - @constraint(optim, gfunc_i(ΔŨvar...) <= 0) + @constraint(optim, gfunc_i(Z̃var...) <= 0) end for i in findall(.!isinf.(con.x̂0max)) gfunc_i = optim[Symbol("g_x̂0max_$(i)")] - @constraint(optim, gfunc_i(ΔŨvar...) <= 0) + @constraint(optim, gfunc_i(Z̃var...) <= 0) end for i in 1:con.nc gfunc_i = optim[Symbol("g_c_$i")] - @constraint(optim, gfunc_i(ΔŨvar...) <= 0) + @constraint(optim, gfunc_i(Z̃var...) <= 0) end return nothing end @@ -705,6 +822,41 @@ function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, gc, ϵ) return g end +""" + con_nonlinprogeq!( + geq, X̂0, Û0, mpc::NonLinMPC, model::NonLinModel, ::MultipleShooting, U0, Z̃ + ) + +Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). + +The method mutates the `geq`, `X̂0` and `Û0` vectors in argument. +""" +function con_nonlinprogeq!( + geq, X̂0, Û0, mpc::NonLinMPC, model::NonLinModel, ::MultipleShooting, U0, Z̃ +) + nx̂, nu, nd, Hp, Hc = mpc.estim.nx̂, model.nu, model.nd, mpc.Hp, mpc.Hc + nΔU, nX̂ = nu*Hc, nx̂*Hp + D̂0 = mpc.D̂0 + X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)] + x̂0 = @views mpc.estim.x̂0[1:nx̂] + d0 = @views mpc.d0[1:nd] + #TODO: allow parallel for loop or threads? + for j=1:Hp + u0 = @views U0[(1 + nu*(j-1)):(nu*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] + f̂!(x̂0next, û0, mpc.estim, model, x̂0, u0, d0) + x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op + x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] + ŝnext = @views geq[(1 + nx̂*(j-1)):(nx̂*j)] + ŝnext .= x̂0next .- x̂0next_Z̃ + x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for) + d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] + end + return geq +end +con_nonlinprogeq!(geq,_,_, ::NonLinMPC, ::NonLinModel, ::SingleShooting, _,_) = geq +con_nonlinprogeq!(geq,_,_, ::NonLinMPC, ::LinModel, ::TranscriptionMethod, _,_) = geq @doc raw""" con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) -> gc diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl new file mode 100644 index 000000000..fdad0879f --- /dev/null +++ b/src/controller/transcription.jl @@ -0,0 +1,702 @@ +""" +Abstract supertype of all transcription methods of [`PredictiveController`](@ref). + +The module currently supports [`SingleShooting`](@ref) and [`MultipleShooting`](@ref). +""" +abstract type TranscriptionMethod end + +@doc raw""" + SingleShooting() + +Construct a direct single shooting [`TranscriptionMethod`](@ref). + +The decision variable in the optimization problem is (excluding the slack ``ϵ``): +```math +\mathbf{Z} = \mathbf{ΔU} = \begin{bmatrix} + \mathbf{Δu}(k+0) \\ + \mathbf{Δu}(k+1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-1) \end{bmatrix} +``` +This method is generally more efficient for small control horizon ``H_c``, stable or mildly +nonlinear plant model/constraints. +""" +struct SingleShooting <: TranscriptionMethod end + +@doc raw""" + MultipleShooting() + +Construct a direct multiple shooting [`TranscriptionMethod`](@ref). + +The decision variable is (excluding ``ϵ``): +```math +\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \end{bmatrix} +``` +thus it also includes the predicted states, expressed as deviation vectors from the +operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)): +```math +\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} + \mathbf{x̂}_i(k+1) - \mathbf{x̂_{op}} \\ + \mathbf{x̂}_i(k+2) - \mathbf{x̂_{op}} \\ + \vdots \\ + \mathbf{x̂}_i(k+H_p) - \mathbf{x̂_{op}} \end{bmatrix} +``` +where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the +observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. This transcription +method is generally more efficient for large control horizon ``H_c``, unstable or highly +nonlinear plant models/constraints. + +Sparse optimizers like `OSQP` or `Ipopt` are recommended for this 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) + return estim.model.nu*Hc +end +function get_nZ(estim::StateEstimator, transcription::MultipleShooting, Hp, Hc) + return estim.model.nu*Hc + estim.nx̂*Hp +end + +@doc raw""" + init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu + +Init decision variables to input increments over ``H_c`` conversion matrix `PΔu`. + +The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{ΔU}``, the input +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 +!!! details "Extended Help" + Following the decision variable definition of the [`TranscriptionMethod`](@ref), the + conversion matrix ``\mathbf{P_{Δu}}``, we have: + - ``\mathbf{P_{Δu}} = \mathbf{I}`` if `transcription` is a [`SingleShooting`](@ref) + - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` + if `transcription` is a [`MultipleShooting`](@ref) +""" +function init_ZtoΔU end + +function init_ZtoΔU( + estim::StateEstimator{NT}, transcription::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 +) 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)] + return PΔu +end + +@doc raw""" + init_ZtoU(estim, transcription, Hp, Hc) -> Pu, Tu + +Init decision variables to inputs over ``H_p`` conversion matrices. + +The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{U}``, the manipulated +inputs over ``H_p``, is computed by: +```math +\mathbf{U} = \mathbf{P_u} \mathbf{Z} + \mathbf{T_u} \mathbf{u}(k-1) +``` +The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + 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{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{T_u} = \begin{bmatrix} + \mathbf{I} \\ + \mathbf{I} \\ + \vdots \\ + \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) + - ``\mathbf{P_u} = [\begin{smallmatrix}\mathbf{P_u^†} & \mathbf{0} \end{smallmatrix}]`` + if `transcription` is a [`MultipleShooting`](@ref) +""" +function init_ZtoU( + estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc +) where {NT<:Real} + model = estim.model + # 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) + 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)] +end + +@doc raw""" + init_predmat( + model::LinModel, estim, transcription::SingleShooting, Hp, Hc + ) -> E, G, J, K, V, ex̂, gx̂, jx̂, kx̂, vx̂ + +Construct the prediction matrices for [`LinModel`](@ref) and [`SingleShooting`](@ref). + +The model predictions are evaluated from the deviation vectors (see [`setop!`](@ref)), the +decision variable ``\mathbf{Z}`` (see [`TranscriptionMethod`](@ref)), and: +```math +\begin{aligned} + \mathbf{Ŷ_0} &= \mathbf{E Z} + \mathbf{G d_0}(k) + \mathbf{J D̂_0} + + \mathbf{K x̂_0}(k) + \mathbf{V u_0}(k-1) + + \mathbf{B} + \mathbf{Ŷ_s} \\ + &= \mathbf{E Z} + \mathbf{F} +\end{aligned} +``` +in which ``\mathbf{x̂_0}(k) = \mathbf{x̂}_i(k) - \mathbf{x̂_{op}}``, with ``i = k`` if +`estim.direct==true`, otherwise ``i = k - 1``. The predicted outputs ``\mathbf{Ŷ_0}`` and +measured disturbances ``\mathbf{D̂_0}`` respectively include ``\mathbf{ŷ_0}(k+j)`` and +``\mathbf{d̂_0}(k+j)`` values with ``j=1`` to ``H_p``, and input increments ``\mathbf{ΔU}``, +``\mathbf{Δu}(k+j)`` from ``j=0`` to ``H_c-1``. The vector ``\mathbf{B}`` contains the +contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` +operating points (for linearization at non-equilibrium point, see [`linearize`](@ref)). The +stochastic predictions ``\mathbf{Ŷ_s=0}`` if `estim` is not a [`InternalModel`](@ref), see +[`init_stochpred`](@ref). The method also computes similar matrices for the predicted +terminal states at ``k+H_p``: +```math +\begin{aligned} + \mathbf{x̂_0}(k+H_p) &= \mathbf{e_x̂ Z} + \mathbf{g_x̂ d_0}(k) + \mathbf{j_x̂ D̂_0} + + \mathbf{k_x̂ x̂_0}(k) + \mathbf{v_x̂ u_0}(k-1) + + \mathbf{b_x̂} \\ + &= \mathbf{e_x̂ Z} + \mathbf{f_x̂} +\end{aligned} +``` +The matrices ``\mathbf{E, G, J, K, V, B, e_x̂, g_x̂, j_x̂, k_x̂, v_x̂, b_x̂}`` are defined in the +Extended Help section. The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at +each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). + +# Extended Help +!!! details "Extended Help" + Using the augmented matrices ``\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}`` in `estim` (see + [`augment_model`](@ref)), and the function ``\mathbf{W}(j) = ∑_{i=0}^j \mathbf{Â}^i``, + the prediction matrices are computed by : + ```math + \begin{aligned} + \mathbf{E} &= \begin{bmatrix} + \mathbf{Ĉ W}(0)\mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ W}(1)\mathbf{B̂_u} & \mathbf{Ĉ W}(0)\mathbf{B̂_u} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ W}(H_p-1)\mathbf{B̂_u} & \mathbf{Ĉ W}(H_p-2)\mathbf{B̂_u} & \cdots & \mathbf{Ĉ W}(H_p-H_c+1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{G} &= \begin{bmatrix} + \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} \\ + \mathbf{Ĉ}\mathbf{Â}^{1} \mathbf{B̂_d} \\ + \vdots \\ + \mathbf{Ĉ}\mathbf{Â}^{H_p-1} \mathbf{B̂_d} \end{bmatrix} \\ + \mathbf{J} &= \begin{bmatrix} + \mathbf{D̂_d} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{Ĉ}\mathbf{Â}^{0} \mathbf{B̂_d} & \mathbf{D̂_d} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{Ĉ}\mathbf{Â}^{H_p-2} \mathbf{B̂_d} & \mathbf{Ĉ}\mathbf{Â}^{H_p-3} \mathbf{B̂_d} & \cdots & \mathbf{D̂_d} \end{bmatrix} \\ + \mathbf{K} &= \begin{bmatrix} + \mathbf{Ĉ}\mathbf{Â}^{1} \\ + \mathbf{Ĉ}\mathbf{Â}^{2} \\ + \vdots \\ + \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ + \mathbf{V} &= \begin{bmatrix} + \mathbf{Ĉ W}(0)\mathbf{B̂_u} \\ + \mathbf{Ĉ W}(1)\mathbf{B̂_u} \\ + \vdots \\ + \mathbf{Ĉ W}(H_p-1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{B} &= \begin{bmatrix} + \mathbf{Ĉ W}(0) \\ + \mathbf{Ĉ W}(1) \\ + \vdots \\ + \mathbf{Ĉ W}(H_p-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \end{aligned} + ``` + For the terminal constraints, the matrices are computed with: + ```math + \begin{aligned} + \mathbf{e_x̂} &= \begin{bmatrix} + \mathbf{W}(H_p-1)\mathbf{B̂_u} & + \mathbf{W}(H_p-2)\mathbf{B̂_u} & + \cdots & + \mathbf{W}(H_p-H_c+1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{g_x̂} &= \mathbf{Â}^{H_p-1} \mathbf{B̂_d} \\ + \mathbf{j_x̂} &= \begin{bmatrix} + \mathbf{Â}^{H_p-2} \mathbf{B̂_d} & + \mathbf{Â}^{H_p-3} \mathbf{B̂_d} & + \cdots & + \mathbf{0} + \end{bmatrix} \\ + \mathbf{k_x̂} &= \mathbf{Â}^{H_p} \\ + \mathbf{v_x̂} &= \mathbf{W}(H_p-1)\mathbf{B̂_u} \\ + \mathbf{b_x̂} &= \mathbf{W}(H_p-1) \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \end{aligned} + ``` +""" +function init_predmat( + model::LinModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc +) where {NT<:Real} + Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d + nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd + # --- pre-compute matrix powers --- + # Apow 3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,Hp+1] = A^Hp + Âpow = Array{NT}(undef, nx̂, nx̂, Hp+1) + Âpow[:,:,1] = I(nx̂) + for j=2:Hp+1 + Âpow[:,:,j] = @views Âpow[:,:,j-1]*Â + end + # Apow_csum 3D array : Apow_csum[:,:,1] = A^0, Apow_csum[:,:,2] = A^1 + A^0, ... + Âpow_csum = cumsum(Âpow, dims=3) + # helper function to improve code clarity and be similar to eqs. in docstring: + getpower(array3D, power) = @views array3D[:,:, power+1] + # --- current state estimates x̂0 --- + kx̂ = getpower(Âpow, Hp) + K = Matrix{NT}(undef, Hp*ny, nx̂) + for j=1:Hp + iRow = (1:ny) .+ ny*(j-1) + K[iRow,:] = Ĉ*getpower(Âpow, j) + end + # --- previous manipulated inputs lastu0 --- + vx̂ = getpower(Âpow_csum, Hp-1)*B̂u + V = Matrix{NT}(undef, Hp*ny, nu) + for j=1:Hp + iRow = (1:ny) .+ ny*(j-1) + V[iRow,:] = Ĉ*getpower(Âpow_csum, j-1)*B̂u + end + # --- decision variables Z --- + nZ = get_nZ(estim, transcription, Hp, Hc) + ex̂ = Matrix{NT}(undef, nx̂, nZ) + E = zeros(NT, Hp*ny, nZ) + for j=1:Hc # truncated with control horizon + iRow = (ny*(j-1)+1):(ny*Hp) + iCol = (1:nu) .+ nu*(j-1) + E[iRow, iCol] = V[iRow .- ny*(j-1),:] + ex̂[: , iCol] = getpower(Âpow_csum, Hp-j)*B̂u + end + # --- current measured disturbances d0 and predictions D̂0 --- + gx̂ = getpower(Âpow, Hp-1)*B̂d + G = Matrix{NT}(undef, Hp*ny, nd) + jx̂ = Matrix{NT}(undef, nx̂, Hp*nd) + J = repeatdiag(D̂d, Hp) + if nd ≠ 0 + for j=1:Hp + iRow = (1:ny) .+ ny*(j-1) + G[iRow,:] = Ĉ*getpower(Âpow, j-1)*B̂d + end + for j=1:Hp + iRow = (ny*j+1):(ny*Hp) + iCol = (1:nd) .+ nd*(j-1) + J[iRow, iCol] = G[iRow .- ny*j,:] + jx̂[: , iCol] = j < Hp ? getpower(Âpow, Hp-j-1)*B̂d : zeros(NT, nx̂, nd) + end + end + # --- state x̂op and state update f̂op operating points --- + coef_bx̂ = getpower(Âpow_csum, Hp-1) + coef_B = Matrix{NT}(undef, ny*Hp, nx̂) + for j=1:Hp + iRow = (1:ny) .+ ny*(j-1) + coef_B[iRow,:] = Ĉ*getpower(Âpow_csum, j-1) + end + f̂op_n_x̂op = estim.f̂op - estim.x̂op + bx̂ = coef_bx̂ * f̂op_n_x̂op + B = coef_B * f̂op_n_x̂op + return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ +end + +@doc raw""" + init_predmat( + model::LinModel, estim, transcription::MultipleShooting, Hp, Hc + ) -> E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ + +Construct the prediction matrices for [`LinModel`](@ref) and [`MultipleShooting`](@ref). + +They are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + They are all appropriately sized zero matrices ``\mathbf{0}``, except for: + ```math + \begin{aligned} + \mathbf{E} &= [\begin{smallmatrix}\mathbf{0} & \mathbf{E^†} \end{smallmatrix}] \\ + \mathbf{E^†} &= \text{diag}\mathbf{(Ĉ,Ĉ,...,Ĉ)} \\ + \mathbf{J} &= \text{diag}\mathbf{(D̂_d,D̂_d,...,D̂_d)} \\ + \mathbf{ex̂} &= [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}] + \end{aligned} + ``` +""" +function init_predmat( + model::LinModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc +) where {NT<:Real} + Ĉ, D̂d = estim.Ĉ, estim.D̂d + nu, nx̂, ny, nd = model.nu, estim.nx̂, model.ny, model.nd + # --- current state estimates x̂0 --- + K = zeros(NT, Hp*ny, nx̂) + kx̂ = zeros(NT, nx̂, nx̂) + # --- previous manipulated inputs lastu0 --- + V = zeros(NT, Hp*ny, nu) + vx̂ = zeros(NT, nx̂, nu) + # --- decision variables Z --- + E = [zeros(NT, Hp*ny, Hc*nu) repeatdiag(Ĉ, Hp)] + ex̂ = [zeros(NT, nx̂, Hc*nu + (Hp-1)*nx̂) I] + # --- current measured disturbances d0 and predictions D̂0 --- + G = zeros(NT, Hp*ny, nd) + gx̂ = zeros(NT, nx̂, nd) + J = repeatdiag(D̂d, Hp) + jx̂ = zeros(NT, nx̂, Hp*nd) + # --- state x̂op and state update f̂op operating points --- + B = zeros(NT, Hp*ny) + bx̂ = zeros(NT, nx̂) + return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ +end + +@doc raw""" + init_predmat(model::SimModel, estim, transcription::MultipleShooting, Hp, Hc) + +Return empty matrices except `ex̂` for [`SimModel`](@ref) and [`MultipleShooting`](@ref). + +The matrix is ``\mathbf{e_x̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]``. +""" +function init_predmat( + model::SimModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc +) where {NT<:Real} + nu, nx̂, nd = model.nu, estim.nx̂, model.nd + nZ = get_nZ(estim, transcription, Hp, Hc) + E = zeros(NT, 0, nZ) + G = zeros(NT, 0, nd) + J = zeros(NT, 0, nd*Hp) + K = zeros(NT, 0, nx̂) + V = zeros(NT, 0, nu) + B = zeros(NT, 0) + ex̂ = [zeros(NT, nx̂, Hc*nu + (Hp-1)*nx̂) I] + gx̂, jx̂, kx̂, vx̂, bx̂ = G, J, K, V, B + return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ +end + +""" + init_predmat(model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc) + +Return empty matrices for all other cases. +""" +function init_predmat( + model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc +) where {NT<:Real} + nu, nx̂, nd = model.nu, estim.nx̂, model.nd + nZ = get_nZ(estim, transcription, Hp, Hc) + E = zeros(NT, 0, nZ) + G = zeros(NT, 0, nd) + J = zeros(NT, 0, nd*Hp) + K = zeros(NT, 0, nx̂) + V = zeros(NT, 0, nu) + B = zeros(NT, 0) + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = E, G, J, K, V, B + return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ +end + +@doc raw""" + init_defectmat(model::LinModel, estim, transcription::MultipleShooting, Hp, Hc) + +Init the matrices for computing the defects over the predicted states. + +An equation similar to the prediction matrices (see +[`init_predmat`](@ref)) computes the defects over the predicted states: +```math +\begin{aligned} + \mathbf{Ŝ} &= \mathbf{E_ŝ Z} + \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} + + \mathbf{K_ŝ x̂_0}(k) + \mathbf{V_ŝ u_0}(k-1) + + \mathbf{B_ŝ} \\ + &= \mathbf{E_ŝ Z} + \mathbf{F_ŝ} +\end{aligned} +``` +They are forced to be ``\mathbf{Ŝ = 0}`` using the optimization equality constraints. The +matrices ``\mathbf{E_ŝ, G_ŝ, J_ŝ, K_ŝ, V_ŝ, B_ŝ}`` are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + The matrices are computed by: + ```math + \begin{aligned} + \mathbf{E_ŝ} &= \begin{bmatrix} + \mathbf{B̂_u} & \mathbf{0} & \cdots & \mathbf{0} & -\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{B̂_u} & \mathbf{B̂_u} & \cdots & \mathbf{0} & \mathbf{Â} & -\mathbf{I} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{B̂_u} & \mathbf{B̂_u} & \cdots & \mathbf{B̂_u} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{Â} & -\mathbf{I} \end{bmatrix} \\ + \mathbf{G_ŝ} &= \begin{bmatrix} + \mathbf{B̂_d} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ + \mathbf{J_ŝ} &= \begin{bmatrix} + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \mathbf{B̂_d} & \mathbf{0} & \cdots & \mathbf{0} & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots & \vdots \\ + \mathbf{0} & \mathbf{0} & \cdots & \mathbf{B̂_d} & \mathbf{0} \end{bmatrix} \\ + \mathbf{K_ŝ} &= \begin{bmatrix} + \mathbf{Â} \\ \mathbf{0} \\ \vdots \\ \mathbf{0} \end{bmatrix} \\ + \mathbf{V_ŝ} &= \begin{bmatrix} + \mathbf{B̂_u} \\ \mathbf{B̂_u} \\ \vdots \\ \mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{B_ŝ} &= \begin{bmatrix} + \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}} \end{bmatrix} + \end{aligned} + ``` +""" +function init_defectmat( + model::LinModel, estim::StateEstimator{NT}, transcription::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 + # --- current state estimates x̂0 --- + Kŝ = [Â; zeros(NT, nx̂*(Hp-1), nx̂)] + # --- previous manipulated inputs lastu0 --- + Vŝ = repeat(B̂u, Hp) + # --- decision variables Z --- + nI_nx̂ = Matrix{NT}(-I, nx̂, nx̂) + Eŝ = [zeros(nx̂*Hp, nu*Hc) repeatdiag(nI_nx̂, Hp)] + for j=1:Hc, i=j:Hp + iRow = (1:nx̂) .+ nx̂*(i-1) + iCol = (1:nu) .+ nu*(j-1) + Eŝ[iRow, iCol] = B̂u + end + for j=1:Hp-1 + iRow = (1:nx̂) .+ nx̂*j + iCol = (1:nx̂) .+ nx̂*(j-1) .+ nu*Hc + Eŝ[iRow, iCol] = Â + end + # --- current measured disturbances d0 and predictions D̂0 --- + Gŝ = [B̂d; zeros(NT, (Hp-1)*nx̂, nd)] + Jŝ = [zeros(nx̂, nd*Hp); repeatdiag(B̂d, Hp-1) zeros(NT, nx̂*(Hp-1), nd)] + # --- state x̂op and state update f̂op operating points --- + Bŝ = repeat(estim.f̂op - estim.x̂op, Hp) + return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ +end + +""" + init_defectmat(model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc) + +Return empty matrices for all other cases (N/A). +""" +function init_defectmat( + model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc +) where {NT<:Real} + nx̂, nu, nd = estim.nx̂, model.nu, model.nd + nZ = get_nZ(estim, transcription, Hp, Hc) + Eŝ = zeros(NT, 0, nZ) + Gŝ = zeros(NT, 0, nd) + Jŝ = zeros(NT, 0, nd*Hp) + Kŝ = zeros(NT, 0, nx̂) + Vŝ = zeros(NT, 0, nu) + Bŝ = zeros(NT, 0) + return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ +end + +@doc raw""" + linconstrainteq!( + mpc::PredictiveController, model::LinModel, transcription::MultipleShooting + ) + +Set `beq` vector for the linear model equality constraints (``\mathbf{A_{eq} Z̃ = b_{eq}}``). + +Also init ``\mathbf{F_ŝ} = \mathbf{G_ŝ d_0}(k) + \mathbf{J_ŝ D̂_0} + \mathbf{K_ŝ x̂_0}(k) + +\mathbf{V_ŝ u_0}(k-1) + \mathbf{B_ŝ}``, see [`init_defectmat`](@ref). +""" +function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::MultipleShooting) + Fŝ = mpc.con.Fŝ + Fŝ .= mpc.con.Bŝ + mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1) + mul!(Fŝ, mpc.con.Vŝ, mpc.estim.lastu0, 1, 1) + if model.nd ≠ 0 + mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1) + mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1) + end + mpc.con.beq .= @. -Fŝ + linconeq = mpc.optim[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, mpc.con.beq) + return nothing +end +linconstrainteq!(::PredictiveController, ::SimModel, ::SingleShooting) = nothing +linconstrainteq!(::PredictiveController, ::SimModel, ::MultipleShooting) = nothing + +@doc raw""" + set_warmstart!(mpc::PredictiveController, transcription::SingleShooting, Z̃var) -> Z̃0 + +Set and return the warm start value of `Z̃var` for [`SingleShooting`](@ref) transcription. + +If supported by `mpc.optim`, it warm-starts the solver at: +```math +\mathbf{ΔŨ} = +\begin{bmatrix} + \mathbf{Δu}(k+0|k-1) \\ + \mathbf{Δu}(k+1|k-1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-2|k-1) \\ + \mathbf{0} \\ + ϵ(k-1) +\end{bmatrix} +``` +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) + nu, Hc, Z̃0 = mpc.estim.model.nu, mpc.Hc, mpc.buffer.Z̃ + # --- input increments ΔU --- + Z̃0[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] + Z̃0[(Hc*nu-nu+1):(Hc*nu)] .= 0 + # --- slack variable ϵ --- + mpc.nϵ == 1 && (Z̃0[end] = mpc.Z̃[end]) + JuMP.set_start_value.(Z̃var, Z̃0) + return Z̃0 +end + +@doc raw""" + set_warmstart!(mpc::PredictiveController, transcription::MultipleShooting, Z̃var) -> Z̃0 + +Set and return the warm start value of `Z̃var` for [`MultipleShooting`](@ref) transcription. + +It warm-starts the solver at: +```math +\mathbf{ΔŨ} = +\begin{bmatrix} + \mathbf{Δu}(k+0|k-1) \\ + \mathbf{Δu}(k+1|k-1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-2|k-1) \\ + \mathbf{0} \\ + \mathbf{x̂_0}(k+1|k-1) \\ + \mathbf{x̂_0}(k+2|k-1) \\ + \vdots \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + \mathbf{x̂_0}(k+H_p-1|k-1) \\ + ϵ(k-1) +\end{bmatrix} +``` +where ``\mathbf{x̂_0}(k+j|k-1)`` is the predicted state for time ``k+j`` computed at the +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) + nu, nx̂, Hp, Hc, Z̃0 = mpc.estim.model.nu, mpc.estim.nx̂, mpc.Hp, mpc.Hc, mpc.buffer.Z̃ + # --- input increments ΔU --- + Z̃0[1:(Hc*nu-nu)] .= @views mpc.Z̃[nu+1:Hc*nu] + Z̃0[(Hc*nu-nu+1):(Hc*nu)] .= 0 + # --- predicted states X̂0 --- + Z̃0[(Hc*nu+1):(Hc*nu+Hp*nx̂-nx̂)] .= @views mpc.Z̃[(Hc*nu+nx̂+1):(Hc*nu+Hp*nx̂)] + Z̃0[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] .= @views mpc.Z̃[(Hc*nu+Hp*nx̂-nx̂+1):(Hc*nu+Hp*nx̂)] + # --- slack variable ϵ --- + mpc.nϵ == 1 && (Z̃0[end] = mpc.Z̃[end]) + JuMP.set_start_value.(Z̃var, Z̃0) + return Z̃0 +end + +getΔŨ!(ΔŨ, mpc::PredictiveController, ::SingleShooting, Z̃) = (ΔŨ .= Z̃) # since mpc.P̃Δu = I +getΔŨ!(ΔŨ, mpc::PredictiveController, ::TranscriptionMethod, Z̃) = mul!(ΔŨ, mpc.P̃Δu, Z̃) +getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_lastu0) + +@doc raw""" + predict!( + Ŷ0, x̂0end, _ , _ , + mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, + _ , Z̃ + ) -> Ŷ0, x̂0end + +Compute the predictions `Ŷ0`, terminal states `x̂0end` if model is a [`LinModel`](@ref). + +The method mutates `Ŷ0` and `x̂0end` vector arguments. The `x̂end` vector is used for +the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computations are +identical for any [`TranscriptionMethod`](@ref) if the model is linear. +""" +function predict!( + Ŷ0, x̂0end, _ , _ , + mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, + _ , Z̃ +) + # in-place operations to reduce allocations : + Ŷ0 .= mul!(Ŷ0, mpc.Ẽ, Z̃) .+ mpc.F + x̂0end .= mul!(x̂0end, mpc.con.ẽx̂, Z̃) .+ mpc.con.fx̂ + return Ŷ0, x̂0end +end + +@doc raw""" + predict!( + Ŷ0, x̂0end, X̂0, Û0, + mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, + U0, _ + ) -> Ŷ0, x̂0end + +Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref). + +The method mutates `Ŷ0`, `x̂0end`, `X̂0` and `Û0` arguments. +""" +function predict!( + Ŷ0, x̂0end, X̂0, Û0, + mpc::PredictiveController, model::NonLinModel, ::SingleShooting, + U0, _ +) + nu, nx̂, ny, nd, Hp, Hc = model.nu, mpc.estim.nx̂, model.ny, model.nd, mpc.Hp, mpc.Hc + D̂0 = mpc.D̂0 + x̂0 = @views mpc.estim.x̂0[1:nx̂] + d0 = @views mpc.d0[1:nd] + for j=1:Hp + u0 = @views U0[(1 + nu*(j-1)):(nu*j)] + û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] + x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] + f̂!(x̂0next, û0, mpc.estim, model, x̂0, u0, d0) + x̂0next .+= mpc.estim.f̂op .- mpc.estim.x̂op + x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] + d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] + ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] + ĥ!(ŷ0, mpc.estim, model, x̂0, d0) + end + Ŷ0 .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0. + x̂0end .= x̂0 + return Ŷ0, x̂0end +end + +@doc raw""" + predict!( + Ŷ0, x̂0end, _ , _ , + mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, + U0, Z̃ + ) -> Ŷ0, x̂0end + +Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`MultipleShooting`](@ref). + +The method mutates `Ŷ0` and `x̂0end` arguments. +""" +function predict!( + Ŷ0, x̂0end, _, _, + mpc::PredictiveController, model::NonLinModel, ::MultipleShooting, + U0, Z̃ +) + nu, ny, nd, nx̂, Hp, Hc = model.nu, model.ny, model.nd, mpc.estim.nx̂, mpc.Hp, mpc.Hc + X̂0 = @views Z̃[(nu*Hc+1):(nu*Hc+nx̂*Hp)] # Z̃ = [ΔU; X̂0; ϵ] + D̂0 = mpc.D̂0 + local x̂0 + for j=1:Hp + x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] + d0 = @views D̂0[(1 + nd*(j-1)):(nd*j)] + ŷ0 = @views Ŷ0[(1 + ny*(j-1)):(ny*j)] + ĥ!(ŷ0, mpc.estim, model, x̂0, d0) + end + Ŷ0 .+= mpc.F # F = Ŷs if mpc.estim is an InternalModel, else F = 0. + x̂0end .= x̂0 + return Ŷ0, x̂0end +end + diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 4a4916af9..d46402f26 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -1,4 +1,40 @@ +struct StateEstimatorBuffer{NT<:Real} + u ::Vector{NT} + û ::Vector{NT} + x̂ ::Vector{NT} + P̂ ::Matrix{NT} + Q̂ ::Matrix{NT} + R̂ ::Matrix{NT} + K̂ ::Matrix{NT} + ym::Vector{NT} + ŷ ::Vector{NT} + d ::Vector{NT} + empty::Vector{NT} +end + +@doc raw""" + StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int) +Create a buffer for `StateEstimator` objects for estimated states and measured outputs. + +The buffer is used to store intermediate results during estimation without allocating. +""" +function StateEstimatorBuffer{NT}( + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int +) where NT <: Real + u = Vector{NT}(undef, nu) + û = Vector{NT}(undef, nu) + x̂ = Vector{NT}(undef, nx̂) + P̂ = Matrix{NT}(undef, nx̂, nx̂) + Q̂ = Matrix{NT}(undef, nx̂, nx̂) + R̂ = Matrix{NT}(undef, nym, nym) + K̂ = Matrix{NT}(undef, nx̂, nym) + ym = Vector{NT}(undef, nym) + ŷ = Vector{NT}(undef, ny) + d = Vector{NT}(undef, nd) + empty = Vector{NT}(undef, 0) + return StateEstimatorBuffer{NT}(u, û, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) +end @doc raw""" init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym @@ -118,9 +154,9 @@ returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`: \end{aligned} ``` An error is thrown if the augmented model is not observable and `verify_obsv == true`. The -augmented operating points `x̂op` and `f̂op` are simply ``\mathbf{x_{op}}`` and -``\mathbf{f_{op}}`` vectors appended with zeros (see [`setop!`](@ref)). See Extended Help -for a detailed definition of the augmented matrices. +augmented operating points ``\mathbf{x̂_{op}}`` and ``\mathbf{f̂_{op}}`` are simply +``\mathbf{x_{op}}`` and ``\mathbf{f_{op}}`` vectors appended with zeros (see [`setop!`](@ref)). +See Extended Help for a detailed definition of the augmented matrices and vectors. # Extended Help !!! details "Extended Help" @@ -143,6 +179,13 @@ for a detailed definition of the augmented matrices. \mathbf{D̂_d} &= \mathbf{D_d} \end{aligned} ``` + and the operating points of the augmented model are: + ```math + \begin{aligned} + \mathbf{x̂_{op}} &= \begin{bmatrix} \mathbf{x_{op}} \\ \mathbf{0} \end{bmatrix} \\ + \mathbf{f̂_{op}} &= \begin{bmatrix} \mathbf{f_{op}} \\ \mathbf{0} \end{bmatrix} + \end{aligned} + ``` """ function augment_model(model::LinModel{NT}, As, Cs_u, Cs_y; verify_obsv=true) where NT<:Real nu, nx, nd = model.nu, model.nx, model.nd diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 68c5bf271..2ebd8ff1c 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -219,7 +219,8 @@ above, and the MHE is in the current form. Else ``p=1``, leading to the predicti See [`UnscentedKalmanFilter`](@ref) for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. This estimator allocates a fair amount of memory -at each time step. +at each time step for the optimization, which is hard-coded as a single shooting +transcription for now. !!! warning See the Extended Help if you get an error like: @@ -1306,13 +1307,14 @@ function get_optim_functions( nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) Nc = nZ̃ + 3 - Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nZ̃), Nc) - V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Nc) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc) - X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Nc) - x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc) - û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) - ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc) + myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: + Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Nc) + V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Nc) + g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc) + X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Nc) + x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc) + û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) + ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc) function update_simulations!(Z̃, Z̃tup::NTuple{N, T}) where {N, T <:Real} if any(new !== old for (new, old) in zip(Z̃tup, Z̃)) # new Z̃tup, update predictions: Z̃1 = Z̃tup[begin] diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 04aceeb2a..96dd64b6b 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -1,10 +1,10 @@ struct NonLinModel{ - NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver, LB<:LinearizationBuffer + NT<:Real, F<:Function, H<:Function, PT<:Any, DS<:DiffSolver, LB<:LinearizationBuffer } <: SimModel{NT} x0::Vector{NT} f!::F h!::H - p::P + p::PT solver::DS Ts::NT t::Vector{NT} @@ -24,8 +24,8 @@ struct NonLinModel{ linbuffer::LB buffer::SimModelBuffer{NT} function NonLinModel{NT}( - f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS, linbuffer::LB - ) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver, LB<:LinearizationBuffer} + f!::F, h!::H, Ts, nu, nx, ny, nd, p::PT, solver::DS, linbuffer::LB + ) where {NT<:Real, F<:Function, H<:Function, PT<:Any, DS<:DiffSolver, LB<:LinearizationBuffer} Ts > 0 || error("Sampling time Ts must be positive") uop = zeros(NT, nu) yop = zeros(NT, ny) @@ -39,7 +39,7 @@ struct NonLinModel{ x0 = zeros(NT, nx) t = zeros(NT, 1) buffer = SimModelBuffer{NT}(nu, nx, ny, nd) - return new{NT, F, H, P, DS, LB}( + return new{NT, F, H, PT, DS, LB}( x0, f!, h!, p, diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 7c6154988..e88cab888 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -19,39 +19,7 @@ julia> u = mpc([5]); round.(u, digits=3) """ abstract type PredictiveController{NT<:Real} end -struct PredictiveControllerBuffer{NT<:Real} - u ::Vector{NT} - ΔŨ::Vector{NT} - D̂ ::Vector{NT} - Ŷ ::Vector{NT} - U ::Vector{NT} - Ẽ ::Matrix{NT} - S̃ ::Matrix{NT} - empty::Vector{NT} -end - -@doc raw""" - PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int) - -Create a buffer for `PredictiveController` objects. - -The buffer is used to store intermediate results during computation without allocating. -""" -function PredictiveControllerBuffer{NT}( - nu::Int, ny::Int, nd::Int, Hp::Int, Hc::Int, nϵ::Int -) where NT <: Real - nΔŨ = nu*Hc + nϵ - u = Vector{NT}(undef, nu) - ΔŨ = Vector{NT}(undef, nΔŨ) - D̂ = Vector{NT}(undef, nd*Hp) - Ŷ = Vector{NT}(undef, ny*Hp) - U = Vector{NT}(undef, nu*Hp) - Ẽ = Matrix{NT}(undef, ny*Hp, nΔŨ) - S̃ = Matrix{NT}(undef, nu*Hp, nΔŨ) - empty = Vector{NT}(undef, 0) - return PredictiveControllerBuffer{NT}(u, ΔŨ, D̂, Ŷ, U, Ẽ, S̃, empty) -end - +include("controller/transcription.jl") include("controller/construct.jl") include("controller/execute.jl") include("controller/explicitmpc.jl") diff --git a/src/sim_model.jl b/src/sim_model.jl index 817ab4c3e..5a8ec6eb6 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -170,18 +170,6 @@ function setstate!(model::SimModel, x) return model end -function Base.show(io::IO, model::SimModel) - nu, nd = model.nu, model.nd - nx, ny = model.nx, model.ny - n = maximum(ndigits.((nu, nx, ny, nd))) + 1 - println(io, "$(typeof(model).name.name) with a sample time Ts = $(model.Ts) s"* - "$(detailstr(model)) and:") - println(io, "$(lpad(nu, n)) manipulated inputs u") - println(io, "$(lpad(nx, n)) states x") - println(io, "$(lpad(ny, n)) outputs y") - print(io, "$(lpad(nd, n)) measured disturbances d") -end - detailstr(model::SimModel) = "" @doc raw""" @@ -365,11 +353,22 @@ to_mat(A::AbstractVector, _ ...) = reshape(A, length(A), 1) to_mat(A::AbstractMatrix, _ ...) = A to_mat(A::Real, dims...) = fill(A, dims) - -"Functor allowing callable `SimModel` object as an alias for `evaloutput`." -(model::SimModel)(d=model.buffer.empty) = evaloutput(model::SimModel, d) - include("model/linmodel.jl") include("model/solver.jl") include("model/linearization.jl") -include("model/nonlinmodel.jl") \ No newline at end of file +include("model/nonlinmodel.jl") + +function Base.show(io::IO, model::SimModel) + nu, nd = model.nu, model.nd + nx, ny = model.nx, model.ny + n = maximum(ndigits.((nu, nx, ny, nd))) + 1 + println(io, "$(typeof(model).name.name) with a sample time Ts = $(model.Ts) s"* + "$(detailstr(model)) and:") + println(io, "$(lpad(nu, n)) manipulated inputs u") + println(io, "$(lpad(nx, n)) states x") + println(io, "$(lpad(ny, n)) outputs y") + print(io, "$(lpad(nd, n)) measured disturbances d") +end + +"Functor allowing callable `SimModel` object as an alias for `evaloutput`." +(model::SimModel)(d=model.buffer.empty) = evaloutput(model::SimModel, d) \ No newline at end of file diff --git a/src/state_estim.jl b/src/state_estim.jl index b2cb2f51d..93420831e 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -18,46 +18,15 @@ julia> ŷ = kf() """ abstract type StateEstimator{NT<:Real} end -struct StateEstimatorBuffer{NT<:Real} - u ::Vector{NT} - û ::Vector{NT} - x̂ ::Vector{NT} - P̂ ::Matrix{NT} - Q̂ ::Matrix{NT} - R̂ ::Matrix{NT} - K̂ ::Matrix{NT} - ym::Vector{NT} - ŷ ::Vector{NT} - d ::Vector{NT} - empty::Vector{NT} -end - -@doc raw""" - StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int) - -Create a buffer for `StateEstimator` objects for estimated states and measured outputs. - -The buffer is used to store intermediate results during estimation without allocating. -""" -function StateEstimatorBuffer{NT}( - nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int -) where NT <: Real - u = Vector{NT}(undef, nu) - û = Vector{NT}(undef, nu) - x̂ = Vector{NT}(undef, nx̂) - P̂ = Matrix{NT}(undef, nx̂, nx̂) - Q̂ = Matrix{NT}(undef, nx̂, nx̂) - R̂ = Matrix{NT}(undef, nym, nym) - K̂ = Matrix{NT}(undef, nx̂, nym) - ym = Vector{NT}(undef, nym) - ŷ = Vector{NT}(undef, ny) - d = Vector{NT}(undef, nd) - empty = Vector{NT}(undef, 0) - return StateEstimatorBuffer{NT}(u, û, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) -end - const IntVectorOrInt = Union{Int, Vector{Int}} +include("estimator/construct.jl") +include("estimator/execute.jl") +include("estimator/kalman.jl") +include("estimator/luenberger.jl") +include("estimator/mhe.jl") +include("estimator/internal_model.jl") + function Base.show(io::IO, estim::StateEstimator) nu, nd = estim.model.nu, estim.model.nd nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu @@ -76,11 +45,4 @@ function print_estim_dim(io::IO, estim::StateEstimator, n) println(io, "$(lpad(nym, n)) measured outputs ym ($(sum(estim.nint_ym)) integrating states)") println(io, "$(lpad(nyu, n)) unmeasured outputs yu") print(io, "$(lpad(nd, n)) measured disturbances d") -end - -include("estimator/construct.jl") -include("estimator/execute.jl") -include("estimator/kalman.jl") -include("estimator/luenberger.jl") -include("estimator/mhe.jl") -include("estimator/internal_model.jl") \ No newline at end of file +end \ No newline at end of file diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 89047f719..52e07bdde 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -36,6 +36,10 @@ mpc13 = LinMPC(model2) @test isa(mpc13, LinMPC{Float32}) @test isa(mpc13.optim, JuMP.GenericModel{Float64}) # OSQP does not support Float32 + mpc14 = LinMPC(model2, transcription=MultipleShooting()) + @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 @test_logs( (:warn, @@ -616,6 +620,7 @@ end h = (x,d,_) -> linmodel2.C*x + linmodel2.Dd*d nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1) + # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0): d = [0.1] preparestate!(nmpc2, [0], d) u = moveinput!(nmpc2, 7d, d) diff --git a/test/runtests.jl b/test/runtests.jl index 8b7ecf403..17fa3df6a 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,15 +1,10 @@ # spell-checker: disable using ModelPredictiveControl -using ControlSystemsBase using Documenter -using LinearAlgebra -using Random: randn -using JuMP, OSQP, Ipopt, DAQP, ForwardDiff -using Plots using Test, TestItemRunner -@run_package_tests +@run_package_tests(verbose=true) include("0_test_module.jl") include("1_test_sim_model.jl")