diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 2c14c4e6d..95df04982 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -31,7 +31,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT} Yop::Vector{NT} Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} - function ExplicitMPC{NT, SE}( + function ExplicitMPC{NT}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp ) where {NT<:Real, SE<:StateEstimator} model = estim.model @@ -158,7 +158,7 @@ function ExplicitMPC( @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end - return ExplicitMPC{NT, SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp) + return ExplicitMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp) end setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.") diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index cc5373bdf..dbaeb3803 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -40,7 +40,7 @@ struct LinMPC{ Yop::Vector{NT} Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} - function LinMPC{NT, SE, JM}( + function LinMPC{NT}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel} model = estim.model @@ -237,7 +237,7 @@ function LinMPC( @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)") end - return LinMPC{NT, SE, JM}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim) + return LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim) end """ diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index a0599aa87..f72be7771 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -4,9 +4,9 @@ struct NonLinMPC{ NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, + P<:Any, JEfunc<:Function, - GCfunc<:Function, - P<:Any + GCfunc<:Function } <: PredictiveController{NT} estim::SE # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be @@ -45,16 +45,16 @@ struct NonLinMPC{ Yop::Vector{NT} Dop::Vector{NT} buffer::PredictiveControllerBuffer{NT} - function NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( + function NonLinMPC{NT}( estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::P, optim::JM ) where { NT<:Real, SE<:StateEstimator, - JM<:JuMP.GenericModel, + JM<:JuMP.GenericModel, + P<:Any, JEfunc<:Function, GCfunc<:Function, - P<:Any } model = estim.model nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂ @@ -80,7 +80,7 @@ struct NonLinMPC{ nΔŨ = size(Ẽ, 2) ΔŨ = zeros(NT, nΔŨ) buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ) - mpc = new{NT, SE, JM, JEfunc, GCfunc, P}( + mpc = new{NT, SE, JM, P, JEfunc, GCfunc}( estim, optim, con, ΔŨ, ŷ, Hp, Hc, nϵ, @@ -317,7 +317,7 @@ function NonLinMPC( L_Hp = diagm(repeat(Lwt, Hp)), Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE ::JEfunc = (_,_,_,_) -> 0.0, + JE ::Function = (_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc = 0, @@ -327,7 +327,6 @@ function NonLinMPC( NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, - JEfunc<:Function, P<:Any } nk = estimate_delays(estim.model) @@ -337,8 +336,7 @@ function NonLinMPC( end validate_JE(NT, JE) gc! = get_mutating_gc(NT, gc) - GCfunc = get_type_mutating_gc(gc!) - return NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}( + return NonLinMPC{NT}( estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, optim ) end @@ -396,9 +394,6 @@ function get_mutating_gc(NT, gc) return gc! end -"Get the type of the mutating version of the custom constrain function `gc!`." -get_type_mutating_gc(::GCfunc) where {GCfunc<:Function} = GCfunc - """ test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index f1b8bbfe1..a32d4349d 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -29,7 +29,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} - function InternalModel{NT, SM}( + function InternalModel{NT}( model::SM, i_ym, Asm, Bsm, Csm, Dsm ) where {NT<:Real, SM<:SimModel} nu, ny, nd = model.nu, model.ny, model.nd @@ -117,7 +117,7 @@ function InternalModel( stoch_ym = c2d(stoch_ym_c, model.Ts, :tustin) end end - return InternalModel{NT, SM}(model, i_ym, stoch_ym.A, stoch_ym.B, stoch_ym.C, stoch_ym.D) + return InternalModel{NT}(model, i_ym, stoch_ym.A, stoch_ym.B, stoch_ym.C, stoch_ym.D) end "Validate if deterministic `model` and stochastic model `Csm, Dsm` for `InternalModel`s." diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 6eb030784..9a0aec83b 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -27,7 +27,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} - function SteadyKalmanFilter{NT, SM}( + function SteadyKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel} nu, ny, nd = model.nu, model.ny, model.nd @@ -182,7 +182,7 @@ function SteadyKalmanFilter( # estimated covariances matrices (variance = σ²) : Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) + return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) end @doc raw""" @@ -196,7 +196,7 @@ function SteadyKalmanFilter( model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel{NT}} Q̂, R̂ = to_mat(Q̂), to_mat(R̂) - return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) + return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) end "Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values." @@ -308,7 +308,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} - function KalmanFilter{NT, SM}( + function KalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel} nu, ny, nd = model.nu, model.ny, model.nd @@ -419,7 +419,7 @@ function KalmanFilter( P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂; direct) + return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂; direct) end @doc raw""" @@ -433,7 +433,7 @@ function KalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) - return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end @doc raw""" @@ -530,7 +530,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} - function UnscentedKalmanFilter{NT, SM}( + function UnscentedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct=true ) where {NT<:Real, SM<:SimModel{NT}} nu, ny, nd = model.nu, model.ny, model.nd @@ -681,7 +681,7 @@ function UnscentedKalmanFilter( P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return UnscentedKalmanFilter{NT, SM}( + return UnscentedKalmanFilter{NT}( model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct ) end @@ -699,7 +699,7 @@ function UnscentedKalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; direct=true ) where {NT<:Real, SM<:SimModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) - return UnscentedKalmanFilter{NT, SM}( + return UnscentedKalmanFilter{NT}( model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, α, β, κ; direct ) end @@ -905,7 +905,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} - function ExtendedKalmanFilter{NT, SM}( + function ExtendedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:SimModel} nu, ny, nd = model.nu, model.ny, model.nd @@ -994,7 +994,7 @@ function ExtendedKalmanFilter( P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + return ExtendedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end @doc raw""" @@ -1008,7 +1008,7 @@ function ExtendedKalmanFilter( model::SM, i_ym, nint_u, nint_ym,P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:SimModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) - return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + return ExtendedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end """ diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 6c0922508..e7cb5c6b8 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -107,7 +107,7 @@ struct MovingHorizonEstimator{ direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} - function MovingHorizonEstimator{NT, SM, JM, CE}( + function MovingHorizonEstimator{NT}( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, optim::JM, covestim::CE; direct=true ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} @@ -406,7 +406,7 @@ function MovingHorizonEstimator( covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) - return MovingHorizonEstimator{NT, SM, JM, CE}( + return MovingHorizonEstimator{NT}( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, optim, covestim; direct ) end diff --git a/src/model/linearization.jl b/src/model/linearization.jl index b7ddc34eb..e9b6f1108 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -1,3 +1,31 @@ + +function jacobianA!(A, jb::JacobianBuffer, model::SimModel, x, u, d) + jb.x .= x; jb.u .= u; jb.d .= d + return ForwardDiff.jacobian!(A, jb.f_x!, jb.xnext, jb.x, jb.f_x_cfg) +end +jacobianA!( _ , _ , model::LinModel, _ , _ , _ ) = model.A +function jacobianBu!(Bu, jb::JacobianBuffer, model::SimModel, x, u, d) + jb.x .= x; jb.u .= u; jb.d .= d + return ForwardDiff.jacobian!(Bu, jb.f_u!, jb.xnext, jb.u, jb.f_u_cfg) +end +jacobianBu!( _ , _ , model::LinModel, _ , _ , _ ) = model.Bu +function jacobianBd!(Bd, jb::JacobianBuffer, model::SimModel, x, u, d) + jb.x .= x; jb.u .= u; jb.d .= d + return ForwardDiff.jacobian!(Bd, jb.f_d!, jb.xnext, jb.d, jb.f_d_cfg) +end +jacobianBd!( _ , _ , model::LinModel, _ , _ , _ ) = model.Bd +function jacobianC!(C, jb::JacobianBuffer, model::SimModel, x, d) + jb.x .= x; jb.d .= d + return ForwardDiff.jacobian!(C, jb.h_x!, jb.y, jb.x, jb.h_x_cfg) +end +jacobianC!( _ , _ , model::LinModel, _ , _ ) = model.C +function jacobianDd!(Dd, jb::JacobianBuffer, model::SimModel, x, d) + jb.x .= x; jb.d .= d + return ForwardDiff.jacobian!(Dd, jb.h_d!, jb.y, jb.d, jb.h_d_cfg) +end +jacobianDd!( _ , _ , model::LinModel, _ , _ ) = model.Dd + + """ LinModel(model::NonLinModel; x=model.x0+model.xop, u=model.uop, d=model.dop) @@ -118,19 +146,25 @@ function linearize!( d0 .= d .- nonlinmodel.dop x0 .= x .- nonlinmodel.xop # --- compute the Jacobians at linearization points --- - A::Matrix{NT}, Bu::Matrix{NT}, Bd::Matrix{NT} = linmodel.A, linmodel.Bu, linmodel.Bd - C::Matrix{NT}, Dd::Matrix{NT} = linmodel.C, linmodel.Dd + #A::Matrix{NT}, Bu::Matrix{NT}, Bd::Matrix{NT} = linmodel.A, linmodel.Bu, linmodel.Bd + #C::Matrix{NT}, Dd::Matrix{NT} = linmodel.C, linmodel.Dd xnext0::Vector{NT}, y0::Vector{NT} = linmodel.buffer.x, linmodel.buffer.y - myf_x0!(xnext0, x0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) - myf_u0!(xnext0, u0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) - myf_d0!(xnext0, d0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) - myh_x0!(y0, x0) = h!(y0, nonlinmodel, x0, d0, model.p) - myh_d0!(y0, d0) = h!(y0, nonlinmodel, x0, d0, model.p) - ForwardDiff.jacobian!(A, myf_x0!, xnext0, x0) - ForwardDiff.jacobian!(Bu, myf_u0!, xnext0, u0) - ForwardDiff.jacobian!(Bd, myf_d0!, xnext0, d0) - ForwardDiff.jacobian!(C, myh_x0!, y0, x0) - ForwardDiff.jacobian!(Dd, myh_d0!, y0, d0) + #myf_x0!(xnext0, x0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) + #myf_u0!(xnext0, u0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) + #myf_d0!(xnext0, d0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p) + #myh_x0!(y0, x0) = h!(y0, nonlinmodel, x0, d0, model.p) + #myh_d0!(y0, d0) = h!(y0, nonlinmodel, x0, d0, model.p) + #ForwardDiff.jacobian!(A, myf_x0!, xnext0, x0) + #ForwardDiff.jacobian!(Bu, myf_u0!, xnext0, u0) + #ForwardDiff.jacobian!(Bd, myf_d0!, xnext0, d0) + #ForwardDiff.jacobian!(C, myh_x0!, y0, x0) + #ForwardDiff.jacobian!(Dd, myh_d0!, y0, d0) + jb = nonlinmodel.buffer.jacobian + jacobianA!(linmodel.A, jb, nonlinmodel, x0, u0, d0) + jacobianBu!(linmodel.Bu, jb, nonlinmodel, x0, u0, d0) + jacobianBd!(linmodel.Bd, jb, nonlinmodel, x0, u0, d0) + jacobianC!(linmodel.C, jb, nonlinmodel, x0, d0) + jacobianDd!(linmodel.Dd, jb, nonlinmodel, x0, d0) # --- compute the nonlinear model output at operating points --- h!(y0, nonlinmodel, x0, d0, model.p) y = y0 diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 24f40ba05..49c26ee40 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -21,7 +21,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} yname::Vector{String} dname::Vector{String} xname::Vector{String} - buffer::SimModelBuffer{NT} + buffer::SimModelBuffer{NT, Nothing} 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) diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 671e7bd47..402e22302 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -1,5 +1,5 @@ struct NonLinModel{ - NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver + NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver, SMB<:SimModelBuffer } <: SimModel{NT} x0::Vector{NT} f!::F @@ -21,10 +21,10 @@ struct NonLinModel{ yname::Vector{String} dname::Vector{String} xname::Vector{String} - buffer::SimModelBuffer{NT} - function NonLinModel{NT, F, H, P, DS}( - f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS - ) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver} + buffer::SMB + function NonLinModel{NT}( + f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS, buffer::SMB + ) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver, SMB<:SimModelBuffer} Ts > 0 || error("Sampling time Ts must be positive") uop = zeros(NT, nu) yop = zeros(NT, ny) @@ -37,8 +37,7 @@ struct NonLinModel{ xname = ["\$x_{$i}\$" for i in 1:nx] x0 = zeros(NT, nx) t = zeros(NT, 1) - buffer = SimModelBuffer{NT}(nu, nx, ny, nd) - return new{NT, F, H, P, DS}( + return new{NT, F, H, P, DS, SMB}( x0, f!, h!, p, @@ -145,8 +144,9 @@ function NonLinModel{NT}( isnothing(solver) && (solver=EmptySolver()) f!, h! = get_mutating_functions(NT, f, h) f!, h! = get_solver_functions(NT, solver, f!, h!, Ts, nu, nx, ny, nd) - F, H, P, DS = get_types(f!, h!, p, solver) - return NonLinModel{NT, F, H, P, DS}(f!, h!, Ts, nu, nx, ny, nd, p, solver) + jacobian = JacobianBuffer{NT}(f!, h!, nu, nx, ny, nd, p) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd, jacobian) + return NonLinModel{NT}(f!, h!, Ts, nu, nx, ny, nd, p, solver, buffer) end function NonLinModel( @@ -224,13 +224,6 @@ function validate_h(NT, h) return ismutating end -"Get the types of `f!`, `h!` and `solver` to construct a `NonLinModel`." -function get_types( - ::F, ::H, ::P, ::DS -) where {F<:Function, H<:Function, P<:Any, DS<:DiffSolver} - return F, H, P, DS -end - "Do nothing if `model` is a [`NonLinModel`](@ref)." steadystate!(::SimModel, _ , _ ) = nothing diff --git a/src/sim_model.jl b/src/sim_model.jl index e1f4e9779..168de565c 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -20,12 +20,74 @@ julia> y = model() """ abstract type SimModel{NT<:Real} end -struct SimModelBuffer{NT<:Real} +struct JacobianBuffer{ + NT<:Real, + FX<:Function, + FU<:Function, + FD<:Function, + HX<:Function, + HU<:Function, + CFX<:ForwardDiff.JacobianConfig, + CFU<:ForwardDiff.JacobianConfig, + CFD<:ForwardDiff.JacobianConfig, + CHU<:ForwardDiff.JacobianConfig, + CHX<:ForwardDiff.JacobianConfig +} + xnext::Vector{NT} + u::Vector{NT} + x::Vector{NT} + y::Vector{NT} + d::Vector{NT} + f_x!::FX + f_u!::FU + f_d!::FD + h_x!::HX + h_d!::HU + f_x_cfg::CFX + f_u_cfg::CFU + f_d_cfg::CFD + h_x_cfg::CHX + h_d_cfg::CHU + +end + +""" + JacobianBuffer(NT::DataType, f!::Function, h!::Function, nu, nx, ny, nd) + +Buffer object for calling `ForwardDiff.jacobian!` on `SimModel` without any allocation. +""" +function JacobianBuffer{NT}( + f!::Function, h!::Function, nu::Int, nx::Int, ny::Int, nd::Int, p +) where NT <: Real + xnext = Vector{NT}(undef, nx) + u = Vector{NT}(undef, nu) + x = Vector{NT}(undef, nx) + y = Vector{NT}(undef, ny) + d = Vector{NT}(undef, nd) + f_x!(xnext, x) = f!(xnext, x, u, d, p) + f_u!(xnext, u) = f!(xnext, x, u, d, p) + f_d!(xnext, d) = f!(xnext, x, u, d, p) + h_x!(y, x) = h!(y, x, d, p) + h_d!(y, d) = h!(y, x, d, p) + f_x_cfg = ForwardDiff.JacobianConfig(f_x!, xnext, x) + f_u_cfg = ForwardDiff.JacobianConfig(f_u!, xnext, u) + f_d_cfg = ForwardDiff.JacobianConfig(f_d!, xnext, d) + h_x_cfg = ForwardDiff.JacobianConfig(h_x!, y, x) + h_d_cfg = ForwardDiff.JacobianConfig(h_d!, y, d) + return JacobianBuffer( + xnext, u, x, y, d, + f_x!, f_u!, f_d!, h_x!, h_d!, + f_x_cfg, f_u_cfg, f_d_cfg, h_x_cfg, h_d_cfg + ) +end + +struct SimModelBuffer{NT<:Real, JB<:Union{JacobianBuffer, Nothing}} u::Vector{NT} x::Vector{NT} y::Vector{NT} d::Vector{NT} empty::Vector{NT} + jacobian::JB end @doc raw""" @@ -35,13 +97,15 @@ Create a buffer for `SimModel` objects for inputs, states, outputs, and disturba 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 +function SimModelBuffer{NT}( + nu::Int, nx::Int, ny::Int, nd::Int, jacobian::JB=nothing +) where {NT <: Real, JB <: Union{JacobianBuffer, Nothing}} 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) + return SimModelBuffer{NT, JB}(u, x, y, d, empty, jacobian) end