diff --git a/README.md b/README.md index bbe274473..23bdc59f5 100644 --- a/README.md +++ b/README.md @@ -81,9 +81,10 @@ for more detailed examples. - 📸 **Linearization**: Auto-differentiation for exact Jacobians. - ⚙️ **Adaptive MPC**: Manual model updates or automatic successive linearization. - 🏎️ **Explicit MPC**: Specialized for unconstrained problems. -- 🚧 **Constraints**: Soft/hard limits on inputs, outputs, increments, and terminal states. +- 🚧 **Bounds**: Soft/hard limits on inputs, outputs, increments, and terminal states. +- 🚫 **Contraints**: Soft/hard custom linear and nonlinear inequality constraints. - 🔁 **Feedback**: Internal model or state estimators (see features below). -- 📡 **Feedforward**: Integrated support for measured disturbances. +- 📡 **Feedforward**: Integrated support for measured disturbances. - 🔮 **Preview**: Custom predictions for setpoints and measured disturbances. - 📈 **Offset-Free**: Automatic model augmentation with integrators. - 📊 **Visuals**: Easy integration with `Plots.jl`. diff --git a/docs/src/index.md b/docs/src/index.md index 3ded65373..8a9a8a851 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -14,7 +14,8 @@ real-time optimization. Modern MPCs based on closed-loop state estimators are th of the package, but classical approaches that rely on internal models are also possible. The `JuMP` and `DifferentiationInterface` dependencies allows the user to test different optimizers and automatic differentiation (AD) backends easily if the performances of the -default settings are not satisfactory. +default settings are not satisfactory. Linear MPC controllers can be exported to lightweight +and standalone C code via the `LinearMPC` package extension. The documentation is divided in two parts: diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 4e8e74d17..ef79ef649 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -20,7 +20,9 @@ ModelPredictiveControl.init_defectmat ModelPredictiveControl.relaxU ModelPredictiveControl.relaxΔU ModelPredictiveControl.relaxŶ +ModelPredictiveControl.relaxW ModelPredictiveControl.relaxterminal +ModelPredictiveControl.augmentdefect ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc @@ -31,7 +33,7 @@ ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveContro ## Update Quadratic Optimization ```@docs -ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any) +ModelPredictiveControl.initpred!(::PredictiveController, ::LinModel, ::Any, ::Any, ::Any, ::Any, ::Any, ::Any) ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel, ::TranscriptionMethod) ModelPredictiveControl.linconstrainteq! ``` diff --git a/ext/LinearMPCext.jl b/ext/LinearMPCext.jl index b4db22980..595df0be9 100644 --- a/ext/LinearMPCext.jl +++ b/ext/LinearMPCext.jl @@ -180,6 +180,7 @@ end function validate_constraints(mpc::ModelPredictiveControl.LinMPC) nΔU = mpc.Hc * mpc.estim.model.nu + mpc.con.nw > 0 && error("Conversion of custom linear inequality constraints is not supported for now.") mpc.weights.isinf_C && return nothing # only hard constraints are entirely supported C_umin, C_umax = -mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end] C_Δumin, C_Δumax = -mpc.con.A_ΔŨmin[1:nΔU, end], -mpc.con.A_ΔŨmax[1:nΔU, end] diff --git a/src/controller/construct.jl b/src/controller/construct.jl index d8857bf7b..8b32bac3b 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -7,6 +7,9 @@ struct PredictiveControllerBuffer{NT<:Real} D̂ ::Vector{NT} Ŷ ::Vector{NT} U ::Vector{NT} + D̂e::Vector{NT} + Ŷe::Vector{NT} + Ue::Vector{NT} Ẽ ::Matrix{NT} P̃u::Matrix{NT} empty::Vector{NT} @@ -22,17 +25,20 @@ The buffer is used to store intermediate results during computation without allo 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̂ + nu, ny, nd = estim.model.nu, estim.model.ny, estim.model.nd 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) + D̂e = Vector{NT}(undef, nd*(Hp+1)) + Ŷe = Vector{NT}(undef, ny*(Hp+1)) + Ue = Vector{NT}(undef, nu*(Hp+1)) Ẽ = 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) + return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, D̂e, Ŷe, Ue, Ẽ, P̃u, empty) end "Include all the objective function weights of [`PredictiveController`](@ref)" @@ -102,18 +108,18 @@ function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1")) Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp")) - size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) - size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) - size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) + size(M_Hp) ≠ (nM,nM) && throw(DimensionMismatch("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) + size(N_Hc) ≠ (nN,nN) && throw(DimensionMismatch("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) + size(L_Hp) ≠ (nL,nL) && throw(DimensionMismatch("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative")) (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative")) (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative")) !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian")) !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian")) !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian")) - size(C) ≠ () && throw(ArgumentError("Cwt should be a real scalar")) + size(C) ≠ () && throw(DimensionMismatch("Cwt should be a real scalar")) C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) - !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar")) + !isnothing(E) && size(E) ≠ () && throw(DimensionMismatch("Ewt should be a real scalar")) end "Include all the data for the constraints of [`PredictiveController`](@ref)" @@ -134,13 +140,23 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} Kŝ ::Matrix{NT} Vŝ ::Matrix{NT} Bŝ ::Vector{NT} - # bounds over the prediction horizon (deviation vectors from operating points): + # custom linear equality constraints: + Ẽw ::Matrix{NT} + Fw ::Vector{NT} + W̄y ::SparseMatrixCSC{NT, Int} + W̄u ::SparseMatrixCSC{NT, Int} + W̄d ::SparseMatrixCSC{NT, Int} + W̄r ::SparseMatrixCSC{NT, Int} + nw ::Int + # bounds over the prediction horizon (deviation vectors from operating points): U0min ::Vector{NT} U0max ::Vector{NT} ΔŨmin ::Vector{NT} ΔŨmax ::Vector{NT} Y0min ::Vector{NT} Y0max ::Vector{NT} + Wmin ::Vector{NT} + Wmax ::Vector{NT} x̂0min ::Vector{NT} x̂0max ::Vector{NT} # A matrices for the linear inequality constraints: @@ -150,6 +166,8 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} A_ΔŨmax ::SparseMatrixCSC{NT, Int} A_Ymin ::Matrix{NT} A_Ymax ::Matrix{NT} + A_Wmin ::Matrix{NT} + A_Wmax ::Matrix{NT} A_x̂min ::Matrix{NT} A_x̂max ::Matrix{NT} A ::Matrix{NT} @@ -164,9 +182,11 @@ struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} beq ::Vector{NT} # nonlinear equality constraints: neq ::Int - # constraint softness parameter vectors for the nonlinear inequality constraints: + # constraint softness parameter vectors needing seperate storage: C_ymin ::Vector{NT} C_ymax ::Vector{NT} + C_wmin ::Vector{NT} + C_wmax ::Vector{NT} c_x̂min ::Vector{NT} c_x̂max ::Vector{NT} # indices of finite numbers in the g vector (nonlinear inequality constraints): @@ -187,13 +207,16 @@ The predictive controllers support both soft and hard constraints, defined by: \mathbf{u_{min} - c_{u_{min}}} ϵ ≤&&\ \mathbf{u}(k+j) &≤ \mathbf{u_{max} + c_{u_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_p - 1 \\ \mathbf{Δu_{min} - c_{Δu_{min}}} ϵ ≤&&\ \mathbf{Δu}(k+j) &≤ \mathbf{Δu_{max} + c_{Δu_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_c - 1 \\ \mathbf{y_{min} - c_{y_{min}}} ϵ ≤&&\ \mathbf{ŷ}(k+j) &≤ \mathbf{y_{max} + c_{y_{max}}} ϵ &&\qquad j = 1, 2 ,..., H_p \\ - \mathbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \mathbf{x̂}_i(k+j) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\qquad j = H_p + \mathbf{w_{min} - c_{w_{min}}} ϵ ≤&&\ \mathbf{w}(k+j) &≤ \mathbf{w_{max} + c_{w_{max}}} ϵ &&\qquad j = 0, 1 ,..., H_p \\ + \mathbf{x̂_{min} - c_{x̂_{min}}} ϵ ≤&&\ \mathbf{x̂}_i(k+H_p) &≤ \mathbf{x̂_{max} + c_{x̂_{max}}} ϵ &&\qquad \end{alignat*} ``` and also ``ϵ ≥ 0``. The last line is the terminal constraints applied on the states at the end of the horizon (see Extended Help). See [`MovingHorizonEstimator`](@ref) constraints -for details on bounds and softness parameters ``\mathbf{c}``. The output and terminal -constraints are all soft by default. See Extended Help for time-varying constraints. +for details on bounds and softness parameters ``\mathbf{c}``. The penultimate line with the +``\mathbf{w}`` vector is for custom linear inequality constraints. See Extended Help for +details on this and time-varying bounds. The output, custom and terminal constraints are all +soft by default. # Arguments !!! info @@ -208,10 +231,12 @@ constraints are all soft by default. See Extended Help for time-varying constrai - `umin=fill(-Inf,nu)` / `umax=fill(+Inf,nu)` : manipulated input bound ``\mathbf{u_{min/max}}`` - `Δumin=fill(-Inf,nu)` / `Δumax=fill(+Inf,nu)` : manipulated input increment bound ``\mathbf{Δu_{min/max}}`` - `ymin=fill(-Inf,ny)` / `ymax=fill(+Inf,ny)` : predicted output bound ``\mathbf{y_{min/max}}`` +- `wmin=fill(-Inf,nw)` / `wmax=fill(+Inf,nw)` : custom linear constraint bound ``\mathbf{w_{min/max}}`` - `x̂min=fill(-Inf,nx̂)` / `x̂max=fill(+Inf,nx̂)` : terminal constraint bound ``\mathbf{x̂_{min/max}}`` - `c_umin=fill(0.0,nu)` / `c_umax=fill(0.0,nu)` : `umin` / `umax` softness weight ``\mathbf{c_{u_{min/max}}}`` - `c_Δumin=fill(0.0,nu)` / `c_Δumax=fill(0.0,nu)` : `Δumin` / `Δumax` softness weight ``\mathbf{c_{Δu_{min/max}}}`` - `c_ymin=fill(1.0,ny)` / `c_ymax=fill(1.0,ny)` : `ymin` / `ymax` softness weight ``\mathbf{c_{y_{min/max}}}`` +- `c_wmin=fill(1.0,nw)` / `c_wmax=fill(1.0,nw)` : `wmin` / `wmax` softness weight ``\mathbf{c_{w_{min/max}}}`` - `c_x̂min=fill(1.0,nx̂)` / `c_x̂max=fill(1.0,nx̂)` : `x̂min` / `x̂max` softness weight ``\mathbf{c_{x̂_{min/max}}}`` - all the keyword arguments above but with a first capital letter, except for the terminal constraints, e.g. `Ymax` or `C_Δumin`: for time-varying constraints (see Extended Help) @@ -256,36 +281,61 @@ LinMPC controller with a sample time Ts = 4.0 s: later at runtime, set the bound to an absolute value sufficiently large when you create the controller (but different than `±Inf`). - It is also possible to specify time-varying constraints over ``H_p`` and ``H_c`` - horizons. In such a case, they are defined by: + It is also possible to specify time-varying constraints over the horizons. In such + cases, they are all defined by: ```math \begin{alignat*}{3} \mathbf{U_{min} - C_{u_{min}}} ϵ ≤&&\ \mathbf{U} &≤ \mathbf{U_{max} + C_{u_{max}}} ϵ \\ \mathbf{ΔU_{min} - C_{Δu_{min}}} ϵ ≤&&\ \mathbf{ΔU} &≤ \mathbf{ΔU_{max} + C_{Δu_{max}}} ϵ \\ - \mathbf{Y_{min} - C_{y_{min}}} ϵ ≤&&\ \mathbf{Ŷ} &≤ \mathbf{Y_{max} + C_{y_{max}}} ϵ + \mathbf{Y_{min} - C_{y_{min}}} ϵ ≤&&\ \mathbf{Ŷ} &≤ \mathbf{Y_{max} + C_{y_{max}}} ϵ \\ + \mathbf{W_{min} - C_{w_{min}}} ϵ ≤&&\ \mathbf{W} &≤ \mathbf{W_{max} + C_{w_{max}}} ϵ \end{alignat*} ``` For this, use the same keyword arguments as above but with a first capital letter: + - `Umin` / `Umax` / `C_umin` / `C_umax` : ``\mathbf{U}`` constraints `(nu*Hp,)`. - `ΔUmin` / `ΔUmax` / `C_Δumin` / `C_Δumax` : ``\mathbf{ΔU}`` constraints `(nu*Hc,)`. - `Ymin` / `Ymax` / `C_ymin` / `C_ymax` : ``\mathbf{Ŷ}`` constraints `(ny*Hp,)`. + - `Wmin` / `Wmax` / `C_wmin` / `C_wmax` : custom linear constraints `(nw*(Hp+1),)`. + + The custom linear inequality constraints are all gathered in the vector: + ```math + \begin{aligned} + \mathbf{W} + &= \begin{bmatrix} + \mathbf{w}(k+0) \\ \mathbf{w}(k+1) \\ \vdots \\ \mathbf{w}(k+H_p) \end{bmatrix} \\ + &= \begin{bmatrix} + \mathbf{W_y ŷ}(k+0) + \mathbf{W_u u}(k+0) + \mathbf{W_d d}(k+0) + \mathbf{W_r r_y}(k+0) \\ + \mathbf{W_y ŷ}(k+1) + \mathbf{W_u u}(k+1) + \mathbf{W_d d̂}(k+1) + \mathbf{W_r r̂_y}(k+1) \\ + \vdots \\ + \mathbf{W_y ŷ}(k+H_p) + \mathbf{W_u u}(k+H_p) + \mathbf{W_d d̂}(k+H_p) + \mathbf{W_r r̂_y}(k+H_p) \end{bmatrix} + \end{aligned} + ``` + The matrices ``\mathbf{W_y}``, ``\mathbf{W_u}``, ``\mathbf{W_d}`` and ``\mathbf{W_r}`` + must have `nw` rows and are provided at construction time. The terms with + ``\mathbf{W_y}`` are present only if the model is a [`LinModel`](@ref). Any `nothing` + value for these matrices is treated as a zero matrix. """ function setconstraint!( mpc::PredictiveController; umin = nothing, umax = nothing, Deltaumin = nothing, Deltaumax = nothing, ymin = nothing, ymax = nothing, + wmin = nothing, wmax = nothing, xhatmin = nothing, xhatmax = nothing, c_umin = nothing, c_umax = nothing, c_Deltaumin = nothing, c_Deltaumax = nothing, c_ymin = nothing, c_ymax = nothing, + c_wmin = nothing, c_wmax = nothing, c_xhatmin = nothing, c_xhatmax = nothing, Umin = nothing, Umax = nothing, DeltaUmin = nothing, DeltaUmax = nothing, Ymin = nothing, Ymax = nothing, + Wmin = nothing, Wmax = nothing, C_umax = nothing, C_umin = nothing, C_Deltaumax = nothing, C_Deltaumin = nothing, C_ymax = nothing, C_ymin = nothing, + C_wmax = nothing, C_wmin = nothing, Δumin = Deltaumin, Δumax = Deltaumax, x̂min = xhatmin, x̂max = xhatmax, c_Δumin = c_Deltaumin, c_Δumax = c_Deltaumax, @@ -296,73 +346,93 @@ function setconstraint!( 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 + nϵ, nw, nc = mpc.nϵ, con.nw, con.nc notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) if isnothing(Umin) && !isnothing(umin) - size(umin) == (nu,) || throw(ArgumentError("umin size must be $((nu,))")) + size(umin) == (nu,) || throw(DimensionMismatch("umin size must be $((nu,))")) for i = 1:nu*Hp con.U0min[i] = umin[(i-1) % nu + 1] - mpc.Uop[i] end elseif !isnothing(Umin) - size(Umin) == (nu*Hp,) || throw(ArgumentError("Umin size must be $((nu*Hp,))")) + size(Umin) == (nu*Hp,) || throw(DimensionMismatch("Umin size must be $((nu*Hp,))")) con.U0min .= Umin .- mpc.Uop end if isnothing(Umax) && !isnothing(umax) - size(umax) == (nu,) || throw(ArgumentError("umax size must be $((nu,))")) + size(umax) == (nu,) || throw(DimensionMismatch("umax size must be $((nu,))")) for i = 1:nu*Hp con.U0max[i] = umax[(i-1) % nu + 1] - mpc.Uop[i] end elseif !isnothing(Umax) - size(Umax) == (nu*Hp,) || throw(ArgumentError("Umax size must be $((nu*Hp,))")) + size(Umax) == (nu*Hp,) || throw(DimensionMismatch("Umax size must be $((nu*Hp,))")) con.U0max .= Umax .- mpc.Uop end if isnothing(ΔUmin) && !isnothing(Δumin) - size(Δumin) == (nu,) || throw(ArgumentError("Δumin size must be $((nu,))")) + size(Δumin) == (nu,) || throw(DimensionMismatch("Δumin size must be $((nu,))")) for i = 1:nu*Hc con.ΔŨmin[i] = Δumin[(i-1) % nu + 1] end elseif !isnothing(ΔUmin) - size(ΔUmin) == (nu*Hc,) || throw(ArgumentError("ΔUmin size must be $((nu*Hc,))")) + size(ΔUmin) == (nu*Hc,) || throw(DimensionMismatch("ΔUmin size must be $((nu*Hc,))")) con.ΔŨmin[1:nu*Hc] .= ΔUmin end if isnothing(ΔUmax) && !isnothing(Δumax) - size(Δumax) == (nu,) || throw(ArgumentError("Δumax size must be $((nu,))")) + size(Δumax) == (nu,) || throw(DimensionMismatch("Δumax size must be $((nu,))")) for i = 1:nu*Hc con.ΔŨmax[i] = Δumax[(i-1) % nu + 1] end elseif !isnothing(ΔUmax) - size(ΔUmax) == (nu*Hc,) || throw(ArgumentError("ΔUmax size must be $((nu*Hc,))")) + size(ΔUmax) == (nu*Hc,) || throw(DimensionMismatch("ΔUmax size must be $((nu*Hc,))")) con.ΔŨmax[1:nu*Hc] .= ΔUmax end if isnothing(Ymin) && !isnothing(ymin) - size(ymin) == (ny,) || throw(ArgumentError("ymin size must be $((ny,))")) + size(ymin) == (ny,) || throw(DimensionMismatch("ymin size must be $((ny,))")) for i = 1:ny*Hp con.Y0min[i] = ymin[(i-1) % ny + 1] - mpc.Yop[i] end elseif !isnothing(Ymin) - size(Ymin) == (ny*Hp,) || throw(ArgumentError("Ymin size must be $((ny*Hp,))")) + size(Ymin) == (ny*Hp,) || throw(DimensionMismatch("Ymin size must be $((ny*Hp,))")) con.Y0min .= Ymin .- mpc.Yop end if isnothing(Ymax) && !isnothing(ymax) - size(ymax) == (ny,) || throw(ArgumentError("ymax size must be $((ny,))")) + size(ymax) == (ny,) || throw(DimensionMismatch("ymax size must be $((ny,))")) for i = 1:ny*Hp con.Y0max[i] = ymax[(i-1) % ny + 1] - mpc.Yop[i] end elseif !isnothing(Ymax) - size(Ymax) == (ny*Hp,) || throw(ArgumentError("Ymax size must be $((ny*Hp,))")) + size(Ymax) == (ny*Hp,) || throw(DimensionMismatch("Ymax size must be $((ny*Hp,))")) con.Y0max .= Ymax .- mpc.Yop end + + if isnothing(Wmin) && !isnothing(wmin) + size(wmin) == (nw,) || throw(DimensionMismatch("wmin size must be $((nw,))")) + for i = 1:nw*(Hp+1) + con.Wmin[i] = wmin[(i-1) % nw + 1] + end + elseif !isnothing(Wmin) + size(Wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmin size must be $((nw*(Hp+1),))")) + con.Wmin .= Wmin + end + if isnothing(Wmax) && !isnothing(wmax) + size(wmax) == (nw,) || throw(DimensionMismatch("wmax size must be $((nw,))")) + for i = 1:nw*(Hp+1) + con.Wmax[i] = wmax[(i-1) % nw + 1] + end + elseif !isnothing(Wmax) + size(Wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("Wmax size must be $((nw*(Hp+1),))")) + con.Wmax .= Wmax + end if !isnothing(x̂min) - size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))")) + size(x̂min) == (nx̂,) || throw(DimensionMismatch("x̂min size must be $((nx̂,))")) con.x̂0min .= x̂min .- mpc.estim.x̂op end if !isnothing(x̂max) - size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))")) + size(x̂max) == (nx̂,) || throw(DimensionMismatch("x̂max size must be $((nx̂,))")) con.x̂0max .= x̂max .- mpc.estim.x̂op end allECRs = ( - c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, - C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, c_x̂min, c_x̂max, + c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax, + C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax, + c_x̂min, c_x̂max, ) if any(ECR -> !isnothing(ECR), allECRs) nϵ == 1 || throw(ArgumentError("Slack variable weight Cwt must be finite to set softness parameters")) @@ -375,47 +445,61 @@ function setconstraint!( isnothing(C_Δumax) && !isnothing(c_Δumax) && (C_Δumax = repeat(c_Δumax, Hc)) isnothing(C_ymin) && !isnothing(c_ymin) && (C_ymin = repeat(c_ymin, Hp)) isnothing(C_ymax) && !isnothing(c_ymax) && (C_ymax = repeat(c_ymax, Hp)) + isnothing(C_wmin) && !isnothing(c_wmin) && (C_wmin = repeat(c_wmin, Hp+1)) + isnothing(C_wmax) && !isnothing(c_wmax) && (C_wmax = repeat(c_wmax, Hp+1)) if !isnothing(C_umin) - size(C_umin) == (nu*Hp,) || throw(ArgumentError("C_umin size must be $((nu*Hp,))")) - any(C_umin .< 0) && error("C_umin weights should be non-negative") + size(C_umin) == (nu*Hp,) || throw(DimensionMismatch("C_umin size must be $((nu*Hp,))")) + any(<(0), C_umin) && error("C_umin weights should be non-negative") con.A_Umin[:, end] .= -C_umin end if !isnothing(C_umax) - size(C_umax) == (nu*Hp,) || throw(ArgumentError("C_umax size must be $((nu*Hp,))")) - any(C_umax .< 0) && error("C_umax weights should be non-negative") + size(C_umax) == (nu*Hp,) || throw(DimensionMismatch("C_umax size must be $((nu*Hp,))")) + any(<(0), C_umax) && error("C_umax weights should be non-negative") con.A_Umax[:, end] .= -C_umax end if !isnothing(C_Δumin) - size(C_Δumin) == (nu*Hc,) || throw(ArgumentError("C_Δumin size must be $((nu*Hc,))")) - any(C_Δumin .< 0) && error("C_Δumin weights should be non-negative") + size(C_Δumin) == (nu*Hc,) || throw(DimensionMismatch("C_Δumin size must be $((nu*Hc,))")) + any(<(0), C_Δumin) && error("C_Δumin weights should be non-negative") con.A_ΔŨmin[1:end-1, end] .= -C_Δumin end if !isnothing(C_Δumax) - size(C_Δumax) == (nu*Hc,) || throw(ArgumentError("C_Δumax size must be $((nu*Hc,))")) - any(C_Δumax .< 0) && error("C_Δumax weights should be non-negative") + size(C_Δumax) == (nu*Hc,) || throw(DimensionMismatch("C_Δumax size must be $((nu*Hc,))")) + any(<(0), C_Δumax) && error("C_Δumax weights should be non-negative") con.A_ΔŨmax[1:end-1, end] .= -C_Δumax end if !isnothing(C_ymin) - size(C_ymin) == (ny*Hp,) || throw(ArgumentError("C_ymin size must be $((ny*Hp,))")) - any(C_ymin .< 0) && error("C_ymin weights should be non-negative") + size(C_ymin) == (ny*Hp,) || throw(DimensionMismatch("C_ymin size must be $((ny*Hp,))")) + any(<(0), C_ymin) && error("C_ymin weights should be non-negative") con.C_ymin .= C_ymin size(con.A_Ymin, 1) ≠ 0 && (con.A_Ymin[:, end] .= -con.C_ymin) # for LinModel end if !isnothing(C_ymax) - size(C_ymax) == (ny*Hp,) || throw(ArgumentError("C_ymax size must be $((ny*Hp,))")) - any(C_ymax .< 0) && error("C_ymax weights should be non-negative") + size(C_ymax) == (ny*Hp,) || throw(DimensionMismatch("C_ymax size must be $((ny*Hp,))")) + any(<(0), C_ymax) && error("C_ymax weights should be non-negative") con.C_ymax .= C_ymax size(con.A_Ymax, 1) ≠ 0 && (con.A_Ymax[:, end] .= -con.C_ymax) # for LinModel end + if !isnothing(C_wmin) + size(C_wmin) == (nw*(Hp+1),) || throw(DimensionMismatch("C_wmin size must be $((nw*(Hp+1),))")) + any(<(0), C_wmin) && error("C_wmin weights should be non-negative") + con.C_wmin .= C_wmin + con.A_Wmin[:, end] .= -C_wmin + end + if !isnothing(C_wmax) + size(C_wmax) == (nw*(Hp+1),) || throw(DimensionMismatch("C_wmax size must be $((nw*(Hp+1),))")) + any(<(0), C_wmax) && error("C_wmax weights should be non-negative") + con.C_wmax .= C_wmax + con.A_Wmax[:, end] .= -C_wmax + end if !isnothing(c_x̂min) - size(c_x̂min) == (nx̂,) || throw(ArgumentError("c_x̂min size must be $((nx̂,))")) - any(c_x̂min .< 0) && error("c_x̂min weights should be non-negative") + size(c_x̂min) == (nx̂,) || throw(DimensionMismatch("c_x̂min size must be $((nx̂,))")) + any(<(0), c_x̂min) && error("c_x̂min weights should be non-negative") con.c_x̂min .= c_x̂min size(con.A_x̂min, 1) ≠ 0 && (con.A_x̂min[:, end] .= -con.c_x̂min) # for LinModel end if !isnothing(c_x̂max) - size(c_x̂max) == (nx̂,) || throw(ArgumentError("c_x̂max size must be $((nx̂,))")) - any(c_x̂max .< 0) && error("c_x̂max weights should be non-negative") + size(c_x̂max) == (nx̂,) || throw(DimensionMismatch("c_x̂max size must be $((nx̂,))")) + any(<(0), c_x̂max) && error("c_x̂max weights should be non-negative") con.c_x̂max .= c_x̂max size(con.A_x̂max, 1) ≠ 0 && (con.A_x̂max[:, end] .= -con.c_x̂max) # for LinModel end @@ -423,14 +507,17 @@ function setconstraint!( i_Umin, i_Umax = .!isinf.(con.U0min), .!isinf.(con.U0max) i_ΔŨmin, i_ΔŨmax = .!isinf.(con.ΔŨmin), .!isinf.(con.ΔŨmax) i_Ymin, i_Ymax = .!isinf.(con.Y0min), .!isinf.(con.Y0max) + i_Wmin, i_Wmax = .!isinf.(con.Wmin), .!isinf.(con.Wmax) 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, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, - i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Ymin, i_Ymax, i_Wmin, i_Wmax, + 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_Wmin, con.A_Wmax, + con.A_x̂min, con.A_x̂max, con.A_ŝ ) A = con.A[con.i_b, :] @@ -444,7 +531,8 @@ function setconstraint!( i_b, i_g = init_matconstraint_mpc( model, transcription, nc, i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, - i_Ymin, i_Ymax, i_x̂min, i_x̂max + i_Ymin, i_Ymax, i_Wmin, i_Wmax, + i_x̂min, i_x̂max, ) if i_b ≠ con.i_b || i_g ≠ con.i_g error("Cannot modify ±Inf constraints after calling moveinput!") @@ -464,7 +552,8 @@ Estimate the default prediction horizon `Hp` for [`LinModel`](@ref). default_Hp(model::LinModel) = DEFAULT_HP0 + estimate_delays(model) "Throw an error when model is not a [`LinModel`](@ref)." function default_Hp(::SimModel) - throw(ArgumentError("Prediction horizon Hp must be explicitly specified if model is not a LinModel.")) + msg = "Prediction horizon Hp must be explicitly specified if model is not a LinModel." + throw(ArgumentError(msg)) end """ @@ -556,6 +645,37 @@ end "Get the actual control Horizon `Hc` (integer) from the move blocking vector `nb`." get_Hc(nb::AbstractVector{Int}) = length(nb) +"Validate the custom linear constraint matrices dimensions." +function validate_custom_lincon(model::SimModel{NT}, Wy, Wu, Wd, Wr) where NT<:Real + nu, nd, ny = model.nu, model.nd, model.ny + if !isa(model, LinModel) && !isnothing(Wy) + throw(ArgumentError("Wy matrix can be specified only with LinModel.")) + end + nw = if !isnothing(Wy) + size(Wy, 1) + elseif !isnothing(Wu) + size(Wu, 1) + elseif !isnothing(Wd) + size(Wd, 1) + elseif !isnothing(Wr) + size(Wr, 1) + else + 0 + end + Wy = isnothing(Wy) ? zeros(NT, nw, ny) : Wy + Wu = isnothing(Wu) ? zeros(NT, nw, nu) : Wu + Wd = isnothing(Wd) ? zeros(NT, nw, nd) : Wd + Wr = isnothing(Wr) ? zeros(NT, nw, ny) : Wr + size(Wy, 2) == ny || throw(DimensionMismatch("Wy must have $ny columns.")) + size(Wu, 2) == nu || throw(DimensionMismatch("Wu must have $nu columns.")) + size(Wd, 2) == nd || throw(DimensionMismatch("Wd must have $nd columns.")) + size(Wr, 2) == ny || throw(DimensionMismatch("Wr must have $ny columns.")) + if size(Wy, 1) != nw || size(Wu, 1) != nw || size(Wd, 1) != nw || size(Wr, 1) != nw + msg = "all custom linear constraint matrices must have the same number of rows." + throw(DimensionMismatch(msg)) + end + return Wy, Wu, Wd, Wr +end """ validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u) @@ -638,8 +758,9 @@ verify_cond(::TranscriptionMethod,_,_) = nothing transcription::TranscriptionMethod, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Wy, Wu, Wd, Wr, gc!=nothing, nc=0 ) -> con, nϵ, P̃Δu, P̃u, Ẽ @@ -653,66 +774,89 @@ function init_defaultcon_mpc( transcription::TranscriptionMethod, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Wy, Wu, Wd, Wr, gc!::GCfunc = nothing, nc = 0 ) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ + nw = size(Wy, 1) + nW = nw*(Hp+1) nϵ = weights.isinf_C ? 0 : 1 u0min, u0max = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) Δumin, Δumax = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu) y0min, y0max = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny) + wmin, wmax = fill(convert(NT,-Inf), nw), fill(convert(NT,+Inf), nw) x̂0min, x̂0max = fill(convert(NT,-Inf), nx̂), fill(convert(NT,+Inf), nx̂) c_umin, c_umax = fill(zero(NT), nu), fill(zero(NT), nu) c_Δumin, c_Δumax = fill(zero(NT), nu), fill(zero(NT), nu) c_ymin, c_ymax = fill(one(NT), ny), fill(one(NT), ny) + c_wmin, c_wmax = fill(one(NT), nw), fill(one(NT), nw) c_x̂min, c_x̂max = fill(one(NT), nx̂), fill(one(NT), nx̂) - U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max = - 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) + U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax = + repeat_constraints( + Hp, Hc, u0min, u0max, Δumin, Δumax, y0min, y0max, wmin, wmax + ) + C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax, C_wmin, C_wmax = + repeat_constraints( + Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax, c_wmin, c_wmax + ) + W̄y = sparse(repeatdiag(Wy, Hp+1)) + W̄u = sparse(repeatdiag(Wu, Hp+1)) + W̄d = sparse(repeatdiag(Wd, Hp+1)) + W̄r = sparse(repeatdiag(Wr, Hp+1)) 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_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, 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_Wmin, i_Wmax = .!isinf.(Wmin), .!isinf.(Wmax) 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, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, A_x̂max, A_x̂min, A_ŝ ) + # dummy fx̂, Fw and Fŝ vectors (updated just before optimization) + fx̂, Fw, Fŝ = zeros(NT, nx̂), zeros(NT, nW), zeros(NT, nx̂*Hp) # 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̂ , - Ẽŝ , 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 , + ẽx̂ , fx̂ , gx̂ , jx̂ , kx̂ , vx̂ , bx̂ , + Ẽŝ , Fŝ , Gŝ , Jŝ , Kŝ , Vŝ , Bŝ , + Ẽw , Fw , W̄y , W̄u , W̄d , W̄r , nw , + U0min , U0max , ΔŨmin , ΔŨmax , + Y0min , Y0max , Wmin , Wmax , x̂0min , x̂0max , + A_Umin , A_Umax , A_ΔŨmin , A_ΔŨmax , + A_Ymin , A_Ymax , A_Wmin , A_Wmax , 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, + C_ymin , C_ymax , C_wmin , C_wmax , c_x̂min , c_x̂max , + i_g , gc! , nc ) return con, nϵ, P̃Δu, P̃u, Ẽ end -"Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons." -function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax) +"Repeat predictive controller constraints over their respective horizons." +function repeat_constraints(Hp, Hc, umin, umax, Δumin, Δumax, ymin, ymax, wmin, wmax) Umin = repeat(umin, Hp) Umax = repeat(umax, Hp) ΔUmin = repeat(Δumin, Hc) ΔUmax = repeat(Δumax, Hc) Ymin = repeat(ymin, Hp) Ymax = repeat(ymax, Hp) - return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax + Wmin = repeat(wmin, Hp+1) + Wmax = repeat(wmax, Hp+1) + return Umin, Umax, ΔUmin, ΔUmax, Ymin, Ymax, Wmin, Wmax end @doc raw""" @@ -834,6 +978,83 @@ function relaxŶ(E::AbstractMatrix{NT}, C_ymin, C_ymax, nϵ) where NT<:Real return A_Ymin, A_Ymax, Ẽ end +@doc raw""" + relaxW(E, Pu, nw, W̄y, W̄u, C_wmin, C_wmax, nϵ) -> A_Wmin, A_Wmax, Ẽw + +Construct and augment the custom linear constraints with slack variable ϵ for softening. + +By introducing the following block-diagonal matrices with ``H_p + 1`` blocks: +```math +\begin{aligned} +\mathbf{W̄_y} &= \mathrm{diag}(\mathbf{W_y}, \mathbf{W_y}, \dots, \mathbf{W_y}) \\ +\mathbf{W̄_u} &= \mathrm{diag}(\mathbf{W_u}, \mathbf{W_u}, \dots, \mathbf{W_u}) \\ +\mathbf{W̄_d} &= \mathrm{diag}(\mathbf{W_d}, \mathbf{W_d}, \dots, \mathbf{W_d}) \\ +\mathbf{W̄_r} &= \mathrm{diag}(\mathbf{W_r}, \mathbf{W_r}, \dots, \mathbf{W_r}) +\end{aligned} +``` +the ``\mathbf{W}`` vector defined in the Extended Help section of [`setconstraint!`](@ref) +can be expressed as: +```math +\mathbf{W} = \mathbf{E_w} \mathbf{Z} + \mathbf{F_w} +``` +in which: +```math +\mathbf{E_w} = \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{0} \\ \mathbf{E} \end{smallmatrix}] + + \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{P_u} \\ \mathbf{p_u} \end{smallmatrix}] +``` +The ``\mathbf{E}`` matrix appears in the linear output prediction equation +``\mathbf{Ŷ_0 = E Z + F}``, and ``\mathbf{P_u}``, in the conversion equation +``\mathbf{U = P_u Z + T_u u}(k-1)``. The ``\mathbf{p_u}`` matrix corresponds to the last +`nu` rows of ``\mathbf{P_u}``. The ``\mathbf{W̄_u}`` term assumes that ``H_c ≤ H_p``, hence +``\mathbf{Δu}(k + H_c) = \mathbf{0}`` and ``\mathbf{u}(k + H_c) = \mathbf{u}(k + H_c - 1)``. +The ``\mathbf{F_w}`` vector is updated at each control period `k` in [`linconstraint!`](@ref) +method, and is defined as: +```math +\mathbf{F_w} = + \mathbf{W̄_y} [\begin{smallmatrix} \mathbf{ŷ}(k) \\ \mathbf{F + Y_{op}} \end{smallmatrix}] + + \mathbf{W̄_u} [\begin{smallmatrix} \mathbf{T_u u}(k-1) \\ \mathbf{u}(k-1) \end{smallmatrix}] + + \mathbf{W̄_d} [\begin{smallmatrix} \mathbf{d}(k) \\ \mathbf{D̂} \end{smallmatrix}] + + \mathbf{W̄_r} [\begin{smallmatrix} \mathbf{r_y}(k) \\ \mathbf{R̂_y} \end{smallmatrix}] +``` +Denoting the decision variables augmented with the slack variable +``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, the function +returns the ``\mathbf{Ẽ_w}`` matrix that appears in the custom constraint equation +``\mathbf{W = Ẽ_w Z̃ + F_w}``, and the ``\mathbf{A}`` matrices for the inequality constraints: +```math +\begin{bmatrix} + \mathbf{A_{W_{min}}} \\ + \mathbf{A_{W_{max}}} +\end{bmatrix} \mathbf{Z̃} ≤ +\begin{bmatrix} + - \mathbf{W_{min} + F_w} \\ + + \mathbf{W_{max} - F_w} +\end{bmatrix} +``` +""" +function relaxW(E::AbstractMatrix{NT}, Pu, Hp, W̄y, W̄u, C_wmin, C_wmax, nϵ) where {NT<:Real} + nW = size(W̄y, 1) + ny = size(W̄y, 2) ÷ (Hp + 1) + nu = size(Pu, 1) ÷ Hp + if iszero(size(E, 1)) + # model is not a LinModel, thus no Wy terms in the custom constraints: + Wy_terms = zeros(NT, nW, size(E, 2)) + else + Wy_terms = W̄y*[zeros(NT, ny, size(E, 2)); E] + end + Wu_terms = W̄u*[Pu; Pu[end-nu+1:end, :]] + Ew = Wy_terms + Wu_terms + if nϵ == 1 # Z̃ = [Z; ϵ] + # ϵ impacts custom constraint calculations: + A_Wmin, A_Wmax = -[Ew C_wmin], [Ew -C_wmax] + # ϵ has no impact on custom constraint predictions: + Ẽw = [Ew zeros(NT, nW, 1)] + else # Z̃ = Z (only hard constraints) + Ẽw = Ew + A_Wmin, A_Wmax = -Ew, Ew + end + return A_Wmin, A_Wmax, Ẽw +end + @doc raw""" relaxterminal(ex̂, c_x̂min, c_x̂max, nϵ) -> A_x̂min, A_x̂max, ẽx̂ diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7f6da0cdf..8342f6924 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -72,7 +72,7 @@ function moveinput!( @warn "preparestate! should be called before moveinput! with current estimators" end validate_args(mpc, ry, d, lastu, D̂, R̂y, R̂u) - initpred!(mpc, mpc.estim.model, d, lastu, D̂, R̂y, R̂u) + initpred!(mpc, mpc.estim.model, ry, d, lastu, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model, mpc.transcription) linconstrainteq!(mpc, mpc.estim.model, mpc.transcription) Z̃ = optim_objective!(mpc) @@ -202,7 +202,7 @@ function addinfo!(info, mpc::PredictiveController) end @doc raw""" - initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R̂y, R̂u) -> nothing + initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing Init linear model prediction matrices `F, q̃, r` and current estimated output `ŷ`. @@ -221,8 +221,8 @@ They are computed with these equations using in-place operations: \end{aligned} ``` """ -function initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, d, lastu, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂, R̂y, R̂u) + F = initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) F .+= mpc.B # F = F + B mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) # F = F + K*x̂0 mul!(F, mpc.V, mpc.lastu0, 1, 1) # F = F + V*lastu0 @@ -254,24 +254,26 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, lastu, D̂, R end @doc raw""" - initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) + initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing Init `lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). """ -function initpred!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) - F = initpred_common!(mpc, model, d, lastu, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) + initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) return nothing end """ - initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, D̂, R̂y, R̂u) -> F + initpred_common!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> F Common computations of `initpred!` for all types of [`SimModel`](@ref). 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, lastu, D̂, R̂y, R̂u) +function initpred_common!( + mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u +) mpc.lastu0 .= lastu .- model.uop mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) @@ -281,6 +283,7 @@ function initpred_common!(mpc::PredictiveController, model::SimModel, d, lastu, mpc.D̂e[1:model.nd] .= d mpc.D̂e[model.nd+1:end] .= D̂ end + mpc.ry .= ry mpc.R̂y .= R̂y mpc.R̂u .= R̂u predictstoch!(mpc.F, mpc, mpc.estim) @@ -300,6 +303,45 @@ end "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)." predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) +@doc raw""" + linconstraint_custom!(mpc::PredictiveController, model::SimModel) + +Init the ``\mathbf{F_w}`` vector for the custom linear inequality constraints. + +See [`relaxW`](@ref) for the definition of the vector. The function does nothing if +`mpc.con.nw < 1`. +""" +function linconstraint_custom!(mpc::PredictiveController, model::SimModel) + mpc.con.nw < 1 && return nothing + ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer + Fw = mpc.con.Fw + Ue_term, D̂e_term, R̂e_term = buffer.Ue, buffer.D̂e, buffer.Ŷe + Fw .= 0 + Ue_term[1:end-nu] .= mpc.Tu_lastu0 .+ mpc.Uop + Ue_term[end-nu+1:end] .= mpc.lastu0 .+ model.uop + mul!(Fw, mpc.con.W̄u, Ue_term, 1, 1) + if model.nd > 0 + D̂e_term[1:nd] .= mpc.d0 .+ model.dop + D̂e_term[nd+1:end] .= mpc.D̂0 .+ mpc.Dop + mul!(Fw, mpc.con.W̄d, D̂e_term, 1, 1) + end + R̂e_term[1:ny] .= mpc.ry + R̂e_term[ny+1:end] .= mpc.R̂y + mul!(Fw, mpc.con.W̄r, R̂e_term, 1, 1) + return linconstraint_custom_outputs!(mpc, model) +end + +"Also include the `W̄y` term in the custom linear constraints for [`LinModel`](@ref)." +function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinModel) + Ŷe_term, Fw, ny = mpc.buffer.Ŷe, mpc.con.Fw, model.ny + Ŷe_term[1:ny] .= mpc.ŷ + Ŷe_term[ny+1:end] .= mpc.F .+ mpc.Yop + mul!(Fw, mpc.con.W̄y, Ŷe_term, 1, 1) + return nothing +end +"Do nothing for other model types." +linconstraint_custom_outputs!(::PredictiveController, ::SimModel) = nothing + """ extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe @@ -626,11 +668,14 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) weights = mpc.weights nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb optim, con = mpc.optim, mpc.con + nZ = get_nZ(estim, transcription, Hp, Hc) + Pu = mpc.P̃u[:, 1:nZ] # --- prediction matrices --- E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) A_Ymin, A_Ymax, Ẽ = relaxŶ(E, con.C_ymin, con.C_ymax, mpc.nϵ) + A_Wmin, A_Wmax, Ẽw = relaxW(E, Pu, Hp, con.W̄y, con.W̄u, con.C_wmin, con.C_wmax, 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 @@ -638,6 +683,13 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) mpc.K .= K mpc.V .= V mpc.B .= B + # --- terminal constraints --- + con.ẽx̂ .= ẽx̂ + con.gx̂ .= gx̂ + con.jx̂ .= jx̂ + con.kx̂ .= kx̂ + con.vx̂ .= vx̂ + con.bx̂ .= bx̂ # --- defect matrices --- Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ) @@ -647,15 +699,13 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) con.Kŝ .= Kŝ con.Vŝ .= Vŝ con.Bŝ .= Bŝ + # --- custom linear constraints --- + con.Ẽw .= Ẽw # --- linear inequality constraints --- - con.ẽx̂ .= ẽx̂ - con.gx̂ .= gx̂ - con.jx̂ .= jx̂ - con.kx̂ .= kx̂ - con.vx̂ .= vx̂ - con.bx̂ .= bx̂ con.A_Ymin .= A_Ymin con.A_Ymax .= A_Ymax + con.A_Wmin .= A_Wmin + con.A_Wmax .= A_Wmax con.A_x̂min .= A_x̂min con.A_x̂max .= A_x̂max con.A .= [ diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 4dfd07283..414155724 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -7,6 +7,7 @@ struct ExplicitMPC{ transcription::SingleShooting Z̃::Vector{NT} ŷ::Vector{NT} + ry::Vector{NT} Hp::Int Hc::Int nϵ::Int @@ -44,7 +45,7 @@ struct ExplicitMPC{ ) where {NT<:Real, SE<:StateEstimator, CW<:ControllerWeights} model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ - ŷ = copy(model.yop) # dummy vals (updated just before optimization) + ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) nϵ = 0 # no slack variable ϵ for ExplicitMPC # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) @@ -54,8 +55,7 @@ struct ExplicitMPC{ PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc, nb) - # dummy val (updated just before optimization): - F = zeros(NT, ny*Hp) + F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): @@ -71,7 +71,7 @@ struct ExplicitMPC{ mpc = new{NT, SE, CW}( estim, transcription, - Z̃, ŷ, + Z̃, ŷ, ry, Hp, Hc, nϵ, nb, weights, R̂u, R̂y, @@ -106,8 +106,9 @@ 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`, `transcription` and `optim`, which are not -supported. It uses a [`SingleShooting`](@ref) transcription method and is allocation-free. +identical to [`LinMPC`](@ref), except for `Cwt`, `Wy`, `Wu`, `Wd`, `Wr`, `transcription` and +`optim`, which are not supported. It uses a [`SingleShooting`](@ref) transcription method +and is allocation-free. This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) with default arguments. diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index d7d30410d..47424d799 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -16,6 +16,7 @@ struct LinMPC{ con::ControllerConstraint{NT, Nothing} Z̃::Vector{NT} ŷ::Vector{NT} + ry::Vector{NT} Hp::Int Hc::Int nϵ::Int @@ -48,7 +49,8 @@ struct LinMPC{ Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} function LinMPC{NT}( - estim::SE, Hp, Hc, nb, weights::CW, + estim::SE, Hp, Hc, nb, weights::CW, + Wy, Wu, Wd, Wr, transcription::TM, optim::JM ) where { NT<:Real, @@ -58,26 +60,27 @@ struct LinMPC{ 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) + nu, ny, nd = model.nu, model.ny, model.nd + ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) lastu0 = zeros(NT, nu) + Wy, Wu, Wd, Wr = validate_custom_lincon(model, Wy, Wu, Wd, Wr) validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) + F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - # dummy vals (updated just before optimization): - F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Wy, Wu, Wd, Wr ) H̃ = init_quadprog(model, transcription, weights, Ẽ, P̃Δu, P̃u) # dummy vals (updated just before optimization): @@ -91,7 +94,7 @@ struct LinMPC{ buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE, CW, TM, JM}( estim, transcription, optim, con, - Z̃, ŷ, + Z̃, ŷ, ry, Hp, Hc, nϵ, nb, weights, R̂u, R̂y, @@ -147,6 +150,7 @@ This method uses the default state estimator, a [`SteadyKalmanFilter`](@ref) wit arguments. This controller allocates memory at each time step for the optimization. # Arguments + - `model::LinModel` : model used for controller predictions and state estimations. - `Hp::Int=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays in `model`. - `Hc::Union{Int, Vector{Int}}=2` : control horizon ``H_c``, custom move blocking pattern is @@ -157,6 +161,10 @@ arguments. This controller allocates memory at each time step for the optimizati - `M_Hp=Diagonal(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``. - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``. +- `Wy=nothing` : custom linear constraint matrix for output (see Extended Help). +- `Wu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Wd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Wr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only. - `transcription=SingleShooting()` : [`SingleShooting`](@ref) or [`MultipleShooting`](@ref). - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in @@ -191,6 +199,11 @@ LinMPC controller with a sample time Ts = 4.0 s: for over-actuated systems, when `nu > ny` (e.g. prioritize solutions with lower economical costs). The default `Lwt` value implies that this feature is disabled by default. + The custom linear constraint matrices `Wy`, `Wu`, `Wd`, and `Wr` allow to define + constraints based on linear combinations of outputs, manipulated inputs, measured + disturbances, and output setpoints, respectively. See the Extended Help section in + [`setconstraint!`](@ref) documentation for more details. + The objective function follows this nomenclature: | VARIABLE | DESCRIPTION | SIZE | @@ -221,13 +234,20 @@ function LinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, transcription::ShootingMethod = 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, transcription, optim) + return LinMPC( + estim; + Hp, Hc, Mwt, Nwt, Lwt, Cwt, M_Hp, N_Hc, L_Hp, Wy, Wu, Wd, Wr, transcription, optim + ) end @@ -270,9 +290,13 @@ function LinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, transcription::ShootingMethod = DEFAULT_LINMPC_TRANSCRIPTION, - optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), + optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false) ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) nk = estimate_delays(estim.model) @@ -283,7 +307,7 @@ function LinMPC( nb = move_blocking(Hp, Hc) Hc = get_Hc(nb) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) - return LinMPC{NT}(estim, Hp, Hc, nb, weights, transcription, optim) + return LinMPC{NT}(estim, Hp, Hc, nb, weights, Wy, Wu, Wd, Wr, transcription, optim) end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 053a1095d..35d39e69a 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -34,6 +34,7 @@ struct NonLinMPC{ oracle::Bool Z̃::Vector{NT} ŷ::Vector{NT} + ry::Vector{NT} Hp::Int Hc::Int nϵ::Int @@ -69,6 +70,7 @@ struct NonLinMPC{ buffer::PredictiveControllerBuffer{NT} function NonLinMPC{NT}( estim::SE, Hp, Hc, nb, weights::CW, + Wy, Wu, Wd, Wr, JE::JEfunc, gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, gradient::GB, jacobian::JB, hessian::HB, oracle @@ -86,26 +88,27 @@ struct NonLinMPC{ GCfunc<:Function, } model = estim.model - nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ - ŷ = copy(model.yop) # dummy vals (updated just before optimization) + nu, ny, nd = model.nu, model.ny, model.nd + ŷ, ry = copy(model.yop), copy(model.yop) # dummy vals (updated just before optimization) # dummy vals (updated just before optimization): R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) lastu0 = zeros(NT, nu) + Wy, Wu, Wd, Wr = validate_custom_lincon(model, Wy, Wu, Wd, Wr) validate_transcription(model, transcription) PΔu = init_ZtoΔU(estim, transcription, Hp, Hc) Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat( model, estim, transcription, Hp, Hc, nb ) + F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb) - # dummy vals (updated just before optimization): - F, fx̂, Fŝ = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp) con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc( estim, weights, transcription, Hp, Hc, PΔu, Pu, E, - ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + ex̂, gx̂, jx̂, kx̂, vx̂, bx̂, + Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ, + Wy, Wu, Wd, Wr, gc!, nc ) warn_cond = iszero(weights.E) ? 1e6 : Inf # condition number warning only if Ewt==0 @@ -123,7 +126,7 @@ struct NonLinMPC{ mpc = new{NT, SE, CW, TM, JM, GB, JB, HB, PT, JEfunc, GCfunc}( estim, transcription, optim, con, gradient, jacobian, hessian, oracle, - Z̃, ŷ, + Z̃, ŷ, ry, Hp, Hc, nϵ, nb, weights, JE, p, @@ -207,13 +210,17 @@ This controller allocates memory at each time step for the optimization. - `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``. - `L_Hp=Diagonal(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. +- `Wy=nothing` : custom linear constraint matrix for output (see Extended Help). +- `Wu=nothing` : custom linear constraint matrix for manipulated input (see Extended Help). +- `Wd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). +- `Wr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Ewt=0.0` : economic costs weight ``E`` (scalar). - `JE=(_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p})``. -- `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom inequality constraint function +- `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``, mutating or not (details in Extended Help). -- `nc=0` : number of custom inequality constraints. +- `nc=0` : number of custom nonlinear 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 @@ -259,6 +266,9 @@ NonLinMPC controller with a sample time Ts = 10.0 s: `NonLinMPC` controllers based on [`LinModel`](@ref) compute the predictions with matrix algebra instead of a `for` loop. This feature can accelerate the optimization, especially for the constraint handling, and is not available in any other package, to my knowledge. + See [`setconstraint!`](@ref) for details about the custom linear inequality constraint + matrices `Wy`, `Wu`, `Wd` and `Wr`. The `Wy` keyword argument can be provided only if + `model` is a [`LinModel`](@ref)). The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the extended vectors ``\mathbf{U_e}`` (`nu*Hp+nu` elements), ``\mathbf{Ŷ_e}`` (`ny+ny*Hp` @@ -326,6 +336,10 @@ function NonLinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, JE ::Function = (_,_,_,_) -> 0.0, @@ -345,6 +359,7 @@ function NonLinMPC( return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, + Wy, Wu, Wd, Wr, transcription, optim, gradient, jacobian, hessian, oracle ) end @@ -393,6 +408,10 @@ function NonLinMPC( M_Hp = Diagonal(repeat(Mwt, Hp)), N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))), L_Hp = Diagonal(repeat(Lwt, Hp)), + Wy = nothing, + Wu = nothing, + Wd = nothing, + Wr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, JE ::Function = (_,_,_,_) -> 0.0, @@ -422,7 +441,7 @@ function NonLinMPC( weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) hessian = validate_hessian(hessian, gradient, oracle, DEFAULT_NONLINMPC_HESSIAN) return NonLinMPC{NT}( - estim, Hp, Hc, nb, weights, JE, gc!, nc, p, + estim, Hp, Hc, nb, weights, Wy, Wu, Wd, Wr, JE, gc!, nc, p, transcription, optim, gradient, jacobian, hessian, oracle ) end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a7b503bea..d18d4365d 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -156,8 +156,7 @@ in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section. 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) + - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` otherwise. The matrix is store as as `SparseMatrixCSC` to support both cases efficiently. """ function init_ZtoΔU end @@ -299,12 +298,12 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). ```math \begin{aligned} \mathbf{Q}(i, m, b) &= \begin{bmatrix} - \mathbf{Ĉ W}(i-b+0)\mathbf{B̂_u} \\ - \mathbf{Ĉ W}(i-b+1)\mathbf{B̂_u} \\ + \mathbf{Ĉ S}(i-b+0)\mathbf{B̂_u} \\ + \mathbf{Ĉ S}(i-b+1)\mathbf{B̂_u} \\ \vdots \\ - \mathbf{Ĉ W}(m-b-1)\mathbf{B̂_u} + \mathbf{Ĉ S}(m-b-1)\mathbf{B̂_u} \end{bmatrix} \\ - \mathbf{W}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ + \mathbf{S}(m) &= ∑_{ℓ=0}^m \mathbf{Â}^ℓ \end{aligned} ``` the prediction matrices are computed from the ``j_ℓ`` integers introduced in the @@ -333,23 +332,23 @@ each control period ``k``, see [`initpred!`](@ref) and [`linconstraint!`](@ref). \mathbf{Ĉ}\mathbf{Â}^{H_p} \end{bmatrix} \\ \mathbf{V} &= \mathbf{Q}(0, H_p, 0) \\ \mathbf{B} &= \begin{bmatrix} - \mathbf{Ĉ W}(0) \\ - \mathbf{Ĉ W}(1) \\ + \mathbf{Ĉ S}(0) \\ + \mathbf{Ĉ S}(1) \\ \vdots \\ - \mathbf{Ĉ W}(H_p-1) \end{bmatrix} \mathbf{\big(f̂_{op} - x̂_{op}\big)} + \mathbf{Ĉ S}(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-j_0-1)\mathbf{B̂_u} & \mathbf{W}(H_p-j_1-1)\mathbf{B̂_u} & \cdots & \mathbf{W}(H_p-j_{H_c-1}-1)\mathbf{B̂_u} \end{bmatrix} \\ + \mathbf{S}(H_p-j_0-1)\mathbf{B̂_u} & \mathbf{S}(H_p-j_1-1)\mathbf{B̂_u} & \cdots & \mathbf{S}(H_p-j_{H_c-1}-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)} + \mathbf{v_x̂} &= \mathbf{S}(H_p-1)\mathbf{B̂_u} \\ + \mathbf{b_x̂} &= \mathbf{S}(H_p-1)\mathbf{\big(f̂_{op} - x̂_{op}\big)} \end{aligned} ``` The complex structure of the ``\mathbf{E}`` and ``\mathbf{e_x̂}`` matrices is due to the @@ -373,12 +372,12 @@ function init_predmat( jℓ_data = [0; cumsum(nb)] # introduced in move_blocking docstring # four helper functions to improve code clarity and be similar to eqs. in docstring: getpower(array3D, power) = @views array3D[:,:, power+1] - W(m) = @views Âpow_csum[:,:, m+1] + S(m) = @views Âpow_csum[:,:, m+1] jℓ(ℓ) = jℓ_data[ℓ+1] function Q!(Q, i, m, b) for ℓ=0:m-i-1 iRows = (1:ny) .+ ny*ℓ - Q[iRows, :] = Ĉ * W(i-b+ℓ) * B̂u + Q[iRows, :] = Ĉ * S(i-b+ℓ) * B̂u end return Q end @@ -390,7 +389,7 @@ function init_predmat( K[iRow,:] = Ĉ*getpower(Âpow, j) end # --- previous manipulated inputs lastu0 --- - vx̂ = W(Hp-1)*B̂u + vx̂ = S(Hp-1)*B̂u V = Matrix{NT}(undef, Hp*ny, nu) Q!(V, 0, Hp, 0) # --- decision variables Z --- @@ -405,7 +404,7 @@ function init_predmat( Q = @views E[iRow, iCol] Q!(Q, i_Q, m_Q, b_Q) end - ex̂[:, iCol] = W(Hp - jℓ(j) - 1)*B̂u + ex̂[:, iCol] = S(Hp - jℓ(j) - 1)*B̂u end # --- current measured disturbances d0 and predictions D̂0 --- gx̂ = getpower(Âpow, Hp-1)*B̂d @@ -425,11 +424,11 @@ function init_predmat( end end # --- state x̂op and state update f̂op operating points --- - coef_bx̂ = W(Hp-1) + coef_bx̂ = S(Hp-1) coef_B = Matrix{NT}(undef, ny*Hp, nx̂) for j=1:Hp iRow = (1:ny) .+ ny*(j-1) - coef_B[iRow,:] = Ĉ*W(j-1) + coef_B[iRow,:] = Ĉ*S(j-1) end f̂op_n_x̂op = estim.f̂op - estim.x̂op bx̂ = coef_bx̂ * f̂op_n_x̂op @@ -674,7 +673,7 @@ end @doc raw""" init_matconstraint_mpc( model::LinModel, transcription::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, i_x̂min, i_x̂max, args... ) -> i_b, i_g, A, Aeq, neq @@ -694,23 +693,36 @@ The argument `nc` is the number of custom nonlinear inequality constraints in 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}}``. +`A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_Wmin, A_Wmax, 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}, ::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, 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] + ( + A_Umin, A_Umax, + A_ΔŨmin, A_ΔŨmax, + A_Ymin, A_Ymax, + A_Wmin, A_Wmax, + A_x̂min, A_x̂max, + A_ŝ + ) = args + A = [ + A_Umin; A_Umax; + A_ΔŨmin; A_ΔŨmax; + A_Ymin; A_Ymax; + A_Wmin; A_Wmax + 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_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Ymin; i_Ymax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = trues(nc) return i_b, i_g, A, Aeq, neq end @@ -718,18 +730,18 @@ end "Init `i_b` without output & terminal constraints if `NonLinModel` and `SingleShooting`." function init_matconstraint_mpc( ::NonLinModel{NT}, ::SingleShooting, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, 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_ŝ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, _ , _ , A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax] Aeq = A_ŝ neq = 0 end - i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax] + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax] i_g = [i_Ymin; i_Ymax; i_x̂min; i_x̂max; trues(nc)] return i_b, i_g, A, Aeq, neq end @@ -737,19 +749,19 @@ end "Init `i_b` without output constraints if `NonLinModel` and other `TranscriptionMethod`." function init_matconstraint_mpc( ::NonLinModel{NT}, ::TranscriptionMethod, nc::Int, - i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_x̂min, i_x̂max, + i_Umin, i_Umax, i_ΔŨmin, i_ΔŨmax, i_Ymin, i_Ymax, i_Wmin, i_Wmax, 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_x̂min, A_x̂max, A_ŝ = args - A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_x̂min; A_x̂max] + A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, _ , _ , A_Wmin, A_Wmax, A_x̂min, A_x̂max, A_ŝ = args + A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Wmin; A_Wmax; A_x̂min; A_x̂max] Aeq = A_ŝ nΔŨ, nZ̃ = size(A_ΔŨmin) neq = nZ̃ - nΔŨ end - i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_x̂min; i_x̂max] + i_b = [i_Umin; i_Umax; i_ΔŨmin; i_ΔŨmax; i_Wmin; i_Wmax; i_x̂min; i_x̂max] i_g = [i_Ymin; i_Ymax; trues(nc)] return i_b, i_g, A, Aeq, neq end @@ -761,10 +773,12 @@ Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ 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). +[`init_predmat`](@ref). The ``\mathbf{F_w}`` vector for the custom linear constraints is +also updated, see [`relaxW`](@ref). """ function linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) + nW = length(mpc.con.Wmin) nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ fx̂ .= mpc.con.bx̂ mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1) @@ -773,6 +787,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) end + linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -786,6 +801,10 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript n += nY mpc.con.b[(n+1):(n+nY)] .= @. +mpc.con.Y0max - mpc.F n += nY + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.Fw + n += nW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.Fw + n += nW mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min + fx̂ n += nx̂ mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max - fx̂ @@ -797,10 +816,12 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript end "Set `b` excluding predicted output constraints for `NonLinModel` and not `SingleShooting`." -function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod) - nU, nΔŨ, nY = length(mpc.con.U0min), length(mpc.con.ΔŨmin), length(mpc.con.Y0min) - nx̂, fx̂ = mpc.estim.nx̂, mpc.con.fx̂ +function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod) + nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) + nW = length(mpc.con.Wmin) + nx̂ = mpc.estim.nx̂ # here, updating fx̂ is not necessary since fx̂ = 0 + linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -810,6 +831,10 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::Transcriptio n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax n += nΔŨ + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.Fw + n += nW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.Fw + n += nW mpc.con.b[(n+1):(n+nx̂)] .= @. -mpc.con.x̂0min n += nx̂ mpc.con.b[(n+1):(n+nx̂)] .= @. +mpc.con.x̂0max @@ -820,8 +845,10 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::Transcriptio end "Also exclude terminal constraints for `NonLinModel` and `SingleShooting`." -function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting) +function linconstraint!(mpc::PredictiveController, model::NonLinModel, ::SingleShooting) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) + nW = length(mpc.con.Wmin) + linconstraint_custom!(mpc, model) n = 0 mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.Tu_lastu0 n += nU @@ -830,6 +857,10 @@ function linconstraint!(mpc::PredictiveController, ::NonLinModel, ::SingleShooti mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin n += nΔŨ mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax + n += nΔŨ + mpc.con.b[(n+1):(n+nW)] .= @. -mpc.con.Wmin + mpc.con.Fw + n += nW + mpc.con.b[(n+1):(n+nW)] .= @. +mpc.con.Wmax - mpc.con.Fw if any(mpc.con.i_b) lincon = mpc.optim[:linconstraint] @views JuMP.set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 897d06c9b..c6e0d2219 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -122,14 +122,16 @@ Also validate initial estimate covariance `P̂_0`, if provided. function validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing) nym = length(i_ym) nx̂ = model.nx + sum(nint_u) + sum(nint_ym) - size(Q̂) ≠ (nx̂, nx̂) && error("Q̂ size $(size(Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂))") + (any(<(0), nint_u) || any(<(0), nint_ym)) && return nothing # a clearer error is thrown later + size(Q̂) ≠ (nx̂, nx̂) && throw(DimensionMismatch("Q̂ size $(size(Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂))")) !ishermitian(Q̂) && error("Q̂ is not Hermitian") - size(R̂) ≠ (nym, nym) && error("R̂ size $(size(R̂)) ≠ nym, nym $((nym, nym))") + size(R̂) ≠ (nym, nym) && throw(DimensionMismatch("R̂ size $(size(R̂)) ≠ nym, nym $((nym, nym))")) !ishermitian(R̂) && error("R̂ is not Hermitian") if ~isnothing(P̂_0) - size(P̂_0) ≠ (nx̂, nx̂) && error("P̂_0 size $(size(P̂_0)) ≠ nx̂, nx̂ $((nx̂, nx̂))") + size(P̂_0) ≠ (nx̂, nx̂) && throw(DimensionMismatch("P̂_0 size $(size(P̂_0)) ≠ nx̂, nx̂ $((nx̂, nx̂))")) !ishermitian(P̂_0) && error("P̂_0 is not Hermitian") end + return nothing end @doc raw""" @@ -210,9 +212,10 @@ function init_integrators(nint::IntVectorOrInt, ny, varname::String) nint = fill(0, ny) end if length(nint) ≠ ny - error("nint_$(varname) length ($(length(nint))) ≠ n$(varname) ($ny)") + msg = "nint_$(varname) length ($(length(nint))) ≠ n$(varname) ($ny)" + throw(DimensionMismatch(msg)) end - any(nint .< 0) && error("nint_$(varname) values should be ≥ 0") + any(nint .< 0) && throw(ArgumentError("nint_$(varname) values should be ≥ 0")) nx = sum(nint) A, C = zeros(nx, nx), zeros(ny, nx) if nx ≠ 0 diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index eb93d4baa..a29a08d85 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -621,61 +621,61 @@ function setconstraint!( notSolvedYet = (JuMP.termination_status(optim) == JuMP.OPTIMIZE_NOT_CALLED) C = estim.C if isnothing(X̂min) && !isnothing(x̂min) - size(x̂min) == (nx̂,) || throw(ArgumentError("x̂min size must be $((nx̂,))")) + size(x̂min) == (nx̂,) || throw(DimensionMismatch("x̂min size must be $((nx̂,))")) con.x̃0min[end-nx̂+1:end] .= x̂min .- estim.x̂op # if C is finite : x̃ = [ε; x̂] for i in 1:nx̂*He con.X̂0min[i] = x̂min[(i-1) % nx̂ + 1] - estim.X̂op[i] end elseif !isnothing(X̂min) - size(X̂min) == (nX̂con,) || throw(ArgumentError("X̂min size must be $((nX̂con,))")) + size(X̂min) == (nX̂con,) || throw(DimensionMismatch("X̂min size must be $((nX̂con,))")) con.x̃0min[end-nx̂+1:end] .= X̂min[1:nx̂] .- estim.x̂op con.X̂0min .= @views X̂min[nx̂+1:end] .- estim.X̂op end if isnothing(X̂max) && !isnothing(x̂max) - size(x̂max) == (nx̂,) || throw(ArgumentError("x̂max size must be $((nx̂,))")) + size(x̂max) == (nx̂,) || throw(DimensionMismatch("x̂max size must be $((nx̂,))")) con.x̃0max[end-nx̂+1:end] .= x̂max .- estim.x̂op # if C is finite : x̃ = [ε; x̂] for i in 1:nx̂*He con.X̂0max[i] = x̂max[(i-1) % nx̂ + 1] - estim.X̂op[i] end elseif !isnothing(X̂max) - size(X̂max) == (nX̂con,) || throw(ArgumentError("X̂max size must be $((nX̂con,))")) + size(X̂max) == (nX̂con,) || throw(DimensionMismatch("X̂max size must be $((nX̂con,))")) con.x̃0max[end-nx̂+1:end] .= X̂max[1:nx̂] .- estim.x̂op con.X̂0max .= @views X̂max[nx̂+1:end] .- estim.X̂op end if isnothing(Ŵmin) && !isnothing(ŵmin) - size(ŵmin) == (nŵ,) || throw(ArgumentError("ŵmin size must be $((nŵ,))")) + size(ŵmin) == (nŵ,) || throw(DimensionMismatch("ŵmin size must be $((nŵ,))")) for i in 1:nŵ*He con.Ŵmin[i] = ŵmin[(i-1) % nŵ + 1] end elseif !isnothing(Ŵmin) - size(Ŵmin) == (nŵ*He,) || throw(ArgumentError("Ŵmin size must be $((nŵ*He,))")) + size(Ŵmin) == (nŵ*He,) || throw(DimensionMismatch("Ŵmin size must be $((nŵ*He,))")) con.Ŵmin .= Ŵmin end if isnothing(Ŵmax) && !isnothing(ŵmax) - size(ŵmax) == (nŵ,) || throw(ArgumentError("ŵmax size must be $((nŵ,))")) + size(ŵmax) == (nŵ,) || throw(DimensionMismatch("ŵmax size must be $((nŵ,))")) for i in 1:nŵ*He con.Ŵmax[i] = ŵmax[(i-1) % nŵ + 1] end elseif !isnothing(Ŵmax) - size(Ŵmax) == (nŵ*He,) || throw(ArgumentError("Ŵmax size must be $((nŵ*He,))")) + size(Ŵmax) == (nŵ*He,) || throw(DimensionMismatch("Ŵmax size must be $((nŵ*He,))")) con.Ŵmax .= Ŵmax end if isnothing(V̂min) && !isnothing(v̂min) - size(v̂min) == (nym,) || throw(ArgumentError("v̂min size must be $((nym,))")) + size(v̂min) == (nym,) || throw(DimensionMismatch("v̂min size must be $((nym,))")) for i in 1:nym*He con.V̂min[i] = v̂min[(i-1) % nym + 1] end elseif !isnothing(V̂min) - size(V̂min) == (nym*He,) || throw(ArgumentError("V̂min size must be $((nym*He,))")) + size(V̂min) == (nym*He,) || throw(DimensionMismatch("V̂min size must be $((nym*He,))")) con.V̂min .= V̂min end if isnothing(V̂max) && !isnothing(v̂max) - size(v̂max) == (nym,) || throw(ArgumentError("v̂max size must be $((nym,))")) + size(v̂max) == (nym,) || throw(DimensionMismatch("v̂max size must be $((nym,))")) for i in 1:nym*He con.V̂max[i] = v̂max[(i-1) % nym + 1] end elseif !isnothing(V̂max) - size(V̂max) == (nym*He,) || throw(ArgumentError("V̂max size must be $((nym*He,))")) + size(V̂max) == (nym*He,) || throw(DimensionMismatch("V̂max size must be $((nym*He,))")) con.V̂max .= V̂max end allECRs = ( @@ -694,7 +694,7 @@ function setconstraint!( isnothing(C_v̂min) && !isnothing(c_v̂min) && (C_v̂min = repeat(c_v̂min, He)) isnothing(C_v̂max) && !isnothing(c_v̂max) && (C_v̂max = repeat(c_v̂max, He)) if !isnothing(C_x̂min) - size(C_x̂min) == (nX̂con,) || throw(ArgumentError("C_x̂min size must be $((nX̂con,))")) + size(C_x̂min) == (nX̂con,) || throw(DimensionMismatch("C_x̂min size must be $((nX̂con,))")) any(C_x̂min .< 0) && error("C_x̂min weights should be non-negative") # if C is finite : x̃ = [ε; x̂] con.A_x̃min[end-nx̂+1:end, end] .= @views -C_x̂min[1:nx̂] @@ -702,7 +702,7 @@ function setconstraint!( size(con.A_X̂min, 1) ≠ 0 && (con.A_X̂min[:, end] = -con.C_x̂min) # for LinModel end if !isnothing(C_x̂max) - size(C_x̂max) == (nX̂con,) || throw(ArgumentError("C_x̂max size must be $((nX̂con,))")) + size(C_x̂max) == (nX̂con,) || throw(DimensionMismatch("C_x̂max size must be $((nX̂con,))")) any(C_x̂max .< 0) && error("C_x̂max weights should be non-negative") # if C is finite : x̃ = [ε; x̂] : con.A_x̃max[end-nx̂+1:end, end] .= @views -C_x̂max[1:nx̂] @@ -710,23 +710,23 @@ function setconstraint!( size(con.A_X̂max, 1) ≠ 0 && (con.A_X̂max[:, end] = -con.C_x̂max) # for LinModel end if !isnothing(C_ŵmin) - size(C_ŵmin) == (nŵ*He,) || throw(ArgumentError("C_ŵmin size must be $((nŵ*He,))")) + size(C_ŵmin) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmin size must be $((nŵ*He,))")) any(C_ŵmin .< 0) && error("C_ŵmin weights should be non-negative") con.A_Ŵmin[:, end] .= -C_ŵmin end if !isnothing(C_ŵmax) - size(C_ŵmax) == (nŵ*He,) || throw(ArgumentError("C_ŵmax size must be $((nŵ*He,))")) + size(C_ŵmax) == (nŵ*He,) || throw(DimensionMismatch("C_ŵmax size must be $((nŵ*He,))")) any(C_ŵmax .< 0) && error("C_ŵmax weights should be non-negative") con.A_Ŵmax[:, end] .= -C_ŵmax end if !isnothing(C_v̂min) - size(C_v̂min) == (nym*He,) || throw(ArgumentError("C_v̂min size must be $((nym*He,))")) + size(C_v̂min) == (nym*He,) || throw(DimensionMismatch("C_v̂min size must be $((nym*He,))")) any(C_v̂min .< 0) && error("C_v̂min weights should be non-negative") con.C_v̂min .= C_v̂min size(con.A_V̂min, 1) ≠ 0 && (con.A_V̂min[:, end] = -con.C_v̂min) # for LinModel end if !isnothing(C_v̂max) - size(C_v̂max) == (nym*He,) || throw(ArgumentError("C_v̂max size must be $((nym*He,))")) + size(C_v̂max) == (nym*He,) || throw(DimensionMismatch("C_v̂max size must be $((nym*He,))")) any(C_v̂max .< 0) && error("C_v̂max weights should be non-negative") con.C_v̂max .= C_v̂max size(con.A_V̂max, 1) ≠ 0 && (con.A_V̂max[:, end] = -con.C_v̂max) # for LinModel diff --git a/src/general.jl b/src/general.jl index 1e994bfe2..d2c67f759 100644 --- a/src/general.jl +++ b/src/general.jl @@ -192,6 +192,11 @@ function repeat!(Y::AbstractVector, a::AbstractVector, n::Int) return Y end +"Convert vectors to single column matrices when necessary." +to_mat(A::AbstractVector, _ ...) = reshape(A, length(A), 1) +to_mat(A::AbstractMatrix, _ ...) = A +to_mat(A::Real, dims...) = fill(A, dims) + "Convert 1-element vectors and normal matrices to Hermitians." to_hermitian(A::AbstractVector) = Hermitian(reshape(A, 1, 1), :L) to_hermitian(A::AbstractMatrix) = Hermitian(A, :L) diff --git a/src/sim_model.jl b/src/sim_model.jl index a7bfa7eb9..26b638b69 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -359,11 +359,6 @@ function validate_args(model::SimModel, d, u=nothing) end end -"Convert vectors to single column matrices when necessary." -to_mat(A::AbstractVector, _ ...) = reshape(A, length(A), 1) -to_mat(A::AbstractMatrix, _ ...) = A -to_mat(A::Real, dims...) = fill(A, dims) - include("model/linmodel.jl") include("model/linearization.jl") include("model/nonlinmodel.jl") diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 55a904d7d..2921b608f 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -49,10 +49,10 @@ @test skalmanfilter9.cov.Q̂ ≈ I(4) @test skalmanfilter9.cov.R̂ ≈ I(2) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=[1,1,1]) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=[-1,0]) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=0, σQ=[1]) - @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=0, σR=[1,1,1]) + @test_throws DimensionMismatch SteadyKalmanFilter(linmodel, nint_ym=[1,1,1]) + @test_throws ArgumentError SteadyKalmanFilter(linmodel, nint_ym=[-1,0]) + @test_throws DimensionMismatch SteadyKalmanFilter(linmodel, nint_ym=0, σQ=[1]) + @test_throws DimensionMismatch SteadyKalmanFilter(linmodel, nint_ym=0, σR=[1,1,1]) @test_throws ErrorException SteadyKalmanFilter(linmodel3, nint_ym=[1, 0, 0]) model_unobs = LinModel([1 0;0 1.5], [1; 0], [1 0], zeros(2,0), zeros(1,0), 1.0) @test_throws ErrorException SteadyKalmanFilter(model_unobs, nint_ym=[1]) @@ -188,7 +188,7 @@ end kalmanfilter8 = KalmanFilter(linmodel2) @test isa(kalmanfilter8, KalmanFilter{Float32}) - @test_throws ErrorException KalmanFilter(linmodel, nint_ym=0, σP_0=[1]) + @test_throws DimensionMismatch KalmanFilter(linmodel, nint_ym=0, σP_0=[1]) end @testitem "KalmanFilter estimator methods" setup=[SetupMPCtests] begin @@ -302,8 +302,8 @@ end lo6 = Luenberger(linmodel2) @test isa(lo6, Luenberger{Float32}) - @test_throws ErrorException Luenberger(linmodel, nint_ym=[1,1,1]) - @test_throws ErrorException Luenberger(linmodel, nint_ym=[-1,0]) + @test_throws DimensionMismatch Luenberger(linmodel, nint_ym=[1,1,1]) + @test_throws ArgumentError Luenberger(linmodel, nint_ym=[-1,0]) @test_throws ErrorException Luenberger(linmodel, poles=[0.5]) @test_throws ErrorException Luenberger(linmodel, poles=fill(1.5, lo1.nx̂)) @test_throws ErrorException Luenberger(LinModel(tf(1,[1, 0]),0.1), poles=[0.5,0.6]) @@ -815,14 +815,10 @@ end @test_throws ErrorException setmodel!(ekf2, deepcopy(nonlinmodel)) end -@testitem "MovingHorizonEstimator construction" setup=[SetupMPCtests] begin +@testitem "MovingHorizonEstimator construction (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - using JuMP, Ipopt, DifferentiationInterface import FiniteDiff linmodel = LinModel(sys,Ts,i_d=[3]) - f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d - h(x,d,model) = model.C*x + model.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) mhe1 = MovingHorizonEstimator(linmodel, He=5) @test mhe1.nym == 2 @@ -831,33 +827,26 @@ end @test mhe1.nx̂ == 6 @test size(mhe1.Ẽ, 2) == 6*mhe1.nx̂ - mhe2 = MovingHorizonEstimator(nonlinmodel, He=5) - @test mhe2.nym == 2 - @test mhe2.nyu == 0 - @test mhe2.nxs == 2 - @test mhe2.nx̂ == 6 - @test size(mhe1.Ẽ, 2) == 6*mhe1.nx̂ - - mhe3 = MovingHorizonEstimator(nonlinmodel, He=5, i_ym=[2]) + mhe3 = MovingHorizonEstimator(linmodel, He=5, i_ym=[2]) @test mhe3.nym == 1 @test mhe3.nyu == 1 @test mhe3.nxs == 1 @test mhe3.nx̂ == 5 - mhe4 = MovingHorizonEstimator(nonlinmodel, He=5, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) + mhe4 = MovingHorizonEstimator(linmodel, He=5, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) @test mhe4.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe4.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) - mhe5 = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=[2,2]) + mhe5 = MovingHorizonEstimator(linmodel, He=5, nint_ym=[2,2]) @test mhe5.nxs == 4 @test mhe5.nx̂ == 8 - mhe6 = MovingHorizonEstimator(nonlinmodel, He=5, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) + mhe6 = MovingHorizonEstimator(linmodel, He=5, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) @test mhe6.cov.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe6.P̂arr_old ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe6.cov.P̂_0 !== mhe6.P̂arr_old - mhe7 = MovingHorizonEstimator(nonlinmodel, He=10) + mhe7 = MovingHorizonEstimator(linmodel, He=10) @test mhe7.He == 10 @test length(mhe7.X̂0) == mhe7.He*6 @test length(mhe7.Y0m) == mhe7.He*2 @@ -865,12 +854,41 @@ end @test length(mhe7.D0) == (mhe7.He+mhe7.direct)*1 @test length(mhe7.Ŵ) == mhe7.He*6 - mhe8 = MovingHorizonEstimator(nonlinmodel, He=5, nint_u=[1, 1], nint_ym=[0, 0]) + mhe8 = MovingHorizonEstimator(linmodel, He=5, nint_u=[1, 1], nint_ym=[0, 0]) @test mhe8.nxs == 2 @test mhe8.nx̂ == 6 @test mhe8.nint_u == [1, 1] @test mhe8.nint_ym == [0, 0] + mhe12 = MovingHorizonEstimator(linmodel, He=5, Cwt=1e3) + @test size(mhe12.Ẽ, 2) == 6*mhe12.nx̂ + 1 + @test mhe12.C == 1e3 + + linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) + mhe13 = MovingHorizonEstimator(linmodel2, He=5) + @test isa(mhe13, MovingHorizonEstimator{Float32}) + + @test_throws ArgumentError MovingHorizonEstimator(linmodel) + @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0) + @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) +end + +@testitem "MovingHorizonEstimator construction (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + linmodel = LinModel(sys,Ts,i_d=[3]) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) + + mhe2 = MovingHorizonEstimator(nonlinmodel, He=5) + @test mhe2.nym == 2 + @test mhe2.nyu == 0 + @test mhe2.nxs == 2 + @test mhe2.nx̂ == 6 + @test size(mhe2.Ẽ, 2) == 6*mhe2.nx̂ + I_6 = Matrix{Float64}(I, 6, 6) I_2 = Matrix{Float64}(I, 2, 2) optim = Model(Ipopt.Optimizer) @@ -886,14 +904,6 @@ end ) @test solver_name(mhe10.optim) == "Ipopt" - mhe12 = MovingHorizonEstimator(nonlinmodel, He=5, Cwt=1e3) - @test size(mhe12.Ẽ, 2) == 6*mhe12.nx̂ + 1 - @test mhe12.C == 1e3 - - linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) - mhe13 = MovingHorizonEstimator(linmodel2, He=5) - @test isa(mhe13, MovingHorizonEstimator{Float32}) - mhe14 = MovingHorizonEstimator( nonlinmodel, He=5, gradient=AutoFiniteDiff(), @@ -904,9 +914,6 @@ end @test mhe14.jacobian == AutoFiniteDiff() @test mhe14.hessian == AutoFiniteDiff() - @test_throws ArgumentError MovingHorizonEstimator(linmodel) - @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0) - @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) @test_throws ErrorException MovingHorizonEstimator( nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Inf; optim, covestim = InternalModel(nonlinmodel) @@ -917,9 +924,79 @@ end ) end -@testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin +@testitem "MovingHorizonEstimator estimation and getinfo (LinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using JuMP, DAQP + linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) + linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) + + mhe2 = MovingHorizonEstimator(linmodel, He=2) + preparestate!(mhe2, [50, 30], [5]) + x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 + @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 + preparestate!(mhe2, [50, 30], [5]) + info = getinfo(mhe2) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + for i in 1:40 + preparestate!(mhe2, [50, 30], [5]) + updatestate!(mhe2, [11, 52], [50, 30], [5]) + end + preparestate!(mhe2, [50, 30], [5]) + @test mhe2([5]) ≈ [50, 30] atol=1e-3 + for i in 1:40 + preparestate!(mhe2, [51, 32], [5]) + updatestate!(mhe2, [10, 50], [51, 32], [5]) + end + preparestate!(mhe2, [51, 32], [5]) + @test mhe2([5]) ≈ [51, 32] atol=1e-3 + + mhe2 = MovingHorizonEstimator(linmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false) + preparestate!(mhe2, [50, 30], [5]) + x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 + @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 + info = getinfo(mhe2) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + for i in 1:40 + preparestate!(mhe2, [50, 30], [5]) + updatestate!(mhe2, [11, 52], [50, 30], [5]) + end + @test mhe2([5]) ≈ [50, 30] atol=1e-2 + for i in 1:40 + preparestate!(mhe2, [51, 32], [5]) + updatestate!(mhe2, [10, 50], [51, 32], [5]) + end + @test mhe2([5]) ≈ [51, 32] atol=1e-2 + + Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) + R̂ = diagm([1, 1].^2) + optim = Model(DAQP.Optimizer) + covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂) + P̂_0 = covestim.cov.P̂ + mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim) + preparestate!(mhe3, [50, 30], [5]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(4) atol=1e-9 + @test mhe3.x̂0 ≈ zeros(4) atol=1e-9 + preparestate!(mhe3, [50, 30], [5]) + info = getinfo(mhe3) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + + linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) + mhe3 = MovingHorizonEstimator(linmodel3, He=1) + preparestate!(mhe3, [0]) + x̂ = updatestate!(mhe3, [0], [0]) + @test x̂ ≈ [0, 0] atol=1e-3 + @test isa(x̂, Vector{Float32}) +end + +@testitem "MovingHorizonEstimator estimation and getinfo (NonLinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff - using JuMP, Ipopt, DAQP + using JuMP, Ipopt linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d @@ -927,6 +1004,7 @@ end nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) + mhe1 = MovingHorizonEstimator(nonlinmodel, He=2) JuMP.set_attribute(mhe1.optim, "tol", 1e-7) preparestate!(mhe1, [50, 30], [5]) @@ -991,69 +1069,6 @@ end end @test mhe1c([5]) ≈ [51, 32] atol=1e-3 - mhe2 = MovingHorizonEstimator(linmodel, He=2) - preparestate!(mhe2, [50, 30], [5]) - x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) - @test x̂ ≈ zeros(6) atol=1e-9 - @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 - preparestate!(mhe2, [50, 30], [5]) - info = getinfo(mhe2) - @test info[:x̂] ≈ x̂ atol=1e-9 - @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 - for i in 1:40 - preparestate!(mhe2, [50, 30], [5]) - updatestate!(mhe2, [11, 52], [50, 30], [5]) - end - preparestate!(mhe2, [50, 30], [5]) - @test mhe2([5]) ≈ [50, 30] atol=1e-3 - for i in 1:40 - preparestate!(mhe2, [51, 32], [5]) - updatestate!(mhe2, [10, 50], [51, 32], [5]) - end - preparestate!(mhe2, [51, 32], [5]) - @test mhe2([5]) ≈ [51, 32] atol=1e-3 - - mhe2 = MovingHorizonEstimator(linmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false) - preparestate!(mhe2, [50, 30], [5]) - x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) - @test x̂ ≈ zeros(6) atol=1e-9 - @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 - info = getinfo(mhe2) - @test info[:x̂] ≈ x̂ atol=1e-9 - @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 - for i in 1:40 - preparestate!(mhe2, [50, 30], [5]) - updatestate!(mhe2, [11, 52], [50, 30], [5]) - end - @test mhe2([5]) ≈ [50, 30] atol=1e-2 - for i in 1:40 - preparestate!(mhe2, [51, 32], [5]) - updatestate!(mhe2, [10, 50], [51, 32], [5]) - end - @test mhe2([5]) ≈ [51, 32] atol=1e-2 - - Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) - R̂ = diagm([1, 1].^2) - optim = Model(DAQP.Optimizer) - covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂) - P̂_0 = covestim.cov.P̂ - mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim) - preparestate!(mhe3, [50, 30], [5]) - x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5]) - @test x̂ ≈ zeros(4) atol=1e-9 - @test mhe3.x̂0 ≈ zeros(4) atol=1e-9 - preparestate!(mhe3, [50, 30], [5]) - info = getinfo(mhe3) - @test info[:x̂] ≈ x̂ atol=1e-9 - @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 - - linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) - mhe3 = MovingHorizonEstimator(linmodel3, He=1) - preparestate!(mhe3, [0]) - x̂ = updatestate!(mhe3, [0], [0]) - @test x̂ ≈ [0, 0] atol=1e-3 - @test isa(x̂, Vector{Float32}) - Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) R̂ = diagm([1, 1].^2) optim = Model(Ipopt.Optimizer) @@ -1085,6 +1100,7 @@ end @test x̂ ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) @test_throws ErrorException setstate!(mhe1, [1,2,3,4,5,6], diagm(.1:.1:.6)) + end @testitem "MovingHorizonEstimator estimation with unfilled window" setup=[SetupMPCtests] begin @@ -1163,35 +1179,51 @@ end linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe1 = MovingHorizonEstimator(linmodel, He=1, nint_ym=0, Cwt=1e3) setconstraint!(mhe1, x̂min=[-51,-52], x̂max=[53,54]) - @test all((mhe1.con.X̂0min, mhe1.con.X̂0max) .≈ ([-51,-52], [53,54])) - @test all((mhe1.con.x̃0min[2:end], mhe1.con.x̃0max[2:end]) .≈ ([-51,-52], [53,54])) + @test mhe1.con.X̂0min ≈ [-51,-52] + @test mhe1.con.X̂0max ≈ [53,54] + @test mhe1.con.x̃0min[2:end] ≈ [-51,-52] + @test mhe1.con.x̃0max[2:end] ≈ [53,54] setconstraint!(mhe1, ŵmin=[-55,-56], ŵmax=[57,58]) - @test all((mhe1.con.Ŵmin, mhe1.con.Ŵmax) .≈ ([-55,-56], [57,58])) + @test mhe1.con.Ŵmin ≈ [-55,-56] + @test mhe1.con.Ŵmax ≈ [57,58] setconstraint!(mhe1, v̂min=[-59,-60], v̂max=[61,62]) - @test all((mhe1.con.V̂min, mhe1.con.V̂max) .≈ ([-59,-60], [61,62])) + @test mhe1.con.V̂min ≈ [-59,-60] + @test mhe1.con.V̂max ≈ [61,62] setconstraint!(mhe1, c_x̂min=[0.01,0.02], c_x̂max=[0.03,0.04]) - @test all((-mhe1.con.A_X̂min[:, end], -mhe1.con.A_X̂max[:, end]) .≈ ([0.01, 0.02], [0.03,0.04])) - @test all((-mhe1.con.A_x̃min[2:end, end], -mhe1.con.A_x̃max[2:end, end]) .≈ ([0.01,0.02], [0.03,0.04])) + @test -mhe1.con.A_X̂min[:, end] ≈ [0.01, 0.02] + @test -mhe1.con.A_X̂max[:, end] ≈ [0.03,0.04] + @test -mhe1.con.A_x̃min[2:end, end] ≈ [0.01,0.02] + @test -mhe1.con.A_x̃max[2:end, end] ≈ [0.03,0.04] setconstraint!(mhe1, c_ŵmin=[0.05,0.06], c_ŵmax=[0.07,0.08]) - @test all((-mhe1.con.A_Ŵmin[:, end], -mhe1.con.A_Ŵmax[:, end]) .≈ ([0.05, 0.06], [0.07,0.08])) + @test -mhe1.con.A_Ŵmin[:, end] ≈ [0.05, 0.06] + @test -mhe1.con.A_Ŵmax[:, end] ≈ [0.07,0.08] setconstraint!(mhe1, c_v̂min=[0.09,0.10], c_v̂max=[0.11,0.12]) - @test all((-mhe1.con.A_V̂min[:, end], -mhe1.con.A_V̂max[:, end]) .≈ ([0.09, 0.10], [0.11,0.12])) + @test -mhe1.con.A_V̂min[:, end] ≈ [0.09, 0.10] + @test -mhe1.con.A_V̂max[:, end] ≈ [0.11,0.12] mhe2 = MovingHorizonEstimator(linmodel, He=4, nint_ym=0, Cwt=1e3) setconstraint!(mhe2, X̂min=-1(1:10), X̂max=1(1:10)) - @test all((mhe2.con.X̂0min, mhe2.con.X̂0max) .≈ (-1(3:10), 1(3:10))) - @test all((mhe2.con.x̃0min[2:end], mhe2.con.x̃0max[2:end]) .≈ (-1(1:2), 1(1:2))) + @test mhe2.con.X̂0min ≈ -1(3:10) + @test mhe2.con.X̂0max ≈ 1(3:10) + @test mhe2.con.x̃0min[2:end] ≈ -1(1:2) + @test mhe2.con.x̃0max[2:end] ≈ 1(1:2) setconstraint!(mhe2, Ŵmin=-1(11:18), Ŵmax=1(11:18)) - @test all((mhe2.con.Ŵmin, mhe2.con.Ŵmax) .≈ (-1(11:18), 1(11:18))) + @test mhe2.con.Ŵmin ≈ -1(11:18) + @test mhe2.con.Ŵmax ≈ 1(11:18) setconstraint!(mhe2, V̂min=-1(31:38), V̂max=1(31:38)) - @test all((mhe2.con.V̂min, mhe2.con.V̂max) .≈ (-1(31:38), 1(31:38))) + @test mhe2.con.V̂min ≈ -1(31:38) + @test mhe2.con.V̂max ≈ 1(31:38) setconstraint!(mhe2, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test all((-mhe2.con.A_X̂min[:, end], -mhe2.con.A_X̂max[:, end]) .≈ (0.01(3:10), 0.02(3:10))) - @test all((-mhe2.con.A_x̃min[2:end, end], -mhe2.con.A_x̃max[2:end, end]) .≈ (0.01(1:2), 0.02(1:2))) + @test -mhe2.con.A_X̂min[:, end] ≈ 0.01(3:10) + @test -mhe2.con.A_X̂max[:, end] ≈ 0.02(3:10) + @test -mhe2.con.A_x̃min[2:end, end] ≈ 0.01(1:2) + @test -mhe2.con.A_x̃max[2:end, end] ≈ 0.02(1:2) setconstraint!(mhe2, C_ŵmin=0.03(11:18), C_ŵmax=0.04(11:18)) - @test all((-mhe2.con.A_Ŵmin[:, end], -mhe2.con.A_Ŵmax[:, end]) .≈ (0.03(11:18), 0.04(11:18))) + @test -mhe2.con.A_Ŵmin[:, end] ≈ 0.03(11:18) + @test -mhe2.con.A_Ŵmax[:, end] ≈ 0.04(11:18) setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38)) - @test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38))) + @test -mhe2.con.A_V̂min[:, end] ≈ 0.05(31:38) + @test -mhe2.con.A_V̂max[:, end] ≈ 0.06(31:38) f(x,u,d,model) = model.A*x + model.Bu*u h(x,d,model) = model.C*x @@ -1200,29 +1232,33 @@ end mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3) setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test all((mhe3.con.C_x̂min, mhe3.con.C_x̂max) .≈ (0.01(3:10), 0.02(3:10))) + @test mhe3.con.C_x̂min ≈ 0.01(3:10) + @test mhe3.con.C_x̂max ≈ 0.02(3:10) setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18)) - @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18))) + @test mhe3.con.C_v̂min ≈ 0.03(11:18) + @test mhe3.con.C_v̂max ≈ 0.04(11:18) # TODO: delete these tests when the deprecated legacy splatting syntax will be. mhe4 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3, oracle=false) setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) - @test all((mhe3.con.C_x̂min, mhe3.con.C_x̂max) .≈ (0.01(3:10), 0.02(3:10))) + @test mhe3.con.C_x̂min ≈ 0.01(3:10) + @test mhe3.con.C_x̂max ≈ 0.02(3:10) setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18)) - @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18))) - - @test_throws ArgumentError setconstraint!(mhe2, x̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, x̂max=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, ŵmin=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, ŵmax=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, v̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, v̂max=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, c_x̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, c_x̂max=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, c_ŵmin=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, c_ŵmax=[+1]) - @test_throws ArgumentError setconstraint!(mhe2, c_v̂min=[-1]) - @test_throws ArgumentError setconstraint!(mhe2, c_v̂max=[+1]) + @test mhe3.con.C_v̂min ≈ 0.03(11:18) + @test mhe3.con.C_v̂max ≈ 0.04(11:18) + + @test_throws DimensionMismatch setconstraint!(mhe2, x̂min=[-1]) + @test_throws DimensionMismatch setconstraint!(mhe2, x̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, ŵmin=[-1]) + @test_throws DimensionMismatch setconstraint!(mhe2, ŵmax=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, v̂min=[-1]) + @test_throws DimensionMismatch setconstraint!(mhe2, v̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_x̂min=[+2]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_x̂max=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_ŵmin=[+2]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_ŵmax=[+1]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_v̂min=[+2]) + @test_throws DimensionMismatch setconstraint!(mhe2, c_v̂max=[+1]) preparestate!(mhe1, [50, 30]) updatestate!(mhe1, [10, 50], [50, 30]) @@ -1583,8 +1619,8 @@ end @testitem "ManualEstimator construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d - h(x,d,model) = model.C*x + model.Du*d + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) manual1 = ManualEstimator(linmodel) @@ -1631,8 +1667,8 @@ end @testitem "ManualEstimator estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d - h(x,d,model) = model.C*x + model.Du*d + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, 0, solver=nothing, p=linmodel) manual1 = ManualEstimator(linmodel) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 0568a8f02..e8f413966 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -6,9 +6,9 @@ @test size(mpc1.Ẽ,1) == 15*mpc1.estim.model.ny mpc2 = LinMPC(model, Hc=4, Cwt=Inf) @test size(mpc2.Ẽ,2) == 4*mpc2.estim.model.nu - mpc3 = LinMPC(model, Hc=4, Cwt=1e6) + mpc3 = LinMPC(model, Hc=4, Cwt=1e4) @test size(mpc3.Ẽ,2) == 4*mpc3.estim.model.nu + 1 - @test mpc3.weights.Ñ_Hc[end] ≈ 1e6 + @test mpc3.weights.Ñ_Hc[end] ≈ 1e4 mpc4 = LinMPC(model, Mwt=[1,2], Hp=15) @test mpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) @test mpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} @@ -53,6 +53,21 @@ mpc16 = LinMPC(model, Hc=[1,2,3,6,6,6], Hp=10, Cwt=Inf) @test mpc16.Hc == 4 # the last 2 elements of Hc are ignored @test size(mpc16.P̃u) == (10*mpc1.estim.model.nu, 4*mpc1.estim.model.nu) + Wy = ones(3, model.ny) + mpc17 = LinMPC(model; Wy) + @test mpc17.con.W̄y ≈ ModelPredictiveControl.repeatdiag(Wy, mpc17.Hp+1) + @test mpc17.con.W̄u ≈ zeros(3*(mpc17.Hp+1), model.nu*(mpc17.Hp+1)) + @test mpc17.con.W̄d ≈ zeros(3*(mpc17.Hp+1), model.nd*(mpc17.Hp+1)) + @test mpc17.con.W̄r ≈ zeros(3*(mpc17.Hp+1), model.ny*(mpc17.Hp+1)) + Wy = ones(2, model.ny) + Wu = 2*ones(2, model.nu) + Wd = 3*ones(2, model.nd) + Wr = 0.5*ones(2, model.ny) + mpc18 = LinMPC(model; Wy, Wu, Wd, Wr) + @test mpc18.con.W̄y ≈ ModelPredictiveControl.repeatdiag(Wy, mpc18.Hp+1) + @test mpc18.con.W̄u ≈ ModelPredictiveControl.repeatdiag(Wu, mpc18.Hp+1) + @test mpc18.con.W̄d ≈ ModelPredictiveControl.repeatdiag(Wd, mpc18.Hp+1) + @test mpc18.con.W̄r ≈ ModelPredictiveControl.repeatdiag(Wr, mpc18.Hp+1) @test_logs( (:warn, @@ -62,14 +77,19 @@ ) @test_throws ArgumentError LinMPC(model, Hc=0) @test_throws ArgumentError LinMPC(model, Hp=1, Hc=2) - @test_throws ArgumentError LinMPC(model, Mwt=[1]) - @test_throws ArgumentError LinMPC(model, Mwt=[1]) - @test_throws ArgumentError LinMPC(model, Lwt=[1]) - @test_throws ArgumentError LinMPC(model, Cwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Mwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Mwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Lwt=[1]) + @test_throws DimensionMismatch LinMPC(model, Cwt=[1]) @test_throws ArgumentError LinMPC(model, Mwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Nwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Lwt=[-1,1]) @test_throws ArgumentError LinMPC(model, Cwt=-1) + @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, model.ny+1)) + @test_throws DimensionMismatch LinMPC(model, Wu=ones(2, model.nu-1)) + @test_throws DimensionMismatch LinMPC(model, Wd=ones(2, model.nd+1)) + @test_throws DimensionMismatch LinMPC(model, Wr=ones(2, model.ny-1)) + @test_throws DimensionMismatch LinMPC(model, Wy=ones(2, model.ny), Wu=ones(3, model.nu)) end @testitem "LinMPC moves and getinfo" setup=[SetupMPCtests] begin @@ -242,67 +262,121 @@ end @testitem "LinMPC set constraints" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra model = LinModel(sys, Ts, i_d=[3]) - mpc = LinMPC(model, Hp=1, Hc=1) + mpc = LinMPC(model, Hp=1, Hc=1, Wr=ones(2, 2)) # test default constraints before modifying any: - @test all((mpc.con.U0min, mpc.con.U0max) .≈ (fill(-Inf, model.nu), fill(Inf, model.nu))) - @test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ (vcat(fill(-Inf, model.nu), 0), vcat(fill(Inf, model.nu), Inf))) - @test all((mpc.con.Y0min, mpc.con.Y0max) .≈ (fill(-Inf, model.ny), fill(Inf, model.ny))) - @test all((mpc.con.x̂0min, mpc.con.x̂0max) .≈ (fill(-Inf, mpc.estim.nx̂), fill(Inf, mpc.estim.nx̂))) - @test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ (fill(0.0, model.nu), fill(0.0, model.nu))) - @test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ (fill(0.0, model.nu), fill(0.0, model.nu))) - @test all((-mpc.con.A_Ymin[:, end], -mpc.con.A_Ymax[:, end]) .≈ (fill(1.0, model.ny), fill(1.0, model.ny))) - @test all((-mpc.con.A_x̂min[:, end], -mpc.con.A_x̂max[:, end]) .≈ (fill(1.0, mpc.estim.nx̂), fill(1.0, mpc.estim.nx̂))) + @test mpc.con.U0min ≈ fill(-Inf, model.nu) + @test mpc.con.U0max ≈ fill(Inf, model.nu) + @test mpc.con.ΔŨmin ≈ vcat(fill(-Inf, model.nu), 0) + @test mpc.con.ΔŨmax ≈ vcat(fill(Inf, model.nu), Inf) + @test mpc.con.Y0min ≈ fill(-Inf, model.ny) + @test mpc.con.Y0max ≈ fill(Inf, model.ny) + @test mpc.con.Wmin ≈ fill(-Inf, 2mpc.con.nw) + @test mpc.con.Wmax ≈ fill(Inf, 2mpc.con.nw) + @test mpc.con.x̂0min ≈ fill(-Inf, mpc.estim.nx̂) + @test mpc.con.x̂0max ≈ fill(Inf, mpc.estim.nx̂) + @test -mpc.con.A_Umin[:, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_Umax[:, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_ΔŨmin[1:end-1, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_ΔŨmax[1:end-1, end] ≈ fill(0.0, model.nu) + @test -mpc.con.A_Ymin[:, end] ≈ fill(1.0, model.ny) + @test -mpc.con.A_Ymax[:, end] ≈ fill(1.0, model.ny) + @test -mpc.con.A_Wmin[:, end] ≈ fill(1.0, 2mpc.con.nw) + @test -mpc.con.A_Wmax[:, end] ≈ fill(1.0, 2mpc.con.nw) + @test -mpc.con.A_x̂min[:, end] ≈ fill(1.0, mpc.estim.nx̂) + @test -mpc.con.A_x̂max[:, end] ≈ fill(1.0, mpc.estim.nx̂) setconstraint!(mpc, umin=[-5, -9.9], umax=[100,99]) - @test all((mpc.con.U0min, mpc.con.U0max) .≈ ([-5, -9.9], [100,99])) + @test mpc.con.U0min ≈ [-5, -9.9] + @test mpc.con.U0max ≈ [100,99] setconstraint!(mpc, Δumin=[-5,-10], Δumax=[6,11]) - @test all((mpc.con.ΔŨmin, mpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + @test mpc.con.ΔŨmin ≈ [-5,-10,0] + @test mpc.con.ΔŨmax ≈ [6,11,Inf] setconstraint!(mpc, ymin=[-6, -11],ymax=[55, 35]) - @test all((mpc.con.Y0min, mpc.con.Y0max) .≈ ([-6,-11], [55,35])) + @test mpc.con.Y0min ≈ [-6,-11] + @test mpc.con.Y0max ≈ [55,35] + setconstraint!(mpc, wmin=[-7, -12], wmax=[75, 65]) + @test mpc.con.Wmin ≈ [-7, -12, -7, -12] + @test mpc.con.Wmax ≈ [75, 65, 75, 65] setconstraint!(mpc, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((mpc.con.x̂0min, mpc.con.x̂0max) .≈ ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test mpc.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test mpc.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(mpc, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) - @test all((-mpc.con.A_Umin[:, end], -mpc.con.A_Umax[:, end]) .≈ ([0.01,0.02], [0.03,0.04])) + @test -mpc.con.A_Umin[:, end] ≈ [0.01,0.02] + @test -mpc.con.A_Umax[:, end] ≈ [0.03,0.04] setconstraint!(mpc, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) - @test all((-mpc.con.A_ΔŨmin[1:end-1, end], -mpc.con.A_ΔŨmax[1:end-1, end]) .≈ ([0.05,0.06], [0.07,0.08])) + @test -mpc.con.A_ΔŨmin[1:end-1, end] ≈ [0.05,0.06] + @test -mpc.con.A_ΔŨmax[1:end-1, end] ≈ [0.07,0.08] setconstraint!(mpc, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-mpc.con.A_Ymin[:, end], -mpc.con.A_Ymax[:, end]) .≈ ([1.00,1.01], [1.02,1.03])) + @test -mpc.con.A_Ymin[:, end] ≈ [1.00,1.01] + @test -mpc.con.A_Ymax[:, end] ≈ [1.02,1.03] + setconstraint!(mpc, c_wmin=[2.00,2.01], c_wmax=[2.02,2.03]) + @test -mpc.con.A_Wmin[:, end] ≈ [2.00,2.01,2.00,2.01] + @test -mpc.con.A_Wmax[:, end] ≈ [2.02,2.03,2.02,2.03] setconstraint!(mpc, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36]) - @test all((-mpc.con.A_x̂min[:, end], -mpc.con.A_x̂max[:, end]) .≈ ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test -mpc.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -mpc.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] model2 = LinModel(tf([2], [10, 1]), 3.0) - mpc2 = LinMPC(model2, Hp=50, Hc=5) + mpc2 = LinMPC(model2, Hp=50, Hc=5, Wr=[1]) setconstraint!(mpc2, Umin=-1(1:50).-1, Umax=+1(1:50).+1) - @test all((mpc2.con.U0min, mpc2.con.U0max) .≈ (-1(1:50).-1, +1(1:50).+1)) + @test mpc2.con.U0min ≈ -1(1:50).-1 + @test mpc2.con.U0max ≈ +1(1:50).+1 setconstraint!(mpc2, ΔUmin=-1(1:5).-2, ΔUmax=+1(1:5).+2) - @test all((mpc2.con.ΔŨmin, mpc2.con.ΔŨmax) .≈ ([-1(1:5).-2; 0], [+1(1:5).+2; Inf])) + @test mpc2.con.ΔŨmin ≈ [-1(1:5).-2; 0] + @test mpc2.con.ΔŨmax ≈ [+1(1:5).+2; Inf] setconstraint!(mpc2, Ymin=-1(1:50).-3, Ymax=+1(1:50).+3) - @test all((mpc2.con.Y0min, mpc2.con.Y0max) .≈ (-1(1:50).-3, +1(1:50).+3)) - - setconstraint!(mpc2, C_umin=+1(1:50).+4, C_umax=+1(1:50).+4) - @test all((-mpc2.con.A_Umin[:, end], -mpc2.con.A_Umax[:, end]) .≈ (+1(1:50).+4, +1(1:50).+4)) - setconstraint!(mpc2, C_Δumin=+1(1:5).+5, C_Δumax=+1(1:5).+5) - @test all((-mpc2.con.A_ΔŨmin[1:end-1, end], -mpc2.con.A_ΔŨmax[1:end-1, end]) .≈ (+1(1:5).+5, +1(1:5).+5)) - setconstraint!(mpc2, C_ymin=+1(1:50).+6, C_ymax=+1(1:50).+6) - @test all((-mpc2.con.A_Ymin[:, end], -mpc2.con.A_Ymax[:, end]) .≈ (+1(1:50).+6, +1(1:50).+6)) - setconstraint!(mpc2, c_umin=[0], c_umax=[0], c_Δumin=[0], c_Δumax=[0], c_ymin=[1], c_ymax=[1]) - - @test_throws ArgumentError setconstraint!(mpc, umin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, umax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, Δumin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, Δumax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, ymin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, ymax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_umin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_umax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_Δumin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_Δumax=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_ymin=[0,0,0]) - @test_throws ArgumentError setconstraint!(mpc, c_ymax=[0,0,0]) + @test mpc2.con.Y0min ≈ -1(1:50).-3 + @test mpc2.con.Y0max ≈ +1(1:50).+3 + setconstraint!(mpc2, Wmin=-1(1:51).-4, Wmax=+1(1:51).+4) + @test mpc2.con.Wmin ≈ -1(1:51).-4 + @test mpc2.con.Wmax ≈ +1(1:51).+4 + + setconstraint!(mpc2, C_umin=+1(1:50).+5, C_umax=+1(1:50).+5) + @test -mpc2.con.A_Umin[:, end] ≈ +1(1:50).+5 + @test -mpc2.con.A_Umax[:, end] ≈ +1(1:50).+5 + setconstraint!(mpc2, C_Δumin=+1(1:5).+6, C_Δumax=+1(1:5).+6) + @test -mpc2.con.A_ΔŨmin[1:end-1, end] ≈ +1(1:5).+6 + @test -mpc2.con.A_ΔŨmax[1:end-1, end] ≈ +1(1:5).+6 + setconstraint!(mpc2, C_ymin=+1(1:50).+7, C_ymax=+1(1:50).+7) + @test -mpc2.con.A_Ymin[:, end] ≈ +1(1:50).+7 + @test -mpc2.con.A_Ymax[:, end] ≈ +1(1:50).+7 + setconstraint!(mpc2, C_wmin=+1(1:51).+8, C_wmax=+1(1:51).+8) + @test -mpc2.con.A_Wmin[:, end] ≈ +1(1:51).+8 + @test -mpc2.con.A_Wmax[:, end] ≈ +1(1:51).+8 + + setconstraint!(mpc2, + c_umin=[0], c_umax=[0], c_Δumin=[0], c_Δumax=[0], + c_ymin=[1], c_ymax=[1], c_wmin=[1], c_wmax=[1] + ) + @test_throws DimensionMismatch setconstraint!(mpc, umin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, umax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, Δumin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, Δumax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, ymin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, ymax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, wmin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, wmax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_umin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_umax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_Δumin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_Δumax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_ymin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_ymax=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_wmin=[0,0,0]) + @test_throws DimensionMismatch setconstraint!(mpc, c_wmax=[0,0,0]) + @test_throws ErrorException setconstraint!(mpc, c_umin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_umax=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_Δumin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_Δumax=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_ymin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_ymax=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_wmin=[-1,-1]) + @test_throws ErrorException setconstraint!(mpc, c_wmax=[-1,-1]) + preparestate!(mpc, mpc.estim.model.yop, mpc.estim.model.dop) moveinput!(mpc, [0, 0], [0]) @test_throws ErrorException setconstraint!(mpc, c_umin=[1, 1], c_umax=[1, 1]) @@ -374,6 +448,37 @@ end info = getinfo(mpc) @test info[:x̂end][1] ≈ 0 atol=1e-1 setconstraint!(mpc, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) + + model2 = LinModel([tf([2], [10, 1]) tf(0.1, [7, 1])], 3.0, i_d=[2]) + model2 = setop!(model2, uop=[25], dop=[30], yop=[50]) + mpc_wy = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wy=[1]) + mpc_wy = setconstraint!(mpc_wy, wmin=[36], wmax=[75]) + preparestate!(mpc_wy, [50], [30]) + u = moveinput!(mpc_wy, [0], [30]) + @test all(isapprox.(getinfo(mpc_wy)[:Ŷ], 36; atol=1e-1)) + u = moveinput!(mpc_wy, [100], [30]) + @test all(isapprox.(getinfo(mpc_wy)[:Ŷ], 75; atol=1e-1)) + mpc_wu = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wu=[1]) + mpc_wu = setconstraint!(mpc_wu, wmin=[4], wmax=[20]) + preparestate!(mpc_wu, [50], [30]) + u = moveinput!(mpc_wu, [0], [30]) + @test all(isapprox.(getinfo(mpc_wu)[:U], 4; atol=1e-1)) + u = moveinput!(mpc_wu, [100], [30]) + @test all(isapprox.(getinfo(mpc_wu)[:U], 20; atol=1e-1)) + mpc_wd = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wd=[1], Wy=[1]) + mpc_wd = setconstraint!(mpc_wd, wmin=[56], wmax=[95]) + preparestate!(mpc_wd, [50], [30]) + u = moveinput!(mpc_wd, [0], [30]) + @test all(isapprox.(getinfo(mpc_wd)[:Ŷ], 56-30; atol=1e-1)) + u = moveinput!(mpc_wd, [100], [30]) + @test all(isapprox.(getinfo(mpc_wd)[:Ŷ], 95-30; atol=1e-1)) + mpc_wr = LinMPC(model2, Nwt=[0], Cwt=Inf, Hp=50, Hc=50, Wr=[1], Wy=[1]) + mpc_wr = setconstraint!(mpc_wr, wmin=[52], wmax=[175]) + preparestate!(mpc_wr, [50], [30]) + u = moveinput!(mpc_wr, [21], [30]) + @test all(isapprox.(getinfo(mpc_wr)[:Ŷ], 52-21; atol=1e-1)) + u = moveinput!(mpc_wr, [100], [30]) + @test all(isapprox.(getinfo(mpc_wr)[:Ŷ], 175-100; atol=1e-1)) end @testitem "LinMPC terminal cost" setup=[SetupMPCtests] begin @@ -660,62 +765,82 @@ end @test mpc.weights.L_Hp ≈ diagm(1.1:1000.1) end -@testitem "NonLinMPC construction" setup=[SetupMPCtests] begin +@testitem "NonLinMPC construction (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra - using JuMP, Ipopt, DifferentiationInterface - import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) nmpc0 = NonLinMPC(linmodel1, Hp=15) @test isa(nmpc0.estim, SteadyKalmanFilter) - f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d - h = (x,d,model) -> model.C*x + model.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing) - nmpc1 = NonLinMPC(nonlinmodel, Hp=15) - @test isa(nmpc1.estim, UnscentedKalmanFilter) - @test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny - nmpc2 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=Inf) - @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu - nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=1e6) - @test size(nmpc3.Ẽ, 2) == 4*nonlinmodel.nu + 1 - @test nmpc3.weights.Ñ_Hc[end] == 1e6 - nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[1,2]) + nmpc3 = NonLinMPC(linmodel1, Hp=15, Hc=4, Cwt=1e4) + @test size(nmpc3.Ẽ, 2) == 4*linmodel1.nu + 1 + @test nmpc3.weights.Ñ_Hc[end] == 1e4 + nmpc4 = NonLinMPC(linmodel1, Hp=15, Mwt=[1,2]) @test nmpc4.weights.M_Hp ≈ Diagonal(diagm(repeat(Float64[1, 2], 15))) @test nmpc4.weights.M_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc5 = NonLinMPC(nonlinmodel, Hp=15 ,Nwt=[3,4], Cwt=1e3, Hc=5) + nmpc5 = NonLinMPC(linmodel1, Hp=15 ,Nwt=[3,4], Cwt=1e3, Hc=5) @test nmpc5.weights.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]])) @test nmpc5.weights.Ñ_Hc isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1]) + nmpc6 = NonLinMPC(linmodel1, Hp=15, Lwt=[0,1]) @test nmpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) @test nmpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) + nmpc7 = NonLinMPC(linmodel1, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) @test nmpc7.weights.E == 1e-3 @test nmpc7.JE([1,2],[3,4],[4,6],10) == 10*dot([1,2],[3,4])+sum([4,6]) - optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) - nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim) - @test solver_name(nmpc8.optim) == "Ipopt" - @test get_attribute(nmpc8.optim, "nlp_scaling_max_gradient") == 1.0 - im = InternalModel(nonlinmodel) - nmpc9 = NonLinMPC(im, Hp=15) - @test isa(nmpc9.estim, InternalModel) nmpc10 = NonLinMPC(linmodel1, nint_u=[1, 1], nint_ym=[0, 0]) @test nmpc10.estim.nint_u == [1, 1] @test nmpc10.estim.nint_ym == [0, 0] - nmpc11 = NonLinMPC(nonlinmodel, Hp=15, nint_u=[1, 1], nint_ym=[0, 0]) - @test nmpc11.estim.nint_u == [1, 1] - @test nmpc11.estim.nint_ym == [0, 0] - nmpc12 = NonLinMPC(nonlinmodel, Hp=10, M_Hp=Hermitian(diagm(1.01:0.01:1.2), :L)) + nmpc12 = NonLinMPC(linmodel1, Hp=10, M_Hp=Hermitian(diagm(1.01:0.01:1.2), :L)) @test nmpc12.weights.M_Hp ≈ diagm(1.01:0.01:1.2) @test nmpc12.weights.M_Hp isa Hermitian{Float64, Matrix{Float64}} - nmpc13 = NonLinMPC(nonlinmodel, Hp=10, N_Hc=Hermitian(diagm([0.1,0.11,0.12,0.13]), :L), Cwt=Inf) + nmpc13 = NonLinMPC(linmodel1, Hp=10, N_Hc=Hermitian(diagm([0.1,0.11,0.12,0.13]), :L), Cwt=Inf) @test nmpc13.weights.Ñ_Hc ≈ diagm([0.1,0.11,0.12,0.13]) @test nmpc13.weights.Ñ_Hc isa Hermitian{Float64, Matrix{Float64}} - nmcp14 = NonLinMPC(nonlinmodel, Hp=10, L_Hp=Hermitian(diagm(0.001:0.001:0.02), :L)) + nmcp14 = NonLinMPC(linmodel1, Hp=10, L_Hp=Hermitian(diagm(0.001:0.001:0.02), :L)) @test nmcp14.weights.L_Hp ≈ diagm(0.001:0.001:0.02) @test nmcp14.weights.L_Hp isa Hermitian{Float64, Matrix{Float64}} - nmpc15 = NonLinMPC(nonlinmodel, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10) + nmpc15 = NonLinMPC(linmodel1, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10) LHS = zeros(1) nmpc15.con.gc!(LHS,[1,2],[3,4],[4,6],10,0.1) @test LHS ≈ [10*dot([1,2],[3,4])+sum([4,6])+0.1] + nmpc17 = NonLinMPC(linmodel1, Hp=10, transcription=MultipleShooting()) + @test nmpc17.transcription == MultipleShooting() + @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ + @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp + + @test_throws DimensionMismatch NonLinMPC(linmodel1, Hp=15, Ewt=[1, 1]) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, JE = (_,_,_)->0.0) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) + + @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, JE=(Ue,_,_,_)->Ue) + @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) +end + +@testitem "NonLinMPC construction (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt, DifferentiationInterface + import FiniteDiff + linmodel1 = LinModel(sys,Ts,i_d=[3]) + nmpc0 = NonLinMPC(linmodel1, Hp=15) + @test isa(nmpc0.estim, SteadyKalmanFilter) + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing) + + nmpc1 = NonLinMPC(nonlinmodel, Hp=15) + @test isa(nmpc1.estim, UnscentedKalmanFilter) + @test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny + nmpc2 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=Inf) + @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu + optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) + nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim) + @test solver_name(nmpc8.optim) == "Ipopt" + @test get_attribute(nmpc8.optim, "nlp_scaling_max_gradient") == 1.0 + im = InternalModel(nonlinmodel) + nmpc9 = NonLinMPC(im, Hp=15) + @test isa(nmpc9.estim, InternalModel) + nmpc11 = NonLinMPC(nonlinmodel, Hp=15, nint_u=[1, 1], nint_ym=[0, 0]) + @test nmpc11.estim.nint_u == [1, 1] + @test nmpc11.estim.nint_ym == [0, 0] gc! = (LHS,_,_,_,_,_)-> (LHS .= 0.0) # useless, only for coverage nmpc16 = NonLinMPC(nonlinmodel, Hp=10, transcription=MultipleShooting(), nc=10, gc=gc!) @test nmpc16.transcription == MultipleShooting() @@ -728,10 +853,6 @@ end @test length(nmpc16.Z̃) == nonlinmodel_c.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp @test nmpc16.con.nc == 10 - nmpc17 = NonLinMPC(linmodel1, Hp=10, transcription=MultipleShooting()) - @test nmpc17.transcription == MultipleShooting() - @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ - @test size(nmpc17.con.Aeq, 1) == nmpc17.estim.nx̂*nmpc17.Hp nmpc18 = NonLinMPC(nonlinmodel, Hp=10, gradient=AutoFiniteDiff(), jacobian=AutoFiniteDiff(), @@ -753,24 +874,19 @@ end @test isa(nmpc15.estim, UnscentedKalmanFilter{Float32}) @test isa(nmpc15.optim, JuMP.GenericModel{Float64}) # Ipopt does not support Float32 - @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1]) @test_throws ArgumentError NonLinMPC(nonlinmodel) - @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, JE = (_,_,_)->0.0) - @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) - @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation()) @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation(2)) @test_throws ErrorException NonLinMPC(linmodel1, oracle=false, hessian=AutoFiniteDiff()) - - @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue) - @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) + @test_throws ArgumentError NonLinMPC(nonlinmodel, Wy=[1 0;0 1]) end -@testitem "NonLinMPC moves and getinfo" setup=[SetupMPCtests] begin +@testitem "NonLinMPC moves and getinfo (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra using DifferentiationInterface import FiniteDiff linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) + Hp = 100 nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=Hp, Hc=1) ry, ru = [15], [4] @@ -792,6 +908,7 @@ end end R̂y, R̂u = fill(ry[1], Hp), fill(ru[1], Hp) p = [1, R̂y, 0, R̂u] + nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, p=p, Hp=Hp, Hc=1) preparestate!(nmpc, [10]) u = moveinput!(nmpc) @@ -801,10 +918,42 @@ end nmpc.p .= [0, R̂y, 1, R̂u] u = moveinput!(nmpc) @test u ≈ [4] atol=5e-2 + + linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) + nmpc6 = NonLinMPC(linmodel3, Hp=10) + preparestate!(nmpc6, [0]) + @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2 + + nmpc9 = NonLinMPC(linmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) + preparestate!(nmpc9, [10]) + u = moveinput!(nmpc9, [20]) + @test u ≈ [2] atol=5e-2 + info = getinfo(nmpc9) + @test info[:u] ≈ u + @test info[:Ŷ][end] ≈ 20 atol=5e-2 + + # coverage of the branch with error termination status (with an infeasible problem): + nmpc_infeas = NonLinMPC(linmodel, Hp=1, Hc=1, Cwt=Inf) + nmpc_infeas = setconstraint!(nmpc_infeas, umin=[+1], umax=[-1]) + preparestate!(nmpc_infeas, [0]) + @test_logs( + (:error, "MPC terminated without solution: returning last solution shifted "* + "(more info in debug log)"), + moveinput!(nmpc_infeas, [0]) + ) + + @test_nowarn ModelPredictiveControl.info2debugstr(info) +end + +@testitem "NonLinMPC moves and getinfo (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2]) f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d h = (x,d,model) -> model.C*x + model.Dd*d nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) + nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=100, Hc=1) preparestate!(nmpc2, [0], [0]) # if d=[0.1], the output will eventually reach 7*0.1=0.7, no action needed (u=0): @@ -816,40 +965,43 @@ end info = getinfo(nmpc2) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 7d[1] atol=5e-2 + nmpc3 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=100, Hc=1) preparestate!(nmpc3, [0], [0]) u = moveinput!(nmpc3, 7d, d) @test u ≈ [0] atol=5e-2 + nmpc4 = NonLinMPC(nonlinmodel, Hp=15, Mwt=[0], Nwt=[0], Lwt=[1]) preparestate!(nmpc4, [0], [0]) u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) @test u ≈ [12] atol=5e-2 + nmpc5 = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf, transcription=MultipleShooting()) nmpc5 = setconstraint!(nmpc5, ymin=[1]) f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u h! = (y,x,_,_) -> y .= x nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1) transcription = TrapezoidalCollocation(0, f_threads=true, h_threads=true) + nmpc5 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc5, [0.0]) u = moveinput!(nmpc5, [1/0.001]) @test u ≈ [1.0] atol=5e-2 - transcription = TrapezoidalCollocation(1) + + transcription = TrapezoidalCollocation(1) nmpc5_1 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc5_1, [0.0]) u = moveinput!(nmpc5_1, [1/0.001]) @test u ≈ [1.0] atol=5e-2 - linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) - nmpc6 = NonLinMPC(linmodel3, Hp=10) - preparestate!(nmpc6, [0]) - @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2 nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2) + nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) ModelPredictiveControl.h!(y, nonlinmodel2, Float32[0,0], Float32[0], nonlinmodel2.p) preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] atol=5e-2 transcription = MultipleShooting() + nmpc8 = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription) preparestate!(nmpc8, [0], [0]) u = moveinput!(nmpc8, [10], [0]) @@ -857,6 +1009,7 @@ end info = getinfo(nmpc8) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 + transcription = MultipleShooting(f_threads=true, h_threads=true) nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription, hessian=true) nmpc8t = setconstraint!(nmpc8t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian @@ -866,13 +1019,7 @@ end info = getinfo(nmpc8t) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 - nmpc9 = NonLinMPC(linmodel, Nwt=[0], Hp=100, Hc=1, transcription=MultipleShooting()) - preparestate!(nmpc9, [10]) - u = moveinput!(nmpc9, [20]) - @test u ≈ [2] atol=5e-2 - info = getinfo(nmpc9) - @test info[:u] ≈ u - @test info[:Ŷ][end] ≈ 20 atol=5e-2 + nmpc10 = setconstraint!(NonLinMPC( nonlinmodel, Nwt=[0], Hp=100, Hc=1, gradient=AutoFiniteDiff(), @@ -886,24 +1033,12 @@ end info = getinfo(nmpc10) @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 + nmpc11 = NonLinMPC(nonlinmodel, Hp=10, Hc=[1, 2, 3, 4], Nwt=[10]) preparestate!(nmpc11, y, [0]) moveinput!(nmpc11, [10], [0]) ΔU_diff = diff(getinfo(nmpc11)[:U]) @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9 - - # coverage of the branch with error termination status (with an infeasible problem): - nmpc_infeas = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf) - nmpc_infeas = setconstraint!(nmpc_infeas, umin=[+1], umax=[-1]) - preparestate!(nmpc_infeas, [0], [0]) - @test_logs( - (:error, "MPC terminated without solution: returning last solution shifted "* - "(more info in debug log)"), - moveinput!(nmpc_infeas, [0], [0]) - ) - - - @test_nowarn ModelPredictiveControl.info2debugstr(info) end @testitem "NonLinMPC step disturbance rejection" setup=[SetupMPCtests] begin @@ -961,8 +1096,8 @@ end @testitem "NonLinMPC and ManualEstimator v.s. default" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(tf(5, [200, 1]), 300.0) - f(x,u,_,p) = p.A*x + p.Bu*u - h(x,_,p) = p.C*x + f = (x,u,_,p) -> p.A*x + p.Bu*u + h = (x,_,p) -> p.C*x model = setop!(NonLinModel(f, h, 300.0, 1, 1, 1; solver=nothing, p=linmodel), yop=[10]) r = [15] outdist = [5] @@ -1013,19 +1148,20 @@ end nmpc_lin = NonLinMPC(linmodel1, Hp=1, Hc=1) setconstraint!(nmpc_lin, ymin=[5,10],ymax=[55, 35]) - @test all((nmpc_lin.con.Y0min, nmpc_lin.con.Y0max) .≈ ([5,10], [55,35])) + @test nmpc_lin.con.Y0min ≈ [5,10] + @test nmpc_lin.con.Y0max ≈ [55,35] setconstraint!(nmpc_lin, c_ymin=[1.0,1.1], c_ymax=[1.2,1.3]) - @test all((-nmpc_lin.con.A_Ymin[:, end], -nmpc_lin.con.A_Ymax[:, end]) .≈ - ([1.0,1.1], [1.2,1.3])) + @test -nmpc_lin.con.A_Ymin[:, end] ≈ [1.0,1.1] + @test -nmpc_lin.con.A_Ymax[:, end] ≈ [1.2,1.3] setconstraint!(nmpc_lin, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc_lin.con.x̂0min, nmpc_lin.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc_lin.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc_lin.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc_lin, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((-nmpc_lin.con.A_x̂min[:, end], -nmpc_lin.con.A_x̂max[:, end]) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d @@ -1033,30 +1169,35 @@ end nmpc = NonLinMPC(nonlinmodel, Hp=1, Hc=1) setconstraint!(nmpc, umin=[-5, -9.9], umax=[100,99]) - @test all((nmpc.con.U0min, nmpc.con.U0max) .≈ ([-5, -9.9], [100,99])) + @test nmpc.con.U0min ≈ [-5, -9.9] + @test nmpc.con.U0max ≈ [100,99] setconstraint!(nmpc, Δumin=[-5,-10], Δumax=[6,11]) - @test all((nmpc.con.ΔŨmin, nmpc.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + @test nmpc.con.ΔŨmin ≈ [-5,-10,0] + @test nmpc.con.ΔŨmax ≈ [6,11,Inf] setconstraint!(nmpc, ymin=[-6, -11],ymax=[55, 35]) - @test all((nmpc.con.Y0min, nmpc.con.Y0max) .≈ ([-6,-11], [55,35])) + @test nmpc.con.Y0min ≈ [-6,-11] + @test nmpc.con.Y0max ≈ [55,35] setconstraint!(nmpc, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc.con.x̂0min, nmpc.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) - @test all((-nmpc.con.A_Umin[:, end], -nmpc.con.A_Umax[:, end]) .≈ - ([0.01,0.02], [0.03,0.04])) + @test -nmpc.con.A_Umin[:, end] ≈ [0.01,0.02] + @test -nmpc.con.A_Umax[:, end] ≈ [0.03,0.04] setconstraint!(nmpc, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) - @test all((-nmpc.con.A_ΔŨmin[1:end-1, end], -nmpc.con.A_ΔŨmax[1:end-1, end]) .≈ - ([0.05,0.06], [0.07,0.08])) + @test -nmpc.con.A_ΔŨmin[1:end-1, end] ≈ [0.05,0.06] + @test -nmpc.con.A_ΔŨmax[1:end-1, end] ≈ [0.07,0.08] setconstraint!(nmpc, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-nmpc.con.A_Ymin, -nmpc.con.A_Ymax) .≈ (zeros(0,3), zeros(0,3))) - @test all((nmpc.con.C_ymin, nmpc.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + @test -nmpc.con.A_Ymin ≈ zeros(0,3) + @test -nmpc.con.A_Ymax ≈ zeros(0,3) + @test nmpc.con.C_ymin ≈ [1.00,1.01] + @test nmpc.con.C_ymax ≈ [1.02,1.03] setconstraint!(nmpc, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((nmpc.con.c_x̂min, nmpc.con.c_x̂max) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test nmpc.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] # TODO: delete these tests when the deprecated legacy splatting syntax will be. gc_leg! = (LHS,_,_,_,_,_) -> (LHS[begin] = -1) @@ -1075,54 +1216,62 @@ end nmpc_leg = NonLinMPC(nonlinmodel, Hp=1, Hc=1, oracle=false) setconstraint!(nmpc_leg, umin=[-5, -9.9], umax=[100,99]) - @test all((nmpc_leg.con.U0min, nmpc_leg.con.U0max) .≈ ([-5, -9.9], [100,99])) + @test nmpc_leg.con.U0min ≈ [-5, -9.9] + @test nmpc_leg.con.U0max ≈ [100,99] setconstraint!(nmpc_leg, Δumin=[-5,-10], Δumax=[6,11]) - @test all((nmpc_leg.con.ΔŨmin, nmpc_leg.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + @test nmpc_leg.con.ΔŨmin ≈ [-5,-10,0] + @test nmpc_leg.con.ΔŨmax ≈ [6,11,Inf] setconstraint!(nmpc_leg, ymin=[-6, -11],ymax=[55, 35]) - @test all((nmpc_leg.con.Y0min, nmpc_leg.con.Y0max) .≈ ([-6,-11], [55,35])) + @test nmpc_leg.con.Y0min ≈ [-6,-11] + @test nmpc_leg.con.Y0max ≈ [55,35] setconstraint!(nmpc_leg, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc_leg.con.x̂0min, nmpc_leg.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc_leg.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc_leg.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc_leg, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) - @test all((-nmpc_leg.con.A_Umin[:, end], -nmpc_leg.con.A_Umax[:, end]) .≈ - ([0.01,0.02], [0.03,0.04])) + @test -nmpc_leg.con.A_Umin[:, end] ≈ [0.01,0.02] + @test -nmpc_leg.con.A_Umax[:, end] ≈ [0.03,0.04] setconstraint!(nmpc_leg, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) - @test all((-nmpc_leg.con.A_ΔŨmin[1:end-1, end], -nmpc_leg.con.A_ΔŨmax[1:end-1, end]) .≈ - ([0.05,0.06], [0.07,0.08])) + @test -nmpc_leg.con.A_ΔŨmin[1:end-1, end] ≈ [0.05,0.06] + @test -nmpc_leg.con.A_ΔŨmax[1:end-1, end] ≈ [0.07,0.08] setconstraint!(nmpc_leg, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-nmpc_leg.con.A_Ymin, -nmpc_leg.con.A_Ymax) .≈ (zeros(0,3), zeros(0,3))) - @test all((nmpc_leg.con.C_ymin, nmpc_leg.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + @test -nmpc_leg.con.A_Ymin ≈ zeros(0,3) + @test -nmpc_leg.con.A_Ymax ≈ zeros(0,3) + @test nmpc_leg.con.C_ymin ≈ [1.00,1.01] + @test nmpc_leg.con.C_ymax ≈ [1.02,1.03] setconstraint!(nmpc_leg, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((nmpc_leg.con.c_x̂min, nmpc_leg.con.c_x̂max) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test nmpc_leg.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc_leg.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] nmpc_ms = NonLinMPC(nonlinmodel, Hp=1, Hc=1, transcription=MultipleShooting()) setconstraint!(nmpc_ms, ymin=[-6, -11],ymax=[55, 35]) - @test all((nmpc_ms.con.Y0min, nmpc_ms.con.Y0max) .≈ ([-6,-11], [55,35])) + @test nmpc_ms.con.Y0min ≈ [-6,-11] + @test nmpc_ms.con.Y0max ≈ [55,35] setconstraint!(nmpc_ms, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) - @test all((nmpc_ms.con.x̂0min, nmpc_ms.con.x̂0max) .≈ - ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + @test nmpc_ms.con.x̂0min ≈ [-21,-22,-23,-24,-25,-26] + @test nmpc_ms.con.x̂0max ≈ [21,22,23,24,25,26] setconstraint!(nmpc_ms, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) - @test all((-nmpc_ms.con.A_Ymin, -nmpc_ms.con.A_Ymax) .≈ (zeros(0,9), zeros(0,9))) - @test all((nmpc_ms.con.C_ymin, nmpc_ms.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + @test -nmpc_ms.con.A_Ymin ≈ zeros(0,9) + @test -nmpc_ms.con.A_Ymax ≈ zeros(0,9) + @test nmpc_ms.con.C_ymin ≈ [1.00,1.01] + @test nmpc_ms.con.C_ymax ≈ [1.02,1.03] setconstraint!(nmpc_ms, c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] ) - @test all((-nmpc_lin.con.A_x̂min[:, end], -nmpc_lin.con.A_x̂max[:, end]) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) - @test all((nmpc_ms.con.c_x̂min, nmpc_ms.con.c_x̂max) .≈ - ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + @test -nmpc_lin.con.A_x̂min[:, end] ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test -nmpc_lin.con.A_x̂max[:, end] ≈ [0.31,0.32,0.33,0.34,0.35,0.36] + @test nmpc_ms.con.c_x̂min ≈ [0.21,0.22,0.23,0.24,0.25,0.26] + @test nmpc_ms.con.c_x̂max ≈ [0.31,0.32,0.33,0.34,0.35,0.36] end -@testitem "NonLinMPC constraint violation" setup=[SetupMPCtests] begin +@testitem "NonLinMPC constraint violation (LinModel)" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue[1:end-1] .- 4.2 .- ϵ); p[2]*(Ŷe[2:end] .- 3.14 .- ϵ)] Hp=50 @@ -1195,7 +1344,14 @@ end info = getinfo(nmpc_lin) @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) +end + +@testitem "NonLinMPC constraint violation (NonLinModel)" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP + gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue[1:end-1] .- 4.2 .- ϵ); p[2]*(Ŷe[2:end] .- 3.14 .- ϵ)] + Hp=50 + linmodel = LinModel(tf([2], [10000, 1]), 3000.0) f = (x,u,_,p) -> p.A*x + p.Bu*u h = (x,_,p) -> p.C*x nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing, p=linmodel) @@ -1376,6 +1532,27 @@ end @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) + linmodel2 = LinModel([tf([2], [2500, 1]) tf(0.1, [2000, 1])], 3000.0, i_d=[2]) + f = (x,u,d,p) -> p.A*x + p.Bu*u + p.Bd*d + h = (x,d,p) -> p.C*x + p.Dd*d + nonlinmodel2 = NonLinModel(f, h, linmodel2.Ts, 1, 2, 1, 1, solver=nothing, p=linmodel2) + nonlinmodel2 = setop!(nonlinmodel2, uop=[25], dop=[30], yop=[50]) + nmpc_wu = NonLinMPC(nonlinmodel2, Nwt=[0], Cwt=Inf, Hp=100, Hc=1, Wu=[1]) + nmpc_wu = setconstraint!(nmpc_wu, wmax=[20]) + preparestate!(nmpc_wu, [50], [30]) + u = moveinput!(nmpc_wu, [100], [30]) + @test all(isapprox.(getinfo(nmpc_wu)[:U], 20.0; atol=1e-1)) + nmpc_wd = NonLinMPC(nonlinmodel2, Nwt=[0], Cwt=Inf, Hp=100, Hc=1, Wu=[1], Wd=[1]) + nmpc_wd = setconstraint!(nmpc_wd, wmax=[45]) + preparestate!(nmpc_wd, [50], [30]) + u = moveinput!(nmpc_wd, [100], [30]) + @test all(isapprox.(getinfo(nmpc_wd)[:U], 45-30; atol=1e-1)) + nmpc_wr = NonLinMPC(nonlinmodel2, Nwt=[0], Cwt=Inf, Hp=100, Hc=1, Wu=[1], Wr=[1]) + nmpc_wr = setconstraint!(nmpc_wr, wmax=[145]) + preparestate!(nmpc_wr, [50], [30]) + u = moveinput!(nmpc_wr, [100], [30]) + @test all(isapprox.(getinfo(nmpc_wr)[:U], 145-100; atol=1e-1)) + end @testitem "NonLinMPC set model" setup=[SetupMPCtests] begin