added: covestim arg. in both MovingHorizonEstimator constructors

Author: franckgaga

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.4.2"
+version = "2.4.3"
 authors = ["Francis Gagnon"]
 
 [deps]

src/estimator/mhe/construct.jl

@@ -313,6 +313,8 @@ transcription for now.
 - `hessian=false` : an `AbstractADType` backend for the Hessian of the Lagrangian, see 
    `gradient` above for the options. The default `false` skip it and use the quasi-Newton
    method of `optim` (see Extended Help).
+- `covestim=nothing`: a [`StateEstimator`](@ref) object for the arrival covariance estimation
+   ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``, `nothing` means the default choice (see Extended Help).
 - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current
    estimator, in opposition to the delayed/predictor form).
 
@@ -449,7 +451,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
       [`JuMP` documentation](@extref JuMP Common-mistakes-when-writing-a-user-defined-operator)
       for common mistakes when writing these functions. Also, an [`UnscentedKalmanFilter`](@ref)
       estimates the arrival covariance by default.
-
+    
     One exception about AD: the selected backend for the Hessian of the Lagrangian function
     with `hessian=true` options is sparse:
     ```julia
@@ -469,7 +471,14 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s:
         )
     )
     ```
-    that is, it will test many coloring orders at preparation and keep the best. 
+    that is, it will test many coloring orders at preparation and keep the best. The
+    argument `covestim` customizes the arrival covariance estimator. The supported types are
+    [`SteadyKalmanFilter`](@ref), [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref)
+    and [`ExtendedKalmanFilter`](@ref). A constant arrival covariance is supported with
+    [`SteadyKalmanFilter`](@ref), and by setting the `P̂` argument of  [`setstate!`](@ref)
+    method at the desired value. For [`NonLinModel`](@ref), construct a
+    [`SteadyKalmanFilter`](@ref) with an arbitrary [`LinModel`](@ref), as long as the number
+    of estimated states `nx̂` matches the MHE.
 
     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 ``ε``. Use the second
@@ -497,6 +506,7 @@ function MovingHorizonEstimator(
     gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT,
     jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN,
     hessian::Union{AbstractADType, Bool, Nothing} = false,
+    covestim::Union{StateEstimator, Nothing} = nothing,
     direct = true,
     σP_0       = sigmaP_0,
     σQ         = sigmaQ,
@@ -513,19 +523,10 @@ function MovingHorizonEstimator(
     isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) 
     return MovingHorizonEstimator(
         model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt;
-        gc, gc!, nc, p, direct, optim, gradient, jacobian, hessian
+        gc, gc!, nc, p, optim, gradient, jacobian, hessian, covestim, direct
     )
 end
 
-"Default optimizer for MHE, depending on the model and the number of custom NL constraints."
-function default_optim_mhe(model::SimModel, nc)
-    if model isa LinModel && iszero(nc)
-        return JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false)
-    else
-        return JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false)
-    end
-end
-
 @doc raw"""
     MovingHorizonEstimator(
         model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
@@ -536,20 +537,14 @@ end
         gradient=AutoForwardDiff(),
         jacobian=AutoForwardDiff(),
         hessian=false,
+        covestim=nothing,
         direct=true,
-        covestim=default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
     )
 
 Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` and `R̂`.
 
 This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂_i}, \mathbf{Q̂, R̂}``,
-where ``\mathbf{P̂_i}`` is the initial estimation covariance, provided by `P̂_0` argument. The
-keyword argument `covestim` also allows specifying a custom [`StateEstimator`](@ref) object
-for the estimation of covariance at the arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. The
-supported types are [`SteadyKalmanFilter`](@ref), [`KalmanFilter`](@ref), 
-[`UnscentedKalmanFilter`](@ref) and [`ExtendedKalmanFilter`](@ref). A constant arrival
-covariance is supported with [`SteadyKalmanFilter`](@ref), and by setting the `P̂` argument 
-of [`setstate!`](@ref) at the desired value.
+where ``\mathbf{P̂_i}`` is the initial estimation covariance, provided by `P̂_0` argument.
 """
 function MovingHorizonEstimator(
     model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
@@ -561,13 +556,16 @@ function MovingHorizonEstimator(
     gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT,
     jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN,
     hessian::Union{AbstractADType, Bool, Nothing} = false,
+    covestim::Union{StateEstimator, Nothing} = nothing,
     direct = true,
-    covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
-) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
+) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel}
     P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
     cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0, He)
     gc! = get_mutating_gc_mhe(NT, gc)
     hessian = validate_hessian(hessian, gradient, DEFAULT_NONLINMHE_HESSIAN)
+    if isnothing(covestim)
+        covestim = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    end
     validate_covestim(cov, covestim)
     return MovingHorizonEstimator{NT}(
         model, 
@@ -578,17 +576,30 @@ function MovingHorizonEstimator(
     )
 end
 
-function default_covestim_mhe(model::LinModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
-    return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+"Default optimizer for MHE, depending on the model and the number of custom NL constraints."
+function default_optim_mhe(model::SimModel, nc)
+    if model isa LinModel && iszero(nc)
+        return JuMP.Model(DEFAULT_LINMHE_OPTIMIZER, add_bridges=false)
+    else
+        return JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_bridges=false)
+    end
 end
+
+"Default arrival covariance estimator for MHE, depending on the model type only."
 function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
-    return UnscentedKalmanFilter(model,  i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    if model isa LinModel
+        return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    else
+        return UnscentedKalmanFilter(model,  i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    end
 end
 
 "Validate covestim type and dimensions."
 function validate_covestim(cov::KalmanCovariances, covestim::KalmanEstimator)
-    if size(cov.P̂) != size(covestim.cov.P̂)
-        throw(ArgumentError("estimation covariance covestim.cov.P̂ size does not match the MHE"))
+    invP̄, P̂ = cov.invP̄, covestim.cov.P̂
+    nx̂ = size(invP̄, 1)
+    if size(invP̄) != size(P̂)
+        throw(ArgumentError("P̂ covariance size $(size(P̂)) of covestim does match nx̂=$nx̂"))
     end
     return nothing
 end