diff --git a/Project.toml b/Project.toml index d0d68d997..f44c14812 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "0.24.0" +version = "0.24.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 7f95c21df..2b94ce2e6 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -34,4 +34,5 @@ ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel) ```@docs ModelPredictiveControl.optim_objective!(::PredictiveController) +ModelPredictiveControl.getinput ``` diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 7a01a547f..fe63f1e4c 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -40,12 +40,6 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel) ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator) ``` -## Evaluate Estimated Output - -```@docs -ModelPredictiveControl.evalŷ -``` - ## Remove Operating Points ```@docs diff --git a/src/controller/construct.jl b/src/controller/construct.jl index f4d588aae..76b2b6c08 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -603,15 +603,15 @@ Init the quadratic programming Hessian `H̃` for MPC. The matrix appear in the quadratic general form: ```math - J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + p + J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + r ``` The Hessian matrix is constant if the model and weights are linear and time invariant (LTI): ```math \mathbf{H̃} = 2 ( \mathbf{Ẽ}'\mathbf{M}_{H_p}\mathbf{Ẽ} + \mathbf{Ñ}_{H_c} + \mathbf{S̃}'\mathbf{L}_{H_p}\mathbf{S̃} ) ``` -The vector ``\mathbf{q̃}`` and scalar ``p`` need recalculation each control period ``k``, see -[`initpred!`](@ref). ``p`` does not impact the minima position. It is thus useless at +The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see +[`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at optimization but required to evaluate the minimal ``J`` value. """ function init_quadprog(::LinModel, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index ea93627fa..04989c2c7 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -42,7 +42,9 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref). ```jldoctest julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1); -julia> ry = [5]; u = moveinput!(mpc, ry); round.(u, digits=3) +julia> preparestate!(mpc, [0]); ry = [5]; + +julia> u = moveinput!(mpc, ry); round.(u, digits=3) 1-element Vector{Float64}: 1.0 ``` @@ -50,25 +52,24 @@ julia> ry = [5]; u = moveinput!(mpc, ry); round.(u, digits=3) function moveinput!( mpc::PredictiveController, ry::Vector = mpc.estim.model.yop, - d ::Vector = mpc.estim.buffer.empty; - Dhat ::Vector = repeat(d, mpc.Hp), - Rhaty::Vector = repeat(ry, mpc.Hp), + d ::Vector = mpc.buffer.empty; + Dhat ::Vector = repeat!(mpc.buffer.D̂, d, mpc.Hp), + Rhaty::Vector = repeat!(mpc.buffer.R̂y, ry, mpc.Hp), Rhatu::Vector = mpc.Uop, D̂ = Dhat, R̂y = Rhaty, R̂u = Rhatu ) + if mpc.estim.direct && !mpc.estim.corrected[] + @warn "preparestate! should be called before moveinput! with current estimators" + end validate_args(mpc, ry, d, D̂, R̂y, R̂u) initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u) linconstraint!(mpc, mpc.estim.model) ΔŨ = optim_objective!(mpc) - Δu = ΔŨ[1:mpc.estim.model.nu] # receding horizon principle: only Δu(k) is used (1st one) - u = mpc.estim.lastu0 + mpc.estim.model.uop + Δu - return u + return getinput(mpc, ΔŨ) end - - @doc raw""" getinfo(mpc::PredictiveController) -> info @@ -102,7 +103,7 @@ available for [`NonLinMPC`](@ref). ```jldoctest julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1, Hc=1); -julia> u = moveinput!(mpc, [10]); +julia> preparestate!(mpc, [0]); u = moveinput!(mpc, [10]); julia> round.(getinfo(mpc)[:Ŷ], digits=3) 1-element Vector{Float64}: @@ -164,7 +165,7 @@ end @doc raw""" initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) -> nothing -Init linear model prediction matrices `F, q̃, p` and current estimated output `ŷ`. +Init linear model prediction matrices `F, q̃, r` and current estimated output `ŷ`. See [`init_predmat`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices. They are computed with these equations using in-place operations: @@ -176,15 +177,15 @@ They are computed with these equations using in-place operations: \mathbf{C_u} &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}}) \\ \mathbf{q̃} &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}] \\ - p &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} + r &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u} \end{aligned} ``` """ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) - ŷ, F, q̃, p = mpc.ŷ, mpc.F, mpc.q̃, mpc.p - ŷ .= evalŷ(mpc.estim, d) + ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r + ŷ .= 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) @@ -201,13 +202,13 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂ Cy = F .- mpc.R̂y0 M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ mul!(q̃, M_Hp_Ẽ', Cy) - p .= dot(Cy, mpc.M_Hp, Cy) + r .= dot(Cy, mpc.M_Hp, Cy) if ~mpc.noR̂u mpc.R̂u0 .= R̂u .- mpc.Uop Cu = mpc.T_lastu0 .- mpc.R̂u0 L_Hp_S̃ = mpc.L_Hp*mpc.S̃ mul!(q̃, L_Hp_S̃', Cu, 1, 1) - p .+= dot(Cu, mpc.L_Hp, Cu) + r .+= dot(Cu, mpc.L_Hp, Cu) end lmul!(2, q̃) return nothing @@ -220,7 +221,7 @@ Init `ŷ, F, d0, D̂0, D̂E, R̂y0, R̂u0` vectors when model is not a [`LinMod """ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0) - mpc.ŷ .= evalŷ(mpc.estim, d) + mpc.ŷ .= evaloutput(mpc.estim, d) predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel if model.nd ≠ 0 mpc.d0 .= d .- model.dop @@ -367,9 +368,9 @@ at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates t function obj_nonlinprog!( U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT} ) where NT <: Real - J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.p[] + J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[] if !iszero(mpc.E) - ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E + ŷ, D̂E = mpc.ŷ, mpc.D̂E U = U0 U .+= mpc.Uop uend = @views U[(end-model.nu+1):end] @@ -501,6 +502,24 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty) return preparestate!(mpc.estim, ym, d) end +@doc raw""" + getinput(mpc::PredictiveController, ΔŨ) -> u + +Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `ΔŨ`. + +The first manipulated input ``\mathbf{u}(k)`` is extracted from the input increments vector +``\mathbf{ΔŨ}`` and applied on the plant (from the receding horizon principle). +""" +function getinput(mpc, ΔŨ) + Δu = mpc.buffer.u + for i in 1:mpc.estim.model.nu + Δu[i] = ΔŨ[i] + end + u = Δu + u .+= mpc.estim.lastu0 .+ mpc.estim.model.uop + return u +end + """ updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂next diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 424f9cfe0..9ba406d8c 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -24,7 +24,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} B::Vector{NT} H̃::Hermitian{NT, Matrix{NT}} q̃::Vector{NT} - p::Vector{NT} + r::Vector{NT} H̃_chol::Cholesky{NT, Matrix{NT}} Ks::Matrix{NT} Ps::Matrix{NT} @@ -34,6 +34,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} Uop::Vector{NT} Yop::Vector{NT} Dop::Vector{NT} + buffer::PredictiveControllerBuffer{NT} function ExplicitMPC{NT, SE}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp ) where {NT<:Real, SE<:StateEstimator} @@ -57,7 +58,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} S̃, Ñ_Hc, Ẽ = S, N_Hc, E # no slack variable ϵ for ExplicitMPC H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) # dummy vals (updated just before optimization): - q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1) + q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) H̃_chol = cholesky(H̃) Ks, Ps = init_stochpred(estim, Hp) # dummy vals (updated just before optimization): @@ -65,6 +66,7 @@ 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) mpc = new{NT, SE}( estim, ΔŨ, ŷ, @@ -73,11 +75,12 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} R̂u0, R̂y0, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, - H̃, q̃, p, + H̃, q̃, r, H̃_chol, Ks, Ps, d0, D̂0, D̂E, Uop, Yop, Dop, + buffer ) return mpc end diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index d7676271f..8b84f8e4b 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -34,7 +34,7 @@ struct LinMPC{ B::Vector{NT} H̃::Hermitian{NT, Matrix{NT}} q̃::Vector{NT} - p::Vector{NT} + r::Vector{NT} Ks::Matrix{NT} Ps::Matrix{NT} d0::Vector{NT} @@ -43,6 +43,7 @@ struct LinMPC{ Uop::Vector{NT} Yop::Vector{NT} Dop::Vector{NT} + buffer::PredictiveControllerBuffer{NT} function LinMPC{NT, SE, JM}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel} @@ -67,13 +68,14 @@ struct LinMPC{ ) H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) # dummy vals (updated just before optimization): - q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1) + q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) Ks, Ps = init_stochpred(estim, Hp) # dummy vals (updated just before optimization): d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) mpc = new{NT, SE, JM}( estim, optim, con, ΔŨ, ŷ, @@ -82,10 +84,11 @@ struct LinMPC{ R̂u0, R̂y0, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, - H̃, q̃, p, + H̃, q̃, r, Ks, Ps, d0, D̂0, D̂E, Uop, Yop, Dop, + buffer ) init_optimization!(mpc, model, optim) return mpc diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index bc341b53a..1f9c8a011 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -36,7 +36,7 @@ struct NonLinMPC{ B::Vector{NT} H̃::Hermitian{NT, Matrix{NT}} q̃::Vector{NT} - p::Vector{NT} + r::Vector{NT} Ks::Matrix{NT} Ps::Matrix{NT} d0::Vector{NT} @@ -45,6 +45,7 @@ struct NonLinMPC{ Uop::Vector{NT} Yop::Vector{NT} Dop::Vector{NT} + buffer::PredictiveControllerBuffer{NT} function NonLinMPC{NT, SE, JM, JEFunc}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, optim::JM ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function} @@ -68,13 +69,14 @@ struct NonLinMPC{ ) H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) # dummy vals (updated just before optimization): - q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1) + q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1) Ks, Ps = init_stochpred(estim, Hp) # dummy vals (updated just before optimization): d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp) Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp) nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) + buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp) mpc = new{NT, SE, JM, JEFunc}( estim, optim, con, ΔŨ, ŷ, @@ -83,10 +85,11 @@ struct NonLinMPC{ R̂u0, R̂y0, noR̂u, S̃, T, T_lastu0, Ẽ, F, G, J, K, V, B, - H̃, q̃, p, + H̃, q̃, r, Ks, Ps, d0, D̂0, D̂E, Uop, Yop, Dop, + buffer ) init_optimization!(mpc, model, optim) return mpc diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index fc6208119..2714fd2b0 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -87,7 +87,8 @@ end Init `estim.x̂0` states from current inputs `u`, measured outputs `ym` and disturbances `d`. The method tries to find a good steady-state for the initial estimate ``\mathbf{x̂}``. It -removes the operating points with [`remove_op!`](@ref) and call [`init_estimate!`](@ref): +stores `u - estim.model.uop` at `estim.lastu0` and removes the operating points with +[`remove_op!`](@ref), and call [`init_estimate!`](@ref): - If `estim.model` is a [`LinModel`](@ref), it finds the steady-state of the augmented model using `u` and `d` arguments, and uses the `ym` argument to enforce that @@ -99,7 +100,7 @@ If applicable, it also sets the error covariance `estim.P̂` to `estim.P̂_0`. # Examples ```jldoctest -julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2]); +julia> estim = SteadyKalmanFilter(LinModel(tf(3, [10, 1]), 0.5), nint_ym=[2], direct=false); julia> u = [1]; y = [3 - 0.1]; x̂ = round.(initstate!(estim, u, y), digits=3) 3-element Vector{Float64}: @@ -107,8 +108,6 @@ julia> u = [1]; y = [3 - 0.1]; x̂ = round.(initstate!(estim, u, y), digits=3) 0.0 -0.1 -julia> preparestate!(estim, y); - julia> x̂ ≈ updatestate!(estim, u, y) true @@ -171,12 +170,15 @@ init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ ) = nothing Evaluate `StateEstimator` outputs `ŷ` from `estim.x̂0` states and disturbances `d`. -It returns `estim` output at the current time step ``\mathbf{ŷ}(k)``. Calling a -[`StateEstimator`](@ref) object calls this `evaloutput` method. +It returns `estim` output at the current time step ``\mathbf{ŷ}(k)``. If `estim.direct` is +`true`, the method [`preparestate!`](@ref) should be called beforehand to correct the state +estimate. + +Calling a [`StateEstimator`](@ref) object calls this `evaloutput` method. # Examples ```jldoctest -julia> kf = SteadyKalmanFilter(setop!(LinModel(tf(2, [10, 1]), 5), yop=[20])); +julia> kf = SteadyKalmanFilter(setop!(LinModel(tf(2, [10, 1]), 5), yop=[20]), direct=false); julia> ŷ = evaloutput(kf) 1-element Vector{Float64}: @@ -184,6 +186,9 @@ julia> ŷ = evaloutput(kf) ``` """ function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real + if estim.direct && !estim.corrected[] + @warn "preparestate! should be called before evaloutput with current estimators" + end validate_args(estim.model, d) ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d d0 .= d .- estim.model.dop diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index d54d66cc9..7d2f04c38 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -326,23 +326,16 @@ function init_estimate!(estim::InternalModel, model::LinModel{NT}, y0m, d0, u0) return nothing end -@doc raw""" - evalŷ(estim::InternalModel, d) -> ŷ - -Get [`InternalModel`](@ref) estimated output `ŷ`. - -[`InternalModel`](@ref) estimator needs current stochastic output ``\mathbf{ŷ_s}(k)`` to -estimate its outputs ``\mathbf{ŷ}(k)``. The method [`preparestate!`](@ref) store this value -inside `estim` object, it should be thus called before `evalŷ`. -""" -function evalŷ(estim::InternalModel, d) +# Compute estimated output with current stochastic estimate `estim.ŷs` for `InternalModel` +function evaloutput(estim::InternalModel, d) if !estim.corrected[] - error("InternalModel: preparestate! must be called before evalŷ") + @warn "preparestate! should be called before evaloutput with InternalModel" end + validate_args(estim.model, d) ŷ0d, d0 = estim.buffer.ŷ, estim.buffer.d d0 .= d .- estim.model.dop - h!(ŷ0d, estim.model, estim.x̂d, d0) - ŷ = ŷ0d + ĥ!(ŷ0d, estim, estim.model, estim.x̂0, d0) + ŷ = ŷ0d ŷ .+= estim.model.yop .+ estim.ŷs return ŷ end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index f18bfad57..2cbb1e711 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -87,7 +87,7 @@ struct MovingHorizonEstimator{ fx̄::Vector{NT} H̃::Hermitian{NT, Matrix{NT}} q̃::Vector{NT} - p::Vector{NT} + r::Vector{NT} P̂_0::Hermitian{NT, Matrix{NT}} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} @@ -128,9 +128,9 @@ struct MovingHorizonEstimator{ invP̄ = Hermitian(inv(P̂_0), :L) invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L) invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) - p = direct ? 0 : 1 + r = direct ? 0 : 1 E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( - model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, p + model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, r ) # dummy values (updated just before optimization): F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He) @@ -139,7 +139,7 @@ struct MovingHorizonEstimator{ ) nZ̃ = size(Ẽ, 2) # dummy values, updated before optimization: - H̃, q̃, p = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) + H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1) Z̃ = zeros(NT, nZ̃) X̂op = repeat(x̂op, He) X̂0, Y0m = zeros(NT, nx̂*He), zeros(NT, nym*He) @@ -159,7 +159,7 @@ struct MovingHorizonEstimator{ As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Ẽ, F, G, J, B, ẽx̄, fx̄, - H̃, q̃, p, + H̃, q̃, r, P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt, X̂op, X̂0, Y0m, U0, D0, Ŵ, x̂0arr_old, P̂arr_old, Nk, diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 3126f79f1..6ee80cc68 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -9,7 +9,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) estim.Nk .= 0 estim.H̃ .= 0 estim.q̃ .= 0 - estim.p .= 0 + estim.r .= 0 if estim.direct # add u0(-1) and d0(-1) to the data windows: estim.U0[1:estim.model.nu] .= u0 @@ -211,15 +211,15 @@ end @doc raw""" initpred!(estim::MovingHorizonEstimator, model::LinModel) -> nothing -Init quadratic optimization matrices `F, fx̄, H̃, q̃, p` for [`MovingHorizonEstimator`](@ref). +Init quadratic optimization matrices `F, fx̄, H̃, q̃, r` for [`MovingHorizonEstimator`](@ref). See [`init_predmat_mhe`](@ref) for the definition of the vectors ``\mathbf{F, f_x̄}``. It also inits `estim.optim` objective function, expressed as the quadratic general form: ```math - J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + p + J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r ``` in which ``\mathbf{Z̃} = [\begin{smallmatrix} ϵ \\ \mathbf{Z} \end{smallmatrix}]``. Note that -``p`` is useless at optimization but required to evaluate the objective minima ``J``. The +``r`` is useless at optimization but required to evaluate the objective minima ``J``. The Hessian ``\mathbf{H̃}`` matrix of the quadratic general form is not constant here because of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are: ```math @@ -232,7 +232,7 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are: \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C, \mathbf{0}, \mathbf{Q̂}_{N_k}^{-1}) \\ \mathbf{H̃} &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k}) \\ \mathbf{q̃} &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃} \\ - p &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} + r &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃} \end{aligned} ``` """ @@ -257,7 +257,7 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) M_Nk_ẼZ̃ = M_Nk*ẼZ̃ @views mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃) @views lmul!(2, estim.q̃[1:nZ̃]) - estim.p .= dot(FZ̃, M_Nk, FZ̃) + estim.r .= dot(FZ̃, M_Nk, FZ̃) estim.H̃.data[1:nZ̃, 1:nZ̃] .= Ñ_Nk @views mul!(estim.H̃.data[1:nZ̃, 1:nZ̃], ẼZ̃', M_Nk_ẼZ̃, 1, 1) @views lmul!(2, estim.H̃.data[1:nZ̃, 1:nZ̃]) @@ -460,7 +460,7 @@ It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the ob function at specific `Z̃` and `V̂` values. """ function obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃) - return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.p[] + return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[] end """ diff --git a/src/general.jl b/src/general.jl index e40bd448d..40f5e6264 100644 --- a/src/general.jl +++ b/src/general.jl @@ -44,6 +44,17 @@ end "Generate a block diagonal matrix repeating `n` times the matrix `A`." repeatdiag(A, n::Int) = kron(I(n), A) +"In-place version of `repeat` but for vectors only." +function repeat!(Y::Vector, a::Vector, n::Int) + na = length(a) + for i=0:n-1 + # stop if Y is too short, another clearer error is thrown later in the code: + na*(i+1) > length(Y) && break + Y[(1+na*i):(na*(i+1))] = a + end + return Y +end + "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/plot_sim.jl b/src/plot_sim.jl index 53c0f15b5..1b92736b3 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -290,7 +290,7 @@ function sim_closedloop!( u = sim_getu!(est_mpc, u_ry, d, ru) ud = u + u_step + u_noise.*randn(plant.nu) Y_data[:, i] .= y - Ŷ_data[:, i] .= evalŷ(estim, d) + Ŷ_data[:, i] .= evaloutput(estim, d) U_Ry_data[:, i] .= u_ry U_data[:, i] .= u Ud_data[:, i] .= ud diff --git a/src/precompile.jl b/src/precompile.jl index 653872aef..1e5ebb72e 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -9,35 +9,41 @@ y = model() mpc_im = setconstraint!(LinMPC(InternalModel(model)), ymin=[45, -Inf]) initstate!(mpc_im, model.uop, y) preparestate!(mpc_im, [55, 30]) +mpc_im.estim() u = mpc_im([55, 30]) sim!(mpc_im, 3) mpc_kf = setconstraint!(LinMPC(KalmanFilter(model)), ymin=[45, -Inf]) initstate!(mpc_kf, model.uop, model()) +preparestate!(mpc_kf, [55, 30]) mpc_kf.estim() u = mpc_kf([55, 30]) sim!(mpc_kf, 3, [55, 30]) mpc_lo = setconstraint!(LinMPC(Luenberger(model)), ymin=[45, -Inf]) initstate!(mpc_lo, model.uop, model()) +preparestate!(mpc_lo, [55, 30]) mpc_lo.estim() u = mpc_lo([55, 30]) sim!(mpc_lo, 3, [55, 30]) mpc_ukf = setconstraint!(LinMPC(UnscentedKalmanFilter(model)), ymin=[45, -Inf]) initstate!(mpc_ukf, model.uop, model()) +preparestate!(mpc_ukf, [55, 30]) mpc_ukf.estim() u = mpc_ukf([55, 3]) sim!(mpc_ukf, 3, [55, 30]) mpc_ekf = setconstraint!(LinMPC(ExtendedKalmanFilter(model)), ymin=[45, -Inf]) initstate!(mpc_ekf, model.uop, model()) +preparestate!(mpc_ekf, [55, 30]) mpc_ekf.estim() u = mpc_ekf([55, 30]) sim!(mpc_ekf, 3, [55, 30]) mpc_skf = setconstraint!(LinMPC(SteadyKalmanFilter(model)), ymin=[45, -Inf]) initstate!(mpc_skf, model.uop, model()) +preparestate!(mpc_skf, [55, 30]) mpc_skf.estim() u = mpc_skf([55, 30]) sim!(mpc_skf, 3, [55, 30]) @@ -45,12 +51,14 @@ sim!(mpc_skf, 3, [55, 30]) mpc_mhe = setconstraint!(LinMPC(MovingHorizonEstimator(model, He=2)), ymin=[45, -Inf]) setconstraint!(mpc_mhe.estim, x̂min=[-50,-50,-50,-50], x̂max=[50,50,50,50]) initstate!(mpc_mhe, model.uop, model()) +preparestate!(mpc_mhe, [55, 30]) mpc_mhe.estim() u = mpc_mhe([55, 30]) sim!(mpc_mhe, 3, [55, 30]) nmpc_skf = setconstraint!(NonLinMPC(SteadyKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf]) initstate!(nmpc_skf, model.uop, model()) +preparestate!(nmpc_skf, [55, 30]) nmpc_skf.estim() u = nmpc_skf([55, 30]) sim!(nmpc_skf, 3, [55, 30]) @@ -61,6 +69,7 @@ res_man = SimResult(model, res.U_data, res.Y_data; X_data=res.X_data) exmpc = ExplicitMPC(model) initstate!(exmpc, model.uop, model()) +preparestate!(exmpc, [55, 30]) exmpc.estim() u = exmpc([55, 30]) sim!(exmpc, 3, [55, 30]) @@ -73,22 +82,26 @@ y = nlmodel() nmpc_im = setconstraint!(NonLinMPC(InternalModel(nlmodel), Hp=10, Cwt=Inf), ymin=[45, -Inf]) initstate!(nmpc_im, nlmodel.uop, y) preparestate!(nmpc_im, [55, 30]) +nmpc_im.estim() u = nmpc_im([55, 30]) sim!(nmpc_im, 3, [55, 30]) nmpc_ukf = setconstraint!(NonLinMPC(UnscentedKalmanFilter(nlmodel), Hp=10, Cwt=1e3), ymin=[45, -Inf]) initstate!(nmpc_ukf, nlmodel.uop, y) +preparestate!(nmpc_ukf, [55, 30]) u = nmpc_ukf([55, 30]) sim!(nmpc_ukf, 3, [55, 30]) nmpc_ekf = setconstraint!(NonLinMPC(ExtendedKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf]) initstate!(nmpc_ekf, model.uop, model()) +preparestate!(nmpc_ekf, [55, 30]) u = nmpc_ekf([55, 30]) sim!(nmpc_ekf, 3, [55, 30]) nmpc_mhe = setconstraint!(NonLinMPC(MovingHorizonEstimator(nlmodel, He=2), Hp=10, Cwt=Inf), ymin=[45, -Inf]) setconstraint!(nmpc_mhe.estim, x̂min=[-50,-50,-50,-50], x̂max=[50,50,50,50]) initstate!(nmpc_mhe, nlmodel.uop, y) +preparestate!(nmpc_mhe, [55, 30]) u = nmpc_mhe([55, 30]) sim!(nmpc_mhe, 3, [55, 30]) @@ -98,5 +111,6 @@ function JE( _ , ŶE, _ ) return dot(Eŷ, I, Eŷ) end empc = setconstraint!(NonLinMPC(nlmodel, Mwt=[0, 0], Hp=10, Cwt=Inf, Ewt=1, JE=JE), ymin=[45, -Inf]) +preparestate!(empc, [55, 30]) u = empc() sim!(empc, 3) \ No newline at end of file diff --git a/src/predictive_control.jl b/src/predictive_control.jl index b6bb27546..54bc7186d 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -9,7 +9,7 @@ Functor allowing callable `PredictiveController` object as an alias for [`movein # Examples ```jldoctest -julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1); +julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1, direct=false); julia> u = mpc([5]); round.(u, digits=3) 1-element Vector{Float64}: @@ -19,6 +19,28 @@ julia> u = mpc([5]); round.(u, digits=3) """ abstract type PredictiveController{NT<:Real} end +struct PredictiveControllerBuffer{NT<:Real} + u ::Vector{NT} + R̂y::Vector{NT} + D̂ ::Vector{NT} + empty::Vector{NT} +end + +@doc raw""" + PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int) + +Create a buffer for `PredictiveController` objects. + +The buffer is used to store intermediate results during computation without allocating. +""" +function PredictiveControllerBuffer{NT}(nu::Int, ny::Int, nd::Int, Hp::Int) where NT <: Real + u = Vector{NT}(undef, nu) + R̂y = Vector{NT}(undef, ny*Hp) + D̂ = Vector{NT}(undef, nd*Hp) + empty = Vector{NT}(undef, 0) + return PredictiveControllerBuffer{NT}(u, R̂y, D̂, empty) +end + include("controller/construct.jl") include("controller/execute.jl") include("controller/explicitmpc.jl") diff --git a/src/sim_model.jl b/src/sim_model.jl index 39f17f60f..37927b142 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -29,7 +29,7 @@ struct SimModelBuffer{NT<:Real} end @doc raw""" - SimModelBuffer(nu::Int, nx::Int, ny::Int, nd::Int) -> SimModelBuffer{NT} + SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int) Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances. diff --git a/src/state_estim.jl b/src/state_estim.jl index 0c801ebe1..b2cb2f51d 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -9,7 +9,7 @@ Functor allowing callable `StateEstimator` object as an alias for [`evaloutput`] # Examples ```jldoctest -julia> kf = KalmanFilter(setop!(LinModel(tf(3, [10, 1]), 2), yop=[20])); +julia> kf = KalmanFilter(setop!(LinModel(tf(3, [10, 1]), 2), yop=[20]), direct=false); julia> ŷ = kf() 1-element Vector{Float64}: @@ -33,11 +33,11 @@ struct StateEstimatorBuffer{NT<:Real} end @doc raw""" - StateEstimatorBuffer(nx̂::Int, nym::Int) -> StateEstimatorBuffer{NT} + StateEstimatorBuffer{NT}(nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int) Create a buffer for `StateEstimator` objects for estimated states and measured outputs. -The buffer is used to store intermediate results during simulation without allocating. +The buffer is used to store intermediate results during estimation without allocating. """ function StateEstimatorBuffer{NT}( nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int @@ -83,12 +83,4 @@ include("estimator/execute.jl") include("estimator/kalman.jl") include("estimator/luenberger.jl") include("estimator/mhe.jl") -include("estimator/internal_model.jl") - -""" - evalŷ(estim::StateEstimator, d) -> ŷ - -Evaluate [`StateEstimator`](@ref) output `ŷ` from measured disturbance `d` and `estim.x̂0`. -""" -evalŷ(estim::StateEstimator, d) = evaloutput(estim, d) - \ No newline at end of file +include("estimator/internal_model.jl") \ No newline at end of file diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index 993994f7e..edfb46897 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -54,6 +54,7 @@ end linmodel = setop!(LinModel(tf(5, [2, 1]), 3), yop=[10]) mpc1 = LinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1) r = [15] + preparestate!(mpc1, [10]) u = moveinput!(mpc1, r) @test u ≈ [1] atol=1e-2 u = mpc1(r) @@ -62,13 +63,16 @@ end @test info[:u] ≈ u @test info[:Ŷ][end] ≈ r[1] atol=1e-2 mpc2 = LinMPC(linmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1) + preparestate!(mpc2, [10]) u = moveinput!(mpc2, r) @test u ≈ [1] atol=1e-2 mpc3 = LinMPC(linmodel, Mwt=[0], Nwt=[0], Lwt=[1]) + preparestate!(mpc3, [10]) u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp)) @test u ≈ [12] atol=1e-2 model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mpc4 = LinMPC(model2) + preparestate!(mpc4, [0]) moveinput!(mpc4, [0]) ≈ [0.0] @test_throws DimensionMismatch moveinput!(mpc1, [0,0,0]) @@ -127,10 +131,10 @@ end setstate!(mpc1, [1,2,3,4]) @test mpc1.estim.x̂0 ≈ [1,2,3,4] setstate!(mpc1, [0,0,0,0]) - preparestate!(mpc1, mpc1.estim()) - updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim()) + preparestate!(mpc1, [50, 30]) + updatestate!(mpc1, mpc1.estim.model.uop, [50, 30]) @test mpc1.estim.x̂0 ≈ [0,0,0,0] - preparestate!(mpc1, mpc1.estim()) + preparestate!(mpc1, [50, 30]) @test_throws ArgumentError updatestate!(mpc1, [0,0]) end @@ -187,6 +191,7 @@ end @test_throws ArgumentError setconstraint!(mpc, c_ymin=[0,0,0]) @test_throws ArgumentError setconstraint!(mpc, c_ymax=[0,0,0]) + 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]) @test_throws ErrorException setconstraint!(mpc, umin=[-Inf,-Inf], umax=[+Inf,+Inf]) @@ -208,6 +213,7 @@ end setconstraint!(mpc, umin=[-3], umax=[3]) setconstraint!(mpc, Δumin=[-1.5], Δumax=[1.5]) setconstraint!(mpc, ymin=[-100], ymax=[100]) + preparestate!(mpc, [0]) moveinput!(mpc, [-10]) info = getinfo(mpc) @test info[:ΔU][begin] ≈ -1.5 atol=1e-1 @@ -276,6 +282,7 @@ end @test mpc.con.Y0min ≈ fill(-54.0 -10, 1000) @test mpc.con.Y0max ≈ fill(56.0 -10, 1000) r = [15] + 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])) @@ -346,6 +353,7 @@ end @testset "ExplicitMPC moves and getinfo" begin mpc1 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1) r = [5] + preparestate!(mpc1, [0]) u = moveinput!(mpc1, r) @test u ≈ [1] atol=1e-2 u = mpc1(r) @@ -354,13 +362,16 @@ end @test info[:u] ≈ u @test info[:Ŷ][end] ≈ r[1] atol=1e-2 mpc2 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1000, Hc=1) + preparestate!(mpc2, [0]) u = moveinput!(mpc2, [5]) @test u ≈ [1] atol=1e-2 mpc3 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1]) + preparestate!(mpc3, [0]) u = moveinput!(mpc3, [0], R̂u=fill(12, mpc3.Hp)) @test u ≈ [12] atol=1e-2 model2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mpc4 = ExplicitMPC(model2) + preparestate!(mpc4, [0]) moveinput!(mpc4, [0]) ≈ [0.0] end @@ -414,10 +425,10 @@ end setstate!(mpc1, [1,2,3,4]) @test mpc1.estim.x̂0 ≈ [1,2,3,4] setstate!(mpc1, [0,0,0,0]) - preparestate!(mpc1, mpc1.estim()) - updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim()) + preparestate!(mpc1, [50, 30]) + updatestate!(mpc1, mpc1.estim.model.uop, [50, 30]) @test mpc1.estim.x̂0 ≈ [0,0,0,0] - preparestate!(mpc1, mpc1.estim()) + preparestate!(mpc1, [50, 30]) @test_throws ArgumentError updatestate!(mpc1, [0,0]) end @@ -433,6 +444,7 @@ end @test mpc.Yop ≈ fill(10.0, 1000) @test mpc.Uop ≈ fill(1.0, 1000) r = [15] + 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])) @@ -512,6 +524,7 @@ end linmodel = setop!(LinModel(tf(5, [2000, 1]), 3000.0), yop=[10]) nmpc_lin = NonLinMPC(linmodel, Nwt=[0], Hp=1000, Hc=1) r = [15] + preparestate!(nmpc_lin, [10]) u = moveinput!(nmpc_lin, r) @test u ≈ [1] atol=5e-2 u = nmpc_lin(r) @@ -523,16 +536,18 @@ end R̂y = fill(r[1], Hp) JE = (_ , ŶE, _ ) -> sum((ŶE[2:end] - R̂y).^2) nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, Hp=Hp, Hc=1) + preparestate!(nmpc, [10]) u = moveinput!(nmpc) @test u ≈ [1] atol=5e-2 # ensure that the current estimated output is updated for correct JE values: - @test nmpc.ŷ ≈ ModelPredictiveControl.evalŷ(nmpc.estim, Float64[]) + @test nmpc.ŷ ≈ evaloutput(nmpc.estim, Float64[]) linmodel2 = LinModel([tf(5, [2000, 1]) tf(7, [8000,1])], 3000.0, i_d=[2]) f = (x,u,d) -> linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d h = (x,d) -> linmodel2.C*x + linmodel2.Dd*d nonlinmodel = NonLinModel(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1) d = [0.1] + preparestate!(nmpc2, [0], [0]) u = moveinput!(nmpc2, 7d, d) @test u ≈ [0] atol=5e-2 u = nmpc2(7d, d) @@ -541,9 +556,11 @@ end @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 7d[1] atol=5e-2 nmpc3 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=1000, 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 = setconstraint!(NonLinMPC(nonlinmodel, Hp=15, Cwt=Inf), ymin=[1]) @@ -554,11 +571,13 @@ end @test ForwardDiff.gradient(vec->g_Y0min_end(vec...), [20.0, 10.0]) ≈ [-5, -5] atol=1e-3 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) nmpc6 = NonLinMPC(linmodel3, Hp=10) + preparestate!(nmpc6, [0]) @test moveinput!(nmpc6, [0]) ≈ [0.0] nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing) nmpc7 = NonLinMPC(nonlinmodel2, Hp=10) y = similar(nonlinmodel2.yop) nonlinmodel2.h!(y, Float32[0,0], Float32[0]) + preparestate!(nmpc7, [0], [0]) @test moveinput!(nmpc7, [0], [0]) ≈ [0.0] end @@ -608,14 +627,16 @@ end linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) f = (x,u,_) -> linmodel.A*x + linmodel.Bu*u h = (x,_) -> linmodel.C*x - nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing) + nonlinmodel = setop!( + NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30] + ) nmpc1 = NonLinMPC(nonlinmodel, Hp=15) @test initstate!(nmpc1, [10, 50], [20, 25]) ≈ zeros(4) setstate!(nmpc1, [1,2,3,4]) @test nmpc1.estim.x̂0 ≈ [1,2,3,4] setstate!(nmpc1, [0,0,0,0]) - preparestate!(nmpc1, nmpc1.estim()) - updatestate!(nmpc1, nmpc1.estim.model.uop, nmpc1.estim()) + preparestate!(nmpc1, [50, 30]) + updatestate!(nmpc1, nmpc1.estim.model.uop, [50, 30]) @test nmpc1.estim.x̂0 ≈ [0,0,0,0] atol=1e-6 end @@ -662,6 +683,7 @@ end setconstraint!(nmpc_lin, umin=[-3], umax=[3]) setconstraint!(nmpc_lin, Δumin=[-1.5], Δumax=[1.5]) setconstraint!(nmpc_lin, ymin=[-100], ymax=[100]) + preparestate!(nmpc_lin, [0]) moveinput!(nmpc_lin, [-20]) info = getinfo(nmpc_lin) @test info[:ΔU][begin] ≈ -1.5 atol=1e-2 @@ -699,6 +721,7 @@ end setconstraint!(nmpc, umin=[-3], umax=[3]) setconstraint!(nmpc, Δumin=[-1.5], Δumax=[1.5]) setconstraint!(nmpc, ymin=[-100], ymax=[100]) + preparestate!(nmpc, [0]) moveinput!(nmpc, [-20]) info = getinfo(nmpc) @test info[:ΔU][begin] ≈ -1.5 atol=1e-2 @@ -740,6 +763,7 @@ end @test mpc.con.Y0min ≈ fill(-54.0 -10, 1000) @test mpc.con.Y0max ≈ fill(56.0 -10, 1000) r = [15] + 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])) diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index d3d8da404..906ef3d77 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -72,6 +72,7 @@ end preparestate!(skalmanfilter1, [50, 30]) @test updatestate!(skalmanfilter1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) @test skalmanfilter1.x̂0 ≈ zeros(4) + preparestate!(skalmanfilter1, [50, 30]) @test evaloutput(skalmanfilter1) ≈ skalmanfilter1() ≈ [50, 30] @test evaloutput(skalmanfilter1, Float64[]) ≈ skalmanfilter1(Float64[]) ≈ [50, 30] @test initstate!(skalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] @@ -86,11 +87,13 @@ end preparestate!(skalmanfilter1, [50, 30]) updatestate!(skalmanfilter1, [11, 52], [50, 30]) end + preparestate!(skalmanfilter1, [50, 30]) @test skalmanfilter1() ≈ [50, 30] atol=1e-3 for i in 1:40 preparestate!(skalmanfilter1, [51, 32]) updatestate!(skalmanfilter1, [10, 50], [51, 32]) end + preparestate!(skalmanfilter1, [51, 32]) @test skalmanfilter1() ≈ [51, 32] atol=1e-3 skalmanfilter2 = SteadyKalmanFilter(linmodel1, nint_u=[1, 1], direct=false) for i in 1:40 @@ -191,6 +194,7 @@ end preparestate!(kalmanfilter1, [50, 30]) @test updatestate!(kalmanfilter1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) @test kalmanfilter1.x̂0 ≈ zeros(4) + preparestate!(kalmanfilter1, [50, 30]) @test evaloutput(kalmanfilter1) ≈ kalmanfilter1() ≈ [50, 30] @test evaloutput(kalmanfilter1, Float64[]) ≈ kalmanfilter1(Float64[]) ≈ [50, 30] @test initstate!(kalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] @@ -200,11 +204,13 @@ end preparestate!(kalmanfilter1, [50, 30]) updatestate!(kalmanfilter1, [11, 52], [50, 30]) end + preparestate!(kalmanfilter1, [50, 30]) @test kalmanfilter1() ≈ [50, 30] atol=1e-3 for i in 1:40 preparestate!(kalmanfilter1, [51, 32]) updatestate!(kalmanfilter1, [10, 50], [51, 32]) end + preparestate!(kalmanfilter1, [51, 32]) @test kalmanfilter1() ≈ [51, 32] atol=1e-3 kalmanfilter2 = KalmanFilter(linmodel1, nint_u=[1, 1], direct=false) for i in 1:40 @@ -231,6 +237,7 @@ end linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) kalmanfilter = KalmanFilter(linmodel, nint_ym=0) @test kalmanfilter. ≈ [0.5] + preparestate!(kalmanfilter, [50.0]) @test evaloutput(kalmanfilter) ≈ [50.0] preparestate!(kalmanfilter, [50.0]) x̂ = updatestate!(kalmanfilter, [2.0], [50.0]) @@ -239,6 +246,7 @@ end newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[3.0], fop=[3.0]) setmodel!(kalmanfilter, newlinmodel) @test kalmanfilter. ≈ [0.2] + preparestate!(kalmanfilter, [55.0]) @test evaloutput(kalmanfilter) ≈ [55.0] @test kalmanfilter.lastu0 ≈ [2.0 - 3.0] preparestate!(kalmanfilter, [55.0]) @@ -300,6 +308,7 @@ end preparestate!(lo1, [50, 30]) @test updatestate!(lo1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) @test lo1.x̂0 ≈ zeros(4) + preparestate!(lo1, [50, 30]) @test evaloutput(lo1) ≈ lo1() ≈ [50, 30] @test evaloutput(lo1, Float64[]) ≈ lo1(Float64[]) ≈ [50, 30] @test initstate!(lo1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] @@ -309,11 +318,13 @@ end preparestate!(lo1, [50, 30]) updatestate!(lo1, [11, 52], [50, 30]) end + preparestate!(lo1, [50, 30]) @test lo1() ≈ [50, 30] atol=1e-3 for i in 1:40 preparestate!(lo1, [51, 32]) updatestate!(lo1, [10, 50], [51, 32]) end + preparestate!(lo1, [51, 32]) @test lo1() ≈ [51, 32] atol=1e-3 lo2 = Luenberger(linmodel1, nint_u=[1, 1], direct=false) for i in 1:40 @@ -420,19 +431,25 @@ end @test internalmodel1.x̂d ≈ internalmodel1.x̂0 ≈ zeros(2) @test internalmodel1.x̂s ≈ ones(2) preparestate!(internalmodel1, [51, 31]) - @test ModelPredictiveControl.evalŷ(internalmodel1, Float64[]) ≈ [51,31] + @test evaloutput(internalmodel1, Float64[]) ≈ [51,31] @test initstate!(internalmodel1, [10, 50], [50, 30]) ≈ zeros(2) - linmodel2 = LinModel(append(tf(3, [5, 1]), tf(2, [10, 1])), 1.0) - stoch_ym = append(tf([2.5, 1],[1.2, 1, 0]),tf([1.5, 1], [1.3, 1, 0])) - estim = InternalModel(linmodel2; stoch_ym) - initstate!(estim, [1, 2], [3+0.1, 4+0.5]) - @test estim.x̂d ≈ estim.Â*estim.x̂d + estim.B̂u*[1, 2] - ŷs = [3+0.1, 4+0.5] - estim() - @test estim.x̂s ≈ estim.Âs*estim.x̂s + estim.B̂s*ŷs @test internalmodel1.x̂s ≈ zeros(2) setstate!(internalmodel1, [1,2]) @test internalmodel1.x̂0 ≈ [1,2] - + linmodel2 = LinModel(append(tf(3, [5, 1]), tf(2, [10, 1])), 1.0) + stoch_ym = append(tf([2.5, 1],[1.2, 1, 0]),tf([1.5, 1], [1.3, 1, 0])) + internalmodel2 = InternalModel(linmodel2; stoch_ym) + initstate!(internalmodel2, [1, 2], [3+0.1, 4+0.5]) + @test internalmodel2.x̂d ≈ internalmodel2.Â*internalmodel2.x̂d + internalmodel2.B̂u*[1, 2] + preparestate!(internalmodel2, [3+0.1, 4+0.5]) + ŷ = evaloutput(internalmodel2) + @test ŷ ≈ [3+0.1, 4+0.5] + x̂s = internalmodel2.x̂s + @test x̂s ≈ internalmodel2.Âs*x̂s + internalmodel2.B̂s*internalmodel2.ŷs + updatestate!(internalmodel2, [1, 2], [-13, -14]) + preparestate!(internalmodel2, [13, 14]) + ŷ = internalmodel2() + @test ŷ ≈ [13, 14] linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) internalmodel3 = InternalModel(linmodel3) preparestate!(internalmodel3, [0]) @@ -446,6 +463,7 @@ end linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) internalmodel = InternalModel(linmodel) @test internalmodel. ≈ [0.5] + preparestate!(internalmodel, [50.0]) @test evaloutput(internalmodel) ≈ [50.0] preparestate!(internalmodel, [50.0]) x̂ = updatestate!(internalmodel, [2.0], [50.0]) @@ -454,6 +472,7 @@ end newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[3.0], fop=[3.0]) setmodel!(internalmodel, newlinmodel) @test internalmodel. ≈ [0.2] + preparestate!(internalmodel, [55.0]) @test evaloutput(internalmodel) ≈ [55.0] @test internalmodel.lastu0 ≈ [2.0 - 3.0] preparestate!(internalmodel, [55.0]) @@ -532,6 +551,7 @@ end preparestate!(ukf1, [50, 30]) @test updatestate!(ukf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9 @test ukf1.x̂0 ≈ zeros(4) atol=1e-9 + preparestate!(ukf1, [50, 30]) @test evaloutput(ukf1) ≈ ukf1() ≈ [50, 30] @test evaloutput(ukf1, Float64[]) ≈ ukf1(Float64[]) ≈ [50, 30] @test initstate!(ukf1, [10, 50], [50, 30+1]) ≈ zeros(4) atol=1e-9 @@ -541,11 +561,13 @@ end preparestate!(ukf1, [50, 30]) updatestate!(ukf1, [11, 52], [50, 30]) end + preparestate!(ukf1, [50, 30]) @test ukf1() ≈ [50, 30] atol=1e-3 for i in 1:40 preparestate!(ukf1, [51, 32]) updatestate!(ukf1, [10, 50], [51, 32]) end + preparestate!(ukf1, [51, 32]) @test ukf1() ≈ [51, 32] atol=1e-3 ukf2 = UnscentedKalmanFilter(linmodel1, nint_u=[1, 1], nint_ym=[0, 0], direct=false) for i in 1:40 @@ -571,6 +593,7 @@ end linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) ukf1 = UnscentedKalmanFilter(linmodel, nint_ym=0) @test ukf1. ≈ [0.5] + preparestate!(ukf1, [50.0]) @test evaloutput(ukf1) ≈ [50.0] preparestate!(ukf1, [50.0]) x̂ = updatestate!(ukf1, [2.0], [50.0]) @@ -579,6 +602,7 @@ end newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[3.0], fop=[3.0]) setmodel!(ukf1, newlinmodel) @test ukf1. ≈ [0.2] + preparestate!(ukf1, [55.0]) @test evaloutput(ukf1) ≈ [55.0] @test ukf1.lastu0 ≈ [2.0 - 3.0] preparestate!(ukf1, [55.0]) @@ -664,6 +688,7 @@ end preparestate!(ekf1, [50, 30]) @test updatestate!(ekf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9 @test ekf1.x̂0 ≈ zeros(4) atol=1e-9 + preparestate!(ekf1, [50, 30]) @test evaloutput(ekf1) ≈ ekf1() ≈ [50, 30] @test evaloutput(ekf1, Float64[]) ≈ ekf1(Float64[]) ≈ [50, 30] @test initstate!(ekf1, [10, 50], [50, 30+1]) ≈ zeros(4); @@ -673,11 +698,13 @@ end preparestate!(ekf1, [50, 30]) updatestate!(ekf1, [11, 52], [50, 30]) end + preparestate!(ekf1, [50, 30]) @test ekf1() ≈ [50, 30] atol=1e-3 for i in 1:40 preparestate!(ekf1, [51, 32]) updatestate!(ekf1, [10, 50], [51, 32]) end + preparestate!(ekf1, [51, 32]) @test ekf1() ≈ [51, 32] atol=1e-3 ekf2 = ExtendedKalmanFilter(linmodel1, nint_u=[1, 1], nint_ym=[0, 0], direct=false) for i in 1:40 @@ -703,6 +730,7 @@ end linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) ekf1 = ExtendedKalmanFilter(linmodel, nint_ym=0) @test ekf1. ≈ [0.5] + preparestate!(ekf1, [50.0]) @test evaloutput(ekf1) ≈ [50.0] preparestate!(ekf1, [50.0]) x̂ = updatestate!(ekf1, [2.0], [50.0]) @@ -711,6 +739,7 @@ end newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[3.0], fop=[3.0]) setmodel!(ekf1, newlinmodel) @test ekf1. ≈ [0.2] + preparestate!(ekf1, [55.0]) @test evaloutput(ekf1) ≈ [55.0] @test ekf1.lastu0 ≈ [2.0 - 3.0] preparestate!(ekf1, [55.0]) @@ -824,6 +853,7 @@ end x̂ = updatestate!(mhe1, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9 @test mhe1.x̂0 ≈ zeros(6) atol=1e-9 + preparestate!(mhe1, [50, 30], [5]) @test evaloutput(mhe1, [5]) ≈ mhe1([5]) ≈ [50, 30] info = getinfo(mhe1) @test info[:x̂] ≈ x̂ atol=1e-9 @@ -836,11 +866,13 @@ end preparestate!(mhe1, [50, 30], [5]) updatestate!(mhe1, [11, 52], [50, 30], [5]) end + preparestate!(mhe1, [50, 30], [5]) @test mhe1([5]) ≈ [50, 30] atol=1e-3 for i in 1:40 preparestate!(mhe1, [51, 32], [5]) updatestate!(mhe1, [10, 50], [51, 32], [5]) end + preparestate!(mhe1, [51, 32], [5]) @test mhe1([5]) ≈ [51, 32] atol=1e-3 mhe1 = MovingHorizonEstimator(nonlinmodel, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false) @@ -872,6 +904,7 @@ end 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]) @test evaloutput(mhe2, [5]) ≈ mhe2([5]) ≈ [50, 30] info = getinfo(mhe2) @test info[:x̂] ≈ x̂ atol=1e-9 @@ -880,11 +913,13 @@ end 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(linmodel1, He=2, nint_u=[1, 1], nint_ym=[0, 0], direct=false) @@ -929,6 +964,7 @@ end x̂ = updatestate!(mhe5, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(4) atol=1e-9 @test mhe5.x̂0 ≈ zeros(4) atol=1e-9 + preparestate!(mhe5, [50, 30], [5]) @test evaloutput(mhe5, [5]) ≈ mhe5([5]) ≈ [50, 30] info = getinfo(mhe5) @test info[:x̂] ≈ x̂ atol=1e-9