diff --git a/Project.toml b/Project.toml index cff7a6323..815db6783 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "2.12.3" +version = "2.13.0" authors = ["Francis Gagnon"] [deps] diff --git a/docs/make.jl b/docs/make.jl index e1f715550..4f107581e 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -13,6 +13,7 @@ links = InterLinks( "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv", "MathOptInterface" => "https://jump.dev/MathOptInterface.jl/stable/objects.inv", "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv", + "SparseConnectivityTracer" => "https://adrianhill.de/SparseConnectivityTracer.jl/stable/objects.inv", "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv", "LowLevelParticleFilters" => "https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/objects.inv", "LinearMPC" => "https://darnstrom.github.io/LinearMPC.jl/stable/objects.inv", diff --git a/docs/src/internals/misc.md b/docs/src/internals/misc.md index 9dbd5b617..f82944fa4 100644 --- a/docs/src/internals/misc.md +++ b/docs/src/internals/misc.md @@ -4,6 +4,13 @@ Pages = ["misc.md"] ``` +## Abstract Types + +```@docs +ModelPredictiveControl.ShootingMethod +ModelPredictiveControl.CollocationMethod +``` + ## Orthogonal Collocation ```@docs diff --git a/docs/src/internals/sim_model.md b/docs/src/internals/sim_model.md index f18f55f9b..820d9d870 100644 --- a/docs/src/internals/sim_model.md +++ b/docs/src/internals/sim_model.md @@ -4,6 +4,19 @@ Pages = ["sim_model.md"] ``` +## Abstract Types + +```@docs +ModelPredictiveControl.SimModelODE +ModelPredictiveControl.SimModelDAE +``` + +## Model Construction + +```@docs +ModelPredictiveControl.init_defectmat_dae +``` + ## State-Space Functions ```@docs @@ -11,8 +24,8 @@ ModelPredictiveControl.f! ModelPredictiveControl.h! ``` -## Steady-State Calculation +## Init State ```@docs -ModelPredictiveControl.steadystate! +ModelPredictiveControl.initstate_core! ``` diff --git a/docs/src/public/sim_model.md b/docs/src/public/sim_model.md index 3e2d49f24..6af55caa7 100644 --- a/docs/src/public/sim_model.md +++ b/docs/src/public/sim_model.md @@ -37,6 +37,12 @@ LinModel NonLinModel ``` +## NonLinModelDAE + +```@docs +NonLinModelDAE +``` + ## Set Variable Names ```@docs diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index c786b558c..723281bc6 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -17,7 +17,7 @@ using DifferentiationInterface: gradient!, value_and_gradient!, prepare_gradient using DifferentiationInterface: jacobian!, value_and_jacobian!, prepare_jacobian using DifferentiationInterface: hessian!, value_gradient_and_hessian!, prepare_hessian using DifferentiationInterface: Constant, Cache -using SparseConnectivityTracer: TracerSparsityDetector +using SparseConnectivityTracer: TracerSparsityDetector, jacobian_sparsity, jacobian_buffer using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern using SparseMatrixColorings: NaturalOrder, LargestFirst, SmallestLast using SparseMatrixColorings: IncidenceDegree, DynamicLargestFirst, RandomOrder @@ -41,7 +41,7 @@ import OSQP, Ipopt import FastGaussQuadrature -export SimModel, LinModel, NonLinModel +export SimModel, LinModel, NonLinModel, NonLinModelDAE export DiffSolver, RungeKutta, ForwardEuler export setop!, setname! export setstate!, setmodel!, preparestate!, updatestate!, evaloutput, linearize, linearize! @@ -57,8 +57,8 @@ export TrapezoidalCollocation, OrthogonalCollocation export SimResult, getinfo, sim! include("general.jl") -include("sim_model.jl") include("transcription.jl") +include("sim_model.jl") include("state_estim.jl") include("predictive_control.jl") include("plot_sim.jl") diff --git a/src/controller/construct.jl b/src/controller/construct.jl index af263ba32..2c195cf60 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -94,7 +94,7 @@ end "Outer constructor to validate and convert weight matrices if necessary." function ControllerWeights( - model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 + model::SimModelODE{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 ) where {NT<:Real} validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) M_Hp, N_Hc, L_Hp = NT.(M_Hp), NT.(N_Hc), NT.(L_Hp) @@ -568,7 +568,7 @@ Estimate the default prediction horizon `Hp` for [`LinModel`](@ref). """ default_Hp(model::LinModel) = DEFAULT_HP0 + estimate_delays(model) "Throw an error when model is not a [`LinModel`](@ref)." -function default_Hp(::SimModel) +function default_Hp(::SimModelODE) msg = "Prediction horizon Hp must be explicitly specified if model is not a LinModel." throw(ArgumentError(msg)) end diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 490081b62..371e53051 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -277,25 +277,25 @@ function initpred!(mpc::PredictiveController, model::LinModel, ry, d, lastu, D̂ end @doc raw""" - initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> nothing + initpred!(mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u) -> nothing Init `lastu0, ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel`](@ref). """ -function initpred!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) +function initpred!(mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u) initpred_common!(mpc, model, ry, d, lastu, D̂, R̂y, R̂u) return nothing end """ - initpred_common!(mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u) -> F + initpred_common!(mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u) -> F -Common computations of `initpred!` for all types of [`SimModel`](@ref). +Common computations of `initpred!` for all types of [`SimModelODE`](@ref). Will also init `mpc.F` with 0 values, or with the stochastic predictions `Ŷs` if `mpc.estim` is an [`InternalModel`](@ref). The function returns `mpc.F`. """ function initpred_common!( - mpc::PredictiveController, model::SimModel, ry, d, lastu, D̂, R̂y, R̂u + mpc::PredictiveController, model::SimModelODE, ry, d, lastu, D̂, R̂y, R̂u ) mpc.lastu0 .= lastu .- model.uop mul!(mpc.Tu_lastu0, mpc.Tu, mpc.lastu0) @@ -327,14 +327,14 @@ end predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing) @doc raw""" - linconstraint_custom!(mpc::PredictiveController, model::SimModel) + linconstraint_custom!(mpc::PredictiveController, model::SimModelODE) Init the ``\mathbf{F_w}`` vector for the custom linear inequality constraints. See [`relaxW`](@ref) for the definition of the vector. The function does nothing if `mpc.con.nw < 1`. """ -function linconstraint_custom!(mpc::PredictiveController, model::SimModel) +function linconstraint_custom!(mpc::PredictiveController, model::SimModelODE) mpc.con.nw < 1 && return nothing ny, nu, nd, buffer = model.ny, model.nu, model.nd, mpc.buffer Fw = mpc.con.Fw @@ -363,7 +363,7 @@ function linconstraint_custom_outputs!(mpc::PredictiveController, model::LinMode return nothing end "Do nothing for other model types." -linconstraint_custom_outputs!(::PredictiveController, ::SimModel) = nothing +linconstraint_custom_outputs!(::PredictiveController, ::SimModelODE) = nothing """ extended_vectors!(Ue, Ŷe, mpc::PredictiveController, U0, Ŷ0) -> Ue, Ŷe @@ -506,7 +506,7 @@ end "By default, no need to update the objective function." -set_objective_linear_coef!(::PredictiveController, ::SimModel, _) = nothing +set_objective_linear_coef!(::PredictiveController, ::SimModelODE, _) = nothing "Update the linear coefficients of the quadratic objective with `mpc.q̃` for `LinModel`." function set_objective_linear_coef!(mpc::PredictiveController, ::LinModel, Z̃var) @@ -790,7 +790,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old) end "No need to set the objective Hessian by default (only needed for quadratic objective)." -set_objective_hessian!(::PredictiveController, ::SimModel, _ ) = nothing +set_objective_hessian!(::PredictiveController, ::SimModelODE, _ ) = nothing "Set the objective Hessian with `mpc.H̃` if the objective is quadratic." function set_objective_hessian!(mpc::PredictiveController, ::LinModel, Z̃var) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 3b3ae1594..52c542dee 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -99,7 +99,7 @@ struct NonLinMPC{ model, estim, transcription, Hp, Hc, nb ) F = zeros(NT, ny*Hp) # dummy value (updated just before optimization) - Mo, Co, λo = init_orthocolloc(model, transcription) + Mo, Co, λo = init_orthocolloc(NT, transcription, model.nx, model.Ts) ES, GS, JS, KS, VS, BS = init_defectmat( model, estim, transcription, Hp, Hc, nb, Co, λo ) @@ -148,9 +148,9 @@ struct NonLinMPC{ end @doc raw""" - NonLinMPC(model::SimModel; ) + NonLinMPC(model::SimModelODE; ) -Construct a nonlinear predictive controller based on [`SimModel`](@ref) `model`. +Construct a nonlinear predictive controller based on [`SimModelODE`](@ref) `model`. Both [`NonLinModel`](@ref) and [`LinModel`](@ref) are supported (see Extended Help). The controller minimizes the following objective function at each discrete time ``k``: @@ -200,7 +200,7 @@ This controller allocates memory at each time step for the optimization. `MethodError: no method matching Float64(::ForwardDiff.Dual)`. # Arguments -- `model::SimModel` : model used for controller predictions and state estimations. +- `model::SimModelODE` : model used for controller predictions and state estimations. - `Hp::Int=10+nk` : prediction horizon ``H_p``, `nk` is the number of delays if `model` is a [`LinModel`](@ref) (must be specified otherwise). - `Hc::Union{Int, Vector{Int}}=2` : control horizon ``H_c``, custom move blocking pattern is @@ -339,7 +339,7 @@ NonLinMPC controller with a sample time Ts = 10.0 s: `10/Cwt` (if not already set), to scale the small values of ``ϵ``. """ function NonLinMPC( - model::SimModel; + model::SimModelODE; Hp::Int = default_Hp(model), Hc::IntVectorOrInt = DEFAULT_HC, Mwt = fill(DEFAULT_MWT, model.ny), @@ -375,7 +375,7 @@ function NonLinMPC( ) end -default_estimator(model::SimModel; kwargs...) = UnscentedKalmanFilter(model; kwargs...) +default_estimator(model::SimModelODE; kwargs...) = UnscentedKalmanFilter(model; kwargs...) default_estimator(model::LinModel; kwargs...) = SteadyKalmanFilter(model; kwargs...) """ @@ -454,7 +454,7 @@ function NonLinMPC( validate_JE(NT, JE) gc! = get_mutating_gc_mpc(NT, gc) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) - hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMPC_HESSIAN) + hessian = validate_hessian(hessian, DEFAULT_NONLINMPC_HESSIAN, gradient) return NonLinMPC{NT}( estim, Hp, Hc, nb, weights, Wy, Wu, Wd, Wr, JE, gc!, nc, p, transcription, optim, gradient, jacobian, hessian @@ -734,11 +734,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end """ - init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) -> nothing + init_optimization!(mpc::NonLinMPC, model::SimModelODE, optim::JuMP.GenericModel) -> nothing Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers. """ -function init_optimization!(mpc::NonLinMPC, model::SimModel, optim::JuMP.GenericModel) +function init_optimization!(mpc::NonLinMPC, model::SimModelODE, optim::JuMP.GenericModel) # --- variables and linear constraints --- con = mpc.con nZ̃ = length(mpc.Z̃) @@ -827,14 +827,14 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J Hp, Hc = mpc.Hp, mpc.Hc ng = length(mpc.con.i_g) nc, neq = mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ + nZ̃, nU, nŶ, nX̂, nK̄ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN = convert(JNT, NaN) J::Vector{JNT} = zeros(JNT, 1) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K::Vector{JNT} = zeros(JNT, nK) + K̄::Vector{JNT} = zeros(JNT, nK̄) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) @@ -847,7 +847,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J Z̃_J = fill(myNaN, nZ̃) # NaN to force update at first call J_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(K̄), Cache(X̂0), Cache(gc), Cache(g), Cache(geq), ) ∇J_prep = prepare_gradient(J!, grad, Z̃_J, J_cache...; strict) @@ -954,13 +954,13 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN i_g = findall(mpc.con.i_g) # convert to non-logical indices for non-allocating @views ng, ngi = length(mpc.con.i_g), sum(mpc.con.i_g) nc, neq = mpc.con.nc, mpc.con.neq - nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ + nZ̃, nU, nŶ, nX̂, nK̄ = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk̄ nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K::Vector{JNT} = zeros(JNT, nK) + K̄::Vector{JNT} = zeros(JNT, nK̄) Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) @@ -968,20 +968,20 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN gi::Vector{JNT}, geq::Vector{JNT} = zeros(JNT, ngi), zeros(JNT, neq) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ----------------------------- - function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function gi!(gi, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g, gi) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_gi(Z̃, λi, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, geq, g, gi) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) gi .= @views g[i_g] return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update at first call ∇gi_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(K̄), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict) @@ -990,7 +990,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN if !isnothing(hess) ∇²gi_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(K̄), Cache(X̂0), Cache(gc), Cache(geq), Cache(g), Cache(gi) ) ∇²gi_prep = prepare_hessian( @@ -1033,18 +1033,18 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) return nothing end - function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc, Z̃) return dot(λeq, geq) end Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update at first call ∇geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(K̄), Cache(X̂0), Cache(gc), Cache(g) ) ∇geq_prep = prepare_jacobian(geq!, geq, jac, Z̃_∇geq, ∇geq_cache...; strict) @@ -1053,7 +1053,7 @@ function get_nonlincon_oracle(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JN if !isnothing(hess) ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K), Cache(X̂0), + Cache(Û0), Cache(K̄), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) ∇²geq_prep = prepare_hessian( @@ -1099,7 +1099,7 @@ end """ update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) -> nothing @@ -1108,17 +1108,17 @@ Update in-place all vectors for the predictions of `mpc` controller at decision The method mutates all the arguments before the `mpc` argument. """ function update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ + ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K̄, X̂0, gc, g, geq, mpc::PredictiveController, Z̃ ) model, transcription = mpc.estim.model, mpc.transcription U0 = getU0!(U0, mpc, Z̃) ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) - Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) + Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, K̄, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) ϵ = getslack(mpc, Z̃) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) g = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ) - geq = con_nonlinprogeq!(geq, X̂0, Û0, K, mpc, model, transcription, U0, Z̃) + geq = con_nonlinprogeq!(geq, X̂0, Û0, K̄, mpc, model, transcription, U0, Z̃) return nothing end diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 041de5ca5..a6d893050 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -1,4 +1,4 @@ -"Get the number of elements in the optimization decision vector `Z`." +"Get the number of elements in the optimization decision vector `Z` for MPC." function get_nZ_mpc(estim::StateEstimator, ::SingleShooting, _ , Hc) return estim.model.nu*Hc end @@ -415,7 +415,7 @@ end @doc raw""" init_defectmat( - model::SimModel, estim::StateEstimator, transcription::TranscriptionMethod, + model::SimModelODE, estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS @@ -454,7 +454,7 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H ``` """ function init_defectmat( - model::SimModel, estim::StateEstimator{NT}, ::TranscriptionMethod, + model::SimModelODE, estim::StateEstimator{NT}, ::TranscriptionMethod, Hp, Hc, ::Any , ::Any=nothing, ::Any=nothing ) where {NT<:Real} nu, nx, nd, nx̂, nxs = model.nu, model.nx, model.nd, estim.nx̂, estim.nxs @@ -483,7 +483,7 @@ end @doc raw""" init_defectmat( - model::SimModel, estim::StateEstimator, transcription::OrthogonalCollocation, + model::SimModelODE, estim::StateEstimator, transcription::OrthogonalCollocation, Hp, Hc, _ , Co, λo ) -> ES, GS, JS, KS, VS, BS @@ -604,14 +604,14 @@ end """ init_defectmat( - model::SimModel, estim::StateEstimator, transcription::SingleShooting, + model::SimModelODE, estim::StateEstimator, transcription::SingleShooting, Hp, Hc, nb, Co=nothing, λo=nothing ) -> ES, GS, JS, KS, VS, BS Return empty matrices for [`SingleShooting`](@ref) transcription (N/A). """ function init_defectmat( - ::SimModel, estim::StateEstimator, transcription::SingleShooting, + ::SimModelODE, estim::StateEstimator, transcription::SingleShooting, Hp, Hc, ::Any, ::Any=nothing, ::Any=nothing ) return init_defectmat_empty(estim, transcription, Hp, Hc) @@ -666,7 +666,7 @@ case, `args` needs to contain all the inequality and equality constraint matric The integer `neq` is the number of nonlinear equality constraints in ``\mathbf{g_{eq}}``. """ function init_matconstraint_mpc( - model::LinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, nϵ, + model::LinModel{NT}, transcription::TranscriptionMethod, Z̃min, Z̃max, nc, _ , U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, args... ) where {NT<:Real} @@ -705,7 +705,7 @@ end "Init `i_b, A` without output & terminal constraints if `NonLinModel` and `SingleShooting`." function init_matconstraint_mpc( - model::NonLinModel{NT}, transcription::SingleShooting, Z̃min, Z̃max, nc, nϵ, + model::NonLinModel{NT}, transcription::SingleShooting, Z̃min, Z̃max, nc, _ , U0min, U0max, ΔUmin, ΔUmax, Y0min, Y0max, Wmin, Wmax, x̂0min, x̂0max, args... ) where {NT<:Real} @@ -781,7 +781,7 @@ end boxconstraint_terminal!(Z̃min, Z̃max, ::SingleShooting, _, _ , _, _, _, _, _) = Z̃min, Z̃max "Unset `i_ΔUmin` and `i_ΔUmax` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deleteΔU_lincon!(i_ΔUmin, i_ΔUmax, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max) +function deleteΔU_lincon!(i_ΔUmin, i_ΔUmax, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max) nΔU = length(i_ΔUmin) ΔUmin, ΔUmax = @views Z̃min[1:nΔU], @views Z̃max[1:nΔU] foreach(i -> !isinf(ΔUmin[i]) && (i_ΔUmin[i] = false), eachindex(ΔUmin)) @@ -790,14 +790,14 @@ function deleteΔU_lincon!(i_ΔUmin, i_ΔUmax, ::SimModel, ::TranscriptionMethod end "Unset `i_x̂min` and `i_x̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nΔU, nX̂) +function deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nΔU, nX̂) nx̂ = length(i_x̂min) x̂0min, x̂0max = @views Z̃min[nΔU+nX̂-nx̂+1:nΔU+nX̂], @views Z̃max[nΔU+nX̂-nx̂+1:nΔU+nX̂] foreach(i -> !isinf(x̂0min[i]) && (i_x̂min[i] = false), eachindex(x̂0min)) foreach(i -> !isinf(x̂0max[i]) && (i_x̂max[i] = false), eachindex(x̂0max)) return i_x̂min, i_x̂max end -deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModel, ::SingleShooting, _, _, _, _) = i_x̂min, i_x̂max +deletex̂end_lincon!(i_x̂min, i_x̂max, ::SimModelODE, ::SingleShooting, _, _, _, _) = i_x̂min, i_x̂max @doc raw""" linconstraint!(mpc::PredictiveController, model::LinModel, ::TranscriptionMethod) @@ -930,7 +930,7 @@ end """ linconstrainteq!( - mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod + mpc::PredictiveController, ::SimModelODE, ::StateEstimator, ::TranscriptionMethod ) By default, fallback to doing same the but using the shorter equations. @@ -940,7 +940,7 @@ constraints of [`OrthogonalCollocation`](@ref), if applicable. See [`init_defect for the equation. """ function linconstrainteq!( - mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod + mpc::PredictiveController, ::SimModelODE, ::StateEstimator, ::TranscriptionMethod ) FS = mpc.con.FS mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS @@ -972,7 +972,7 @@ end "No linear equality constraints for other cases of [`InternalModel`](@ref)." linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel, ::TranscriptionMethod) = nothing "No linear equality constraints for all cases of [`SingleShooting`](@ref) (N/A)." -linconstrainteq!(::PredictiveController, ::SimModel, ::StateEstimator, ::SingleShooting) = nothing +linconstrainteq!(::PredictiveController, ::SimModelODE, ::StateEstimator, ::SingleShooting) = nothing linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel, ::SingleShooting) = nothing @doc raw""" @@ -1147,7 +1147,7 @@ end @doc raw""" predict!( - Ŷ0, x̂0end, X̂0, Û0, K, + Ŷ0, x̂0end, X̂0, Û0, K̄, mpc::PredictiveController, model::NonLinModel, transcription::SingleShooting, U0, _ ) -> Ŷ0, x̂0end @@ -1165,7 +1165,7 @@ The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K` arguments. The augm for ``j = 0, 1, ... , H_p``. """ function predict!( - Ŷ0, x̂0end, X̂0, Û0, K, + Ŷ0, x̂0end, X̂0, Û0, K̄, mpc::PredictiveController, model::NonLinModel, ::SingleShooting, U0, _ ) @@ -1176,7 +1176,7 @@ function predict!( for j=1:Hp u0 = @views U0[(1 + nu*(j-1)):(nu*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] x̂0next = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] f̂!(x̂0next, û0, k̄, mpc.estim, model, x̂0, u0, d̂0) x̂0 = @views X̂0[(1 + nx̂*(j-1)):(nx̂*j)] @@ -1313,14 +1313,14 @@ end @doc raw""" con_nonlinprogeq!( - geq, X̂0, Û0, K + geq, X̂0, Û0, K̄ mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) -> geq Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). -The method mutates the `geq`, `X̂0`, `Û0` and `K` vectors in argument. The defects of the +The method mutates the `geq`, `X̂0`, `Û0` and `K̄` vectors in argument. The defects of the stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). The defects of the deterministic states are computed with: ```math @@ -1334,7 +1334,7 @@ state update function [`f!`](@ref). The disturbed input ``\mathbf{û_0}`` is def [`f̂!`](@ref) documentation. """ function con_nonlinprogeq!( - geq, X̂0, Û0, K, + geq, X̂0, Û0, K̄, mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, U0, Z̃ ) @@ -1354,7 +1354,7 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄ = @views K[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] @@ -1366,7 +1366,7 @@ end @doc raw""" con_nonlinprogeq!( - geq, _ , Û0, K̇ + geq, _ , Û0, K̄ mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) -> geq @@ -1391,7 +1391,7 @@ in which ``h`` is the hold order `transcription.h` and the disturbed input ``\ma is defined in [`f̂!`](@ref) documentation. """ function con_nonlinprogeq!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, U0, Z̃ ) @@ -1413,17 +1413,17 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̇ = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*(j-1) + nx)] - k̇1, k̇2 = @views k̇[1:nx], k̇[nx+1:2*nx] + k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2*nx] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the # last iteration (j-1) may not be executed (iterations are re-orderable) model.f!(k̇1, x̂d_Z̃, û0, d̂0, model.p) else - k̇1 .= @views K̇[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 + k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 end if h < 1 model.f!(k̇2, x̂dnext_Z̃, û0, d̂0next, model.p) @@ -1440,7 +1440,7 @@ end @doc raw""" con_nonlinprogeq!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) -> geq @@ -1469,7 +1469,7 @@ described in [`init_orthocolloc`](@ref). The defects for the continuity constrai stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). """ function con_nonlinprogeq!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, mpc::PredictiveController, model::NonLinModel, transcription::OrthogonalCollocation, U0, Z̃ ) @@ -1480,7 +1480,7 @@ function con_nonlinprogeq!( Mo, no, τ = mpc.Mo, transcription.no, transcription.τ nk̄ = get_nk̄(model, transcription) D̂0 = mpc.D̂0 - X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk̄*Hp)] + X̂0_Z̃, K̄_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk̄*Hp)] D̂temp = mpc.buffer.D̂ Û0 = disturbedinput!(Û0, mpc.estim, mpc.estim.x̂0, X̂0_Z̃, U0) @threadsif f_threads for j=1:Hp @@ -1492,22 +1492,22 @@ function con_nonlinprogeq!( d̂0 = @views D̂0[(1 + nd*(j-2)):(nd*(j-1))] end û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄dot = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] - k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] + k̄_Z̃ = @views K̄_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] d̂0next = @views D̂0[(1 + nd*(j-1)):(nd*j)] - ŝk = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] + ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] # ----------------- collocation constraint defects ----------------------------- - Δk = k̄dot + Δk = k̄ for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ end - mul!(ŝk, Mo, Δk) + mul!(ŝk̄, Mo, Δk) d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)] if h > 0 ûi = similar(û0) # TODO: remove this allocation end for i=1:no - k̇i = @views k̄dot[(1 + (i-1)*nx):(i*nx)] + k̇i = @views k̄[(1 + (i-1)*nx):(i*nx)] ki_Z̃ = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] d̂i .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next if h < 1 @@ -1519,10 +1519,10 @@ function con_nonlinprogeq!( model.f!(k̇i, ki_Z̃, ûi, d̂i, model.p) end end - ŝk .-= k̄dot + ŝk̄ .-= k̄ end return geq end "No eq. constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq +con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModelODE,::TranscriptionMethod,_,_)=geq diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 7b258fe9c..e9e418920 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -174,7 +174,7 @@ where ``\mathbf{e}(k)`` is an unknown zero mean white noise and ``\mathbf{A_s} = it is thus ignored. The function [`init_integrators`](@ref) builds the state-space matrices. """ function init_estimstoch( - model::SimModel{NT}, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt + model::SimModelODE{NT}, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt ) where {NT<:Real} nu, ny, nym = model.nu, model.ny, length(i_ym) As_u , Cs_u , nint_u = init_integrators(nint_u , nu , "u") @@ -198,7 +198,7 @@ function validate_ym(model::SimModel, i_ym) end "Convert the measured outputs stochastic model `stoch_ym` to all outputs `stoch_y`." -function stoch_ym2y(model::SimModel{NT}, i_ym, Asm, Bsm, Csm, Dsm) where {NT<:Real} +function stoch_ym2y(model::SimModelODE{NT}, i_ym, Asm, Bsm, Csm, Dsm) where {NT<:Real} As = Asm Bs = Bsm Cs = zeros(NT, model.ny, size(Csm,2)) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 62d6456af..f4c73a95a 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -14,7 +14,7 @@ function remove_op!(estim::StateEstimator, ym, d, u=nothing) end @doc raw""" - f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModel, x̂0, u0, d0) -> nothing + f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModelODE, x̂0, u0, d0) -> nothing Mutating state update function ``\mathbf{f̂}`` of the augmented model. @@ -61,7 +61,7 @@ The operating points are handled inside ``\mathbf{f̂}``. See Extended Help for are computed by [`augment_model`](@ref) (almost always zeros in practice for [`NonLinModel`](@ref)). """ -function f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModel, x̂0, u0, d0) +function f̂!(x̂0next, û0, k, estim::StateEstimator, model::SimModelODE, x̂0, u0, d0) return f̂!(x̂0next, û0, k, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0) end @@ -92,11 +92,11 @@ function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) end """ - f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) + f̂!(x̂0next, û0, k, model::SimModelODE, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) -Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. +Same than [`f̂!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. """ -function f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) +function f̂!(x̂0next, û0, k, model::SimModelODE, As, Cs_u, f̂op, x̂op, x̂0, u0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] @views xdnext, xsnext = x̂0next[1:model.nx], x̂0next[model.nx+1:end] @@ -109,11 +109,11 @@ function f̂!(x̂0next, û0, k, model::SimModel, As, Cs_u, f̂op, x̂op, x̂0, end @doc raw""" - ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing + ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, d0) -> nothing Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ref). """ -function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) +function ĥ!(ŷ0, estim::StateEstimator, model::SimModelODE, x̂0, d0) return ĥ!(ŷ0, model, estim.Cs_y, x̂0, d0) end @@ -129,11 +129,11 @@ function ĥ!(ŷ0, estim::StateEstimator, ::LinModel, x̂0, d0) end """ - ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0) + ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) -Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. +Same than [`ĥ!`](@ref) for [`SimModelODE`](@ref) but without the `estim` argument. """ -function ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0) +function ĥ!(ŷ0, model::SimModelODE, Cs_y, x̂0, d0) # `@views` macro avoid copies with matrix slice operator e.g. [a:b] @views xd, xs = x̂0[1:model.nx], x̂0[model.nx+1:end] h!(ŷ0, model, xd, d0, model.p) # y0 = h(xd, d0) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index ac65017de..cf51d59f7 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -1,4 +1,4 @@ -struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} +struct InternalModel{NT<:Real, SM<:SimModelODE} <: StateEstimator{NT} model::SM x̂op::Vector{NT} f̂op::Vector{NT} @@ -30,7 +30,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} buffer::StateEstimatorBuffer{NT} function InternalModel{NT}( model::SM, i_ym, Asm, Bsm, Csm, Dsm - ) where {NT<:Real, SM<:SimModel} + ) where {NT<:Real, SM<:SimModelODE} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ nym, nyu = validate_ym(model, i_ym) validate_internalmodel(model, nym, Csm, Dsm) @@ -61,7 +61,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} end @doc raw""" - InternalModel(model::SimModel; i_ym=1:model.ny, stoch_ym=ss(I,I,I,I,model.Ts)) + InternalModel(model::SimModelODE; i_ym=1:model.ny, stoch_ym=ss(I,I,I,I,model.Ts)) Construct an internal model estimator based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). @@ -108,7 +108,7 @@ function InternalModel( model::SM; i_ym::AbstractVector{Int} = 1:model.ny, stoch_ym::LTISystem = (In = I(length(i_ym)); ss(In, In, In, In, model.Ts)) -) where {NT<:Real, SM<:SimModel{NT}} +) where {NT<:Real, SM<:SimModelODE{NT}} stoch_ym = minreal(ss(stoch_ym)) if iscontinuous(stoch_ym) stoch_ym = c2d(stoch_ym, model.Ts, :tustin) @@ -124,7 +124,7 @@ function InternalModel( end "Validate if deterministic `model` and stochastic model `Csm, Dsm` for `InternalModel`s." -function validate_internalmodel(model::SimModel, nym, Csm, Dsm) +function validate_internalmodel(model::SimModelODE, nym, Csm, Dsm) validate_poles(model) if size(Csm,1) ≠ nym || size(Dsm,1) ≠ nym error("Stochastic model output quantity ($(size(Csm,1))) is different from "* @@ -144,7 +144,7 @@ function validate_poles(model::LinModel) end return nothing end -validate_poles(::SimModel) = nothing +validate_poles(::SimModelODE) = nothing @doc raw""" matrices_internalmodel(model::LinModel) -> Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op @@ -162,7 +162,7 @@ function matrices_internalmodel(model::LinModel) return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op end "Return empty matrices, and `x̂op` & `f̂op` vectors, if `model` is not a [`LinModel`](@ref)." -function matrices_internalmodel(model::SimModel{NT}) where NT<:Real +function matrices_internalmodel(model::SimModelODE{NT}) where NT<:Real nu, nx, nd, ny = model.nu, model.nx, model.nd, model.ny Â, B̂u, Ĉ, B̂d, D̂d = zeros(NT,0,nx), zeros(NT,0,nu), zeros(NT,ny,0), zeros(NT,0,nd), zeros(NT,ny,0) x̂op, f̂op = copy(model.xop), copy(model.fop) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 52d573d77..a29761697 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1,4 +1,4 @@ -"Abstract supertype of all Kalman-type state estimators." +"Supertype of all Kalman-type state estimators." abstract type KalmanEstimator{NT<:Real} <: StateEstimator{NT} end struct SteadyKalmanFilter{ @@ -527,7 +527,7 @@ end struct UnscentedKalmanFilter{ NT<:Real, - SM<:SimModel, + SM<:SimModelODE, KC<:KalmanCovariances } <: KalmanEstimator{NT} model::SM @@ -567,7 +567,7 @@ struct UnscentedKalmanFilter{ buffer::StateEstimatorBuffer{NT} function UnscentedKalmanFilter{NT}( model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; direct=true - ) where {NT<:Real, SM<:SimModel{NT}, KC<:KalmanCovariances} + ) where {NT<:Real, SM<:SimModelODE{NT}, KC<:KalmanCovariances} nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄ 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) @@ -600,9 +600,9 @@ struct UnscentedKalmanFilter{ end @doc raw""" - UnscentedKalmanFilter(model::SimModel; ) + UnscentedKalmanFilter(model::SimModelODE; ) -Construct an unscented Kalman Filter with the [`SimModel`](@ref) `model`. +Construct an unscented Kalman Filter with the [`SimModelODE`](@ref) `model`. Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The unscented Kalman filter is based on the process model : @@ -632,7 +632,7 @@ This estimator is allocation-free if `model` simulations do not allocate. !!! info Keyword arguments with *`emphasis`* are non-Unicode alternatives. -- `model::SimModel` : (deterministic) model for the estimations. +- `model::SimModelODE` : (deterministic) model for the estimations. - `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest are unmeasured ``\mathbf{y^u}``. - `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate @@ -710,7 +710,7 @@ function UnscentedKalmanFilter( α = alpha, β = beta, κ = kappa, -) where {NT<:Real, SM<:SimModel{NT}} +) where {NT<:Real, SM<:SimModelODE{NT}} # estimated covariances matrices (variance = σ²) : P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) @@ -729,7 +729,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat """ 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}} +) where {NT<:Real, SM<:SimModelODE{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) return UnscentedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct) @@ -907,7 +907,7 @@ end struct ExtendedKalmanFilter{ NT<:Real, - SM<:SimModel, + SM<:SimModelODE, KC<:KalmanCovariances, JB<:AbstractADType, FF<:Function, @@ -952,7 +952,7 @@ struct ExtendedKalmanFilter{ jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, direct=true ) where { NT<:Real, - SM<:SimModel, + SM<:SimModelODE, KC<:KalmanCovariances, JB<:AbstractADType, FF<:Function, @@ -988,9 +988,9 @@ struct ExtendedKalmanFilter{ end @doc raw""" - ExtendedKalmanFilter(model::SimModel; ) + ExtendedKalmanFilter(model::SimModelODE; ) -Construct an extended Kalman Filter with the [`SimModel`](@ref) `model`. +Construct an extended Kalman Filter with the [`SimModelODE`](@ref) `model`. Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model is identical to [`UnscentedKalmanFilter`](@ref). By default, the Jacobians of the augmented @@ -1004,7 +1004,7 @@ differentiation. This estimator is allocation-free if `model` simulations do not !!! info Keyword arguments with *`emphasis`* are non-Unicode alternatives. -- `model::SimModel` : (deterministic) model for the estimations. +- `model::SimModelODE` : (deterministic) model for the estimations. - `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest are unmeasured ``\mathbf{y^u}``. - `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate @@ -1068,7 +1068,7 @@ function ExtendedKalmanFilter( σQint_u = sigmaQint_u, σPint_ym_0 = sigmaPint_ym_0, σQint_ym = sigmaQint_ym, -) where {NT<:Real, SM<:SimModel{NT}} +) where {NT<:Real, SM<:SimModelODE{NT}} # estimated covariances matrices (variance = σ²) : P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) @@ -1089,7 +1089,7 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mat """ function ExtendedKalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian=AutoForwardDiff(), direct=true -) where {NT<:Real, SM<:SimModel{NT}} +) where {NT<:Real, SM<:SimModelODE{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) linfuncF̂!, linfuncĤ! = get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index c94eccb68..5bfbe7b68 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -39,7 +39,7 @@ function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n; firstchars=no nZ̃, nε = length(estim.Z̃), estim.nε nA, nAeq = sum(estim.con.i_b), size(estim.con.Aeq, 1) ng, nc, neq = sum(estim.con.i_g), estim.con.nc, estim.con.neq - m = maximum(ndigits.((nZ̃, nA, ng))) + 1 + m = maximum(ndigits.((nZ̃, nA, nAeq, ng, neq))) + 1 i_nZ̃min, i_nZ̃max = @. !isinf(estim.con.Z̃min), !isinf(estim.con.Z̃max) nZ̃bounds = sum(i_nZ̃min) + sum(i_nZ̃max) println(io) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2e22e49eb..eb5bfa208 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -187,7 +187,7 @@ struct MovingHorizonEstimator{ E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ = init_predmat_mhe( model, transcription, direct, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) - Mo, Co, λo = init_orthocolloc(model, transcription) + Mo, Co, λo = init_orthocolloc(NT, transcription, model.nx, model.Ts) ES, GS, JS, BS = init_defectmat_mhe( model, transcription, direct, He, Â, B̂u, B̂d, x̂op, f̂op, As, Co, λo ) @@ -617,7 +617,7 @@ function MovingHorizonEstimator( P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0, He) gc! = get_mutating_gc_mhe(NT, gc) - hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMHE_HESSIAN) + hessian = validate_hessian(hessian, DEFAULT_NONLINMHE_HESSIAN, gradient) if isnothing(covestim) covestim = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end @@ -642,7 +642,7 @@ function default_optim_mhe(model::SimModel, nc) end "Default arrival covariance estimator for MHE, depending on the model type only." -function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) +function default_covestim_mhe(model::SimModelODE, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) if model isa LinModel return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) else @@ -1430,7 +1430,7 @@ function get_nonlinobj_op( He = estim.He nc, neq, ng = con.nc, con.neq, length(con.i_g) nŴ, nV̂, nX̂, ng, nZ̃ = He*nx̂, He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - nK, nU, nŶ = He*nk̄, He*nu, He*nŷ + nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myNaN = convert(JNT, NaN) @@ -1440,13 +1440,13 @@ function get_nonlinobj_op( V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) - K::Vector{JNT} = zeros(JNT, nK) + K̄::Vector{JNT} = zeros(JNT, nK̄) Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) - function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function J!(Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return obj_nonlinprog(estim, model, x̄, V̂, Ŵ, Z̃) end @@ -1455,7 +1455,7 @@ function get_nonlinobj_op( Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation window for the preparation of the gradient: @@ -1548,7 +1548,7 @@ function get_nonlincon_oracle( i_g = findall(con.i_g) # convert to non-logical indices for non-allocating @views ngi = sum(con.i_g) nŴ, nV̂, nX̂, nZ̃ = He*nx̂, He*nym, He*nx̂, length(estim.Z̃) - nK, nU, nŶ = He*nk̄, He*nu, He*nŷ + nK̄, nU, nŶ = He*nk̄, He*nu, He*nŷ nŴe, nX̂e, nV̂e = (He+1)*nx̂, (He+1)*nx̂, (He+1)*nym strict = Val(true) myNaN, myInf = convert(JNT, NaN), convert(JNT, Inf) @@ -1557,23 +1557,23 @@ function get_nonlincon_oracle( V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) Ŵe::Vector{JNT} = zeros(JNT, nŴe) V̂e::Vector{JNT}, X̂e::Vector{JNT} = zeros(JNT, nV̂e), zeros(JNT, nX̂e) - K::Vector{JNT} = zeros(JNT, nK) + K̄::Vector{JNT} = zeros(JNT, nK̄) Û0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) gi::Vector{JNT} = zeros(JNT, ngi) λi::Vector{JNT}, λeq::Vector{JNT} = rand(JNT, ngi), rand(JNT, neq) # -------------- inequality constraint: nonlinear oracle ------------------------- - function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function gi!(gi, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return nothing end - function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, gi) + function ℓ_gi(Z̃, λi, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, gi) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) gi .= @views g[i_g] return dot(λi, gi) @@ -1583,7 +1583,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) # temporarily "fill" the estimation windows for the preparation of the gradient: @@ -1597,7 +1597,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq), Cache(gi) ) @@ -1643,15 +1643,15 @@ function get_nonlincon_oracle( eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²gi_func! ) # ------------- equality constraints : nonlinear oracle ------------------------------ - function geq!(geq, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g) + function geq!(geq, Z̃, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return nothing end - function ℓ_geq(Z̃, λeq, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq) + function ℓ_geq(Z̃, λeq, x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq) update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim, Z̃ ) return dot(λeq, geq) end @@ -1660,7 +1660,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g) ) estim.Nk[] = He # see comment above @@ -1673,7 +1673,7 @@ function get_nonlincon_oracle( Cache(x̂0arr), Cache(x̄), Cache(Ŵ), Cache(V̂), Cache(X̂0), Cache(Ŵe), Cache(V̂e), Cache(X̂e), - Cache(Û0), Cache(K), Cache(Ŷ0), + Cache(Û0), Cache(K̄), Cache(Ŷ0), Cache(gc), Cache(g), Cache(geq) ) estim.Nk[] = He # see comment above diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 5593586b7..c7cffa7c0 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -866,7 +866,7 @@ end """ update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) -> nothing @@ -875,18 +875,18 @@ Update in-place the vectors for the predictions of `estim` estimator at decision The method mutates all the arguments before `estim` argument. """ function update_predictions!( - x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ + x̂0arr, x̄, Ŵ, V̂, X̂0, Ŵe, V̂e, X̂e, Û0, K̄, Ŷ0, gc, g, geq, estim::MovingHorizonEstimator, Z̃ ) model, transcription = estim.model, estim.transcription x̂0arr = getarrival!(x̂0arr, estim, Z̃) x̄ = getx̄!(x̄, estim, x̂0arr) Ŵ = getŴ!(Ŵ, estim, transcription, Z̃) - V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K, Ŷ0, estim, model, transcription, x̂0arr, Ŵ, Z̃) + V̂, X̂0 = predict_mhe!(V̂, X̂0, Û0, K̄, Ŷ0, estim, model, transcription, x̂0arr, Ŵ, Z̃) Ŵe, V̂e, X̂e = extended_vectors!(Ŵe, V̂e, X̂e, estim, Ŵ, V̂, X̂0, x̂0arr) ε = getslack(estim, Z̃) gc = con_custom_mhe!(gc, estim, X̂e, V̂e, Ŵe, x̄, ε) g = con_nonlinprog_mhe!(g, estim, model, transcription, X̂0, V̂, gc, ε) - geq = con_nonlinprogeq_mhe!(geq, X̂0, Û0, K, estim, model, transcription, x̂0arr, Ŵ, Z̃) + geq = con_nonlinprogeq_mhe!(geq, X̂0, Û0, K̄, estim, model, transcription, x̂0arr, Ŵ, Z̃) return nothing end diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index ff28ead4f..3ca0b340e 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -369,14 +369,14 @@ end """ init_predmat_mhe( - model::SimModel, transcription::SingleShooting, direct::Bool, + model::SimModelODE, transcription::SingleShooting, direct::Bool, He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Return empty matrices for [`SingleShooting`](@ref) and non-`LinModel`, except for `ex̄`. """ function init_predmat_mhe( - model::SimModel{NT}, transcription::SingleShooting, ::Bool, + model::SimModelODE{NT}, transcription::SingleShooting, ::Bool, He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) @@ -397,14 +397,14 @@ end """ init_predmat_mhe( - model::SimModel, transcription::TranscriptionMethod, direct::Bool + model::SimModelODE, transcription::TranscriptionMethod, direct::Bool He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂ Return `ex̄, EX̂, GX̂, JX̂, BX̂` and empty matrices non-`LinModel` and other [`TranscriptionMethod`](@ref). """ function init_predmat_mhe( - model::SimModel{NT}, transcription::TranscriptionMethod, ::Bool, + model::SimModelODE{NT}, transcription::TranscriptionMethod, ::Bool, He, Â, _ , Ĉm, _ , _ , _ , _ ) where {NT<:Real} nym, nx̂ = size(Ĉm, 1), size(Â, 2) @@ -510,7 +510,7 @@ end @doc raw""" init_defectmat_mhe( - model::SimModel, transcription::TranscriptionMethod, direct::Bool, + model::SimModelODE, transcription::TranscriptionMethod, direct::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) -> ES, GS, JS, BS @@ -546,7 +546,7 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, ::TranscriptionMethod, ::Bool, + model::SimModelODE{NT}, ::TranscriptionMethod, ::Bool, He, Â, _ , _ , _ , _ , As, _ , _ ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) @@ -569,7 +569,7 @@ end @doc raw""" init_defectmat_mhe( - model::SimModel, transcription::OrthogonalCollocation, direct::Bool + model::SimModelODE, transcription::OrthogonalCollocation, direct::Bool He, Â, _ , _ , _ , _ , As, Co, λo ) -> ES, GS, JS, BS @@ -612,7 +612,7 @@ The matrix ``\mathbf{E_S}`` is defined in the Extended Help section. ``` """ function init_defectmat_mhe( - model::SimModel{NT}, transcription::OrthogonalCollocation, ::Bool, + model::SimModelODE{NT}, transcription::OrthogonalCollocation, ::Bool, He, Â, _ , _ , _ , _ , As, Co, λo ) where {NT<:Real} nx̂, nxs = size(Â, 2), size(As, 2) @@ -637,9 +637,9 @@ function init_defectmat_mhe( return ES, GS, JS, BS end -"Return empty matrices for [`SingleShooting`](@ref) transcription on any `SimModel` (N/A)." +"Return empty matrices for [`SingleShooting`](@ref) transcription on any `SimModelODE` (N/A)." function init_defectmat_mhe( - model::SimModel{NT}, transcription::SingleShooting, ::Bool, + model::SimModelODE{NT}, transcription::SingleShooting, ::Bool, He, Â, _ , _ , _ , _ , _ , _ , _ ) where {NT<:Real} nx̂ = size(Â, 2) @@ -648,7 +648,7 @@ function init_defectmat_mhe( end function init_defectmat_mhe_empty( - model::SimModel{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ + model::SimModelODE{NT}, transcription::TranscriptionMethod, He, nx̂, nŵ ) where {NT<:Real} nu, nd = model.nu, model.nd nk̄ = get_nk̄(model, transcription) @@ -784,7 +784,7 @@ boxconstraint_states!(Z̃min, Z̃max, ::SingleShooting, _, _, _, _, _, _) = Z̃m "Unset `i_x̂min` and `i_x̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." function deletex̂arr_lincon!( - i_x̂min, i_x̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nε + i_x̂min, i_x̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nε ) nx̂ = length(i_x̂min) x̂0min, x̂0max = @views Z̃min[(nε+1):(nε+nx̂)], @views Z̃max[(nε+1):(nε+nx̂)] @@ -795,7 +795,7 @@ end "Unset `i_X̂min` and `i_X̂max` elements if finite box constraints in `Z̃min` and `Z̃max`." function deleteX̂_lincon!( - i_X̂min, i_X̂max, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max, nε, nx̂ + i_X̂min, i_X̂max, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max, nε, nx̂ ) nx̃ = nε + nx̂ nX̂ = length(i_X̂min) @@ -804,10 +804,10 @@ function deleteX̂_lincon!( foreach(i -> !isinf(X̂0max[i]) && (i_X̂max[i] = false), eachindex(i_X̂max)) return i_X̂min, i_X̂max end -deleteX̂_lincon!(i_X̂min, i_X̂max, ::SimModel, ::SingleShooting, _, _, _, _) = i_X̂min, i_X̂max +deleteX̂_lincon!(i_X̂min, i_X̂max, ::SimModelODE, ::SingleShooting, _, _, _, _) = i_X̂min, i_X̂max "Unset `i_Ŵmin` and `i_Ŵmax` elements if finite box constraints in `Z̃min` and `Z̃max`." -function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModel, ::TranscriptionMethod, Z̃min, Z̃max) +function deleteŴ_lincon!(i_Ŵmin, i_Ŵmax, ::SimModelODE, ::TranscriptionMethod, Z̃min, Z̃max) nŴ = length(i_Ŵmin) Ŵmin, Ŵmax = @views Z̃min[end-nŴ+1:end], Z̃max[end-nŴ+1:end] foreach(i -> !isinf(Ŵmin[i]) && (i_Ŵmin[i] = false), eachindex(i_Ŵmin)) @@ -978,7 +978,7 @@ end """ linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModelODE, transcription::TranscriptionMethod ) By default, only update `Aeq` when `Nk < He` for other [`TranscriptionMethod`](@ref). @@ -988,7 +988,7 @@ vector is only zeros for this specific case. See [`init_defectmat_mhe`](@ref) fo equations. """ function linconstrainteq!( - estim::MovingHorizonEstimator, ::SimModel, transcription::TranscriptionMethod + estim::MovingHorizonEstimator, ::SimModelODE, transcription::TranscriptionMethod ) optim, con, Nk = estim.optim, estim.con, estim.Nk[] nŝ = size(con.Aeq, 1) ÷ estim.He # number of state defects per time step @@ -1022,7 +1022,7 @@ function linconstrainteq!( return nothing end "No linear equality constraints for all cases of [`SingleShooting`](@ref)." -linconstrainteq!(::MovingHorizonEstimator, ::SimModel, ::SingleShooting) = nothing +linconstrainteq!(::MovingHorizonEstimator, ::SimModelODE, ::SingleShooting) = nothing @doc raw""" set_warmstart_mhe!( @@ -1320,7 +1320,7 @@ end @doc raw""" predict_mhe!( - V̂, X̂0, Û0, K, Ŷ0, + V̂, X̂0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, x̂0arr, Ŵ, _ ) -> V̂, X̂0 @@ -1332,7 +1332,7 @@ The function mutates `V̂`, `X̂0`, `Û0`, `K` and `Ŷ0` vector arguments. The and by adding the estimated process noise ``\mathbf{ŵ}``. """ function predict_mhe!( - V̂, X̂0, Û0, K, Ŷ0, + V̂, X̂0, Û0, K̄, Ŷ0, estim::MovingHorizonEstimator, model::NonLinModel, ::SingleShooting, x̂0arr, Ŵ, _ ) @@ -1344,7 +1344,7 @@ function predict_mhe!( u0 = @views estim.U0[(1+nu*(j-1)):(nu*j)] d0 = @views estim.D0[(1+nd*(j+p-1)):(nd*(j+p))] ŵ = @views Ŵ[(1+nŵ*(j-1)):(nŵ*j)] - k̄ = @views K[(1+nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1+nk̄*(j-1)):(nk̄*j)] û0 = @views Û0[(1+nu*(j-1)):(nu*j)] x̂0next = @views X̂0[(1+nx̂*(j-1)):(nx̂*j)] f̂!(x̂0next, û0, k̄, estim, model, x̂0, u0, d0) @@ -1521,7 +1521,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K, + geq, X̂0, Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::MultipleShooting, x̂0arr, Ŵ, Z̃ ) -> geq @@ -1539,7 +1539,7 @@ for ``j = 0, 1, ... , N_k-1`` and in which the augmented state vectors ``\mathbf extracted from the decision variable `Z̃`. The function ``\mathbf{f̂}`` is defined at [`f̂!`](@ref). """ function con_nonlinprogeq_mhe!( - geq, X̂0, Û0, K, + geq, X̂0, Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::MultipleShooting, x̂0arr, Ŵ, Z̃ ) @@ -1559,7 +1559,7 @@ function con_nonlinprogeq_mhe!( x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)] end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] - k̄ = @views K[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext = @views X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] @@ -1575,7 +1575,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::TrapezoidalCollocation, x̂0arr, Ŵ, Z̃ ) -> geq @@ -1602,7 +1602,7 @@ in which ``h`` is the hold order `transcription.h` and the disturbed input ``\ma is defined in [`f̂!`](@ref) documentation. """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::TrapezoidalCollocation, x̂0arr, Ŵ, Z̃ ) @@ -1625,18 +1625,18 @@ function con_nonlinprogeq_mhe!( end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄dot = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] ŵd = @views Ŵ[(1 + nŵ*(j-1)):(nŵ*(j-1) + nw)] x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)] ŝdnext = @views geq[(1 + nx*(j-1)):(nx*j)] - k̇1, k̇2 = @views k̄dot[1:nx], k̄dot[nx+1:2*nx] + k̇1, k̇2 = @views k̄[1:nx], k̄[nx+1:2*nx] d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] if f_threads || h < 1 || j < 2 # we need to recompute k1 with multi-threading, even with h==1, since the # last iteration (j-1) may not be executed (iterations are re-orderable) model.f!(k̇1, x̂d_Z̃, û0, d0, model.p) else - k̇1 .= @views K̇[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 + k̇1 .= @views K̄[(1 + nk̄*(j-1)-nx):(nk̄*(j-1))] # k2 of of the last iter. j-1 end if h < 1 model.f!(k̇2, x̂dnext_Z̃, û0, d0next, model.p) @@ -1654,7 +1654,7 @@ end @doc raw""" con_nonlinprogeq_mhe!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, ::OrthogonalCollocation, x̂0arr, _ , Z̃ ) -> geq @@ -1685,7 +1685,7 @@ stochastic states are linear equality constraints (see [`init_defectmat_mhe`](@r estimated process noise ``\mathbf{ŵ}(ℓ+j)`` are incorporated in the continuity constraint. """ function con_nonlinprogeq_mhe!( - geq, _ , Û0, K̇, + geq, _ , Û0, K̄, estim::MovingHorizonEstimator, model::NonLinModel, transcription::OrthogonalCollocation, x̂0arr, _ , Z̃ ) @@ -1708,22 +1708,22 @@ function con_nonlinprogeq_mhe!( end d0 = @views estim.D0[(1 + nd*(j+p-1)):(nd*(j+p))] û0 = @views Û0[(1 + nu*(j-1)):(nu*j)] - k̄dot = @views K̇[(1 + nk̄*(j-1)):(nk̄*j)] + k̄ = @views K̄[(1 + nk̄*(j-1)):(nk̄*j)] k̄_Z̃ = @views K_Z̃[(1 + nk̄*(j-1)):(nk̄*j)] - ŝk = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] + ŝk̄ = @views geq[(1 + nk̄*(j-1)):(nk̄*j)] d0next = @views estim.D0[(1 + nd*(j+p)):(nd*(j+p+1))] # ----------------- collocation constraint defects ----------------------------- - Δk = k̄dot + Δk = k̄ for i=1:no Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃ end - mul!(ŝk, Mo, Δk) + mul!(ŝk̄, Mo, Δk) di = @views Dtemp[(1 + nd*(j-1)):(nd*j)] if h > 0 ûi = similar(û0) # TODO: remove this allocation end for i=1:no - k̇i = @views k̄dot[(1 + (i-1)*nx):(i*nx)] + k̇i = @views k̄[(1 + (i-1)*nx):(i*nx)] ki_Z̃ = @views k̄_Z̃[(1 + (i-1)*nx):(i*nx)] di .= (1-τ[i]).*d0 .+ τ[i].*d0next if h < 1 @@ -1735,11 +1735,11 @@ function con_nonlinprogeq_mhe!( model.f!(k̇i, ki_Z̃, ûi, di, model.p) end end - ŝk .-= k̄dot + ŝk̄ .-= k̄ end Nk < He && (geq[nk̄*Nk+1:end] .= 0) return geq end "No nonlinear eq. const. for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged." -con_nonlinprogeq_mhe!(geq,_,_,_,::MovingHorizonEstimator, ::SimModel, ::TranscriptionMethod, _,_,_) = geq \ No newline at end of file +con_nonlinprogeq_mhe!(geq,_,_,_,::MovingHorizonEstimator, ::SimModelODE, ::TranscriptionMethod, _,_,_) = geq \ No newline at end of file diff --git a/src/general.jl b/src/general.jl index e7272e689..e81d7c6f6 100644 --- a/src/general.jl +++ b/src/general.jl @@ -62,20 +62,21 @@ end "Convert getinfo dictionary to a debug string (without any truncation)." function info2debugstr(info) + sol_keys = filter(key->startswith(string(key), "sol"), keys(info)) mystr = "Content of getinfo dictionary:\n" for (key, value) in info - (key == :sol) && continue + key in sol_keys && continue # skip the sol keys for now if key in HIDDEN_GETINFO_KEYS_MHE || key in HIDDEN_GETINFO_KEYS_MPC # skip the redundant non-Unicode keys continue end mystr *= " :$key => $value\n" end - if haskey(info, :sol) - split_sol = split(string(info[:sol]), "\n") + for sol_key in sol_keys + split_sol = split(string(info[sol_key]), "\n") # Add the treeview prefix to each line solstr = join((" " * line for line in split_sol), "\n") - mystr *= " :sol => \n" * solstr * "\n" # Ensure a trailing newline + mystr *= " :$sol_key => \n" * solstr * "\n" # Ensure a trailing newline end return mystr end @@ -196,7 +197,7 @@ get_ncolors(::Prep) = nothing get_ncolors(prep::Union{SparseJacobianPrep, SparseHessianPrep}) = ncolors(prep) "Validate `hessian` keyword argument and return the differentiation `backend`." -function validate_hessian(hessian, gradient, default) +function validate_hessian(hessian, default, gradient=nothing) if hessian == true backend = default elseif hessian == false || isnothing(hessian) @@ -204,7 +205,7 @@ function validate_hessian(hessian, gradient, default) else backend = hessian end - if !isnothing(backend) + if !isnothing(gradient) && !isnothing(backend) hess = dense_backend(backend) grad = dense_backend(gradient) if hess != grad diff --git a/src/model/linearization.jl b/src/model/linearization.jl index eb916dca3..2963c9b75 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -53,7 +53,7 @@ end @doc raw""" - linearize(model::SimModel; x=model.x0+model.xop, u=model.uop, d=model.dop) -> linmodel + linearize(model::SimModelODE; x=model.x0+model.xop, u=model.uop, d=model.dop) -> linmodel Linearize `model` at the operating points `x`, `u`, `d` and return the [`LinModel`](@ref). @@ -120,7 +120,7 @@ julia> linmodel.A `h` functions must be compatible with this feature though. See [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator) for common mistakes when writing these functions. """ -function linearize(model::SimModel{NT}; kwargs...) where NT<:Real +function linearize(model::SimModelODE{NT}; kwargs...) where NT<:Real nu, nx, ny, nd = model.nu, model.nx, model.ny, model.nd A = Matrix{NT}(undef, nx, nx) Bu = Matrix{NT}(undef, nx, nu) @@ -136,7 +136,7 @@ function linearize(model::SimModel{NT}; kwargs...) where NT<:Real end """ - linearize!(linmodel::LinModel, model::SimModel; ) -> linmodel + linearize!(linmodel::LinModel, model::SimModelODE; ) -> linmodel Linearize `model` and store the result in `linmodel` (in-place). @@ -157,7 +157,7 @@ julia> linearize!(linmodel, model, x=[20.0], u=[0.0]); linmodel.A ``` """ function linearize!( - linmodel::LinModel, model::SimModel; + linmodel::LinModel, model::SimModelODE; x=(model.buffer.x.=model.x0.+model.xop), u=model.uop, d=model.dop ) nonlinmodel = model @@ -193,7 +193,7 @@ function linearize!( end "Call `linfunc!` function to compute the Jacobians of `model` at the linearization point." -function linearize_core!(linmodel::LinModel, model::SimModel, x, u, d) +function linearize_core!(linmodel::LinModel, model::SimModelODE, x, u, d) xnext, y = linmodel.buffer.x, linmodel.buffer.y A, Bu, C, Bd, Dd = linmodel.A, linmodel.Bu, linmodel.C, linmodel.Bd, linmodel.Dd cst_x = Constant(x) diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index b16dc3351..087d71d37 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -1,4 +1,4 @@ -struct LinModel{NT<:Real} <: SimModel{NT} +struct LinModel{NT<:Real} <: SimModelODE{NT} A ::Matrix{NT} Bu ::Matrix{NT} C ::Matrix{NT} @@ -252,8 +252,13 @@ optional parameter `NT` explicitly set the number type of vectors (default to `F LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where NT<:Real LinModel(A, Bu, C, Bd, Dd, Ts) = LinModel{Float64}(A, Bu, C, Bd, Dd, Ts) +function validate_transcription(::LinModel, ::CollocationMethod) + throw(ArgumentError("Collocation methods are not supported for LinModel.")) + return nothing +end + @doc raw""" - steadystate!(model::LinModel, u0, d0) + initstate_core!(model::LinModel, u0, d0) Set `model.x0` to `u0` and `d0` steady-state if `model` is a [`LinModel`](@ref). @@ -265,7 +270,7 @@ with constant manipulated inputs ``\mathbf{u_0 = u - u_{op}}`` and measured disturbances ``\mathbf{d_0 = d - d_{op}}``. The Moore-Penrose pseudo-inverse computes ``\mathbf{(I - A)^{-1}}`` to support integrating `model` (integrator states will be 0). """ -function steadystate!(model::LinModel, u0, d0) +function initstate_core!(model::LinModel, u0, d0) x_tmp = model.buffer.x x_tmp .= model.fop .- model.xop mul!(x_tmp, model.Bu, u0, 1, 1) diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index ebc774ff5..9e7072980 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -1,4 +1,4 @@ -"Abstract supertype of all differential equation solvers." +"Supertype of all differential equation solvers." abstract type DiffSolver end "Empty solver for nonlinear discrete-time models." @@ -22,7 +22,7 @@ struct NonLinModel{ PT<:Any, JB<:AbstractADType, LF<:Function -} <: SimModel{NT} +} <: SimModelODE{NT} x0::Vector{NT} solver::DS f!::F @@ -93,11 +93,11 @@ end NonLinModel{NT}(f::Function, h::Function, Ts, nu, nx, ny, nd=0; ) NonLinModel{NT}(f!::Function, h!::Function, Ts, nu, nx, ny, nd=0; ) -Construct a nonlinear model from state-space functions `f`/`f!` and `h`/`h!`. +Construct a nonlinear ODE model from state-space functions `f`/`f!` and `h`/`h!`. Both continuous and discrete-time models are supported. The default arguments assume -continuous dynamics. Use `solver=nothing` for the discrete case (see Extended Help). The -functions are defined as: +continuous ordinary differentiation equations (ODE). Use `solver=nothing` for the discrete +case (see Extended Help). The functions are defined as: ```math \begin{aligned} \mathbf{ẋ}(t) &= \mathbf{f}\Big( \mathbf{x}(t), \mathbf{u}(t), \mathbf{d}(t), \mathbf{p} \Big) \\ @@ -130,7 +130,7 @@ form. The optional parameter `NT` explicitly set the number type of vectors (def [`ExtendedKalmanFilter`](@ref), [`MovingHorizonEstimator`](@ref) and [`linearize`](@ref), except if a finite difference backend is used (e.g. [`AutoFiniteDiff`](@extref DifferentiationInterface List)). -See also [`LinModel`](@ref). +See also [`LinModel`](@ref), and [`NonLinModelDAE`](@ref) to include algebraic equations. # Arguments - `f::Function` or `f!`: state function of the model. @@ -294,8 +294,10 @@ function validate_h(NT, h) return ismutating end -"Do nothing if `model` is a [`NonLinModel`](@ref)." -steadystate!(::SimModel, _ , _ ) = nothing +function validate_transcription(::NonLinModel{<:Real, <:EmptySolver}, ::CollocationMethod) + throw(ArgumentError("Collocation methods require continuous-time NonLinModel.")) + return nothing +end """ LinModel(model::NonLinModel; x=model.x0+model.xop, u=model.uop, d=model.dop) diff --git a/src/model/nonlinmodeldae.jl b/src/model/nonlinmodeldae.jl new file mode 100644 index 000000000..6e236d145 --- /dev/null +++ b/src/model/nonlinmodeldae.jl @@ -0,0 +1,843 @@ +const DEFAULT_NONLINDAE_HESSIAN = AutoForwardDiff() + +struct NonLinModelDAE{ + NT<:Real, + TM<:CollocationMethod, + JMS<:JuMP.GenericModel, + JMO<:JuMP.GenericModel, + JB<:AbstractADType, + HB<:Union{AbstractADType, Nothing}, + FQ <:Function, + H <:Function, + PT<:Any, +} <: SimModelDAE{NT} + x0::Vector{NT} + a0::Vector{NT} + transcription::TM + # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be + # different since solvers that support non-Float64 are scarce. + optim_state::JMS + optim_output::JMO + jacobian::JB + hessian::HB + Z::Vector{NT} + fq!::FQ + h!::H + p::PT + Mo::Matrix{NT} + Co::Matrix{NT} + λo::NT + Ks::Matrix{NT} + Es::Matrix{NT} + Fs::Vector{NT} + Aeq::Matrix{NT} + beq::Vector{NT} + neq::Int + Ts::NT + t::Vector{NT} + nu::Int + nx::Int + na::Int + ny::Int + nd::Int + uop::Vector{NT} + yop::Vector{NT} + dop::Vector{NT} + xop::Vector{NT} + fop::Vector{NT} + uname::Vector{String} + yname::Vector{String} + dname::Vector{String} + xname::Vector{String} + x0_optim::Vector{NT} + u0_optim::Vector{NT} + d0_optim::Vector{NT} + iszero_Ha::Bool + buffer::SimModelBuffer{NT} + function NonLinModelDAE{NT}( + fq!::FQ, h!::H, Ts, nu, nx, na, ny, nd, + p::PT, + transcription::TM, + optim_state::JMS, + optim_output::JMO, + jacobian::JB, hessian::HB + ) where { + NT<:Real, + TM<:CollocationMethod, + JMS<:JuMP.GenericModel, + JMO<:JuMP.GenericModel, + JB<:AbstractADType, + HB<:Union{AbstractADType, Nothing}, + FQ<:Function, + H<:Function, + PT<:Any + } + Ts > 0 || error("Sampling time Ts must be positive") + uop = zeros(NT, nu) + yop = zeros(NT, ny) + dop = zeros(NT, nd) + xop = zeros(NT, nx) + fop = zeros(NT, nx) + uname = ["\$u_{$i}\$" for i in 1:nu] + yname = ["\$y_{$i}\$" for i in 1:ny] + dname = ["\$d_{$i}\$" for i in 1:nd] + xname = ["\$x_{$i}\$" for i in 1:nx] + x0, a0 = zeros(NT, nx), zeros(NT, na) + t = zeros(NT, 1) + # the updatestate!(model, u, d) API does not know the input `u` of the next time + # step k+1, so only piecewise constant input `u` is supported here: + transcription.h > 0 && error("Only zero-order hold (h=0) is supported for simulations of DAEs") + iszero_Ha = validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) + Mo, Co, λo = init_orthocolloc(NT, transcription, nx, Ts) + nZ = get_nZ_dae(transcription, nx, na) + Z = zeros(NT, get_nZ_dae(transcription, nx, na)) + Es, Ks, Aeq = init_defectmat_dae(NT, transcription, nx, na, Co, λo) + Fs = zeros(NT, size(Aeq, 1)) + beq = zeros(NT, size(Aeq, 1)) + neq = nZ - size(Aeq, 1) # number of nonlinear equality constraints + x0_optim, u0_optim, d0_optim = zeros(NT, nx), zeros(NT, nu), zeros(NT, nd) + buffer = SimModelBuffer{NT}(nu, nx, ny, nd, 0, na) + model = new{NT, TM, JMS, JMO, JB, HB, FQ, H, PT}( + x0, a0, + transcription, + optim_state, optim_output, jacobian, hessian, + Z, + fq!, h!, + p, + Mo, Co, λo, + Ks, Es, Fs, + Aeq, beq, neq, + Ts, t, + nu, nx, na, ny, nd, + uop, yop, dop, xop, fop, + uname, yname, dname, xname, + x0_optim, u0_optim, d0_optim, + iszero_Ha, + buffer + ) + init_optimization!(model, model.optim_state, model.optim_output) + return model + end +end + +@doc raw""" + NonLinModelDAE{NT}(fq::Function, h::Function, Ts, nu, nx, na, ny, nd=0; ) + NonLinModelDAE{NT}(fq!::Function, h!::Function, Ts, nu, nx, na, ny, nd=0; ) + +Construct a nonlinear DAE model from state-space functions `fq`/`fq!` and `h`/`h!`. + +It supports continuous differential and algebraic equations (DAE). The functions are +provided in the semi-explicit form: +```math +\begin{aligned} + \mathbf{ẋ}(t) &= \mathbf{f}\Big( \mathbf{x}(t), \mathbf{a}(t), \mathbf{u}(t), \mathbf{d}(t), \mathbf{p} \Big) \\ + \mathbf{0} &= \mathbf{q}\Big( \mathbf{x}(t), \mathbf{a}(t), \mathbf{u}(t), \mathbf{d}(t), \mathbf{p} \Big) \\ + \mathbf{y}(t) &= \mathbf{h}\Big( \mathbf{x}(t), \mathbf{a}(t), \mathbf{d}(t), \mathbf{p} \Big) +\end{aligned} +``` +where ``\mathbf{x}``, ``\mathbf{y}``, ``\mathbf{u}``, ``\mathbf{d}`` and ``\mathbf{p}`` are +defined in [`NonLinModel`](@ref), and ``\mathbf{a}`` is the algebraic variable with `na` +elements. The ``\mathbf{f}`` and ``\mathbf{q}`` functions are combined into a single method +`fq`/`fq!` since they typically share common computations. If `res` represents the result of +``\mathbf{q(x, a, u, d, p)}`` (or residuals), the functions can be implemented in two +possible ways: + +1. **Non-mutating functions** (out-of-place): define them as `fq(x, a, u, d, p) -> (ẋ, res)` + and `h(x, a, d, p) -> y`. This syntax is simple and intuitive but it allocates more memory. +2. **Mutating functions** (in-place): define them as `fq!(ẋ, res, x, a, u, d, p) -> nothing` + and `h!(y, x, a, d, p) -> nothing`. This syntax reduces the allocations and potentially + the computational burden as well. + +!!! tip + Replace the `a`, `d` or `p` argument with `_` in your functions if not needed (see + Examples below). + +The optional parameter `NT` explicitly set the number type of vectors (default to `Float64`). +Open loop simulations rely on a [`CollocationMethod`](@ref) and `JuMP.jl` as a root solver +to avoid new dependencies, and also to provide a similar solving environnement as +[`NonLinMPC`](@ref), for troubleshooting. Computing the current model output ``\mathbf{y}(t)`` +also require solving the algebraic equation ``\mathbf{q}`` using `JuMP.jl`. + +!!! warning + The two functions must be in pure Julia to use the model in [`NonLinMPC`](@ref) and + [`MovingHorizonEstimator`](@ref), except if a finite difference backend is used (e.g. + [`AutoFiniteDiff`](@extref DifferentiationInterface List)). + +See also [`NonLinModel`](@ref) for ODEs. + +# Arguments +- `fq::Function` or `fq!`: combined state and algebraic function of the model. +- `h::Function` or `h!`: output function of the model. +- `Ts`: sampling time of the model in seconds. +- `nu`: number of manipulated inputs. +- `nx`: number of states. +- `na`: number of algebraic variables. +- `ny`: number of outputs. +- `nd=0`: number of measured disturbances. +- `p=[]`: parameters of the model (any type). +- `transcription=OrthogonalCollocation()` : a [`TrapezoidalCollocation`](@ref) or + [`OrthogonalCollocation`](@ref) instance for open-loop simulations. +- `optim_state=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer for [`updatestate!`](@ref), + provided as a [`JuMP.Model`](@extref) object (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). +- `optim_output=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer for [`evaloutput`](@ref), + provided as a [`JuMP.Model`](@extref) object (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer). +- `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the + nonlinear constraints, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List) +- `hessian=false` : an `AbstractADType` backend or `Bool` for the Hessian of the Lagrangian, + see `jacobian` above for the options. The default `false` skip it and use the + quasi-Newton method of `optim` (see Extended Help). + +# Examples +```jldoctest +julia> fq!(ẋ, res, x, a, u, _ , p) = (ẋ .= p*x .+ a; res .= a .- u; nothing); + +julia> h!(y, x, _ , _ , _ ) = (y .= 0.1x; nothing); + +julia> model = NonLinModelDAE(fq!, h!, 5.0, 1, 1, 1, 1, p=-0.2) +NonLinModelDAE with a sample time Ts = 5.0 s: +├ state optimizer: Ipopt +├ output optimizer: Ipopt +├ transcription: OrthogonalCollocation (3 collocation points) +├ jacobian: AutoForwardDiff +├ hessian: nothing +└ dimensions: + │ ├ 1 manipulated inputs u + │ ├ 1 states x + │ ├ 1 algebraic variables a + │ ├ 1 outputs y + │ └ 0 measured disturbances d + └ optimization: + ├ 8 decision variables Z + ├ 1 linear equality constraints Aeq + └ 7 nonlinear equality constraints geq +``` + +# Extended Help +!!! details "Extended Help" + If the dynamics are a function of the time, simply add a measured disturbance defined as + ``d(t) = t``. This object does not support the ``\mathbf{u}`` argument in ``\mathbf{h}`` + function, see the Extended Help of [`LinModel`](@ref) for the justification. More + precisely, it only supports strictly proper DAEs, so the constructor will verify there + are no global direct transmissions from ``\mathbf{u}`` to ``\mathbf{y}`` with the + functions ``mathbf{q}`` and ``\mathbf{h}`` using [`SparseConnectivityTracer.jl`](@extref SparseConnectivityTracer.jl). + + By default, a dense [`ForwardDiff`](@extref ForwardDiff) backend is used for the + Jacobians of the nonlinear equality constraints. This is also the default backend for + the Hessians if `hessian=true`. +""" +function NonLinModelDAE{NT}( + fq::Function, h::Function, Ts::Real, nu::Int, nx::Int, na::Int, ny::Int, nd::Int=0; + p=NT[], + transcription = OrthogonalCollocation(), + optim_state = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), + optim_output = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), + jacobian = DEFAULT_JACDENSE, + hessian = false, +) where {NT<:Real} + fq!, h! = get_mutating_functions_dae(NT, fq, h) + hessian = validate_hessian(hessian, DEFAULT_NONLINDAE_HESSIAN) + return NonLinModelDAE{NT}( + fq!, h!, Ts, nu, nx, na, ny, nd, p, + transcription, optim_state, optim_output, jacobian, hessian + ) +end + +function NonLinModelDAE( + fq::Function, h::Function, Ts::Real, + nu::Int, nx::Int, na::Int, ny::Int, nd::Int=0; + p=Float64[], + transcription = OrthogonalCollocation(), + optim_state = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), + optim_output = JuMP.Model(DEFAULT_NLP_OPTIMIZER, add_bridges=false), + jacobian = DEFAULT_JACDENSE, + hessian = false, +) + return NonLinModelDAE{Float64}( + fq, h, Ts, nu, nx, na, ny, nd; + p, transcription, optim_state, optim_output, jacobian, hessian + ) +end + +"Get the mutating versions of the functions `fq` and `h` for a DAE model." +function get_mutating_functions_dae(NT, fq, h) + ismutating_f_q = validate_fq_dae(NT, fq) + fq! = if ismutating_f_q + fq + else + function fq!(ẋ, res, x, a, u, d, p) + ẋ_ret, res_ret = fq(x, a, u, d, p) + ẋ .= ẋ_ret + res .= res_ret + return nothing + end + end + ismutating_h = validate_h_dae(NT, h) + h! = if ismutating_h + h + else + function h!(y, x, a, d, p) + y .= h(x, a, d, p) + return nothing + end + end + return fq!, h! +end + +""" + validate_fq_dae(NT, fq) -> ismutating + +Validate `fq` function argument signature for DAEs and return `true` if mutating. +""" +function validate_fq_dae(NT, fq) + ismutating = hasmethod( + fq, + # ẋ , res , x , a , u , d , p + Tuple{ Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any} + ) + isnonmutating = hasmethod( + fq, + # x, , a , u , d , p + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any} + ) + if !(ismutating || isnonmutating) + error( + "the state function has no method with type signature "* + "fq(x::Vector{$(NT)}, a::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or mutating form "* + "fq!(ẋ::Vector{$(NT)}, res::Vector{$(NT)}, x::Vector{$(NT)}, a::Vector{$(NT)}, u::Vector{$(NT)}, d::Vector{$(NT)}, p::Any)" + ) + end + return ismutating +end + +""" + validate_h_dae(NT, h) -> ismutating + +Validate `h` function argument signature for DAEs and return `true` if mutating. +""" +function validate_h_dae(NT, h) + ismutating = hasmethod( + h, + # y , x , a , d , p + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any} + ) + isnonmutating = hasmethod( + h, + # x , a , d , p + Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any} + ) + if !(ismutating || isnonmutating) + error( + "the output function has no method with type signature "* + "h(x::Vector{$(NT)}, a::Vector{$(NT)}, d::Vector{$(NT)}, p::Any) or mutating form "* + "h!(y::Vector{$(NT)}, x::Vector{$(NT)}, a::Vector{$(NT)}, d::Vector{$(NT)}, p::Any)" + ) + end + return ismutating +end + +""" + validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) -> iszero_Ha + +Validate if the DAE model is strictly proper with `SparseConnectivityTracer.jl`. + +It also returns `iszero_Ha` indicating wether or not that algebraic variable is used in `h!` +function. +""" +function validate_strictly_proper(NT, fq!, h!, nu, nx, na, ny, nd, p) + msg = """ + This package does not support a direct transmission from the input u to the output y. + See the Extended Help of LinModel for the justification. + """ + detector = TracerSparsityDetector() + ẋ, q, y = jacobian_buffer(zeros(nx), detector), zeros(na), zeros(ny) + x0, a0, u0, d0 = zeros(NT, nx), zeros(NT, na), zeros(NT, nu), zeros(NT, nd) + funcQu! = (q, u) -> fq!(ẋ, q, x0, a0, u, d0, p) + funcQa! = (q, a) -> fq!(ẋ, q, x0, a, u0, d0, p) + funcHa! = (y, a) -> h!(y, x0, a, d0, p) + S_∂y∂u, iszero_Ha = try + S_Qu = jacobian_sparsity(funcQu!, q, u0, detector) + S_Qa = jacobian_sparsity(funcQa!, q, a0, detector) + S_Ha = jacobian_sparsity(funcHa!, y, a0, detector) + S_∂y∂u = S_Ha/S_Qa*S_Qu + S_∂y∂u, iszero(S_Ha) + catch + @warn( + """ + Could not validate if the DAE is strictly proper with SparseConnectivityTracer.jl. + $msg""" + ) + spzeros(ny, nu), false + end + if !iszero(S_∂y∂u) + error( + """ + The DAE is not globally strictly proper according to SparseConnectivityTracer.jl. + $(msg)The resulting sparsity structure of ∂h/∂u is provided below (should be all zeros). + """, + sprint(show, MIME"text/plain"(), S_∂y∂u), + ) + end + return iszero_Ha +end + +"Get the number of elements in the optimization decision vector `Z` for DAE solving." +function get_nZ_dae(transcription::OrthogonalCollocation, nx, na) + return nx + transcription.no*nx + na + transcription.no*na +end +get_nZ_dae(::TrapezoidalCollocation, nx, na) = nx + 2na + +@doc raw""" + init_defectmat_dae(NT, ::OrthogonalCollocation, nx, na, Co, λo) -> Es, Ks, Aeq + +Init the matrices for computing the defect of the next state. + +Knowing that the decision vector ``\mathbf{Z}`` contain ``\mathbf{x̂_0}(k+1)``, +``\mathbf{a_0}(k+0)``, ``\mathbf{k̄}(k+0)`` and ``\mathbf{ā}(k+0)`` vectors with an +[`OrthogonalCollocation`](@ref), this linear equation compute the defect of the states at +time ``k+1``: +```math +\begin{aligned} + \mathbf{s}(k+1) &= \mathbf{E_s Z + K_s x_0}(k) \\ + &= \mathbf{E_s Z + F_s} +\end{aligned} +``` +It is forced to be ``\mathbf{s}(k+1) = \mathbf{0}`` using the optimization equality +constraints. +""" +function init_defectmat_dae(NT, transcription::OrthogonalCollocation, nx, na, Co, λo) + nā = transcription.no*na + Ks = λo*I(nx) + Esx = -I + Esk̄ = Co + Esa = zeros(NT, nx, na) + Esā = zeros(NT, nx, nā) + Es = [Esx Esa Esk̄ Esā] + Aeq = Es + return Es, Ks, Aeq +end + +""" + init_defectmat_dae(NT, ::CollocationMethod, nx, na, _ , _ ) -> Es, Ks, Aeq + +No linear equality constraint for other [`CollocationMethod`](@ref)s, return empty matrices. +""" +function init_defectmat_dae(NT, ::CollocationMethod, nx, na, _ , _ ) + Ks = zeros(NT, 0, nx) + Es = zeros(NT, 0, nx + 2na) + Aeq = Es + return Es, Ks, Aeq +end + +""" + init_optimization!( + model::NonLinModelDAE, optim_state::JuMP.GenericModel, optim_output::JuMP.GenericModel + ) -> nothing + +Init the two nonlinear optimization problems for [`NonLinModelDAE`](@ref) model. +""" +function init_optimization!( + model::NonLinModelDAE, optim_state::JuMP.GenericModel, optim_output::JuMP.GenericModel +) + if optim_state === optim_output + throw(ArgumentError("optim_state and optim_output must be different JuMP models")) + end + geq_oracle, q_oracle = get_nonlincon_oracle(model, optim_state, optim_output) + # --- collocation problem: optim_state --- + JuMP.num_variables(optim_state) == 0 || JuMP.empty!(optim_state) + JuMP.set_silent(optim_state) + nZ = length(model.Z) + @variable(optim_state, Zvar[i=1:nZ]) + Aeq = model.Aeq + beq = model.beq + @constraint(optim_state, linconstrainteq, Aeq*Zvar .== beq) + @constraint(optim_state, nonlinconstrainteq, Zvar in geq_oracle) + # --- algebraic equation: optim_output --- + JuMP.num_variables(optim_output) == 0 || JuMP.empty!(optim_output) + JuMP.set_silent(optim_output) + na = model.na + @variable(optim_output, a0var[i=1:na]) + @constraint(optim_output, nonlinconstraintq, a0var in q_oracle) + return nothing +end + +""" + get_nonlincon_oracle( + model::NonLinModelDAE, optim_state::JuMP.GenericModel, optim_output::JuMP.GenericModel + ) -> geq_oracle, q_oracle + +Return the nonlinear equality constraint oracles for [`NonLinModelDAE`](@ref) `model`. + +Return `geq_oracle` and `q_oracle`, the equality [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) +for the collocation problem algebraic equation, respectively. This method is really +intricate because the oracles are used inside the nonlinear optimization, so they must be +type-stable and as efficient as possible. All the function outputs and derivatives are +cached and updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). +""" +function get_nonlincon_oracle( + model::NonLinModelDAE, ::JuMP.GenericModel{JNT}, ::JuMP.GenericModel{JNT} +) where JNT<:Real + transcription = model.transcription + jac, hess = model.jacobian, model.hessian + nx, na, neq, nk̄ = model.nx, model.na, model.neq, get_nk̄(model, transcription) + nZ = length(model.Z) + strict = Val(true) + myNaN = convert(JNT, NaN) + k̄::Vector{JNT} = zeros(JNT, nk̄) + geq::Vector{JNT}, λeq::Vector{JNT} = zeros(JNT, neq), rand(JNT, neq) + q::Vector{JNT}, λq::Vector{JNT} = zeros(JNT, na), rand(JNT, na) + ẋ::Vector{JNT} = zeros(JNT, nx) + # -------------- collocation constraint: nonlinear oracle ------------------------- + function geq!(geq, Z, k̄) + update_predictions!(k̄, geq, model, Z) + return nothing + end + function ℓ_geq(Z, λeq, k̄, geq) + update_predictions!(k̄, geq, model, Z) + return dot(λeq, geq) + end + Z_∇geq = fill(myNaN, nZ) # NaN to force update at first call + ∇geq_prep = prepare_jacobian(geq!, geq, jac, Z_∇geq, Cache(k̄); strict) + ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ, neq) + ∇geq_structure = init_diffstructure(∇geq) + if !isnothing(hess) + ∇²geq_prep = prepare_hessian( + ℓ_geq, hess, Z_∇geq, Constant(λeq), Cache(k̄), Cache(geq); strict + ) + ∇²ℓ_geq = init_diffmat(JNT, hess, ∇²geq_prep, nZ, nZ) + ∇²geq_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_geq)) + end + function update_con_eq!(geq, ∇geq, Z_∇geq, Z_arg) + if isdifferent(Z_arg, Z_∇geq) + Z_∇geq .= Z_arg + value_and_jacobian!(geq!, geq, ∇geq, ∇geq_prep, jac, Z_∇geq, Cache(k̄)) + end + return nothing + end + function geq_func!(geq_arg, Z_arg) + update_con_eq!(geq, ∇geq, Z_∇geq, Z_arg) + return geq_arg .= geq + end + function ∇geq_func!(∇geq_arg, Z_arg) + update_con_eq!(geq, ∇geq, Z_∇geq, Z_arg) + return fill_diffstructure!(∇geq_arg, ∇geq, ∇geq_structure) + end + function ∇²geq_func!(∇²ℓ_arg, Z_arg, λ_arg) + Z_∇geq .= Z_arg + λeq .= λ_arg + hessian!( + ℓ_geq, ∇²ℓ_geq, ∇²geq_prep, hess, Z_∇geq, Constant(λeq), Cache(k̄), Cache(geq) + ) + return fill_diffstructure!(∇²ℓ_arg, ∇²ℓ_geq, ∇²geq_structure) + end + geq_min = geq_max = zeros(JNT, neq) + geq_oracle = MOI.VectorNonlinearOracle(; + dimension = nZ, + l = geq_min, + u = geq_max, + eval_f = geq_func!, + jacobian_structure = ∇geq_structure, + eval_jacobian = ∇geq_func!, + hessian_lagrangian_structure = isnothing(hess) ? Tuple{Int,Int}[] : ∇²geq_structure, + eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²geq_func! + ) + # -------------- algebraic equation: nonlinear oracle ------------------------- + function q!(q, a, ẋ) + return model.fq!(ẋ, q, model.x0_optim, a, model.u0_optim, model.d0_optim, model.p) + end + function ℓ_q(a, λq, ẋ, q) + model.fq!(ẋ, q, model.x0_optim, a, model.u0_optim, model.d0_optim, model.p) + return dot(λq, q) + end + a_∇q = fill(myNaN, na) # NaN to force update at first call + ∇q_prep = prepare_jacobian(q!, q, jac, a_∇q, Cache(ẋ); strict) + ∇q = init_diffmat(JNT, jac, ∇q_prep, na, na) + ∇q_structure = init_diffstructure(∇q) + if !isnothing(hess) + ∇²q_prep = prepare_hessian(ℓ_q, hess, a_∇q, Constant(λq), Cache(ẋ), Cache(q); strict) + ∇²ℓ_q = init_diffmat(JNT, hess, ∇²q_prep, na, na) + ∇²q_structure = lowertriangle_indices(init_diffstructure(∇²ℓ_q)) + end + function update_con_q!(q, ∇q, a_∇q, a_arg) + if isdifferent(a_arg, a_∇q) + a_∇q .= a_arg + value_and_jacobian!(q!, q, ∇q, ∇q_prep, jac, a_∇q, Cache(ẋ)) + end + return nothing + end + function q_func!(q_arg, a_arg) + update_con_q!(q, ∇q, a_∇q, a_arg) + return q_arg .= q + end + function ∇q_func!(∇q_arg, a_arg) + update_con_q!(q, ∇q, a_∇q, a_arg) + return fill_diffstructure!(∇q_arg, ∇q, ∇q_structure) + end + function ∇²q_func!(∇²ℓ_arg, a_arg, λ_arg) + a_∇q .= a_arg + λq .= λ_arg + hessian!(ℓ_q, ∇²ℓ_q, ∇²q_prep, hess, a_∇q, Constant(λq), Cache(ẋ), Cache(q)) + return fill_diffstructure!(∇²ℓ_arg, ∇²ℓ_q, ∇²q_structure) + end + q_min = q_max = zeros(JNT, na) + q_oracle = MOI.VectorNonlinearOracle(; + dimension = na, + l = q_min, + u = q_max, + eval_f = q_func!, + jacobian_structure = ∇q_structure, + eval_jacobian = ∇q_func!, + hessian_lagrangian_structure = isnothing(hess) ? Tuple{Int,Int}[] : ∇²q_structure, + eval_hessian_lagrangian = isnothing(hess) ? nothing : ∇²q_func! + ) + return geq_oracle, q_oracle +end + +""" + update_predictions!(k̄, geq, model, Z) + +TBW +""" +function update_predictions!(k̄, geq, model, Z) + x0, u0, d0 = model.x0_optim, model.u0_optim, model.d0_optim + con_nonlinprogeq!(geq, k̄, model, model.transcription, x0, u0, d0, Z) + return nothing +end + +function con_nonlinprogeq!( + geq, k̄, model::NonLinModelDAE, ::TrapezoidalCollocation, x0, u0, d0, Z +) + nx, na = model.nx, model.na + Ts = model.Ts + x0next_Z, a0_Z, a0next_Z = @views Z[1:nx], Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+2na)] + sknext, q1, q2 = @views geq[1:nx], geq[(nx+1):(nx+na)], geq[(nx+na+1):(nx+2na)] + k̇1, k̇2 = @views k̄[1:nx], k̄[(nx+1):(2nx)] + model.fq!(k̇1, q1, x0, a0_Z, u0, d0, model.p) + model.fq!(k̇2, q2, x0next_Z, a0next_Z, u0, d0, model.p) + sknext .= @. x0 - x0next_Z + 0.5*Ts*(k̇1 + k̇2) + return geq +end + +function con_nonlinprogeq!( + geq, k̄, model::NonLinModelDAE, transcription::OrthogonalCollocation, x0, u0, d0, Z +) + nx, na = model.nx, model.na + Mo, no = model.Mo, transcription.no + nk̄, nā = get_nk̄(model, transcription), no*na + a0_Z, k̄_Z, ā_Z = @views Z[(nx+1):(nx+na)], Z[(nx+na+1):(nx+na+nk̄)], Z[(nx+na+nk̄+1):end] + q0, sk̄, q̄ = @views geq[1:na], geq[(na+1):(na+nk̄)], geq[(na+nk̄+1):(na+nk̄+nā)] + @views model.fq!(k̄[1:nx], q0, x0, a0_Z, u0, d0, model.p) + Δk = k̄ + for i=1:no + Δk[(1 + (i-1)*nx):(i*nx)] = @views k̄_Z[(1 + (i-1)*nx):(i*nx)] .- x0 + end + mul!(sk̄, Mo, Δk) + for i=1:no + k̇i = @views k̄[(1 + (i-1)*nx):(i*nx)] + qi = @views q̄[(1 + (i-1)*na):(i*na)] + ki_Z = @views k̄_Z[(1 + (i-1)*nx):(i*nx)] + ai_Z = @views ā_Z[(1 + (i-1)*na):(i*na)] + model.fq!(k̇i, qi, ki_Z, ai_Z, u0, d0, model.p) + end + sk̄ .-= k̄ + return geq +end + +@doc raw""" + initstate_core!(model::NonLinModelDAE, u0, d0) + +Warm-start `model.Z` and `model.a0` at zero if `model` is a [`NonLinModelDAE`](@ref). + +The field `model.a0` and `model.Z` respectively warm-start [`evaloutput`](@ref) and +[`updatestate!`](@ref) solving. The method also set `model.optim_u0` and `model.optim_d0` at +`u0` and `d0` values. The `model.u0` field is used to solve the algebraic equation +``\mathbf{q}`` in [`evaloutput`](@ref) method, but it should not impact the result in theory +since `model` is strictly proper w.r.t. `u0`. +""" +function initstate_core!(model::NonLinModelDAE, u0, d0) + model.Z .= 0 + model.a0 .= 0 + model.x0_optim .= model.x0 + model.u0_optim .= u0 + model.d0_optim .= d0 + return nothing +end + +@doc raw""" + f!(x0next, _ , model::NonLinModelDAE, x0, u0, d0, _ ) -> nothing + +Solve the optimization `model.optim` problem for [`NonLinModelDAE`](@ref). + +After solving, the next state ``\mathbf{x_0}(k+1)`` will be stored in-place in the `x0next` +argument. The next algebraic variable ``\mathbf{a_0}(k+1)`` will be also stored at +`model.a0`. +""" +function f!(x0next, _ , model::NonLinModelDAE, x0, u0, d0, _ ) + nx, na = model.nx, model.na + model.x0_optim .= x0 + model.u0_optim .= u0 + model.d0_optim .= d0 + linconstrainteq!(model, model.transcription) + Zvar = model.optim_state[:Zvar] + Z = solve!(model, model.optim_state, Zvar, model.Z) + x0next .= @views Z[1:nx] + model.a0 .= @views Z[(nx + 1):(nx + na)] + model.Z .= Z + return nothing +end + +""" + h!(y0, model::NonLinModelDAE, x0, d0, p) -> nothing + +Solve the algebraic equation to get `a0` and call `model.h!` for [`NonLinModelDAE`](@ref). + +If `model.iszero_Ha` is `true`, the algebraic variable is not used in `model.h!` according +to [`SparseConnectivityTracer.jl`](@extref SparseConnectivityTracer.jl), the algebraic +equation solving is thus skipped and `model.h!` is called directly. +""" +function h!(y0, model::NonLinModelDAE, x0, d0, p) + if !model.iszero_Ha + model.x0_optim .= x0 + model.d0_optim .= d0 + # model.u0_optim is not updated since u0 not available, but model is strictly proper + # hence possible impacts on a0 vector but no direct impacts on y0 vector in the end. + a0var = model.optim_output[:a0var] + a0 = solve!(model, model.optim_output, a0var, model.a0) + model.a0 .= a0 + else # model.h! is not a function of a0, this vector is not needed here: + a0 = model.buffer.a + end + model.h!(y0, x0, a0, d0, p) + return nothing +end + +function linconstrainteq!(model::NonLinModelDAE, ::OrthogonalCollocation) + mul!(model.Fs, model.Ks, model.x0_optim) + model.beq .= @. -model.Fs + linconeq = model.optim_state[:linconstrainteq] + JuMP.set_normalized_rhs(linconeq, model.beq) + return nothing +end +linconstrainteq!(::NonLinModelDAE, ::CollocationMethod) = nothing + +""" + solve!(model::NonLinModelDAE, optim, Zvar, Zs) + +Solve optimization problem `optim` with the JuMP variable `Zvar` warm-started at `Zs`. +""" +function solve!(model::NonLinModelDAE, optim, Zvar, Zs) + JuMP.set_start_value.(Zvar, Zs) + JuMP.optimize!(optim) + if !issolved(optim) + status = JuMP.termination_status(optim) + if iserror(optim) + @error( + "DAE terminated without solution: returning last solution "* + "(more info in debug log)", + status + ) + else + @warn( + "DAE termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "* + "anyway (more info in debug log)", + status + ) + end + @debug info2debugstr(getinfo(model)) + end + Z = iserror(optim) ? Zs : JuMP.value.(Zvar) + return Z +end + +@doc raw""" + getinfo(model::NonLinModelDAE) -> info + +Get additional info about `model` [`NonLinModelDAE`](@ref) solution for troubleshooting. + +The function should be called after calling [`updatestate!`](@ref) on `model` object. It +returns the dictionary `info` with the following fields: + +- `:xnext` : next state, ``\mathbf{x}(k+1)`` +- `:q` : current algebraic equation residuals `res`, ``\mathbf{q(x, a, u, d, p)}`` +- `:y` : current output, ``\mathbf{y}(k)`` +- `:x` : current state, ``\mathbf{x}(k)`` +- `:a` : current algebraic variable, ``\mathbf{a}(k)`` +- `:u` : current manipulated input, ``\mathbf{u}(k)`` +- `:d` : current measured disturbances, ``\mathbf{u}(k)`` + +The following two fields are also available if the related method is called at least once: + +- `:sol_state` : solution summary of [`updatestate!`](@ref) optimizer for printing +- `:sol_output` : solution summary of [`evaloutput`](@ref) optimizer for printing + +# Examples +```jldoctest +julia> fq!(ẋ, res, x, a, u, _ , p) = (ẋ .= p*x .+ a; res .= a .- u; nothing); + +julia> h!(y, x, _ , _ , _ ) = (y .= 0.1x; nothing); + +julia> model = NonLinModelDAE(fq!, h!, 5.0, 1, 1, 1, 1, p=-0.2); + +julia> u = [7]; updatestate!(model, u); + +julia> a = round.(getinfo(model)[:a], digits=6) +1-element Vector{Float64}: + 7.0 +``` +""" +function getinfo(model::NonLinModelDAE{NT}) where NT<:Real + x0, u0, d0 = model.x0_optim, model.u0_optim, model.d0_optim + a0, p = model.a0, model.p + buffer = model.buffer + ẋ, q, y0 = buffer.x, buffer.a, buffer.y + model.fq!(ẋ, q, x0, a0, u0, d0, p) + model.h!(y0, x0, a0, d0, p) + y = y0 + y .+ model.yop + x, u, d = buffer.x, buffer.u, buffer.d + x .= x0 .+ model.xop + u .= u0 .+ model.uop + d .= d0 .+ model.dop + a = a0 + info = Dict{Symbol, Any}() + info[:xnext] = model.x0 + model.xop + info[:q] = q + info[:y] = y + info[:x] = x + info[:a] = a + info[:u] = u + info[:d] = d + if JuMP.termination_status(model.optim_state) ≠ JuMP.OPTIMIZE_NOT_CALLED + info[:sol_state] = JuMP.solution_summary(model.optim_state, verbose=true) + end + if JuMP.termination_status(model.optim_output) ≠ JuMP.OPTIMIZE_NOT_CALLED + info[:sol_output] = JuMP.solution_summary(model.optim_output, verbose=true) + end + return info +end + +function Base.show(io::IO, model::NonLinModelDAE) + nu, nd = model.nu, model.nd + nx, ny = model.nx, model.ny + na = model.na + n = maximum(ndigits.((nu, nx, ny, nd))) + 1 + println(io, "$(nameof(typeof(model))) with a sample time Ts = $(model.Ts) s:") + println(io, "├ state optimizer: $(JuMP.solver_name(model.optim_state))") + println(io, "├ output optimizer: $(JuMP.solver_name(model.optim_output))") + println(io, "├ transcription: $(transcription_str(model.transcription))") + println(io, "├ jacobian: $(backend_str(model.jacobian))") + println(io, "├ hessian: $(backend_str(model.hessian))") + println(io, "└ dimensions:") + println(io, " │ ├$(lpad(nu, n)) manipulated inputs u") + println(io, " │ ├$(lpad(nx, n)) states x") + println(io, " │ ├$(lpad(na, n)) algebraic variables a") + println(io, " │ ├$(lpad(ny, n)) outputs y") + println(io, " │ └$(lpad(nd, n)) measured disturbances d") + nZ = length(model.Z) + nAeq = size(model.Aeq, 1) + neq = model.neq + m = maximum(ndigits.((nZ, nAeq, neq))) + 1 + println(io, " └ optimization:") + println(io, " ├$(lpad(nZ, m)) decision variables Z") + println(io, " ├$(lpad(nAeq, m)) linear equality constraints Aeq") + print(io, " └$(lpad(neq, m)) nonlinear equality constraints geq") +end \ No newline at end of file diff --git a/src/precompile.jl b/src/precompile.jl index 72aa7b689..d49bd98d9 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -21,6 +21,15 @@ function h!(y, x, _ , p) end p = (sys2.A, sys2.B, sys2.C) +function fq_dae!(ẋ, res, x, a, u, _ , _ ) + ẋ[1] = -0.5*(x[1] - 0.2*u[1]) + res .= (x .- a) + return nothing +end +function h_dae!(y, x, a, _ , _ ) + y .= 2 .* x .+ a +end + function JE( _ , Ŷe, _ , R̂y , _ ) Ŷ = @views Ŷe[3:end] Ȳ = R̂y - Ŷ @@ -95,6 +104,8 @@ R̂y = repeat([55; 30], 3) linearizemodel = linearize(nlmodel) setmodel!(mpc_kf, linearizemodel) + daemodel = NonLinModelDAE(fq_dae!, h_dae!, 1.0, 1, 1, 1, 1) + sim!(daemodel, 2, [10]) end end # @setup_workload \ No newline at end of file diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 3772d4ade..8d94185a2 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -1,7 +1,7 @@ @doc raw""" abstract type PredictiveController end -Abstract supertype of all predictive controllers. +Supertype of all predictive controllers. --- @@ -38,7 +38,7 @@ function Base.show(io::IO, mpc::PredictiveController) println(io, "$(nameof(typeof(mpc))) controller with a sample time Ts = $(model.Ts) s:") println(io, "├ estimator: $(nameof(typeof(mpc.estim)))") println(io, "├ model: $(nameof(typeof(model)))") - println(io, "├ optimizer: $(JuMP.solver_name(mpc.optim)) ") + println(io, "├ optimizer: $(JuMP.solver_name(mpc.optim))") println(io, "├ transcription: $(transcription_str(mpc.transcription))") print_backends(io, mpc) println(io, "└ dimensions:") diff --git a/src/sim_model.jl b/src/sim_model.jl index 4c819bc50..fc4b2ec78 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -1,7 +1,7 @@ @doc raw""" abstract type SimModel end -Abstract supertype of [`LinModel`](@ref) and [`NonLinModel`](@ref) types. +Supertype of [`LinModel`](@ref) and [`NonLinModel`](@ref) types. --- @@ -20,9 +20,27 @@ julia> y = model() """ abstract type SimModel{NT<:Real} end +""" + abstract type SimModelODE <: SimModel + +Abstract subtype of [`SimModel`](@ref) for ordinary differential equations. + +""" +abstract type SimModelODE{NT<:Real} <: SimModel{NT} end + + +""" + abstract type SimModelDAE <: SimModel + +Abstract subtype of [`SimModel`](@ref) for differential and algebraic equations. + +""" +abstract type SimModelDAE{NT<:Real} <: SimModel{NT} end + struct SimModelBuffer{NT<:Real} u::Vector{NT} x::Vector{NT} + a::Vector{NT} y::Vector{NT} d::Vector{NT} k̄::Vector{NT} @@ -30,25 +48,27 @@ struct SimModelBuffer{NT<:Real} end @doc raw""" - SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0) + SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0, na::Int=0) Create a buffer for `SimModel` objects for inputs, states, outputs, and disturbances. The buffer is used to store temporary results during simulation without allocating. The argument `ni` is the number of intermediate stage of the [`DiffSolver`](@ref), when -applicable. +applicable. The field `na` is for the algebraic variables of [`NonLinModelDAE`](@ref). """ -function SimModelBuffer{NT}(nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0) where {NT<:Real} +function SimModelBuffer{NT}( + nu::Int, nx::Int, ny::Int, nd::Int, ni::Int=0, na::Int=0 +) where {NT<:Real} u = Vector{NT}(undef, nu) x = Vector{NT}(undef, nx) + a = Vector{NT}(undef, na) # for NonLinModelDAE only (empty by default) y = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) k̄ = Vector{NT}(undef, nx*(ni+1)) # the "+1" is necessary because of super-sampling empty = Vector{NT}(undef, 0) - return SimModelBuffer{NT}(u, x, y, d, k̄, empty) + return SimModelBuffer{NT}(u, x, a, y, d, k̄, empty) end - @doc raw""" setop!(model; uop=nothing, yop=nothing, dop=nothing, xop=nothing, fop=nothing) -> model @@ -182,7 +202,7 @@ end Init `model.x0` with manipulated inputs `u` and meas. dist. `d` steady-state. The method tries to initialize the model state ``\mathbf{x}`` at steady-state. It removes -the operating points on `u` and `d` and calls [`steadystate!`](@ref): +the operating points on `u` and `d` and calls [`initstate_core!`](@ref): - If `model` is a [`LinModel`](@ref), the method computes the steady-state of current inputs `u` and measured disturbances `d`. @@ -205,7 +225,7 @@ function initstate!(model::SimModel, u, d=model.buffer.empty) u0, d0 = model.buffer.u, model.buffer.d u0 .= u .- model.uop d0 .= d .- model.dop - steadystate!(model, u0, d0) + initstate_core!(model, u0, d0) x = model.buffer.x x .= model.x0 .+ model.xop return x @@ -339,6 +359,9 @@ function periodsleep(model::SimModel, busywait=false) return nothing end +"The [`TranscriptionMethod`](@ref) is compatible with the [`SimModel`](@ref) by default." +validate_transcription(::SimModel, ::TranscriptionMethod) = nothing + """ validate_args(model::SimModel, d, u=nothing) @@ -352,9 +375,14 @@ function validate_args(model::SimModel, d, u=nothing) end end +"Get length of the `k` vector with all the solver intermediate steps or all the collocation pts." +get_nk̄(model::SimModel, ::ShootingMethod) = model.nk̄ +get_nk̄(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no + include("model/linmodel.jl") include("model/linearization.jl") include("model/nonlinmodel.jl") +include("model/nonlinmodeldae.jl") function Base.show(io::IO, model::SimModel) nu, nd = model.nu, model.nd @@ -372,5 +400,12 @@ end "Print additional details of `model` if any (no details by default)." print_details(::IO, ::SimModel) = nothing +""" + initstate_core!(::SimModel, u0, d0) + +Do nothing at all by default. +""" +initstate_core!(::SimModel, _ , _ ) = nothing + "Functor allowing callable `SimModel` object as an alias for `evaloutput`." (model::SimModel)(d=model.buffer.empty) = evaloutput(model::SimModel, d) \ No newline at end of file diff --git a/src/state_estim.jl b/src/state_estim.jl index 1c5943147..9454cc913 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -1,7 +1,7 @@ @doc raw""" abstract type StateEstimator end -Abstract supertype of all state estimators. +Supertype of all state estimators. --- diff --git a/src/transcription.jl b/src/transcription.jl index 9501dbb9b..375609c50 100644 --- a/src/transcription.jl +++ b/src/transcription.jl @@ -1,15 +1,35 @@ -const COLLOCATION_NODE_TYPE = Float64 +const COLLOCATION_NODE_TYPE::Type = Float64 """ abstract type TranscriptionMethod end -Abstract supertype of all transcription methods for the optimization problems. +Supertype of all transcription methods for the optimization problems. + +The [`ShootingMethod`](@ref) subtype includes the following concrete types: + +- [`SingleShooting`](@ref) +- [`MultipleShooting`](@ref) + +and the [`CollocationMethod`](@ref) subtype includes the following concrete types: + +- [`TrapezoidalCollocation`](@ref) +- [`OrthogonalCollocation`](@ref) + +""" +abstract type TranscriptionMethod end -The module currently supports [`SingleShooting`](@ref), [`MultipleShooting`](@ref), -[`TrapezoidalCollocation`](@ref) and [`OrthogonalCollocation`](@ref) transcription methods. """ -abstract type TranscriptionMethod end -abstract type ShootingMethod <: TranscriptionMethod end + abstract type ShootingMethod + +Abstract subtype of [`TranscriptionMethod`](@ref) for shooting methods. +""" +abstract type ShootingMethod <: TranscriptionMethod end + +""" + abstract type CollocationMethod + +Abstract subtype of [`TranscriptionMethod`](@ref) for direct collocation methods. +""" abstract type CollocationMethod <: TranscriptionMethod end @doc raw""" @@ -20,11 +40,11 @@ Construct a direct single shooting [`TranscriptionMethod`](@ref). In the case of [`PredictiveController`](@ref) types, the decision variable in the optimization problem is (excluding the slack ``ϵ``, and without any custom move blocking): ```math -\mathbf{Z} = \mathbf{ΔU} = \begin{bmatrix} - \mathbf{Δu}(k+0) \\ - \mathbf{Δu}(k+1) \\ - \vdots \\ - \mathbf{Δu}(k+H_c-1) \end{bmatrix} +\mathbf{Z} = \mathbf{ΔU} = \begin{bmatrix} + \mathbf{Δu}(k+0) \\ + \mathbf{Δu}(k+1) \\ + \vdots \\ + \mathbf{Δu}(k+H_c-1) \end{bmatrix} ``` This method computes the predictions by calling the augmented discrete-time model recursively over the prediction horizon ``H_p`` in the objective function, or by updating @@ -37,16 +57,14 @@ plant model/constraints. The Extended Help details transcription of !!! details "Extended Help" For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): ```math - \mathbf{Z} - = \begin{bmatrix} - \mathbf{x̂_0}(k-N_k+p) \\ - \mathbf{Ŵ} \\ - \mathbf{0_ŵ} - \end{bmatrix} - = \begin{bmatrix} - \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}} \\ - \mathbf{Ŵ} \\ - \mathbf{0_ŵ} \end{bmatrix} + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} + = \begin{bmatrix} + \mathbf{x̂}_k(k-N_k+p) - \mathbf{x̂_{op}} \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} ``` The vector ``\mathbf{0_ŵ}`` with `nx̂*(He-Nk)` zeros is for the unused decision variables at the beginning, when the data windows are growing (``N_k < H_e``). The number of @@ -67,11 +85,11 @@ The decision variable of [`PredictiveController`](@ref) is (excluding ``ϵ``): thus it also includes the predicted states, expressed as deviation vectors from the operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)): ```math -\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} - \mathbf{x̂}_i(k+1) - \mathbf{x̂_{op}} \\ - \mathbf{x̂}_i(k+2) - \mathbf{x̂_{op}} \\ - \vdots \\ - \mathbf{x̂}_i(k+H_p) - \mathbf{x̂_{op}} \end{bmatrix} +\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} + \mathbf{x̂}_i(k+1) - \mathbf{x̂_{op}} \\ + \mathbf{x̂}_i(k+2) - \mathbf{x̂_{op}} \\ + \vdots \\ + \mathbf{x̂}_i(k+H_p) - \mathbf{x̂_{op}} \end{bmatrix} ``` where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. Note that @@ -93,21 +111,21 @@ provided in the Extended Help. !!! details "Extended Help" For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): ```math - \mathbf{Z} = \begin{bmatrix} - \mathbf{x̂_0}(k-N_k+p) \\ - \mathbf{X̂_0} \\ - \mathbf{0_x̂} \\ - \mathbf{Ŵ} \\ - \mathbf{0_ŵ} \end{bmatrix} + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{X̂_0} \\ + \mathbf{0_x̂} \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} ``` thus the deviation value of arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)`` is kept out of the estimated states over ``N_k``: ```math - \mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} - \mathbf{x̂}_k(k-N_k+p+1) - \mathbf{x̂_{op}} \\ - \mathbf{x̂}_k(k-N_k+p+2) - \mathbf{x̂_{op}} \\ - \vdots \\ - \mathbf{x̂}_k(k+p) - \mathbf{x̂_{op}} \end{bmatrix} + \mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} = \begin{bmatrix} + \mathbf{x̂}_k(k-N_k+p+1) - \mathbf{x̂_{op}} \\ + \mathbf{x̂}_k(k-N_k+p+2) - \mathbf{x̂_{op}} \\ + \vdots \\ + \mathbf{x̂}_k(k+p) - \mathbf{x̂_{op}} \end{bmatrix} ``` Similarly to [`SingleShooting`](@ref), the ``\mathbf{0_x̂}`` and ``\mathbf{0_ŵ}`` vectors with zeros is for the unused decision variables at the beginning. @@ -125,13 +143,15 @@ end Construct an implicit trapezoidal [`TranscriptionMethod`](@ref) with `h`th order hold. -This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref)s -only. The decision variables are the same as for [`MultipleShooting`](@ref), hence similar -computational costs. See the same docstring for descriptions of `f_threads` and `h_threads` -keywords. The `h` argument is `0` or `1`, for piecewise constant or linear manipulated -inputs ``\mathbf{u}`` (`h=1` is slightly less expensive). Note that the various [`DiffSolver`](@ref) -here assume zero-order hold, so `h=1` will induce a plant-model mismatch if the plant is -simulated with these solvers. Measured disturbances ``\mathbf{d}`` are piecewise linear. +This is the simplest collocation method. It supports continuous-time [`NonLinModel`](@ref) +and [`NonLinModelDAE`](@ref). For [`NonLinModel`](@ref), the decision variables are the same +as for [`MultipleShooting`](@ref), hence similar computational costs. See the same docstring +for descriptions of `f_threads` and `h_threads` keywords. The Extended Help details +the decision variables for [`NonLinModelDAE`](@ref). The `h` argument is `0` or `1`, for +piecewise constant or linear manipulated inputs ``\mathbf{u}`` (`h=1` is slightly less +expensive). Note that the various [`DiffSolver`](@ref) here assume zero-order hold, so `h=1` +will induce a plant-model mismatch if the plant is simulated with these solvers. Measured +disturbances ``\mathbf{d}`` are piecewise linear. This transcription computes the predictions by calling the continuous-time model in the equality constraint function and by using the implicit trapezoidal rule. It can handle @@ -149,6 +169,50 @@ transcription method. # Extended Help !!! details "Extended Help" + The algebraic vectors at the future time step ``\mathbf{a_0}`` is included in the + decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{x_0}(k+1) \\ + \mathbf{a_0}(k+0) \\ + \mathbf{a_0}(k+1) \end{bmatrix} + ``` + For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{ΔU} \\ + \mathbf{X̂_0} \\ + \mathbf{a_0}(k+0) \\ + \mathbf{A_0} \end{bmatrix} + \quad \text{and} \quad + \mathbf{A_0} = \begin{bmatrix} + \mathbf{a_0}(k+1) \\ + \mathbf{a_0}(k+2) \\ + \vdots \\ + \mathbf{a_0}(k+H_p) \end{bmatrix} + ``` + and, for [`MovingHorizonEstimator`](@ref) with DAEs: + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{X̂_0} \\ + \mathbf{0_x̂} \\ + \mathbf{a_0}(k-N_k+p) \\ + \mathbf{A_0} \\ + \mathbf{0_a} \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} + \quad \text{and} \quad + \mathbf{A_0} = \begin{bmatrix} + \mathbf{a_0}(k-N_k+p+1) \\ + \mathbf{a_0}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{a_0}(k+p) \end{bmatrix} + ``` + See [`MultipleShooting`](@ref) for the exact definition of ``\mathbf{X̂_0}`` on the last + two cases. All the ``\mathbf{0_{(•)}}`` are vectors with zeros for the unused decision + variables at the beginning (``N_k < He``). + Note that the stochastic model of the unmeasured disturbances is strictly linear and discrete-time, as described in [`ModelPredictiveControl.init_estimstoch`](@ref). Collocation methods require continuous-time dynamics. Because of this, and also to @@ -176,31 +240,30 @@ end h::Int=0, no::Int=3; f_threads=false, h_threads=false, roots=:gaussradau ) -Construct an orthogonal collocation on finite elements [`TranscriptionMethod`](@ref). +Construct an orthogonal collocation [`TranscriptionMethod`](@ref). -Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref)s -only. The `h` argument is the hold order for ``\mathbf{u}`` (`0` or `1`), and the `no` -argument, the number of collocation points ``n_o``. The decision variable of -[`PredictiveController`](@ref) is similar to [`MultipleShooting`](@ref), but it also -includes the collocation points: +Also known as pseudo-spectral method. It supports continuous-time [`NonLinModel`](@ref) +and [`NonLinModelDAE`](@ref). The `h` argument is the hold order for ``\mathbf{u}`` (`0` or +`1`), and the `no` argument, the number of collocation points ``n_o``. The decision variable +of [`PredictiveController`](@ref) with [`NonLinModel`](@ref) is similar to +[`MultipleShooting`](@ref), but it also includes the collocation points: ```math -\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K} \end{bmatrix} +\mathbf{Z} = \begin{bmatrix} \mathbf{ΔU} \\ \mathbf{X̂_0} \\ \mathbf{K̄} \end{bmatrix} ``` -where ``\mathbf{K}`` encompasses all the intermediate stages of the deterministic states +where ``\mathbf{K̄}`` encompasses all the intermediate stages of the deterministic states (the first `nx` elements of ``\mathbf{x̂}``): ```math -\mathbf{K} = \begin{bmatrix} - \mathbf{k̄}(k+0) \\ - \mathbf{k̄}(k+1) \\ - \vdots \\ - \mathbf{k̄}(k+H_p-1) -\end{bmatrix} \quad \text{and} \quad +\mathbf{K̄} = \begin{bmatrix} + \mathbf{k̄}(k+0) \\ + \mathbf{k̄}(k+1) \\ + \vdots \\ + \mathbf{k̄}(k+H_p-1) \end{bmatrix} +\quad \text{and} \quad \mathbf{k̄}(k+j) = \begin{bmatrix} - \mathbf{k}_1(k+j) \\ - \mathbf{k}_2(k+j) \\ - \vdots \\ - \mathbf{k}_{n_o}(k+j) -\end{bmatrix} + \mathbf{k}_1(k+j) \\ + \mathbf{k}_2(k+j) \\ + \vdots \\ + \mathbf{k}_{n_o}(k+j) \end{bmatrix} ``` The `roots` keyword argument is either `:gaussradau` or `:gausslegendre`, for Gauss-Radau or Gauss-Legendre quadrature, respectively. See [`MultipleShooting`](@ref) docstring for info @@ -208,7 +271,7 @@ on `f_threads` and `h_threads` keywords. This transcription computes thecpredict enforcing the collocation and continuity constraints at the collocationc points. It is efficient for highly stiff systems, but generally more expensive than the other methods for non-stiff systems. See Extended Help for details and the transcription of -[`MovingHorizonEstimator`](@ref) objects. +[`MovingHorizonEstimator`](@ref) objects and [`NonLinModelDAE`](@ref). !!! warning Except if you construct your MPC with a [`MovingHorizonEstimator`](@ref) based on a @@ -222,38 +285,96 @@ this transcription method (sparser formulation than [`MultipleShooting`](@ref)). # Extended Help !!! details "Extended Help" - As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic - states are left out of the ``\mathbf{K}`` vector to reduce the dimensions, and also - because collocation methods require continuous-time dynamics and the stochastic model is - discrete. - - For [`MovingHorizonEstimator`](@ref), the decision variable is (excluding slack `ε`): + For [`MovingHorizonEstimator`](@ref) based on [`NonLinModel`](@ref), the decision + variable is (excluding slack `ε`): + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{X̂_0} \\ + \mathbf{0_x̂} \\ + \mathbf{K̄} \\ + \mathbf{0_k̄} \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} + \quad \text{and} \quad + \mathbf{K̄} = \begin{bmatrix} + \mathbf{k̄}(k-N_k+p+0) \\ + \mathbf{k̄}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{k̄}(k+p-1) \end{bmatrix} + ``` + The text above defines ``\mathbf{k̄}``. The Extended Help of [`SingleShooting`](@ref) and + [`MultipleShooting`](@ref) introduces all the other variables. + + The case of [`NonLinModelDAE`](@ref) requires the introduction the vector with the + algebraic variables at the collocation points: + ```math + \mathbf{ā}(k+j) = \begin{bmatrix} + \mathbf{ā}_1(k+j) \\ + \mathbf{ā}_2(k+j) \\ + \vdots \\ + \mathbf{ā}_{n_o}(k+j) \end{bmatrix} + ``` + Although not strictly needed, the current algebraic variable ``\mathbf{a_0}(k+0)`` is + still included in the decision vector for open-loop simulations of [`NonLinModelDAE`](@ref): + ```math + \mathbf{Z} = \begin{bmatrix} + \mathbf{x_0}(k+1) \\ + \mathbf{a_0}(k+0) \\ + \mathbf{k̄}(k+0) \\ + \mathbf{ā}(k+0) \end{bmatrix} + ``` + For [`NonLinMPC`](@ref) based on [`NonLinModelDAE`](@ref), the decision vector is: ```math - \mathbf{Z} = \begin{bmatrix} - \mathbf{x̂_0}(k-N_k+p) \\ - \mathbf{X̂_0} \\ - \mathbf{0_x̂} \\ - \mathbf{K} \\ - \mathbf{0_k̄} \\ - \mathbf{Ŵ} \\ - \mathbf{0_ŵ} \end{bmatrix} + \mathbf{Z} = \begin{bmatrix} + \mathbf{ΔU} \\ + \mathbf{X̂_0} \\ + \mathbf{A_0} \\ + \mathbf{K̄} \\ + \mathbf{Ā} \end{bmatrix} + \quad \text{and} \quad + \mathbf{Ā} = \begin{bmatrix} + \mathbf{ā}(k+0) \\ + \mathbf{ā}(k+1) \\ + \vdots \\ + \mathbf{ā}(k+H_p-1) \end{bmatrix} ``` - The Extended Help of [`SingleShooting`](@ref) and [`MultipleShooting`](@ref) introduces - all these variables, except for the vector with the intermediate stages of the - deterministic states at the collation points: + and, for [`MovingHorizonEstimator`](@ref) with [`NonLinModelDAE`](@ref): ```math - \mathbf{K} = \begin{bmatrix} - \mathbf{k̄}(k-N_k+p+0) \\ - \mathbf{k̄}(k-N_k+p+1) \\ - \vdots \\ - \mathbf{k̄}(k+p-1) \end{bmatrix} + \mathbf{Z} = \begin{bmatrix} + \mathbf{x̂_0}(k-N_k+p) \\ + \mathbf{X̂_0} \\ + \mathbf{0_x̂} \\ + \mathbf{A_0} \\ + \mathbf{0_a} \\ + \mathbf{K̄} \\ + \mathbf{0_k̄} \\ + \mathbf{Ā} \\ + \mathbf{0_ā} \\ + \mathbf{Ŵ} \\ + \mathbf{0_ŵ} \end{bmatrix} + \quad \text{and} \quad + \mathbf{Ā} = \begin{bmatrix} + \mathbf{ā}(k-N_k+p+0) \\ + \mathbf{ā}(k-N_k+p+1) \\ + \vdots \\ + \mathbf{ā}(k+p-1) \end{bmatrix} ``` + See the Extended Help of [`TrapezoidalCollocation`](@ref) for the exact definition of + ``\mathbf{A_0}`` on the last two cases. All the ``\mathbf{0_{(•)}}`` are vectors with + zeros for the unused decision variables at the beginning (``N_k < H_e``). + The collocation points are located at the roots of orthogonal polynomials, which is "optimal" for approximating the state trajectories with polynomials of degree ``n_o``. The method then enforces the system dynamics at these points. The Gauss-Legendre scheme is more accurate than Gauss-Radau but only A-stable, while the latter being L-stable. See [`init_orthocolloc`](@ref), [`con_nonlinprogeq!`](@ref) and [`con_nonlinprogeq_mhe!`](@ref) for more details. + + As explained in the Extended Help of [`TrapezoidalCollocation`](@ref), the stochastic + states are left out of the ``\mathbf{K̄}`` vector to reduce the dimensions, and also + because collocation methods require continuous-time dynamics and the stochastic model is + discrete. """ struct OrthogonalCollocation <: CollocationMethod h::Int @@ -284,7 +405,7 @@ struct OrthogonalCollocation <: CollocationMethod end @doc raw""" - init_orthocolloc(model::SimModel, transcription::OrthogonalCollocation) -> Mo, Co, λo + init_orthocolloc(NT, transcription::OrthogonalCollocation, nx, Ts) -> Mo, Co, λo Init the differentiation and continuity matrices for [`OrthogonalCollocation`](@ref). @@ -292,7 +413,7 @@ Introducing ``τ_i``, the ``i``th root of the orthogonal polynomial normalized t interval ``[0, 1]`` with ``τ_0=0``, the trajectories for each state are approximated by a distinct polynomial of degree ``n_o``. The differentiation matrix ``\mathbf{M_o}``, the continuity matrix ``\mathbf{C_o}`` and the continuity coefficient ``λ_o`` are pre-computed -with the identity matrix ``\mathbf{I}`` of size `(model.nx, model.nx)` and: +with the identity matrix ``\mathbf{I}`` of size `(nx, nx)` and: ```math \begin{aligned} \mathbf{P_o} &= \begin{bmatrix} @@ -311,7 +432,7 @@ with the identity matrix ``\mathbf{I}`` of size `(model.nx, model.nx)` and: λ_o &= L_0(1) \end{aligned} ``` -where ``T_s`` is the sampling time `model.Ts`, ``\mathbf{P_o}`` is a matrix to evaluate the +where ``T_s`` is the sampling time `Ts`, ``\mathbf{P_o}`` is a matrix to evaluate the polynomial values w/o the coefficients and Y-intercept, and ``\mathbf{Ṗ_o}``, to evaluate its derivatives. The Lagrange polynomial ``L_j(τ)`` bases are defined as: ```math @@ -378,10 +499,8 @@ objects (only used for [`MovingHorizonEstimator`](@ref)). Note that handling the process noise in the continuity constraint implicitly assumes that it's a discrete stochastic process (like all the other [`StateEstimator`](@ref) types in this package). """ -function init_orthocolloc( - model::SimModel{NT}, transcription::OrthogonalCollocation -) where {NT<:Real} - nx, no = model.nx, transcription.no +function init_orthocolloc(NT, transcription::OrthogonalCollocation, nx, Ts) + no = transcription.no τ = transcription.τ Po = Matrix{NT}(undef, nx*no, nx*no) # polynomial matrix (w/o the Y-intercept term) Ṗo = Matrix{NT}(undef, nx*no, nx*no) # polynomial derivative matrix @@ -392,7 +511,7 @@ function init_orthocolloc( Po[iRows, iCols] = (τ[i]^j)*I_nx Ṗo[iRows, iCols] = (j*τ[i]^(j-1))*I_nx end - Mo = sparse((Ṗo/Po)/model.Ts) + Mo = sparse((Ṗo/Po)/Ts) Co = Matrix{NT}(undef, nx, nx*no) for j=1:no iCols = (1:nx) .+ nx*(j-1) @@ -404,11 +523,11 @@ function init_orthocolloc( end """ - init_orthocolloc(model::SimModel, transcription::TranscriptionMethod) + init_orthocolloc(NT, transcription::TranscriptionMethod, _ , _ ) Return empty sparse matrices and `NaN` value for other [`TranscriptionMethod`](@ref) types. """ -init_orthocolloc(::SimModel, ::TranscriptionMethod) = spzeros(0,0), spzeros(0,0), NaN +init_orthocolloc(NT,::TranscriptionMethod,_,_) = spzeros(NT,0,0), spzeros(NT,0,0), NT(NaN) "Evaluate the Lagrange basis polynomial ``L_j`` at `τ=1`." function lagrange_end(j, transcription::OrthogonalCollocation) @@ -428,20 +547,6 @@ end default_jacobian(::SingleShooting) = DEFAULT_JACDENSE default_jacobian(::TranscriptionMethod) = DEFAULT_JACSPARSE -function validate_transcription(::LinModel, ::CollocationMethod) - throw(ArgumentError("Collocation methods are not supported for LinModel.")) - return nothing -end -function validate_transcription(::NonLinModel{<:Real, <:EmptySolver}, ::CollocationMethod) - throw(ArgumentError("Collocation methods require continuous-time NonLinModel.")) - return nothing -end -validate_transcription(::SimModel, ::TranscriptionMethod) = nothing - -"Get length of the `k̄` vector with all the solver intermediate steps or all the collocation pts." -get_nk̄(model::SimModel, ::ShootingMethod) = model.nk̄ -get_nk̄(model::SimModel, transcription::CollocationMethod) = model.nx*transcription.no - transcription_str(transription::TranscriptionMethod) = string(nameof(typeof(transription))) function transcription_str(transription::OrthogonalCollocation) return "$(nameof(typeof(transription))) ($(transription.no) collocation points)" diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index e6c23dbf2..03aa6295c 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -162,15 +162,15 @@ end linmodel1 = LinModel(sys,Ts,i_u=[1,2]) f1!(x,u,_,model) = model.A*x + model.Bu*u h1!(x,_,model) = model.C*x - nonlinmodel1 = NonLinModel(f1!,h1!,Ts,2,2,2,solver=nothing,p=linmodel1) - @test nonlinmodel1.nx == 2 - @test nonlinmodel1.nu == 2 - @test nonlinmodel1.nd == 0 - @test nonlinmodel1.ny == 2 - xnext, y = nonlinmodel1.buffer.x, nonlinmodel1.buffer.y - nonlinmodel1.f!(xnext, [0,0],[0,0],[1],nonlinmodel1.p) - @test xnext ≈ zeros(2,) - nonlinmodel1.h!(y,[0,0],[1],nonlinmodel1.p) + daemodel = NonLinModel(f1!,h1!,Ts,2,2,2,solver=nothing,p=linmodel1) + @test daemodel.nx == 2 + @test daemodel.nu == 2 + @test daemodel.nd == 0 + @test daemodel.ny == 2 + ẋ, y = daemodel.buffer.x, daemodel.buffer.y + daemodel.f!(ẋ, [0,0],[0,0],[1],daemodel.p) + @test ẋ ≈ zeros(2,) + daemodel.h!(y,[0,0],[1],daemodel.p) @test y ≈ zeros(2,) linmodel2 = LinModel(sys,Ts,i_d=[3]) @@ -182,9 +182,9 @@ end @test nonlinmodel2.nu == 2 @test nonlinmodel2.nd == 1 @test nonlinmodel2.ny == 2 - xnext, y = nonlinmodel2.buffer.x, nonlinmodel2.buffer.y - nonlinmodel2.f!(xnext,[0,0,0,0],[0,0],[0],nonlinmodel2.p) - @test xnext ≈ zeros(4,) + ẋ, y = nonlinmodel2.buffer.x, nonlinmodel2.buffer.y + nonlinmodel2.f!(ẋ,[0,0,0,0],[0,0],[0],nonlinmodel2.p) + @test ẋ ≈ zeros(4,) nonlinmodel2.h!(y,[0,0,0,0],[0],nonlinmodel2.p) @test y ≈ zeros(2,) @@ -203,9 +203,9 @@ end return nothing end nonlinmodel4 = NonLinModel(f1!, h1!, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel2) - xnext, y = nonlinmodel4.buffer.x, nonlinmodel4.buffer.y - nonlinmodel4.f!(xnext,[0,0,0,0],[0,0],[0],nonlinmodel4.p) - @test xnext ≈ zeros(4) + ẋ, y = nonlinmodel4.buffer.x, nonlinmodel4.buffer.y + nonlinmodel4.f!(ẋ,[0,0,0,0],[0,0],[0],nonlinmodel4.p) + @test ẋ ≈ zeros(4) nonlinmodel4.h!(y,[0,0,0,0],[0],nonlinmodel4.p) @test y ≈ zeros(2) @@ -221,9 +221,9 @@ end @test string(solver) == "4th order Runge-Kutta differential equation solver with 1 supersamples." nonlinmodel5 = NonLinModel(f3, h3, 1.0, 1, 2, 1, 1, solver=solver, p=p) - xnext, k̄, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.k̄, nonlinmodel5.buffer.y - ModelPredictiveControl.f!(xnext, k̄, nonlinmodel5, [0; 0], [0], [0], nonlinmodel5.p) - @test xnext ≈ zeros(2) + ẋ, k̄, y = nonlinmodel5.buffer.x, nonlinmodel5.buffer.k̄, nonlinmodel5.buffer.y + ModelPredictiveControl.f!(ẋ, k̄, nonlinmodel5, [0; 0], [0], [0], nonlinmodel5.p) + @test ẋ ≈ zeros(2) ModelPredictiveControl.h!(y, nonlinmodel5, [0; 0], [0], nonlinmodel5.p) @test y ≈ zeros(1) @@ -239,15 +239,15 @@ end return nothing end nonlinmodel6 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=RungeKutta(), p=p) - xnext, k̄, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.k̄, nonlinmodel6.buffer.y - ModelPredictiveControl.f!(xnext, k̄, nonlinmodel6, [0; 0], [0], [0], nonlinmodel6.p) - @test xnext ≈ zeros(2) + ẋ, k̄, y = nonlinmodel6.buffer.x, nonlinmodel6.buffer.k̄, nonlinmodel6.buffer.y + ModelPredictiveControl.f!(ẋ, k̄, nonlinmodel6, [0; 0], [0], [0], nonlinmodel6.p) + @test ẋ ≈ zeros(2) ModelPredictiveControl.h!(y, nonlinmodel6, [0; 0], [0], nonlinmodel6.p) @test y ≈ zeros(1) nonlinmodel7 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, solver=ForwardEuler(), p=p) - xnext, k̄, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.k̄, nonlinmodel7.buffer.y - ModelPredictiveControl.f!(xnext, k̄, nonlinmodel7, [0; 0], [0], [0], nonlinmodel7.p) - @test xnext ≈ zeros(2) + ẋ, k̄, y = nonlinmodel7.buffer.x, nonlinmodel7.buffer.k̄, nonlinmodel7.buffer.y + ModelPredictiveControl.f!(ẋ, k̄, nonlinmodel7, [0; 0], [0], [0], nonlinmodel7.p) + @test ẋ ≈ zeros(2) ModelPredictiveControl.h!(y, nonlinmodel7, [0; 0], [0], nonlinmodel7.p) @test y ≈ zeros(1) nonlinmodel8 = NonLinModel(f2!, h2!, 1.0, 1, 2, 1, 1, p=p, jacobian=AutoFiniteDiff()) @@ -302,15 +302,15 @@ end Ts = 1.0 f1!(x,u,d,_) = x.^5 .+ u.^4 .+ d.^3 h1!(x,d,_) = x.^2 .+ d - nonlinmodel1 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=nothing) + daemodel = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=nothing) x, u, d = [2.0], [3.0], [4.0] - linmodel1 = linearize(nonlinmodel1; x, u, d) + linmodel1 = linearize(daemodel; x, u, d) @test linmodel1.A ≈ 5*x.^4 @test linmodel1.Bu ≈ 4*u.^3 @test linmodel1.Bd ≈ 3*d.^2 @test linmodel1.C ≈ 2*x.^1 @test linmodel1.Dd ≈ 1*d.^0 - linmodel1b = LinModel(nonlinmodel1; x, u, d) + linmodel1b = LinModel(daemodel; x, u, d) @test linmodel1.A ≈ linmodel1b.A @test linmodel1.Bu ≈ linmodel1b.Bu @test linmodel1.Bd ≈ linmodel1b.Bd @@ -395,16 +395,16 @@ end @testitem "NonLinModel real time simulations" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = LinModel(tf(2, [10, 1]), 0.25) - nonlinmodel1 = NonLinModel( + daemodel = NonLinModel( (x,u,_,_)->linmodel1.A*x + linmodel1.Bu*u, (x,_,_)->linmodel1.C*x, linmodel1.Ts, 1, 1, 1, 0, solver=nothing ) times1 = zeros(5) for i=1:5 - times1[i] = savetime!(nonlinmodel1) - updatestate!(nonlinmodel1, [1]) - periodsleep(nonlinmodel1) + times1[i] = savetime!(daemodel) + updatestate!(daemodel, [1]) + periodsleep(daemodel) end @test all(isapprox.(diff(times1[2:end]), 0.25, atol=0.05)) linmodel2 = LinModel(tf(2, [0.1, 1]), 0.25) @@ -420,4 +420,149 @@ end periodsleep(nonlinmodel2, true) end @test all(isapprox.(diff(times2[2:end]), 0.25, atol=0.05)) +end + +@testitem "NonLinModelDAE construction" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using JuMP, Ipopt + using DifferentiationInterface + import FiniteDiff + + function fq!(ẋ, res, x, a, u, _ , p) + ẋ[] = -p[] * (x[] - 0.2 * u[]) + res[] = x[] - a[] + return nothing + end + function fq(x, a, u, _ , p) + ẋ = -p .* (x .- 0.2 .* u) + res = x .- a + return ẋ, res + end + function h!(y, x, a, _ , _ ) + y[] = 2*x[]+ a[] + end + function h(x, a, _ , _ ) + y = 2 .* x .+ a + return y + end + nu, nx, na, ny = 1, 1, 1, 1 + Ts = 1 + p = [0.5] + + dae = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p) + @test dae.nx == nx + @test dae.na == na + @test dae.nu == nu + @test dae.nd == 0 + @test dae.ny == ny + @test dae.iszero_Ha == false + ẋ, q, y = dae.buffer.x, dae.buffer.a, dae.buffer.y + dae.fq!(ẋ, q, [0], [0], [0], [0], dae.p) + @test ẋ ≈ zeros(1) + @test q ≈ zeros(1) + dae.h!(y, [0], [0], [0], dae.p) + @test y ≈ zeros(1) + + dae_oop = NonLinModelDAE(fq, h, Ts, nu, nx, na, ny; p) + dae_oop.fq!(ẋ, q, [0], [0], [0], [0], dae_oop.p) + @test ẋ ≈ zeros(1) + @test q ≈ zeros(1) + dae_oop.h!(y, [0], [0], [0], dae_oop.p) + @test y ≈ zeros(1) + + transcription = TrapezoidalCollocation() + dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription) + @test dae2.transcription isa TrapezoidalCollocation + @test length(dae2.Z) == 3 + @test size(dae2.Aeq, 1) == 0 + + transcription = OrthogonalCollocation(0, 4, roots=:gausslegendre) + dae3 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription) + @test dae3.transcription isa OrthogonalCollocation + @test length(dae3.Z) == 1 + 1 + 4 + 4 + @test size(dae3.Aeq, 1) == 1 + + optim_state = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb"=>"yes")) + optim_output = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb"=>"yes")) + dae4 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, optim_state, optim_output) + @test solver_name(dae4.optim_state) == "Ipopt" + @test solver_name(dae4.optim_output) == "Ipopt" + + jacobian = AutoFiniteDiff() + hessian = true + dae5 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, jacobian, hessian) + @test dae5.jacobian isa AutoFiniteDiff + @test dae5.hessian isa AutoForwardDiff + + function h_no_a!(y, x, _ , _ , _ ) + y .= 2 .* x + end + dae6 = NonLinModelDAE(fq!, h_no_a!, Ts, nu, nx, na, ny; p) + @test dae6.iszero_Ha == true + + dae7 = NonLinModelDAE{Float32}(fq!,h!, Ts, nu, nx, na, ny; p) + @test isa(dae7, NonLinModelDAE{Float32}) + + @test_throws ErrorException NonLinModelDAE( + (x,u,p)->(x+u+p, 0.0), + (x,a,d,p)->(x+a+d+p), Ts, 1, 1, 1, 1) + @test_throws ErrorException NonLinModelDAE( + (x,a,u,d,p)->(x+u+p, a-x), + (x,a,d)->(x+a+d), Ts, 1, 1, 1, 1) + @test_throws ArgumentError NonLinModelDAE( + fq!, h!, Ts, 1, 1, 1, 1; p, optim_state, optim_output=optim_state + ) + + # DAE with direct transmission from input to output: + function fq_dt!(ẋ, res, x, a, u, _ , p) + ẋ[] = -p[] * (x[] - 0.2 * u[]) + res[] = a[] - u[] + end + h_dt!(y, x, a, _ , _ ) = (y[] = 2*x[] + a[]) + @test_throws ErrorException NonLinModelDAE( + fq_dt!, h_dt!, Ts, nu, nx, na, ny; p + ) +end + +@testitem "NonLinModelDAE sim methods" setup=[SetupMPCtests] begin + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff + + function fq!(ẋ, res, x, a, u, _ , p) + ẋ[] = -p[] * (x[] - 0.2 * u[]) + res[] = x[] - a[] + return nothing + end + function h!(y, x, a, _ , _ ) + y[] = 2*x[]+ a[] + end + nu, nx, na, ny = 1, 1, 1, 1 + Ts = 1 + p = [1.0] + + transcription = OrthogonalCollocation(0, 4, roots=:gausslegendre) + dae = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription) + u = [0.0] + d = Float64[] + + @test updatestate!(dae, u) ≈ zeros(1) + @test updatestate!(dae, u, d) ≈ zeros(1) + @test dae.x0 ≈ zeros(1) + @test evaloutput(dae) ≈ dae() ≈ zeros(1) + + transcription = TrapezoidalCollocation() + dae2 = NonLinModelDAE(fq!, h!, Ts, nu, nx, na, ny; p, transcription) + @test updatestate!(dae2, u) ≈ zeros(1) + @test updatestate!(dae2, u, d) ≈ zeros(1) + @test dae2.x0 ≈ zeros(1) + @test evaloutput(dae2) ≈ dae2() ≈ zeros(1) + + x = initstate!(dae, [10]) # do nothing for NonLinModelDAE + @test evaloutput(dae) ≈ [0] + + @test_throws DimensionMismatch updatestate!(dae, zeros(2)) + @test_throws DimensionMismatch updatestate!(dae, zeros(1), zeros(1)) + @test_throws DimensionMismatch evaloutput(dae, zeros(1)) + end \ No newline at end of file