Merge pull request #206 from JuliaControl/manual_estimator

added: `ManualEstimator` to turn off built-in state estimation

Author: franckgaga

docs/src/public/state_estim.md

@@ -81,6 +81,12 @@ MovingHorizonEstimator
 InternalModel
 ```
 
+## ManualEstimator
+
+```@docs
+ManualEstimator
+```
+
 ## Default Model Augmentation
 
 ```@docs

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

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}

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 "*

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 "*

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

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; <keyword arguments>)
+
+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

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:

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