added: doc correction and reduce allocations for all Kalman filters

Project.toml

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

README.md

@@ -126,9 +126,9 @@ for more detailed examples.
   - [x] manipulated inputs
   - [x] measured outputs
 - [x] bumpless manual to automatic transfer for control with a proper initial estimate
-- [ ] estimators in two possible forms:
+- [x] estimators in two possible forms:
+  - [x] filter (or current) form to improve accuracy and robustness
   - [x] predictor (or delayed) form to reduce computational load
-  - [ ] filter (or current) form to improve accuracy and robustness
 - [x] moving horizon estimator in two formulations:
   - [x] linear plant models (quadratic optimization)
   - [x] nonlinear plant models (nonlinear optimization)

docs/src/manual/nonlinmpc.md

@@ -378,7 +378,7 @@ nothing # hide
 
 The [`setmodel!`](@ref) method must be called after solving the optimization problem with
 [`moveinput!`](@ref) and before updating the state estimate with [`updatestate!`](@ref),
-that is, when both ``\mathbf{u}(k)`` and ``\mathbf{x̂}_{k-1}(k)`` are available as the new
+that is, when both ``\mathbf{u}(k)`` and ``\mathbf{x̂}_{k}(k)`` are available as the new
 operating point. The [`SimResult`](@ref) object is for plotting purposes only. The adaptive
 [`LinMPC`](@ref) performances are similar to the nonlinear MPC, both for the 180° setpoint:
 

src/controller/construct.jl

@@ -94,10 +94,10 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil
     Terminal constraints provide closed-loop stability guarantees on the nominal plant model.
     They can render an unfeasible problem however. In practice, a sufficiently large
     prediction horizon ``H_p`` without terminal constraints is typically enough for 
-    stability. If `mpc.estim.direct==true`, the state is estimated at ``i = k`` (the current
-    time step), otherwise at ``i = k - 1``. Note that terminal constraints are applied on the
-    augmented state vector ``\mathbf{x̂}`` (see [`SteadyKalmanFilter`](@ref) for details on
-    augmentation).
+    stability. If `mpc.estim.direct==true`, the estimator computes the states at ``i = k`` 
+    (the current time step), otherwise at ``i = k - 1``. Note that terminal constraints are
+    applied on the augmented state vector ``\mathbf{x̂}`` (see [`SteadyKalmanFilter`](@ref)
+    for details on augmentation).
 
     For variable constraints, the bounds can be modified after calling [`moveinput!`](@ref),
     that is, at runtime, but not the softness parameters ``\mathbf{c}``. It is not possible

src/estimator/construct.jl

@@ -146,13 +146,13 @@ end
 Return empty matrices, and `x̂op` & `f̂op` vectors, if `model` is not a [`LinModel`](@ref).
 """
 function augment_model(model::SimModel{NT}, As, args... ; verify_obsv=false) where NT<:Real
-    nu, nx, nd = model.nu, model.nx, model.nd
+    nu, nx, nd, ny = model.nu, model.nx, model.nd, model.ny
     nxs = size(As, 1)
     Â   = zeros(NT, 0, nx+nxs)
     B̂u  = zeros(NT, 0, nu)
-    Ĉ   = zeros(NT, 0, nx+nxs)
+    Ĉ   = zeros(NT, ny, 0)
     B̂d  = zeros(NT, 0, nd)
-    D̂d  = zeros(NT, 0, nd)
+    D̂d  = zeros(NT, ny, 0)
     x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)]
     return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
 end

src/estimator/execute.jl

@@ -153,8 +153,8 @@ in which ``\mathbf{Ĉ^m, D̂_d^m}`` are the rows of `estim.Ĉ, estim.D̂d`  th
 measured outputs ``\mathbf{y^m}``.
 """
 function init_estimate!(estim::StateEstimator, ::LinModel, y0m, d0, u0)
-    Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d
-    Ĉm, D̂dm = @views Ĉ[estim.i_ym, :], D̂d[estim.i_ym, :] # measured outputs ym only
+    Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d
+    Ĉm, D̂dm = estim.Ĉm, estim.D̂dm
     # TODO: use estim.buffer.x̂ to reduce allocations
     estim.x̂0 .= [I - Â; Ĉm]\[B̂u*u0 + B̂d*d0 + estim.f̂op - estim.x̂op; y0m - D̂dm*d0]
     return nothing
@@ -390,6 +390,8 @@ function setmodel_estimator!(estim::StateEstimator, model, _ , _ , _ , Q̂, R̂)
     estim.Ĉ  .= Ĉ
     estim.B̂d .= B̂d
     estim.D̂d .= D̂d
+    estim.Ĉm  .= @views Ĉ[estim.i_ym, :]
+    estim.D̂dm .= @views D̂d[estim.i_ym, :]
     # --- update state estimate and its operating points ---
     estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point
     estim.x̂op .= x̂op

src/estimator/internal_model.jl

@@ -16,11 +16,13 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     Bs::Matrix{NT}
     Cs::Matrix{NT}
     Ds::Matrix{NT}
-    Â ::Matrix{NT}
-    B̂u::Matrix{NT}
-    Ĉ ::Matrix{NT}
-    B̂d::Matrix{NT}
-    D̂d::Matrix{NT}
+    Â   ::Matrix{NT}
+    B̂u  ::Matrix{NT}
+    Ĉ   ::Matrix{NT}
+    B̂d  ::Matrix{NT}
+    D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     Âs::Matrix{NT}
     B̂s::Matrix{NT}
     direct::Bool
@@ -36,6 +38,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         nxs = size(As,1)
         nx̂ = model.nx
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)
+        Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:]
         Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
         lastu0 = zeros(NT, nu)
         # x̂0 and x̂d are same object (updating x̂d will update x̂0):
@@ -50,7 +53,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Bs, Cs, Ds, 
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             Âs, B̂s,
             direct, corrected,
             buffer
@@ -156,8 +159,8 @@ function matrices_internalmodel(model::LinModel)
 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
-    nu, nx, nd = model.nu, model.nx, model.nd
-    Â, B̂u, Ĉ, B̂d, D̂d = zeros(NT,0,nx), zeros(NT,0,nu), zeros(NT,0,nx), zeros(NT,0,nd), zeros(NT,0,nd)
+    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)
     return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
 end
@@ -225,6 +228,8 @@ function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ )
     estim.Ĉ  .= Ĉ
     estim.B̂d .= B̂d
     estim.D̂d .= D̂d
+    estim.Ĉm  .= @views Ĉ[estim.i_ym,:]
+    estim.D̂dm .= @views D̂d[estim.i_ym,:]
     # --- update state estimate and its operating points ---
     estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point
     estim.x̂op .= x̂op