diff --git a/Project.toml b/Project.toml index e4364e03b..3148df94d 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.1.0" +version = "1.1.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 89103e84b..e6c5e5939 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -27,7 +27,7 @@ struct ControllerWeights{NT<:Real} end "Include all the data for the constraints of [`PredictiveController`](@ref)" -struct ControllerConstraint{NT<:Real, GCfunc<:Function} +struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} ẽx̂ ::Matrix{NT} fx̂ ::Vector{NT} gx̂ ::Matrix{NT} @@ -434,9 +434,7 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u) size(d) ≠ (nd,) && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)")) size(D̂) ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)")) size(R̂y) ≠ (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)")) - if ~mpc.noR̂u - size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) - end + size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)")) end @@ -668,7 +666,7 @@ end """ init_defaultcon_mpc( estim, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, - gc!=(_,_,_,_,_,_)->nothing, nc=0 + gc!=nothing, nc=0 ) -> con, S̃, Ẽ Init `ControllerConstraint` struct with default parameters based on estimator `estim`. @@ -676,10 +674,9 @@ Init `ControllerConstraint` struct with default parameters based on estimator `e Also return `S̃` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`. """ function init_defaultcon_mpc( - estim::StateEstimator{NT}, - Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, - gc!::GCfunc=(_,_,_,_,_,_)->nothing, nc=0 -) where {NT<:Real, GCfunc<:Function} + estim::StateEstimator{NT}, Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, + gc!::GCfunc=nothing, nc=0 +) where {NT<:Real, GCfunc<:Union{Nothing, Function}} model = estim.model nu, ny, nx̂ = model.nu, model.ny, estim.nx̂ nϵ = isinf(C) ? 0 : 1 diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7044fc63b..d113641ce 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -191,32 +191,34 @@ They are computed with these equations using in-place operations: function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r + Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃ ŷ .= evaloutput(mpc.estim, d) - predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel - F .+= mpc.B - mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) - mul!(F, mpc.V, mpc.estim.lastu0, 1, 1) + predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel + 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.estim.lastu0, 1, 1) # F = F + V*lastu0 if model.nd ≠ 0 mpc.d0 .= d .- model.dop mpc.D̂0 .= D̂ .- mpc.Dop mpc.D̂e[1:model.nd] .= d mpc.D̂e[model.nd+1:end] .= D̂ - mul!(F, mpc.G, mpc.d0, 1, 1) - mul!(F, mpc.J, mpc.D̂0, 1, 1) + mul!(F, mpc.G, mpc.d0, 1, 1) # F = F + G*d0 + mul!(F, mpc.J, mpc.D̂0, 1, 1) # F = F + J*D̂0 end + # --- output setpoint tracking term --- mpc.R̂y .= R̂y - Cy = F .- (R̂y .- mpc.Yop) - M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ - mul!(q̃, M_Hp_Ẽ', Cy) + Cy .= F .- (R̂y .- mpc.Yop) + mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ) + mul!(q̃, M_Hp_Ẽ', Cy) # q̃ = M_Hp*Ẽ'*Cy r .= dot(Cy, mpc.weights.M_Hp, Cy) - if ~mpc.noR̂u - mpc.R̂u .= R̂u - Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) - L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃ - mul!(q̃, L_Hp_S̃', Cu, 1, 1) - r .+= dot(Cu, mpc.weights.L_Hp, Cu) - end - lmul!(2, q̃) + # --- input setpoint tracking term --- + mpc.R̂u .= R̂u + Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) + mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃) + mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu + r .+= dot(Cu, mpc.weights.L_Hp, Cu) + # --- finalize --- + lmul!(2, q̃) # q̃ = 2*q̃ return nothing end @@ -228,7 +230,7 @@ Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) mpc.ŷ .= evaloutput(mpc.estim, d) - predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel + predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel if model.nd ≠ 0 mpc.d0 .= d .- model.dop mpc.D̂0 .= D̂ .- mpc.Dop @@ -236,9 +238,7 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂ mpc.D̂e[model.nd+1:end] .= D̂ end mpc.R̂y .= R̂y - if ~mpc.noR̂u - mpc.R̂u .= R̂u - end + mpc.R̂u .= R̂u return nothing end @@ -371,15 +371,15 @@ The function mutates `Ue`, `Ŷe` and `Ū` in arguments, without assuming any i function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) ny, nu = model.ny, model.nu # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] --- - U0 = Ū - U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 - Ue[1:end-nu] .= U0 .+ mpc.Uop + U = Ū + U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop + Ue[1:end-nu] .= U # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp): - Ue[end-nu+1:end] .= @views Ue[end-2nu+1:end-nu] + Ue[end-nu+1:end] .= @views U[end-nu+1:end] # --- extended output predictions Ŷe = [ŷ(k); Ŷ] --- Ŷe[1:ny] .= mpc.ŷ Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop - return Ue, Ŷe + return Ue, Ŷe end """ @@ -418,13 +418,9 @@ function obj_nonlinprog!( # --- move suppression and slack variable term --- JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ) # --- input setpoint tracking term --- - if !mpc.noR̂u - Ū .= @views Ue[1:end-nu] - Ū .= mpc.R̂u .- Ū - JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) - else - JR̂u = 0.0 - end + Ū .= @views Ue[1:end-nu] + Ū .= mpc.R̂u .- Ū + JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) # --- economic term --- E_JE = obj_econ(mpc, model, Ue, Ŷe) return JR̂y + JΔŨ + JR̂u + E_JE diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 1d83d1db4..1941126a6 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -8,7 +8,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} - noR̂u::Bool S̃::Matrix{NT} T::Matrix{NT} T_lastu0::Vector{NT} @@ -46,7 +45,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - noR̂u = iszero(L_Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc) # dummy val (updated just before optimization): @@ -62,13 +60,13 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) mpc = new{NT, SE}( estim, ΔŨ, ŷ, Hp, Hc, nϵ, weights, - R̂u, R̂y, noR̂u, + R̂u, R̂y, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 66e428876..1be5895e9 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -9,7 +9,7 @@ struct LinMPC{ # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - con::ControllerConstraint{NT} + con::ControllerConstraint{NT, Nothing} ΔŨ::Vector{NT} ŷ ::Vector{NT} Hp::Int @@ -18,7 +18,6 @@ struct LinMPC{ weights::ControllerWeights{NT} R̂u::Vector{NT} R̂y::Vector{NT} - noR̂u::Bool S̃::Matrix{NT} T::Matrix{NT} T_lastu0::Vector{NT} @@ -50,7 +49,6 @@ struct LinMPC{ weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - noR̂u = iszero(L_Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): @@ -67,13 +65,13 @@ struct LinMPC{ Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) mpc = new{NT, SE, JM}( estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, weights, - R̂u, R̂y, noR̂u, + R̂u, R̂y, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 8ba2a3e97..ec365ad0c 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -2,16 +2,17 @@ const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"s struct NonLinMPC{ NT<:Real, - SE<:StateEstimator, + SE<:StateEstimator, JM<:JuMP.GenericModel, JEfunc<:Function, + GCfunc<:Function, P<:Any } <: PredictiveController{NT} estim::SE # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - con::ControllerConstraint{NT} + con::ControllerConstraint{NT, GCfunc} ΔŨ::Vector{NT} ŷ ::Vector{NT} Hp::Int @@ -22,7 +23,6 @@ struct NonLinMPC{ p::P R̂u::Vector{NT} R̂y::Vector{NT} - noR̂u::Bool S̃::Matrix{NT} T::Matrix{NT} T_lastu0::Vector{NT} @@ -45,18 +45,23 @@ struct NonLinMPC{ Yop::Vector{NT} Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} - function NonLinMPC{NT, SE, JM, JEfunc, P}( - estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc, nc, p::P, optim::JM - ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEfunc<:Function, P<:Any} + function NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( + estim::SE, + Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::P, optim::JM + ) where { + NT<:Real, + SE<:StateEstimator, + JM<:JuMP.GenericModel, + JEfunc<:Function, + GCfunc<:Function, + P<:Any + } model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ ŷ = copy(model.yop) # dummy vals (updated just before optimization) - validate_JE(NT, JE) - gc! = get_mutating_gc(NT, gc) weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # dummy vals (updated just before optimization): R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) - noR̂u = iszero(L_Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): @@ -74,14 +79,14 @@ struct NonLinMPC{ test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) - buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) - mpc = new{NT, SE, JM, JEfunc, P}( + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) + mpc = new{NT, SE, JM, JEfunc, GCfunc, P}( estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, weights, JE, p, - R̂u, R̂y, noR̂u, + R̂u, R̂y, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, H̃, q̃, r, @@ -234,7 +239,7 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE::Function = (_,_,_,_) -> 0.0, + JE ::Function = (_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc::Int = 0, @@ -312,7 +317,7 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE ::JEfunc = (_,_,_,_) -> 0.0, + JE ::JEfunc = (_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc = 0, @@ -330,8 +335,11 @@ function NonLinMPC( @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end - return NonLinMPC{NT, SE, JM, JEfunc, P}( - estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc, nc, p, optim + validate_JE(NT, JE) + gc! = get_mutating_gc(NT, gc) + GCfunc = get_type_mutating_gc(gc!) + return NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( + estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, optim ) end @@ -388,6 +396,9 @@ function get_mutating_gc(NT, gc) return gc! end +"Get the type of the mutating version of the custom constrain function `gc!`." +get_type_mutating_gc(::GCfunc) where {GCfunc<:Function} = GCfunc + """ test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) @@ -514,7 +525,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc = get_tmp(gc_cache, ΔŨ1) Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) Ue, Ŷe = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation) mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ)::T @@ -533,7 +544,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc = get_tmp(gc_cache, ΔŨ1) Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ) Ue, Ŷe = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) - ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) + ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation) mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ) end diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 54bc7186d..f10772dae 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -23,6 +23,10 @@ struct PredictiveControllerBuffer{NT<:Real} u ::Vector{NT} R̂y::Vector{NT} D̂ ::Vector{NT} + Ŷ ::Vector{NT} + U ::Vector{NT} + Ẽ ::Matrix{NT} + S̃ ::Matrix{NT} empty::Vector{NT} end @@ -33,12 +37,18 @@ Create a buffer for `PredictiveController` objects. The buffer is used to store intermediate results during computation without allocating. """ -function PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int, Hp::Int) where NT <: Real +function PredictiveControllerBuffer{NT}( + nu::Int, ny::Int, nd::Int, Hp::Int, Hc::Int, nϵ::Int +) where NT <: Real u = Vector{NT}(undef, nu) R̂y = Vector{NT}(undef, ny*Hp) D̂ = Vector{NT}(undef, nd*Hp) + Ŷ = Vector{NT}(undef, ny*Hp) + U = Vector{NT}(undef, nu*Hp) + Ẽ = Matrix{NT}(undef, ny*Hp, nu*Hc + nϵ) + S̃ = Matrix{NT}(undef, nu*Hp, nu*Hc + nϵ) empty = Vector{NT}(undef, 0) - return PredictiveControllerBuffer{NT}(u, R̂y, D̂, empty) + return PredictiveControllerBuffer{NT}(u, R̂y, D̂, Ŷ, U, Ẽ, S̃, empty) end include("controller/construct.jl") diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index 5afbbfe07..02f876274 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -714,10 +714,10 @@ end end @testset "NonLinMPC constraint violation" begin - gc( _ , Ŷe, _ , p , ϵ) = p[]*(Ŷe .- 3.14 .- ϵ) + gc(Ue, Ŷe, _ ,p , ϵ) = [p[1]*(Ue .- 4.2 .- ϵ); p[2]*(Ŷe .- 3.14 .- ϵ)] linmodel = LinModel(tf([2], [10000, 1]), 3000.0) - nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5, gc=gc, nc=50+1, p=[0]) + nmpc_lin = NonLinMPC(linmodel, Hp=50, Hc=5, gc=gc, nc=2*(50+1), p=[0; 0]) setconstraint!(nmpc_lin, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc_lin, umin=[-3], umax=[3]) @@ -766,11 +766,18 @@ end info = getinfo(nmpc_lin) @test info[:x̂end][1] ≈ 0 atol=1e-1 - nmpc_lin.p[] = 1 setconstraint!(nmpc_lin, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc_lin, umin=[-10], umax=[10]) setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15]) setconstraint!(nmpc_lin, ymin=[-100], ymax=[100]) + + nmpc_lin.p .= [1; 0] + moveinput!(nmpc_lin, [20]) + info = getinfo(nmpc_lin) + @test info[:U][end] ≈ 4.2 atol=1e-1 + @test info[:U][begin] ≈ 4.2 atol=1e-1 + + nmpc_lin.p .= [0; 1] moveinput!(nmpc_lin, [20]) info = getinfo(nmpc_lin) @test info[:Ŷ][end] ≈ 3.14 atol=1e-1 @@ -779,7 +786,7 @@ end f = (x,u,_,_) -> linmodel.A*x + linmodel.Bu*u h = (x,_,_) -> linmodel.C*x nonlinmodel = NonLinModel(f, h, linmodel.Ts, 1, 1, 1, solver=nothing) - nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=50+1, p=[0]) + nmpc = NonLinMPC(nonlinmodel, Hp=50, Hc=5, gc=gc, nc=2*(50+1), p=[0; 0]) setconstraint!(nmpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc, umin=[-3], umax=[3]) @@ -828,11 +835,18 @@ end info = getinfo(nmpc) @test info[:x̂end][1] ≈ 0 atol=1e-1 - nmpc.p[] = 1 setconstraint!(nmpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) setconstraint!(nmpc, umin=[-10], umax=[10]) setconstraint!(nmpc, Δumin=[-15], Δumax=[15]) setconstraint!(nmpc, ymin=[-100], ymax=[100]) + + nmpc.p .= [1; 0] + moveinput!(nmpc, [20]) + info = getinfo(nmpc) + @test info[:U][end] ≈ 4.2 atol=1e-1 + @test info[:U][begin] ≈ 4.2 atol=1e-1 + + nmpc.p .= [0; 1] moveinput!(nmpc, [20]) info = getinfo(nmpc) @test info[:Ŷ][end] ≈ 3.14 atol=1e-1 @@ -840,7 +854,7 @@ end end @testset "NonLinMPC set model" begin - estim = KalmanFilter(setop!(LinModel(tf(5, [2, 1]), 3), yop=[10], uop=[1])) + estim = KalmanFilter(setop!(LinModel(tf(5, [200, 1]), 300), yop=[10], uop=[1])) mpc = NonLinMPC(estim, Nwt=[0], Cwt=1e4, Hp=1000, Hc=1) mpc = setconstraint!(mpc, umin=[-24], umax=[26]) mpc = setconstraint!(mpc, ymin=[-54], ymax=[56]) @@ -854,7 +868,7 @@ end preparestate!(mpc, [10]) u = moveinput!(mpc, r) @test u ≈ [2] atol=1e-2 - setmodel!(mpc, setop!(LinModel(tf(5, [2, 1]), 3), yop=[20], uop=[11])) + setmodel!(mpc, setop!(LinModel(tf(5, [200, 1]), 300), yop=[20], uop=[11])) @test mpc.Yop ≈ fill(20.0, 1000) @test mpc.Uop ≈ fill(11.0, 1000) @test mpc.con.U0min ≈ fill(-24.0 - 1 + 1 - 11, 1000) @@ -864,7 +878,7 @@ end r = [40] u = moveinput!(mpc, r) @test u ≈ [15] atol=1e-2 - setmodel!(mpc, setop!(LinModel(tf(10, [2, 1]), 3), yop=[20], uop=[11])) + setmodel!(mpc, setop!(LinModel(tf(10, [200, 1]), 300), yop=[20], uop=[11])) r = [40] u = moveinput!(mpc, r) @test u ≈ [13] atol=1e-2