Merge pull request #374 from JuliaControl/covestim_mhe_const

added: `covestim` arg. in both `MovingHorizonEstimator` constructors + honor `P̂_0`

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

@@ -278,25 +278,25 @@ transcription for now.
 - `model::SimModel` : (deterministic) model for the estimations.
 - `He=nothing` : estimation horizon ``H_e``, must be specified.
 - `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest 
-    are unmeasured ``\mathbf{y^u}``.
+   are unmeasured ``\mathbf{y^u}``.
 - `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate
-    covariance ``\mathbf{P}(0)``, specified as a standard deviation vector.
+   covariance ``\mathbf{P}(0)``, specified as a standard deviation vector.
 - `σQ=fill(1/model.nx,model.nx)` or *`sigmaQ`* : main diagonal of the process noise
-    covariance ``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
+   covariance ``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
 - `σ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.
+   ``\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 (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).
+   disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help).
 - `σ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).
+   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
-    disturbances at manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators).
+   disturbances at manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators).
 - `σQint_ym=fill(1,sum(nint_ym))` or *`sigmaQint_u`* : same than `σQ` for the unmeasured
-    disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
+   disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
 - `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured
-    disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators).
+   disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators).
 - `Cwt=Inf` : slack variable weight ``C``, default to `Inf` meaning hard constraints only.
 - `gc=(_,_,_,_,_,_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function 
    ``\mathbf{g_c}(\mathbf{X̂_e, V̂_e, Ŵ_e, U_e, Y_e^m, D_e, P̄, x̄, p}, ε)``, mutating or not 
@@ -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,15 @@ 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 using
+    the [`SteadyKalmanFilter`](@ref). The constant is fixed by `σP_0`, `σPint_ym_0` and
+    `σPint_u_0` arguments. If `isnothing(σP_0)`, it is fixed at `covestim.cov.P̂`, which is
+    the steady-state value ``\mathbf{P̂}(∞)`` for the [`SteadyKalmanFilter`](@ref). 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 +507,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,
@@ -507,25 +518,16 @@ function MovingHorizonEstimator(
     σQint_ym   = sigmaQint_ym,
 ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel}
     # estimated covariances matrices (variance = σ²) :
-    P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2)
+    P̂_0 = isnothing(σP_0) ? nothing : Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2)
     Q̂   = Diagonal([σQ;  σQint_u;  σQint_ym ].^2)
     R̂   = Diagonal([σR;].^2)
     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 +538,15 @@ 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. Its value is provided by `P̂_0`
+argument. If `isnothing(P̂_0)`, its value will be fetch in `covestim.cov.P̂`.
 """
 function MovingHorizonEstimator(
     model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf;
@@ -561,14 +558,24 @@ 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}
+    if isnothing(P̂_0)
+        if isnothing(covestim)
+            throw(ArgumentError("a covestim argument should be specified to fetch its covariance P̂"))
+        end
+        P̂_0 = covestim.cov.P̂ 
+    end
     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)
+    setstate!(covestim, covestim.x̂0 + covestim.x̂op, P̂_0)
     return MovingHorizonEstimator{NT}(
         model, 
         He, i_ym, nint_u, nint_ym, cov, Cwt,
@@ -578,17 +585,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

test/2_test_state_estim.jl

@@ -866,7 +866,20 @@ end
 
     linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mhe13 = MovingHorizonEstimator(linmodel2, He=5)
-    @test isa(mhe13, MovingHorizonEstimator{Float32})    
+    @test isa(mhe13, MovingHorizonEstimator{Float32})   
+    
+    covestim = SteadyKalmanFilter(linmodel)
+    σP_0 = 1:4
+    σPint_ym_0 = 5:6
+    mhe14 = MovingHorizonEstimator(linmodel; He=1, σP_0, σPint_ym_0, covestim)
+    @test mhe14.cov.invP̄ ≈ inv(diagm((1:6).^2))
+    preparestate!(mhe14, linmodel.yop, linmodel.dop)
+    @test mhe14.cov.invP̄ ≈ inv(diagm((1:6).^2))
+
+    covestim = SteadyKalmanFilter(linmodel)
+    σP_0 = nothing
+    mhe15 = MovingHorizonEstimator(linmodel; He=1, σP_0, covestim)
+    @test mhe15.cov.invP̄ ≈ inv(covestim.cov.P̂)
 
     function gcl(X̂e, _ , _ , _ , _ , _ , _ , _ , nx, ε)
         gc = X̂e .- 100 .- ε   
@@ -883,6 +896,7 @@ end
     @test_throws ArgumentError MovingHorizonEstimator(linmodel)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0)
     @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1)
+    @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=1, σP_0=nothing)
 end
 
 @testitem "MHE construction (NonLinModel)" setup=[SetupMPCtests] begin