diff --git a/Project.toml b/Project.toml index 3148df94d..7cdcf1203 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.1" +version = "1.1.2" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 0739a2b52..1aa620cf5 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -454,7 +454,35 @@ Init manipulated input increments to inputs conversion matrices. The conversion from the input increments ``\mathbf{ΔU}`` to manipulated inputs over ``H_p`` are calculated by: ```math -\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1) \\ +\mathbf{U} = \mathbf{S} \mathbf{ΔU} + \mathbf{T} \mathbf{u}(k-1) +``` +The ``\mathbf{S}`` and ``\mathbf{T}`` matrices are defined in the Extended Help section. + +# Extended Help +!!! details "Extended Help" + The ``\mathbf{U}`` vector and the two conversion matrices are defined as: + ```math + \mathbf{U} = \begin{bmatrix} + \mathbf{u}(k + 0) \\ + \mathbf{u}(k + 1) \\ + \vdots \\ + \mathbf{u}(k + H_c - 1) \\ + \vdots \\ + \mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad + \mathbf{S} = \begin{bmatrix} + \mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\ + \mathbf{I} & \mathbf{I} & \cdots & \mathbf{0} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \\ + \vdots & \vdots & \ddots & \vdots \\ + \mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \end{bmatrix} , \quad + \mathbf{T} = \begin{bmatrix} + \mathbf{I} \\ + \mathbf{I} \\ + \vdots \\ + \mathbf{I} \\ + \vdots \\ + \mathbf{I} \end{bmatrix} ``` """ function init_ΔUtoU(model::SimModel{NT}, Hp, Hc) where {NT<:Real} @@ -753,12 +781,12 @@ constraints: \mathbf{A_{U_{max}}} \end{bmatrix} \mathbf{ΔŨ} ≤ \begin{bmatrix} - - \mathbf{(U_{min} - U_{op}) + T} \mathbf{u_0}(k-1) \\ - + \mathbf{(U_{max} - U_{op}) - T} \mathbf{u_0}(k-1) + - \mathbf{(U_{min}) + T} \mathbf{u}(k-1) \\ + + \mathbf{(U_{max}) - T} \mathbf{u}(k-1) \end{bmatrix} ``` -in which ``\mathbf{U_{min}, U_{max}}`` and ``\mathbf{U_{op}}`` vectors respectively contains -``\mathbf{u_{min}, u_{max}}`` and ``\mathbf{u_{op}}`` repeated ``H_p`` times. +in which ``\mathbf{U_{min}}`` and ``\mathbf{U_{max}}`` vectors respectively contains +``\mathbf{u_{min}}`` and ``\mathbf{u_{max}}`` repeated ``H_p`` times. """ function relaxU(::SimModel{NT}, nϵ, C_umin, C_umax, S) where NT<:Real if nϵ == 1 # ΔŨ = [ΔU; ϵ] diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 9e5f0b534..f6f7beffa 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -179,8 +179,8 @@ They are computed with these equations using in-place operations: \begin{aligned} \mathbf{F} &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k) + \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s} \\ - \mathbf{C_y} &= \mathbf{F} - (\mathbf{R̂_y - Y_{op}}) \\ - \mathbf{C_u} &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}}) \\ + \mathbf{C_y} &= \mathbf{F} + \mathbf{Y_{op}} - \mathbf{R̂_y} \\ + \mathbf{C_u} &= \mathbf{T}\mathbf{u}(k-1) - \mathbf{R̂_u} \\ \mathbf{q̃} &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\ r &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} @@ -189,7 +189,9 @@ 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) + lastu = mpc.buffer.u + lastu .= mpc.estim.lastu0 .+ model.uop + mul!(mpc.T_lastu, mpc.T, lastu) ŷ, 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) @@ -210,7 +212,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ # --- output setpoint tracking term --- mpc.R̂y .= R̂y if !mpc.weights.iszero_M_Hp[] - Cy .= F .- (R̂y .- mpc.Yop) + Cy .= F .+ mpc.Yop .- R̂y mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ) mul!(q̃, M_Hp_Ẽ', Cy, 1, 1) # q̃ = q̃ + M_Hp*Ẽ'*Cy r .+= dot(Cy, mpc.weights.M_Hp, Cy) # r = r + Cy'*M_Hp*Cy @@ -218,7 +220,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ # --- input setpoint tracking term --- mpc.R̂u .= R̂u if !mpc.weights.iszero_L_Hp[] - Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) + Cu .= mpc.T_lastu .- R̂u mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃) mul!(q̃, L_Hp_S̃', Cu, 1, 1) # q̃ = q̃ + L_Hp*S̃'*Cu r .+= dot(Cu, mpc.weights.L_Hp, Cu) # r = r + Cu'*L_Hp*Cu @@ -234,7 +236,9 @@ end Init `ŷ, 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, D̂, R̂y, R̂u) - mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) + lastu = mpc.buffer.u + lastu .= mpc.estim.lastu0 .+ model.uop + mul!(mpc.T_lastu, mpc.T, lastu) mpc.ŷ .= evaloutput(mpc.estim, d) predictstoch!(mpc, mpc.estim) # init F with Ŷs for InternalModel if model.nd ≠ 0 @@ -281,9 +285,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel) mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1) end n = 0 - mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.T_lastu0 + mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu n += nU - mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.T_lastu0 + mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu n += nU mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin n += nΔŨ @@ -307,9 +311,9 @@ end function linconstraint!(mpc::PredictiveController, ::SimModel) nU, nΔŨ = length(mpc.con.U0min), length(mpc.con.ΔŨmin) n = 0 - mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min + mpc.T_lastu0 + mpc.con.b[(n+1):(n+nU)] .= @. -mpc.con.U0min - mpc.Uop + mpc.T_lastu n += nU - mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max - mpc.T_lastu0 + mpc.con.b[(n+1):(n+nU)] .= @. +mpc.con.U0max + mpc.Uop - mpc.T_lastu n += nU mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin n += nΔŨ @@ -378,7 +382,7 @@ function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) ny, nu = model.ny, model.nu # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] --- U = Ū - U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop + U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu Ue[1:end-nu] .= U # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp): Ue[end-nu+1:end] .= @views U[end-nu+1:end] diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 1941126a6..2c14c4e6d 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -10,7 +10,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} R̂y::Vector{NT} S̃::Matrix{NT} T::Matrix{NT} - T_lastu0::Vector{NT} + T_lastu::Vector{NT} Ẽ::Matrix{NT} F::Vector{NT} G::Matrix{NT} @@ -44,7 +44,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L) L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L) # dummy vals (updated just before optimization): - R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc) # dummy val (updated just before optimization): @@ -67,7 +67,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} Hp, Hc, nϵ, weights, R̂u, R̂y, - S̃, T, T_lastu0, + S̃, T, T_lastu, Ẽ, F, G, J, K, V, B, H̃, q̃, r, H̃_chol, diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 1be5895e9..cc5373bdf 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -20,7 +20,7 @@ struct LinMPC{ R̂y::Vector{NT} S̃::Matrix{NT} T::Matrix{NT} - T_lastu0::Vector{NT} + T_lastu::Vector{NT} Ẽ::Matrix{NT} F::Vector{NT} G::Matrix{NT} @@ -48,7 +48,7 @@ struct LinMPC{ ŷ = copy(model.yop) # dummy vals (updated just before optimization) weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt) # dummy vals (updated just before optimization): - R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): @@ -72,7 +72,7 @@ struct LinMPC{ Hp, Hc, nϵ, weights, R̂u, R̂y, - S̃, T, T_lastu0, + S̃, T, T_lastu, Ẽ, F, G, J, K, V, B, H̃, q̃, r, Ks, Ps, diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 6023ac2d5..c1080ae60 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -25,7 +25,7 @@ struct NonLinMPC{ R̂y::Vector{NT} S̃::Matrix{NT} T::Matrix{NT} - T_lastu0::Vector{NT} + T_lastu::Vector{NT} Ẽ::Matrix{NT} F::Vector{NT} G::Matrix{NT} @@ -61,7 +61,7 @@ struct NonLinMPC{ ŷ = copy(model.yop) # dummy vals (updated just before optimization) weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) # dummy vals (updated just before optimization): - R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) + R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp) S, T = init_ΔUtoU(model, Hp, Hc) E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc) # dummy vals (updated just before optimization): @@ -87,7 +87,7 @@ struct NonLinMPC{ weights, JE, p, R̂u, R̂y, - S̃, T, T_lastu0, + S̃, T, T_lastu, Ẽ, F, G, J, K, V, B, H̃, q̃, r, Ks, Ps, diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index 02f876274..20e58acc5 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -221,52 +221,58 @@ end model = LinModel(tf([2], [10, 1]), 3.0) mpc = LinMPC(model, Hp=50, Hc=5) - setconstraint!(mpc, x̂min=[-1e3,-Inf], x̂max=[1e3,+Inf]) - setconstraint!(mpc, umin=[-3], umax=[3]) - setconstraint!(mpc, Δumin=[-1.5], Δumax=[1.5]) + setconstraint!(mpc, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) + setconstraint!(mpc, umin=[-10], umax=[10]) + setconstraint!(mpc, Δumin=[-15], Δumax=[15]) setconstraint!(mpc, ymin=[-100], ymax=[100]) preparestate!(mpc, [0]) - moveinput!(mpc, [-10]) + + setconstraint!(mpc, umin=[-3], umax=[4]) + moveinput!(mpc, [-100]) info = getinfo(mpc) - @test info[:ΔU][begin] ≈ -1.5 atol=1e-1 - @test info[:U][end] ≈ -3 atol=1e-1 - moveinput!(mpc, [10]) + @test all(isapprox.(info[:U], -3; atol=1e-1)) + moveinput!(mpc, [100]) info = getinfo(mpc) - @test info[:ΔU][begin] ≈ 1.5 atol=1e-1 - @test info[:U][end] ≈ 3 atol=1e-1 - + @test all(isapprox.(info[:U], 4; atol=1e-1)) setconstraint!(mpc, umin=[-10], umax=[10]) + + setconstraint!(mpc, Δumin=[-1.5], Δumax=[1.25]) + moveinput!(mpc, [-100]) + info = getinfo(mpc) + @test all(isapprox.(info[:ΔU], -1.5; atol=1e-1)) + moveinput!(mpc, [100]) + info = getinfo(mpc) + @test all(isapprox.(info[:ΔU], 1.25; atol=1e-1)) setconstraint!(mpc, Δumin=[-15], Δumax=[15]) - setconstraint!(mpc, ymin=[-0.5], ymax=[0.5]) - moveinput!(mpc, [-10]) + + setconstraint!(mpc, ymin=[-0.5], ymax=[0.9]) + moveinput!(mpc, [-100]) info = getinfo(mpc) - @test info[:Ŷ][end] ≈ -0.5 atol=1e-1 - moveinput!(mpc, [10]) + @test all(isapprox.(info[:Ŷ], -0.5; atol=1e-1)) + moveinput!(mpc, [100]) info = getinfo(mpc) - @test info[:Ŷ][end] ≈ 0.5 atol=1e-1 + @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1)) + setconstraint!(mpc, ymin=[-100], ymax=[100]) - setconstraint!(mpc, umin=[-10], umax=[10]) - setconstraint!(mpc, Δumin=[-15], Δumax=[15]) - setconstraint!(mpc, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.5; fill(+100, 49)]) + setconstraint!(mpc, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.9; fill(+100, 49)]) moveinput!(mpc, [-10]) info = getinfo(mpc) - @test info[:Ŷ][end] ≈ -10 atol=1e-1 @test info[:Ŷ][begin] ≈ -0.5 atol=1e-1 + @test info[:Ŷ][end] ≈ -10 atol=1e-1 moveinput!(mpc, [10]) info = getinfo(mpc) + @test info[:Ŷ][begin] ≈ 0.9 atol=1e-1 @test info[:Ŷ][end] ≈ 10 atol=1e-1 - @test info[:Ŷ][begin] ≈ 0.5 atol=1e-1 + setconstraint!(mpc, ymin=[-100], ymax=[100]) - setconstraint!(mpc, umin=[-1e3], umax=[+1e3]) - setconstraint!(mpc, Δumin=[-1e3], Δumax=[+1e3]) - setconstraint!(mpc, ymin=[-1e3], ymax=[+1e3]) setconstraint!(mpc, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf]) - moveinput!(mpc, [-10]) + moveinput!(mpc, [-100]) info = getinfo(mpc) @test info[:x̂end][1] ≈ 0 atol=1e-1 - moveinput!(mpc, [10]) + moveinput!(mpc, [100]) info = getinfo(mpc) @test info[:x̂end][1] ≈ 0 atol=1e-1 + setconstraint!(mpc, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) end @testset "LinMPC terminal cost" begin @@ -719,33 +725,40 @@ end linmodel = LinModel(tf([2], [10000, 1]), 3000.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]) - setconstraint!(nmpc_lin, Δumin=[-1.5], Δumax=[1.5]) + setconstraint!(nmpc_lin, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) + setconstraint!(nmpc_lin, umin=[-10], umax=[10]) + setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15]) setconstraint!(nmpc_lin, ymin=[-100], ymax=[100]) preparestate!(nmpc_lin, [0]) - moveinput!(nmpc_lin, [-20]) + + setconstraint!(nmpc_lin, umin=[-3], umax=[4]) + moveinput!(nmpc_lin, [-100]) info = getinfo(nmpc_lin) - @test info[:ΔU][begin] ≈ -1.5 atol=1e-1 - @test info[:U][end] ≈ -3 atol=1e-1 - moveinput!(nmpc_lin, [20]) + @test all(isapprox.(info[:U], -3; atol=1e-1)) + moveinput!(nmpc_lin, [100]) info = getinfo(nmpc_lin) - @test info[:ΔU][begin] ≈ 1.5 atol=1e-1 - @test info[:U][end] ≈ 3 atol=1e-1 - + @test all(isapprox.(info[:U], 4; atol=1e-1)) setconstraint!(nmpc_lin, umin=[-10], umax=[10]) + + setconstraint!(nmpc_lin, Δumin=[-1.5], Δumax=[1.25]) + moveinput!(nmpc_lin, [-100]) + info = getinfo(nmpc_lin) + @test all(isapprox.(info[:ΔU], -1.5; atol=1e-1)) + moveinput!(nmpc_lin, [100]) + info = getinfo(nmpc_lin) + @test all(isapprox.(info[:ΔU], 1.25; atol=1e-1)) setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15]) - setconstraint!(nmpc_lin, ymin=[-0.5], ymax=[0.5]) - moveinput!(nmpc_lin, [-20]) + + setconstraint!(nmpc_lin, ymin=[-0.5], ymax=[0.9]) + moveinput!(nmpc_lin, [-100]) info = getinfo(nmpc_lin) - @test info[:Ŷ][end] ≈ -0.5 atol=1e-1 - moveinput!(nmpc_lin, [20]) + @test all(isapprox.(info[:Ŷ], -0.5; atol=1e-1)) + moveinput!(nmpc_lin, [100]) info = getinfo(nmpc_lin) - @test info[:Ŷ][end] ≈ 0.5 atol=1e-1 + @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1)) + setconstraint!(nmpc_lin, ymin=[-100], ymax=[100]) - setconstraint!(nmpc_lin, umin=[-10], umax=[10]) - setconstraint!(nmpc_lin, Δumin=[-15], Δumax=[15]) - setconstraint!(nmpc_lin, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.5; fill(+100, 49)]) + setconstraint!(nmpc_lin, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.9; fill(+100, 49)]) moveinput!(nmpc_lin, [-10]) info = getinfo(nmpc_lin) @test info[:Ŷ][end] ≈ -10 atol=1e-1 @@ -753,68 +766,68 @@ end moveinput!(nmpc_lin, [10]) info = getinfo(nmpc_lin) @test info[:Ŷ][end] ≈ 10 atol=1e-1 - @test info[:Ŷ][begin] ≈ 0.5 atol=1e-1 + @test info[:Ŷ][begin] ≈ 0.9 atol=1e-1 + setconstraint!(nmpc_lin, ymin=[-100], ymax=[100]) - setconstraint!(nmpc_lin, umin=[-1e3], umax=[+1e3]) - setconstraint!(nmpc_lin, Δumin=[-1e3], Δumax=[+1e3]) - setconstraint!(nmpc_lin, ymin=[-1e3], ymax=[+1e3]) setconstraint!(nmpc_lin, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf]) - moveinput!(nmpc_lin, [-10]) + moveinput!(nmpc_lin, [-100]) info = getinfo(nmpc_lin) @test info[:x̂end][1] ≈ 0 atol=1e-1 - moveinput!(nmpc_lin, [10]) + moveinput!(nmpc_lin, [100]) info = getinfo(nmpc_lin) @test info[:x̂end][1] ≈ 0 atol=1e-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]) + setconstraint!(nmpc_lin, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) nmpc_lin.p .= [1; 0] - moveinput!(nmpc_lin, [20]) + moveinput!(nmpc_lin, [100]) info = getinfo(nmpc_lin) - @test info[:U][end] ≈ 4.2 atol=1e-1 - @test info[:U][begin] ≈ 4.2 atol=1e-1 + @test all(isapprox.(info[:U], 4.2; atol=1e-1)) nmpc_lin.p .= [0; 1] - moveinput!(nmpc_lin, [20]) + moveinput!(nmpc_lin, [100]) info = getinfo(nmpc_lin) - @test info[:Ŷ][end] ≈ 3.14 atol=1e-1 - @test info[:Ŷ][begin] ≈ 3.14 atol=1e-1 + @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) - 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) + + 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) 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]) - setconstraint!(nmpc, Δumin=[-1.5], Δumax=[1.5]) + setconstraint!(nmpc, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) + setconstraint!(nmpc, umin=[-10], umax=[10]) + setconstraint!(nmpc, Δumin=[-15], Δumax=[15]) setconstraint!(nmpc, ymin=[-100], ymax=[100]) preparestate!(nmpc, [0]) - moveinput!(nmpc, [-20]) + + setconstraint!(nmpc, umin=[-3], umax=[4]) + moveinput!(nmpc, [-100]) info = getinfo(nmpc) - @test info[:ΔU][begin] ≈ -1.5 atol=1e-1 - @test info[:U][end] ≈ -3 atol=1e-1 - moveinput!(nmpc, [20]) + @test all(isapprox.(info[:U], -3; atol=1e-1)) + moveinput!(nmpc, [100]) info = getinfo(nmpc) - @test info[:ΔU][begin] ≈ 1.5 atol=1e-1 - @test info[:U][end] ≈ 3 atol=1e-1 - + @test all(isapprox.(info[:U], 4; atol=1e-1)) setconstraint!(nmpc, umin=[-10], umax=[10]) - setconstraint!(nmpc, Δumin=[-15], Δumax=[15]) - setconstraint!(nmpc, ymin=[-0.5], ymax=[0.5]) - moveinput!(nmpc, [-20]) + + setconstraint!(nmpc, Δumin=[-1.5], Δumax=[1.25]) + moveinput!(nmpc, [-100]) info = getinfo(nmpc) - @test info[:Ŷ][end] ≈ -0.5 atol=1e-1 - moveinput!(nmpc, [20]) + @test all(isapprox.(info[:ΔU], -1.5; atol=1e-1)) + moveinput!(nmpc, [100]) info = getinfo(nmpc) - @test info[:Ŷ][end] ≈ 0.5 atol=1e-1 - - setconstraint!(nmpc, umin=[-10], umax=[10]) + @test all(isapprox.(info[:ΔU], 1.25; atol=1e-1)) setconstraint!(nmpc, Δumin=[-15], Δumax=[15]) - setconstraint!(nmpc, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.5; fill(+100, 49)]) + + setconstraint!(nmpc, ymin=[-0.5], ymax=[0.9]) + moveinput!(nmpc, [-100]) + info = getinfo(nmpc) + @test all(isapprox.(info[:Ŷ], -0.5; atol=1e-1)) + moveinput!(nmpc, [100]) + info = getinfo(nmpc) + @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1)) + setconstraint!(nmpc, ymin=[-100], ymax=[100]) + + setconstraint!(nmpc, Ymin=[-0.5; fill(-100, 49)], Ymax=[0.9; fill(+100, 49)]) moveinput!(nmpc, [-10]) info = getinfo(nmpc) @test info[:Ŷ][end] ≈ -10 atol=1e-1 @@ -822,11 +835,9 @@ end moveinput!(nmpc, [10]) info = getinfo(nmpc) @test info[:Ŷ][end] ≈ 10 atol=1e-1 - @test info[:Ŷ][begin] ≈ 0.5 atol=1e-1 + @test info[:Ŷ][begin] ≈ 0.9 atol=1e-1 + setconstraint!(nmpc, ymin=[-100], ymax=[100]) - setconstraint!(nmpc, umin=[-1e3], umax=[+1e3]) - setconstraint!(nmpc, Δumin=[-1e3], Δumax=[+1e3]) - setconstraint!(nmpc, ymin=[-1e3], ymax=[+1e3]) setconstraint!(nmpc, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf]) moveinput!(nmpc, [-10]) info = getinfo(nmpc) @@ -834,23 +845,18 @@ end moveinput!(nmpc, [10]) info = getinfo(nmpc) @test info[:x̂end][1] ≈ 0 atol=1e-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]) + setconstraint!(nmpc, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) nmpc.p .= [1; 0] - moveinput!(nmpc, [20]) + moveinput!(nmpc, [100]) info = getinfo(nmpc) - @test info[:U][end] ≈ 4.2 atol=1e-1 - @test info[:U][begin] ≈ 4.2 atol=1e-1 + @test all(isapprox.(info[:U], 4.2; atol=1e-1)) nmpc.p .= [0; 1] - moveinput!(nmpc, [20]) + moveinput!(nmpc, [100]) info = getinfo(nmpc) - @test info[:Ŷ][end] ≈ 3.14 atol=1e-1 - @test info[:Ŷ][begin] ≈ 3.14 atol=1e-1 + @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) + end @testset "NonLinMPC set model" begin