diff --git a/docs/src/public/state_estim.md b/docs/src/public/state_estim.md index 2d2d9a794..07a28b0fd 100644 --- a/docs/src/public/state_estim.md +++ b/docs/src/public/state_estim.md @@ -81,6 +81,12 @@ MovingHorizonEstimator InternalModel ``` +## ManualEstimator + +```@docs +ManualEstimator +``` + ## Default Model Augmentation ```@docs diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index adefdfef2..b3a74ad61 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -36,6 +36,7 @@ export savetime!, periodsleep export StateEstimator, InternalModel, Luenberger export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter export MovingHorizonEstimator +export ManualEstimator export default_nint, initstate! export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput! export TranscriptionMethod, SingleShooting, MultipleShooting diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 2e6d11eea..294b32d50 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -1,3 +1,6 @@ +const MSG_LINMODEL_ERR = "estim.model type must be a LinModel, see ManualEstimator docstring "* + "to use a nonlinear state estimator with a linear controller" + struct PredictiveControllerBuffer{NT<:Real} u ::Vector{NT} Z̃ ::Vector{NT} diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index f6f646792..72f6b4142 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -157,7 +157,7 @@ function ExplicitMPC( N_Hc = Diagonal(repeat(Nwt, Hc)), L_Hp = Diagonal(repeat(Lwt, Hp)), ) where {NT<:Real, SE<:StateEstimator{NT}} - isa(estim.model, LinModel) || error("estim.model type must be a LinModel") + isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) nk = estimate_delays(estim.model) if Hp ≤ nk @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 03122b651..ae3d48108 100644 --- a/src/controller/linmpc.jl +++ b/src/controller/linmpc.jl @@ -256,7 +256,7 @@ function LinMPC( transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION, optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false), ) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel} - isa(estim.model, LinModel) || error("estim.model type must be a LinModel") + isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR) nk = estimate_delays(estim.model) if Hp ≤ nk @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "* diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 6f1ceedeb..90500dbad 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -978,9 +978,9 @@ differentiation. This estimator is allocation-free if `model` simulations do not - `σR=fill(1,length(i_ym))` or *`sigmaR`* : main diagonal of the sensor noise covariance ``\mathbf{R}`` of `model` measured outputs, specified as a standard deviation vector. - `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at - the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). + the manipulated inputs (vector), use `nint_u=0` for no integrator. - `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured - disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). + disturbances at the measured outputs, use `nint_ym=0` for no integrator. - `σQint_u=fill(1,sum(nint_u))` or *`sigmaQint_u`* : same than `σQ` but for the unmeasured disturbances at manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators). - `σPint_u_0=fill(1,sum(nint_u))` or *`sigmaPint_u_0`* : same than `σP_0` but for the unmeasured diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl new file mode 100644 index 000000000..a1ed20450 --- /dev/null +++ b/src/estimator/manual.jl @@ -0,0 +1,111 @@ +struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT} + model::SM + lastu0::Vector{NT} + x̂op ::Vector{NT} + f̂op ::Vector{NT} + x̂0 ::Vector{NT} + i_ym::Vector{Int} + nx̂ ::Int + nym::Int + nyu::Int + nxs::Int + As ::Matrix{NT} + Cs_u::Matrix{NT} + Cs_y::Matrix{NT} + nint_u ::Vector{Int} + nint_ym::Vector{Int} + Â ::Matrix{NT} + B̂u ::Matrix{NT} + Ĉ ::Matrix{NT} + B̂d ::Matrix{NT} + D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} + direct::Bool + corrected::Vector{Bool} + buffer::StateEstimatorBuffer{NT} + function ManualEstimator{NT}( + model::SM, i_ym, nint_u, nint_ym + ) where {NT<:Real, SM<:SimModel{NT}} + 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) + nxs = size(As, 1) + nx̂ = model.nx + nxs + Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) + Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] + lastu0 = zeros(NT, nu) + x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] + direct = false + corrected = [true] + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) + return new{NT, SM}( + model, + lastu0, x̂op, f̂op, x̂0, + i_ym, nx̂, nym, nyu, nxs, + As, Cs_u, Cs_y, nint_u, nint_ym, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, + direct, corrected, + buffer + ) + end +end + +@doc raw""" + ManualEstimator(model::SimModel; ) + +Construct a manual state estimator for `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). + +This [`StateEstimator`](@ref) type allows the construction of [`PredictiveController`](@ref) +objects but turns off the built-in state estimation. The user must manually provides the +estimate ``\mathbf{x̂}_{k}(k)`` or ``\mathbf{x̂}_{k-1}(k)`` through [`setstate!`](@ref) at +each time step. Calling [`preparestate!`](@ref) and [`updatestate!`](@ref) will not modify +the estimate. See Extended Help for usage examples. + +# Arguments +- `model::SimModel` : (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}``. +- `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at + the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). +- `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured + disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). + +# Examples +```jldoctest +julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5); + +julia> estim = ManualEstimator(model, nint_ym=0) # disable augmentation with integrators +ManualEstimator estimator with a sample time Ts = 0.5 s, LinModel and: + 1 manipulated inputs u (0 integrating states) + 2 estimated states x̂ + 2 measured outputs ym (0 integrating states) + 0 unmeasured outputs yu + 0 measured disturbances d +``` + +# Extended Help +!!! details "Extended Help" + A first use case is a linear predictive controller based on nonlinear state estimation. + The [`ManualEstimator`](@ref) serves as a wrapper to provide the minimal required + information to construct a [`PredictiveController`](@ref). e.g.: + + ```julia + a=1 + ``` +""" +function ManualEstimator( + model::SM; + i_ym::AbstractVector{Int} = 1:model.ny, + nint_u ::IntVectorOrInt = 0, + nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u), +) where {NT<:Real, SM<:SimModel{NT}} + return ManualEstimator{NT}(model, i_ym, nint_u, nint_ym) +end + +""" + update_estimate!(estim::ManualEstimator, y0m, d0, u0) + +Do nothing for [`ManualEstimator`](@ref). +""" +update_estimate!(estim::ManualEstimator, y0m, d0, u0) = nothing \ No newline at end of file diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index 25566a7c6..a1d9c9ccc 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -90,7 +90,7 @@ functions are defined as: ``` where ``\mathbf{x}``, ``\mathbf{y}``, ``\mathbf{u}``, ``\mathbf{d}`` and ``\mathbf{p}`` are respectively the state, output, manipulated input, measured disturbance and parameter -vectors. As a mather of fact, the parameter argument `p` can be any Julia objects but use a +vectors. As a matter of fact, the parameter argument `p` can be any Julia objects but use a mutable type if you want to change them later e.g.: a vector. If the dynamics is a function of the time, simply add a measured disturbance defined as ``d(t) = t``. The functions can be implemented in two possible ways: diff --git a/src/state_estim.jl b/src/state_estim.jl index 93420831e..b681ef2d1 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -26,6 +26,7 @@ include("estimator/kalman.jl") include("estimator/luenberger.jl") include("estimator/mhe.jl") include("estimator/internal_model.jl") +include("estimator/manual.jl") function Base.show(io::IO, estim::StateEstimator) nu, nd = estim.model.nu, estim.model.nd