doc: add info on custom covariance estimator for MHE

franckgaga · franckgaga · commit 3f02ec383a9a · 2024-03-28T12:28:20.000-04:00
diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md
@@ -79,7 +79,7 @@ savefig(ans, "plot1_NonLinMPC.svg"); nothing # hide
 An [`UnscentedKalmanFilter`](@ref) estimates the plant state :
 
 ```@example 1
-α=0.1; σQ=[0.1, 0.5]; σR=[0.5]; nint_u=[1]; σQint_u=[0.1]
+α=0.01; σQ=[0.1, 0.5]; σR=[0.5]; nint_u=[1]; σQint_u=[0.1]
 estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u)
 ```
 
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -428,7 +428,7 @@ represents the measured outputs of ``\mathbf{ĥ}`` function (and unmeasured one
 
 # Arguments
 - `model::SimModel` : (deterministic) model for the estimations.
-- `α=1e-3` : alpha parameter, spread of the state distribution ``(0 ≤ α ≤ 1)``.
+- `α=1e-3` : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``.
 - `β=2` : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``.
 - `κ=0` : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``.
 - `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
@@ -565,22 +565,7 @@ noise, respectively.
      ISBN9780470045343.
 """
 function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:Real
-    return update_estimate_ukf!(estim, u, ym, d, estim.P̂, estim.x̂)
-end
-
-"""
-    update_estimate_ukf!(estim::StateEstimator, u, ym, d, P̂, x̂=nothing)
-
-Update Unscented Kalman Filter estimates and covariance matrices.
-
-Allows code reuse for [`UnscentedKalmanFilter`](@ref) and [`MovingHorizonEstimator`](@ref).
-See  [`update_estimate!(::UnscentedKalmanFilter, ::Any, ::Any, ::Any)`(@ref) docstring
-for the equations. If `isnothing(x̂)`, only the covariance `P̂` is updated.
-"""
-function update_estimate_ukf!(
-    estim::StateEstimator{NT}, u, ym, d, P̂, x̂=nothing
-) where NT<:Real
-    Q̂, R̂, K̂ = estim.Q̂, estim.R̂, estim.K̂
+    x̂, P̂, Q̂, R̂, K̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂
     nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
     # --- initialize matrices ---
@@ -815,34 +800,29 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing)
 end
 
 """
-    update_estimate_kf!(estim::StateEstimator, u, ym, d, Â, Ĉm, P̂, x̂=nothing)
+    update_estimate_kf!(estim::StateEstimator, u, ym, d, Â, Ĉm, P̂, x̂)
 
 Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices.
 
 Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter`](@ref).
 They update the state `x̂` and covariance `P̂` with the same equations. The extended filter
 substitutes the augmented model matrices with its Jacobians (`Â = F̂` and `Ĉm = Ĥm`).
 The implementation uses in-place operations and explicit factorization to reduce
-allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. If `isnothing(x̂)`,
-only the covariance `P̂` is updated.
+allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations.
 """
-function update_estimate_kf!(
-    estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂, x̂=nothing
-) where NT<:Real
-    Q̂, R̂, M̂ = estim.Q̂, estim.R̂, estim.M̂
+function update_estimate_kf!(estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂, x̂) where NT<:Real
+    Q̂, R̂, M̂, K̂ = estim.Q̂, estim.R̂, estim.M̂, estim.K̂
     mul!(M̂, P̂, Ĉm')
     rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂)))
-    if !isnothing(x̂)
-        mul!(estim.K̂, Â, M̂)
-        x̂next, ŷ = Vector{NT}(undef, estim.nx̂), Vector{NT}(undef, estim.model.ny)
-        ĥ!(ŷ, estim, estim.model, x̂, d)
-        ŷm = @views ŷ[estim.i_ym]
-        v̂  = ŷm
-        v̂ .= ym .- ŷm
-        f̂!(x̂next, estim, estim.model, x̂, u, d)
-        mul!(x̂next, estim.K̂, v̂, 1, 1)
-        estim.x̂ .= x̂next
-    end
+    mul!(K̂, Â, M̂)
+    x̂next, ŷ = Vector{NT}(undef, estim.nx̂), Vector{NT}(undef, estim.model.ny)
+    ĥ!(ŷ, estim, estim.model, x̂, d)
+    ŷm = @views ŷ[estim.i_ym]
+    v̂  = ŷm
+    v̂ .= ym .- ŷm
+    f̂!(x̂next, estim, estim.model, x̂, u, d)
+    mul!(x̂next, K̂, v̂, 1, 1)
+    estim.x̂ .= x̂next
     P̂.data .= Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂ # .data is necessary for Hermitians
     return nothing
 end
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -187,9 +187,9 @@ N_k =                     \begin{cases}
 ```
 The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` encompass the estimated process noise
 ``\mathbf{ŵ}(k-j)`` and sensor noise ``\mathbf{v̂}(k-j)`` from ``j=N_k-1`` to ``0``. The 
-Extended Help defines the two vectors and the scalar ``ϵ``. See [`UnscentedKalmanFilter`](@ref)
-for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. The
-covariance ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated with an [`ExtendedKalmanFilter`](@ref).
+Extended Help defines the two vectors, the slack variable ``ϵ``, and the estimation of the
+covariance at arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+1)``. See [`UnscentedKalmanFilter`](@ref)
+for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances.
 
 !!! warning
     See the Extended Help if you get an error like:    
@@ -248,18 +248,23 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, Ipopt optimizer
     The slack variable ``ϵ`` relaxes the constraints if enabled, see [`setconstraint!`](@ref). 
     It is disabled by default for the MHE (from `Cwt=Inf`) but it should be activated for
     problems with two or more types of bounds, to ensure feasibility (e.g. on the estimated
-    state and sensor noise).
-
-    For [`LinModel`](@ref), the optimization is treated as a quadratic program with a
-    time-varying Hessian, which is generally cheaper than nonlinear programming.
+    state and sensor noise). 
     
-    For [`NonLinModel`](@ref), the optimization relies on automatic differentiation (AD).
-    Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` 
-    functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
-    for common mistakes when writing these functions. 
+    The optimization and the estimation of the covariance at arrival 
+    ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` depend on `model`:
+
+    - If `model` is a [`LinModel`](@ref), the optimization is treated as a quadratic program
+      with a time-varying Hessian, which is generally cheaper than nonlinear programming. By
+      default, a [`KalmanFilter`](@ref) estimates the arrival covariance (customizable).
+    - Else, a nonlinear program with automatic differentiation (AD) solves the optimization.
+      Optimizers generally benefit from exact derivatives like AD. However, the `f` and `h` 
+      functions must be compatible with this feature. See [Automatic differentiation](https://jump.dev/JuMP.jl/stable/manual/nlp/#Automatic-differentiation)
+      for common mistakes when writing these functions. An [`UnscentedKalmanFilter`](@ref)
+      estimates the arrival covariance by default.
         
     Note that if `Cwt≠Inf`, the attribute `nlp_scaling_max_gradient` of `Ipopt` is set to 
-    `10/Cwt` (if not already set), to scale the small values of ``ϵ``.
+    `10/Cwt` (if not already set), to scale the small values of ``ϵ``. Use the second
+    constructor to specify the covariance estimation method.
 """
 function MovingHorizonEstimator(
     model::SM;
