Merge pull request #209 from JuliaControl/manual_estimator_test

Added tests and improve doc for `ManualEstimator`

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.6.2"
+version = "1.7.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -129,6 +129,7 @@ for more detailed examples.
   - extended Kalman filter
   - unscented Kalman filter
   - moving horizon estimator
+- disable built-in observer to manually provide your own state estimate
 - easily estimate unmeasured disturbances by adding one or more integrators at the:
   - manipulated inputs
   - measured outputs

docs/make.jl

@@ -12,6 +12,7 @@ links = InterLinks(
     "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv",
     "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv",
     "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv",
+    "LowLevelParticleFilters" => "https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/objects.inv",
 )
 
 DocMeta.setdocmeta!(

src/estimator/manual.jl

@@ -55,10 +55,13 @@ end
 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
+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.
+each time step ``k``. The states in the estimator must obviously represent the same thing as
+in the controller, both for the deterministic and the stochastic model of the unmeasured
+disturbances (`nint_u` and `nint_ym` arguments). Calling [`preparestate!`](@ref) and 
+[`updatestate!`](@ref) on this object will do nothing at all. See Extended Help for usage 
+examples.
 
 # Arguments
 - `model::SimModel` : (deterministic) model for the estimations.
@@ -84,13 +87,51 @@ ManualEstimator estimator with a sample time Ts = 0.5 s, LinModel and:
 
 # 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.:
+    A first use case is a linear predictive controller based on nonlinear state estimation,
+    for example a nonlinear [`MovingHorizonEstimator`](@ref) (MHE). Note that the model
+    augmentation scheme with `nint_u` and `nint_ym` options must be identical for both
+    [`StateEstimator`](@ref) objects (see [`SteadyKalmanFilter`](@ref) extended help for
+    details on augmentation). The [`ManualEstimator`](@ref) serves as a wrapper to provide
+    the minimal required information to construct any [`PredictiveController`](@ref) object:
 
-    ```julia
-    a=1
+    ```jldoctest
+    julia> function man_sim()
+               f(x,u,_,_) = 0.5*sin.(x + u)
+               h(x,_,_) = x
+               model = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing)
+               linModel = linearize(model, x=[0], u=[0])
+               man = ManualEstimator(linModel, nint_u=[1])
+               mpc = LinMPC(man)
+               estim = MovingHorizonEstimator(model, nint_u=[1], He=5)
+               estim = setconstraint!(estim, v̂min=[-0.001], v̂max=[0.001])
+               initstate!(estim, [0], [0])
+               y_data, ŷ_data = zeros(5), zeros(5)
+               for i=1:5
+                   y = model()                     # simulated measurement
+                   x̂ = preparestate!(estim, y)     # correct nonlinear MHE state estimate
+                   ŷ = estim()                     # nonlinear MHE estimated output
+                   setstate!(mpc, x̂)               # update MPC with the MHE corrected state 
+                   u = moveinput!(mpc, [0])
+                   y_data[i], ŷ_data[i] = y[1], ŷ[1]
+                   updatestate!(estim, u, y)       # update nonlinear MHE estimation
+                   updatestate!(model, u .+ 0.5)   # update simulator with load disturbance
+               end
+               return collect([y_data ŷ_data]')
+           end;
+
+    julia> YandŶ = round.(man_sim(), digits=6)
+    2×5 Matrix{Float64}:
+      0.0  0.239713  0.227556  0.157837  0.098629
+     -0.0  0.238713  0.226556  0.156837  0.097629
     ```
+
+    A second use case is to allow the user to manually provide the state estimate computed
+    from an external source, e.g. an observer from [`LowLevelParticleFilters`](@extref LowLevelParticleFilters).
+    A custom stochastic model for the unmeasured disturbances (other than integrated white 
+    noise) can be specified by constructing a [`SimModel`](@ref) object with the augmented
+    state-space matrices/functions directly, and by setting `nint_u=0` and `nint_ym=0`. See
+    [`Disturbance-gallery`](@extref LowLevelParticleFilters) for examples of other
+    disturbance models.
 """
 function ManualEstimator(
     model::SM;

src/estimator/mhe/execute.jl

@@ -15,6 +15,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0)
         estim.U0[1:estim.model.nu] .= u0
         estim.D0[1:estim.model.nd] .= d0
     end
+    estim.lastu0 .= u0
     # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0)
     invert_cov!(estim, estim.P̂_0)
     estim.P̂arr_old  .= estim.P̂_0

src/precompile.jl

@@ -56,6 +56,11 @@ mpc_mhe.estim()
 u = mpc_mhe([55, 30])
 sim!(mpc_mhe, 2, [55, 30])
 
+mpc_man = setconstraint!(LinMPC(ManualEstimator(model)), ymin=[45, -Inf])
+initstate!(mpc_man, model.uop, model())
+setstate!(mpc_man, ones(4))
+u = mpc_man([55, 30])
+
 nmpc_skf = setconstraint!(NonLinMPC(SteadyKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf])
 initstate!(nmpc_skf, model.uop, model())
 preparestate!(nmpc_skf, [55, 30])