diff --git a/src/controller/execute.jl b/src/controller/execute.jl index b598462eb..7b648bce0 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -1,18 +1,15 @@ @doc raw""" - initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop) -> x̂ + initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂ Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero. """ -function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop) +function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) mpc.ΔŨ .= 0 return initstate!(mpc.estim, u, ym, d) end @doc raw""" - moveinput!( - mpc::PredictiveController, ry=mpc.estim.model.yop, d=mpc.estim.model.dop; - - ) -> u + moveinput!(mpc::PredictiveController, ry=mpc.estim.model.yop, d=[]; ) -> u Compute the optimal manipulated input value `u` for the current control period. @@ -32,7 +29,7 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref). - `mpc::PredictiveController` : solve optimization problem of `mpc`. - `ry=mpc.estim.model.yop` : current output setpoints ``\mathbf{r_y}(k)``. -- `d=mpc.estim.model.dop` : current measured disturbances ``\mathbf{d}(k)``. +- `d=[]` : current measured disturbances ``\mathbf{d}(k)``. - `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}``, constant in the future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``. - `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}``, constant @@ -54,7 +51,7 @@ 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.model.dop; + d ::Vector = mpc.estim.buffer.empty; Dhat ::Vector = repeat(d, mpc.Hp), Rhaty::Vector = repeat(ry, mpc.Hp), Rhatu::Vector = mpc.Uop, @@ -508,11 +505,11 @@ end set_objective_linear_coef!(::PredictiveController, _ ) = nothing """ - updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop) -> x̂ + updatestate!(mpc::PredictiveController, u, ym, d=[]) -> x̂ Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). """ -function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.dop) +function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) return updatestate!(mpc.estim, u, ym, d) end updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym")) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index c52b47362..a5e56fe59 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -7,11 +7,13 @@ Also store current inputs without operating points `u0` in `estim.lastu0`. This used for [`PredictiveController`](@ref) computations. """ function remove_op!(estim::StateEstimator, u, ym, d) - u0 = u - estim.model.uop - ym0 = ym - estim.model.yop[estim.i_ym] - d0 = d - estim.model.dop - estim.lastu0[:] = u0 - return u0, ym0, d0 + u0, d0 = estim.buffer.u, estim.buffer.d + y0m = estim.buffer.ym + u0 .= u .- estim.model.uop + y0m .= @views ym .- estim.model.yop[estim.i_ym] + d0 .= d .- estim.model.dop + estim.lastu0 .= u0 + return u0, y0m, d0 end @doc raw""" @@ -79,7 +81,7 @@ end @doc raw""" - initstate!(estim::StateEstimator, u, ym, d=estim.model.dop) -> x̂ + initstate!(estim::StateEstimator, u, ym, d=[]) -> x̂ Init `estim.x̂0` states from current inputs `u`, measured outputs `ym` and disturbances `d`. @@ -111,7 +113,7 @@ julia> evaloutput(estim) ≈ y true ``` """ -function initstate!(estim::StateEstimator, u, ym, d=estim.model.dop) +function initstate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) # --- validate arguments --- validate_args(estim, u, ym, d) # --- init state estimate ---- @@ -150,6 +152,7 @@ measured outputs ``\mathbf{y^m}``. function init_estimate!(estim::StateEstimator, ::LinModel, u0, ym0, d0) Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d Ĉm, D̂dm = @views Ĉ[estim.i_ym, :], D̂d[estim.i_ym, :] # measured outputs ym only + # TODO: use estim.buffer.x̂ to reduce allocations estim.x̂0 .= [I - Â; Ĉm]\[B̂u*u0 + B̂d*d0 + estim.f̂op - estim.x̂op; ym0 - D̂dm*d0] return nothing end @@ -161,7 +164,7 @@ Left `estim.x̂0` estimate unchanged if `model` is not a [`LinModel`](@ref). init_estimate!(::StateEstimator, ::SimModel, _ , _ , _ ) = nothing @doc raw""" - evaloutput(estim::StateEstimator, d=estim.model.dop) -> ŷ + evaloutput(estim::StateEstimator, d=[]) -> ŷ Evaluate `StateEstimator` outputs `ŷ` from `estim.x̂0` states and disturbances `d`. @@ -176,10 +179,10 @@ julia> ŷ = evaloutput(kf) 20.0 ``` """ -function evaloutput(estim::StateEstimator{NT}, d=estim.model.dop) where NT <: Real +function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real validate_args(estim.model, d) - ŷ0 = Vector{NT}(undef, estim.model.ny) - d0 = d - estim.model.dop + ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d + d0 .= d .- estim.model.dop ĥ!(ŷ0, estim, estim.model, estim.x̂0, d0) ŷ = ŷ0 ŷ .+= estim.model.yop @@ -187,10 +190,10 @@ function evaloutput(estim::StateEstimator{NT}, d=estim.model.dop) where NT <: Re end "Functor allowing callable `StateEstimator` object as an alias for `evaloutput`." -(estim::StateEstimator)(d=estim.model.dop) = evaloutput(estim, d) +(estim::StateEstimator)(d=estim.buffer.empty) = evaloutput(estim, d) @doc raw""" - updatestate!(estim::StateEstimator, u, ym, d=estim.model.dop) -> x̂ + updatestate!(estim::StateEstimator, u, ym, d=[]) -> x̂ Update `estim.x̂0` estimate with current inputs `u`, measured outputs `ym` and dist. `d`. @@ -207,10 +210,10 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y 0.0 ``` """ -function updatestate!(estim::StateEstimator, u, ym, d=estim.model.dop) +function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) validate_args(estim, u, ym, d) u0, ym0, d0 = remove_op!(estim, u, ym, d) - x̂0next = update_estimate!(estim, u0, ym0, d0) + x̂0next = update_estimate!(estim, u0, ym0, d0) x̂next = x̂0next x̂next .+= estim.x̂op return x̂next diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 190a08f22..8ba4f86d5 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -22,9 +22,11 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} D̂d::Matrix{NT} Âs::Matrix{NT} B̂s::Matrix{NT} + buffer::StateEstimatorBuffer{NT} function InternalModel{NT, SM}( model::SM, i_ym, Asm, Bsm, Csm, Dsm ) where {NT<:Real, SM<:SimModel} + nu, ny, nd = model.nu, model.ny, model.nd nym, nyu = validate_ym(model, i_ym) validate_internalmodel(model, nym, Csm, Dsm) As, Bs, Cs, Ds = stoch_ym2y(model, i_ym, Asm, Bsm, Csm, Dsm) @@ -32,17 +34,19 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} nx̂ = model.nx Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model) Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds) - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) # x̂0 and x̂d are same object (updating x̂d will update x̂0): x̂d = x̂0 = zeros(NT, model.nx) x̂s = zeros(NT, nxs) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, i_ym, nx̂, nym, nyu, nxs, As, Bs, Cs, Ds, Â, B̂u, Ĉ, B̂d, D̂d, - Âs, B̂s + Âs, B̂s, + buffer ) end end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index d50f2084a..71088176c 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -22,9 +22,11 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} K̂::Matrix{NT} + buffer::StateEstimatorBuffer{NT} function SteadyKalmanFilter{NT, SM}( model::SM, i_ym, nint_u, nint_ym, Q̂, R̂ ) where {NT<:Real, SM<:LinModel} + nu, ny, nd = model.nu, model.ny, model.nd nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -33,7 +35,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} validate_kfcov(nym, nx̂, Q̂, R̂) K̂ = try Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6 - R̂_kalman = zeros(NT, model.ny, model.ny) + R̂_kalman = zeros(NT, ny, ny) R̂_kalman[i_ym, i_ym] = R̂ ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂_kalman, R̂_kalman)[:, i_ym] catch my_error @@ -45,9 +47,10 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} rethrow() end end - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, @@ -55,7 +58,8 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, Q̂, R̂, - K̂ + K̂, + buffer ) end end @@ -234,21 +238,24 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} R̂::Hermitian{NT, Matrix{NT}} K̂::Matrix{NT} M̂::Matrix{NT} + buffer::StateEstimatorBuffer{NT} function KalmanFilter{NT, SM}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂ ) where {NT<:Real, SM<:LinModel} + nu, ny, nd = model.nu, model.ny, model.nd nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) nx̂ = model.nx + nxs Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) P̂ = copy(P̂_0) K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -256,7 +263,8 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, P̂_0, Q̂, R̂, - K̂, M̂ + K̂, M̂, + buffer ) end end @@ -402,9 +410,11 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} γ::NT m̂::Vector{NT} Ŝ::Diagonal{NT, Vector{NT}} + buffer::StateEstimatorBuffer{NT} function UnscentedKalmanFilter{NT, SM}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ ) where {NT<:Real, SM<:SimModel{NT}} + nu, ny, nd = model.nu, model.ny, model.nd nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -412,7 +422,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ) - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂_0 = Hermitian(P̂_0, :L) @@ -421,6 +431,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} M̂ = Hermitian(zeros(NT, nym, nym), :L) X̂0, Ŷ0m = zeros(NT, nx̂, nσ), zeros(NT, nym, nσ) sqrtP̂ = LowerTriangular(zeros(NT, nx̂, nx̂)) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -429,7 +440,8 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Â, B̂u, Ĉ, B̂d, D̂d, P̂_0, Q̂, R̂, K̂, M̂, X̂0, Ŷ0m, sqrtP̂, - nσ, γ, m̂, Ŝ + nσ, γ, m̂, Ŝ, + buffer ) end end @@ -698,23 +710,26 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} M̂::Matrix{NT} F̂_û::Matrix{NT} Ĥ ::Matrix{NT} + buffer::StateEstimatorBuffer{NT} function ExtendedKalmanFilter{NT, SM}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂ ) where {NT<:Real, SM<:SimModel} + nu, ny, nd = model.nu, model.ny, model.nd nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) nx̂ = model.nx + nxs Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] P̂_0 = Hermitian(P̂_0, :L) Q̂ = Hermitian(Q̂, :L) R̂ = Hermitian(R̂, :L) P̂ = copy(P̂_0) K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym) - F̂_û, Ĥ = zeros(NT, nx̂+model.nu, nx̂), zeros(NT, model.ny, nx̂) + F̂_û, Ĥ = zeros(NT, nx̂+nu, nx̂), zeros(NT, ny, nx̂) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -723,7 +738,8 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Â, B̂u, Ĉ, B̂d, D̂d, P̂_0, Q̂, R̂, K̂, M̂, - F̂_û, Ĥ + F̂_û, Ĥ, + buffer ) end end diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 2c2692272..3be3ecc23 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -20,9 +20,11 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} B̂d ::Matrix{NT} D̂d ::Matrix{NT} K̂::Matrix{NT} + buffer::StateEstimatorBuffer{NT} function Luenberger{NT, SM}( model, i_ym, nint_u, nint_ym, poles ) where {NT<:Real, SM<:LinModel} + nu, ny, nd = model.nu, model.ny, model.nd nym, nyu = validate_ym(model, i_ym) validate_luenberger(model, nint_u, nint_ym, poles) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -34,15 +36,17 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} catch error("Cannot compute the Luenberger gain K̂ with specified poles.") end - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, - K̂ + K̂, + buffer ) end end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index de9127771..62b9999e5 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -102,10 +102,11 @@ struct MovingHorizonEstimator{ x̂0arr_old::Vector{NT} P̂arr_old ::Hermitian{NT, Matrix{NT}} Nk::Vector{Int} + buffer::StateEstimatorBuffer{NT} function MovingHorizonEstimator{NT, SM, JM, CE}( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, optim::JM, covestim::CE ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} - nu, nd = model.nu, model.nd + nu, ny, nd = model.nu, model.ny, model.nd He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) Cwt < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) nym, nyu = validate_ym(model, i_ym) @@ -114,7 +115,7 @@ struct MovingHorizonEstimator{ nx̂ = model.nx + nxs Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) - lastu0 = zeros(NT, model.nu) + lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] P̂_0 = Hermitian(P̂_0, :L) Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) @@ -140,6 +141,7 @@ struct MovingHorizonEstimator{ x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(P̂_0) Nk = [0] + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) estim = new{NT, SM, JM, CE}( model, optim, con, covestim, Z̃, lastu0, x̂op, f̂op, x̂0, @@ -151,7 +153,8 @@ struct MovingHorizonEstimator{ H̃, q̃, p, P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt, X̂op, X̂0, Y0m, U0, D0, Ŵ, - x̂0arr_old, P̂arr_old, Nk + x̂0arr_old, P̂arr_old, Nk, + buffer ) init_optimization!(estim, model, optim) return estim diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index a119ef1aa..2f7202820 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -19,6 +19,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} yname::Vector{String} dname::Vector{String} xname::Vector{String} + buffer::SimModelBuffer{NT} function LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where {NT<:Real} A, Bu = to_mat(A, 1, 1), to_mat(Bu, 1, 1) nu, nx = size(Bu, 2), size(A, 2) @@ -44,13 +45,15 @@ struct LinModel{NT<:Real} <: SimModel{NT} dname = ["\$d_{$i}\$" for i in 1:nd] xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, x0, Ts, nu, nx, ny, nd, uop, yop, dop, xop, fop, - uname, yname, dname, xname + uname, yname, dname, xname, + buffer ) end end @@ -243,6 +246,7 @@ disturbances ``\mathbf{d_0 = d - d_{op}}``. The Moore-Penrose pseudo-inverse com function steadystate!(model::LinModel, u0, d0) M = I - model.A rtol = sqrt(eps(real(float(oneunit(eltype(M)))))) # pinv docstring recommendation + #TODO: use model.buffer.x to reduce allocations model.x0 .= pinv(M; rtol)*(model.Bu*u0 + model.Bd*d0 + model.fop - model.xop) return nothing end diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 610d77935..b983ae2f8 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -17,6 +17,7 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod yname::Vector{String} dname::Vector{String} xname::Vector{String} + buffer::SimModelBuffer{NT} function NonLinModel{NT, F, H, DS}( f!::F, h!::H, solver::DS, Ts, nu, nx, ny, nd ) where {NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} @@ -31,13 +32,15 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod dname = ["\$d_{$i}\$" for i in 1:nd] xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT, F, H, DS}( x0, f!, h!, solver, Ts, nu, nx, ny, nd, uop, yop, dop, xop, fop, - uname, yname, dname, xname + uname, yname, dname, xname, + buffer ) end end diff --git a/src/plot_sim.jl b/src/plot_sim.jl index c5d499725..e86052d6a 100644 --- a/src/plot_sim.jl +++ b/src/plot_sim.jl @@ -41,9 +41,11 @@ Simply call `plot` on them. # Examples ```jldoctest -julia> model = LinModel(tf(1, [1, 1]), 1.0); N = 5; U_data = fill(1.0, 1, N); +julia> model = LinModel(tf(1, [1, 1]), 1.0); -julia> Y_data = reduce(hcat, (updatestate!(model, U_data[:, i]); model()) for i=1:N) +julia> N = 5; U_data = fill(1.0, 1, N); Y_data = zeros(1, N); + +julia> for i=1:N; updatestate!(model, U_data[:, i]); Y_data[:, i] = model(); end; Y_data 1×5 Matrix{Float64}: 0.632121 0.864665 0.950213 0.981684 0.993262 diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 2df46b535..b6bb27546 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -3,7 +3,7 @@ Abstract supertype of all predictive controllers. --- - (mpc::PredictiveController)(ry, d=mpc.estim.model.dop; kwargs...) -> u + (mpc::PredictiveController)(ry, d=[]; kwargs...) -> u Functor allowing callable `PredictiveController` object as an alias for [`moveinput!`](@ref). @@ -42,7 +42,7 @@ end "Functor allowing callable `PredictiveController` object as an alias for `moveinput!`." function (mpc::PredictiveController)( ry::Vector = mpc.estim.model.yop, - d ::Vector = mpc.estim.model.dop; + d ::Vector = mpc.estim.buffer.empty; kwargs... ) return moveinput!(mpc, ry, d; kwargs...) diff --git a/src/sim_model.jl b/src/sim_model.jl index 835e47c19..d24f5307e 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -5,7 +5,7 @@ Abstract supertype of [`LinModel`](@ref) and [`NonLinModel`](@ref) types. --- - (model::SimModel)(d=model.dop) -> y + (model::SimModel)(d=[]) -> y Functor allowing callable `SimModel` object as an alias for [`evaloutput`](@ref). @@ -20,6 +20,31 @@ julia> y = model() """ abstract type SimModel{NT<:Real} end +struct SimModelBuffer{NT<:Real} + u::Vector{NT} + x::Vector{NT} + y::Vector{NT} + d::Vector{NT} + empty::Vector{NT} +end + +@doc raw""" + SimModelBuffer(nu::Int, nx::Int, ny::Int, nd::Int) -> SimModelBuffer{NT} + +Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances. + +The buffer is used to store intermediate results during simulation without allocating. +""" +function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int) where NT <: Real + u = Vector{NT}(undef, nu) + x = Vector{NT}(undef, nx) + y = Vector{NT}(undef, ny) + d = Vector{NT}(undef, nd) + empty = Vector{NT}(undef, 0) + return SimModelBuffer{NT}(u, x, y, d, empty) +end + + @doc raw""" setop!(model; uop=nothing, yop=nothing, dop=nothing, xop=nothing, fop=nothing) -> model @@ -160,7 +185,7 @@ end detailstr(model::SimModel) = "" @doc raw""" - initstate!(model::SimModel, u, d=model.dop) -> x + initstate!(model::SimModel, u, d=[]) -> x Init `model.x0` with manipulated inputs `u` and measured disturbances `d` steady-state. @@ -182,16 +207,19 @@ julia> x ≈ updatestate!(model, u) true ``` """ -function initstate!(model::SimModel, u, d=model.dop) +function initstate!(model::SimModel, u, d=model.buffer.empty) validate_args(model::SimModel, d, u) - u0, d0 = u - model.uop, d - model.dop + u0, d0 = model.buffer.u, model.buffer.d + u0 .= u .- model.uop + d0 .= d .- model.dop steadystate!(model, u0, d0) - x = model.x0 + model.xop + x = model.buffer.x + x .= model.x0 .+ model.xop return x end """ - updatestate!(model::SimModel, u, d=model.dop) -> x + updatestate!(model::SimModel, u, d=[]) -> x Update `model.x0` states with current inputs `u` and measured disturbances `d`. @@ -204,10 +232,11 @@ julia> x = updatestate!(model, [1]) 1.0 ``` """ -function updatestate!(model::SimModel{NT}, u, d=model.dop) where NT <: Real +function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real validate_args(model::SimModel, d, u) - xnext0 = Vector{NT}(undef, model.nx) - u0, d0 = u - model.uop, d - model.dop + u0, d0, xnext0 = model.buffer.u, model.buffer.d, model.buffer.x + u0 .= u .- model.uop + d0 .= d .- model.dop f!(xnext0, model, model.x0, u0, d0) xnext0 .+= model.fop .- model.xop model.x0 .= xnext0 @@ -217,7 +246,7 @@ function updatestate!(model::SimModel{NT}, u, d=model.dop) where NT <: Real end """ - evaloutput(model::SimModel, d=model.dop) -> y + evaloutput(model::SimModel, d=[]) -> y Evaluate `SimModel` outputs `y` from `model.x0` states and measured disturbances `d`. @@ -232,10 +261,10 @@ julia> y = evaloutput(model) 20.0 ``` """ -function evaloutput(model::SimModel{NT}, d=model.dop) where NT <: Real +function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real validate_args(model, d) - y0 = Vector{NT}(undef, model.ny) - d0 = d - model.dop + d0, y0 = model.buffer.d, model.buffer.y + d0 .= d .- model.dop h!(y0, model, model.x0, d0) y = y0 y .+= model.yop @@ -262,7 +291,7 @@ to_mat(A::Real, dims...) = fill(A, dims) "Functor allowing callable `SimModel` object as an alias for `evaloutput`." -(model::SimModel)(d=model.dop) = evaloutput(model::SimModel, d) +(model::SimModel)(d=model.buffer.empty) = evaloutput(model::SimModel, d) include("model/linmodel.jl") include("model/solver.jl") diff --git a/src/state_estim.jl b/src/state_estim.jl index 523ccbfd2..e9b047a0d 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -3,7 +3,7 @@ Abstract supertype of all state estimators. --- - (estim::StateEstimator)(d=estim.model.dop) -> ŷ + (estim::StateEstimator)(d=[]) -> ŷ Functor allowing callable `StateEstimator` object as an alias for [`evaloutput`](@ref). @@ -18,6 +18,34 @@ julia> ŷ = kf() """ abstract type StateEstimator{NT<:Real} end +struct StateEstimatorBuffer{NT<:Real} + u ::Vector{NT} + x̂ ::Vector{NT} + ym::Vector{NT} + ŷ ::Vector{NT} + d ::Vector{NT} + empty::Vector{NT} +end + +@doc raw""" + StateEstimatorBuffer(nx̂::Int, nym::Int) -> StateEstimatorBuffer{NT} + +Create a buffer for `StateEstimator` objects for estimated states and measured outputs. + +The buffer is used to store intermediate results during simulation without allocating. +""" +function StateEstimatorBuffer{NT}( + nu::Int, nx̂::Int, nym::Int, ny::Int, nd::Int +) where NT <: Real + u = Vector{NT}(undef, nu) + x̂ = Vector{NT}(undef, nx̂) + ym = Vector{NT}(undef, nym) + ŷ = Vector{NT}(undef, ny) + d = Vector{NT}(undef, nd) + empty = Vector{NT}(undef, 0) + return StateEstimatorBuffer{NT}(u, x̂, ym, ŷ, d, empty) +end + const IntVectorOrInt = Union{Int, Vector{Int}} function Base.show(io::IO, estim::StateEstimator)