From 48c201f329f5a68ef5c792c5937a88b7dd22556f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Aug 2024 16:58:31 -0400 Subject: [PATCH 1/6] added: `SimModelBuffer` object to reduce the allocations default `d` argument is now `model.buffer.empty` (restore the error handling when no d is provided) --- src/controller/execute.jl | 17 +++++++---------- src/estimator/execute.jl | 14 +++++++------- src/model/linmodel.jl | 5 ++++- src/model/nonlinmodel.jl | 5 ++++- src/predictive_control.jl | 4 ++-- src/sim_model.jl | 39 +++++++++++++++++++++++++++++++-------- src/state_estim.jl | 2 +- 7 files changed, 56 insertions(+), 30 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index b598462eb..576eb1e2d 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.model.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.model.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.model.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..815bbf49f 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -79,7 +79,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 +111,7 @@ julia> evaloutput(estim) ≈ y true ``` """ -function initstate!(estim::StateEstimator, u, ym, d=estim.model.dop) +function initstate!(estim::StateEstimator, u, ym, d=estim.model.buffer.empty) # --- validate arguments --- validate_args(estim, u, ym, d) # --- init state estimate ---- @@ -161,7 +161,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,7 +176,7 @@ julia> ŷ = evaloutput(kf) 20.0 ``` """ -function evaloutput(estim::StateEstimator{NT}, d=estim.model.dop) where NT <: Real +function evaloutput(estim::StateEstimator{NT}, d=estim.model.buffer.empty) where NT <: Real validate_args(estim.model, d) ŷ0 = Vector{NT}(undef, estim.model.ny) d0 = d - estim.model.dop @@ -187,10 +187,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.model.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,7 +207,7 @@ 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=[]) validate_args(estim, u, ym, d) u0, ym0, d0 = remove_op!(estim, u, ym, d) x̂0next = update_estimate!(estim, u0, ym0, d0) diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index a119ef1aa..9d427c32e 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}(nx, nu, 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 diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 610d77935..1c78e506e 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}(nx, nu, 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/predictive_control.jl b/src/predictive_control.jl index 2df46b535..ef7af6c1b 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.model.buffer.empty; kwargs... ) return moveinput!(mpc, ry, d; kwargs...) diff --git a/src/sim_model.jl b/src/sim_model.jl index 835e47c19..cc75068e9 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,29 @@ 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, nx, ny, nd) -> SimModelBuffer{NT} + +Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances. +""" +function SimModelBuffer{NT}(nu, nx, ny, nd) 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 +183,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,7 +205,7 @@ 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 steadystate!(model, u0, d0) @@ -191,7 +214,7 @@ function initstate!(model::SimModel, u, d=model.dop) 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,7 +227,7 @@ 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 @@ -217,7 +240,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,7 +255,7 @@ 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 @@ -262,7 +285,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..ba3dc5fca 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). From 9f1612ee5d7c314edd16d2d963826547545daa7a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Aug 2024 17:16:46 -0400 Subject: [PATCH 2/6] debug: error in I/O dimensions of the buffer --- src/model/linmodel.jl | 3 ++- src/model/nonlinmodel.jl | 2 +- src/sim_model.jl | 28 ++++++++++++++++++---------- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 9d427c32e..2f7202820 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -45,7 +45,7 @@ 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}(nx, nu, ny, nd) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, x0, @@ -246,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 1c78e506e..b983ae2f8 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -32,7 +32,7 @@ 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}(nx, nu, ny, nd) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT, F, H, DS}( x0, f!, h!, diff --git a/src/sim_model.jl b/src/sim_model.jl index cc75068e9..ab345a43f 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -32,6 +32,8 @@ end SimModelBuffer(nu, nx, ny, nd) -> 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, nx, ny, nd) where NT <: Real u = Vector{NT}(undef, nu) @@ -207,9 +209,12 @@ true """ 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 @@ -229,13 +234,15 @@ julia> x = updatestate!(model, [1]) """ 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 = model.buffer.u, model.buffer.d + u0 .= u .- model.uop + d0 .= d .- model.dop + xnext0 = model.buffer.x f!(xnext0, model, model.x0, u0, d0) xnext0 .+= model.fop .- model.xop model.x0 .= xnext0 - xnext = xnext0 - xnext .+= model.xop + xnext = model.buffer.x + xnext .= xnext0 .+ model.xop return xnext end @@ -257,11 +264,12 @@ julia> y = evaloutput(model) """ 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 = model.buffer.d + d0 .= d .- model.dop + y0 = model.buffer.y h!(y0, model, model.x0, d0) - y = y0 - y .+= model.yop + y = model.buffer.y + y .= y0 .+ model.yop return y end From 62006e4e35584c6b0e1517268cfef2dc54af1721 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 1 Aug 2024 17:54:33 -0400 Subject: [PATCH 3/6] weird bug: the simulations in my own test script are slightly different, to investigate --- src/estimator/execute.jl | 23 +++++++++++++---------- src/estimator/internal_model.jl | 5 ++++- src/estimator/kalman.jl | 20 ++++++++++++++++---- src/estimator/luenberger.jl | 5 ++++- src/estimator/mhe/construct.jl | 5 ++++- src/sim_model.jl | 4 ++-- src/state_estim.jl | 18 ++++++++++++++++++ 7 files changed, 61 insertions(+), 19 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 815bbf49f..15134bc39 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.model.buffer.u, estim.model.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""" @@ -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 @@ -178,11 +181,11 @@ julia> ŷ = evaloutput(kf) """ function evaloutput(estim::StateEstimator{NT}, d=estim.model.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.model.buffer.y, estim.model.buffer.d + d0 .= d .- estim.model.dop ĥ!(ŷ0, estim, estim.model, estim.x̂0, d0) - ŷ = ŷ0 - ŷ .+= estim.model.yop + ŷ = estim.model.buffer.y + ŷ .= ŷ0 .+ estim.model.yop return ŷ end @@ -207,7 +210,7 @@ julia> x̂ = updatestate!(kf, [1], [0]) # x̂[2] is the integrator state (nint_y 0.0 ``` """ -function updatestate!(estim::StateEstimator, u, ym, d=[]) +function updatestate!(estim::StateEstimator, u, ym, d=estim.model.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) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 190a08f22..0fd400e11 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -22,6 +22,7 @@ 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} @@ -36,13 +37,15 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} # 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}(nx̂, nym) 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..ca29f156b 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -22,6 +22,7 @@ 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} @@ -48,6 +49,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} lastu0 = zeros(NT, model.nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) + buffer = StateEstimatorBuffer{NT}(nx̂, nym) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, @@ -55,7 +57,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,6 +237,7 @@ 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} @@ -249,6 +253,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} P̂_0 = Hermitian(P̂_0, :L) P̂ = copy(P̂_0) K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nx̂, nym) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -256,7 +261,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,6 +408,7 @@ 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}} @@ -421,6 +428,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}(nx̂, nym) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -429,7 +437,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,6 +707,7 @@ 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} @@ -715,6 +725,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} 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̂) + buffer = StateEstimatorBuffer{NT}(nx̂, nym) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -723,7 +734,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..9d35ae344 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -20,6 +20,7 @@ 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} @@ -36,13 +37,15 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} end lastu0 = zeros(NT, model.nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] + buffer = StateEstimatorBuffer{NT}(nx̂, nym) 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..3ed566b13 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -102,6 +102,7 @@ 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}} @@ -140,6 +141,7 @@ struct MovingHorizonEstimator{ x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(P̂_0) Nk = [0] + buffer = StateEstimatorBuffer{NT}(nx̂, nym) 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/sim_model.jl b/src/sim_model.jl index ab345a43f..edde4cd75 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -29,13 +29,13 @@ struct SimModelBuffer{NT<:Real} end @doc raw""" - SimModelBuffer(nu, nx, ny, nd) -> SimModelBuffer{NT} + 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, nx, ny, nd) where NT <: Real +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) diff --git a/src/state_estim.jl b/src/state_estim.jl index ba3dc5fca..1e0b39627 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -18,6 +18,24 @@ julia> ŷ = kf() """ abstract type StateEstimator{NT<:Real} end +struct StateEstimatorBuffer{NT<:Real} + x̂ ::Vector{NT} + ym::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}(nx̂::Int, nym::Int) where NT <: Real + x̂ = Vector{NT}(undef, nx̂) + ym = Vector{NT}(undef, nym) + return StateEstimatorBuffer{NT}(x̂, ym) +end + const IntVectorOrInt = Union{Int, Vector{Int}} function Base.show(io::IO, estim::StateEstimator) From bbbca9058614fefe108cc89a680eaec120e36310 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 5 Aug 2024 08:41:35 -0400 Subject: [PATCH 4/6] buffer `StateEstimator`with more attributes --- src/estimator/internal_model.jl | 5 +++-- src/estimator/kalman.jl | 24 ++++++++++++++---------- src/estimator/luenberger.jl | 5 +++-- src/estimator/mhe/construct.jl | 6 +++--- src/state_estim.jl | 14 ++++++++++++-- 5 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 0fd400e11..8ba4f86d5 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -26,6 +26,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{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) @@ -33,11 +34,11 @@ 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}(nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index ca29f156b..71088176c 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -26,6 +26,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{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) @@ -34,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 @@ -46,10 +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}(nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, @@ -241,19 +242,20 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{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}(nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -412,6 +414,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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) @@ -419,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) @@ -428,7 +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}(nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, P̂, @@ -711,21 +714,22 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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̂) - buffer = StateEstimatorBuffer{NT}(nx̂, nym) + 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̂, diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 9d35ae344..3be3ecc23 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -24,6 +24,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{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) @@ -35,9 +36,9 @@ 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}(nx̂, nym) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( model, lastu0, x̂op, f̂op, x̂0, diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 3ed566b13..62b9999e5 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -106,7 +106,7 @@ struct MovingHorizonEstimator{ 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) @@ -115,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) @@ -141,7 +141,7 @@ struct MovingHorizonEstimator{ x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(P̂_0) Nk = [0] - buffer = StateEstimatorBuffer{NT}(nx̂, nym) + 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, diff --git a/src/state_estim.jl b/src/state_estim.jl index 1e0b39627..e9b047a0d 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -19,8 +19,12 @@ 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""" @@ -30,10 +34,16 @@ Create a buffer for `StateEstimator` objects for estimated states and measured o The buffer is used to store intermediate results during simulation without allocating. """ -function StateEstimatorBuffer{NT}(nx̂::Int, nym::Int) where NT <: Real +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) - return StateEstimatorBuffer{NT}(x̂, ym) + ŷ = 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}} From d284c3e54fa8b2e84553c5339652f5671df0c030 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 5 Aug 2024 09:29:17 -0400 Subject: [PATCH 5/6] debug: separate `SimModel` and `StateEstimator` buffers --- src/controller/execute.jl | 6 +++--- src/estimator/execute.jl | 16 ++++++++-------- src/predictive_control.jl | 2 +- src/sim_model.jl | 8 ++++---- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 576eb1e2d..7b648bce0 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -3,7 +3,7 @@ Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero. """ -function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.buffer.empty) +function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty) mpc.ΔŨ .= 0 return initstate!(mpc.estim, u, ym, d) end @@ -51,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.buffer.empty; + d ::Vector = mpc.estim.buffer.empty; Dhat ::Vector = repeat(d, mpc.Hp), Rhaty::Vector = repeat(ry, mpc.Hp), Rhatu::Vector = mpc.Uop, @@ -509,7 +509,7 @@ set_objective_linear_coef!(::PredictiveController, _ ) = nothing Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref). """ -function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.model.buffer.empty) +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 15134bc39..482e1eb81 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -113,7 +113,7 @@ julia> evaloutput(estim) ≈ y true ``` """ -function initstate!(estim::StateEstimator, u, ym, d=estim.model.buffer.empty) +function initstate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) # --- validate arguments --- validate_args(estim, u, ym, d) # --- init state estimate ---- @@ -179,18 +179,18 @@ julia> ŷ = evaloutput(kf) 20.0 ``` """ -function evaloutput(estim::StateEstimator{NT}, d=estim.model.buffer.empty) where NT <: Real +function evaloutput(estim::StateEstimator{NT}, d=estim.buffer.empty) where NT <: Real validate_args(estim.model, d) - ŷ0, d0 = estim.model.buffer.y, estim.model.buffer.d + ŷ0, d0 = estim.buffer.ŷ, estim.buffer.d d0 .= d .- estim.model.dop ĥ!(ŷ0, estim, estim.model, estim.x̂0, d0) - ŷ = estim.model.buffer.y - ŷ .= ŷ0 .+ estim.model.yop + ŷ = ŷ0 + ŷ .+= estim.model.yop return ŷ end "Functor allowing callable `StateEstimator` object as an alias for `evaloutput`." -(estim::StateEstimator)(d=estim.model.buffer.empty) = evaloutput(estim, d) +(estim::StateEstimator)(d=estim.buffer.empty) = evaloutput(estim, d) @doc raw""" updatestate!(estim::StateEstimator, u, ym, d=[]) -> x̂ @@ -210,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.buffer.empty) +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/predictive_control.jl b/src/predictive_control.jl index ef7af6c1b..b6bb27546 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -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.buffer.empty; + 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 edde4cd75..a358bf14d 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -241,8 +241,8 @@ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: f!(xnext0, model, model.x0, u0, d0) xnext0 .+= model.fop .- model.xop model.x0 .= xnext0 - xnext = model.buffer.x - xnext .= xnext0 .+ model.xop + xnext = xnext0 + xnext .+= model.xop return xnext end @@ -268,8 +268,8 @@ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real d0 .= d .- model.dop y0 = model.buffer.y h!(y0, model, model.x0, d0) - y = model.buffer.y - y .= y0 .+ model.yop + y = y0 + y .+= model.yop return y end From 0ac2a7c5513d5f62be1918af2729675c68774b86 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 5 Aug 2024 11:24:48 -0400 Subject: [PATCH 6/6] debug: `SimResults` doctest --- src/estimator/execute.jl | 2 +- src/plot_sim.jl | 6 ++++-- src/sim_model.jl | 6 ++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 482e1eb81..a5e56fe59 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -7,7 +7,7 @@ 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, d0 = estim.model.buffer.u, estim.model.buffer.d + 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] 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/sim_model.jl b/src/sim_model.jl index a358bf14d..d24f5307e 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -234,10 +234,9 @@ julia> x = updatestate!(model, [1]) """ function updatestate!(model::SimModel{NT}, u, d=model.buffer.empty) where NT <: Real validate_args(model::SimModel, d, u) - u0, d0 = model.buffer.u, model.buffer.d + u0, d0, xnext0 = model.buffer.u, model.buffer.d, model.buffer.x u0 .= u .- model.uop d0 .= d .- model.dop - xnext0 = model.buffer.x f!(xnext0, model, model.x0, u0, d0) xnext0 .+= model.fop .- model.xop model.x0 .= xnext0 @@ -264,9 +263,8 @@ julia> y = evaloutput(model) """ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real validate_args(model, d) - d0 = model.buffer.d + d0, y0 = model.buffer.d, model.buffer.y d0 .= d .- model.dop - y0 = model.buffer.y h!(y0, model, model.x0, d0) y = y0 y .+= model.yop