added: setstate! now allows cov estimate P̂ modification

Author: franckgaga

src/controller/execute.jl

@@ -488,12 +488,11 @@ Call `periodsleep(mpc.estim.model)`.
 periodsleep(mpc::PredictiveController, busywait=false) = periodsleep(mpc.estim.model, busywait)
 
 """
-    setstate!(mpc::PredictiveController, x̂) -> mpc
+    setstate!(mpc::PredictiveController, x̂, P̂=nothing) -> mpc
 
-Set `mpc.estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`.
+Call [`setstate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
-setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)
-
+setstate!(mpc::PredictiveController, x̂, P̂=nothing) = (setstate!(mpc.estim, x̂, P̂); return mpc)
 
 @doc raw"""
     setmodel!(mpc::PredictiveController, model=mpc.estim.model; <keyword arguments>) -> mpc

src/estimator/execute.jl

@@ -330,16 +330,28 @@ function validate_args(estim::StateEstimator, ym, d, u=nothing)
 end
 
 """
-    setstate!(estim::StateEstimator, x̂) -> estim
+    setstate!(estim::StateEstimator, x̂, P̂=nothing) -> estim
 
-Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`. 
+Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. 
+
+The covariance error estimate `P̂` can be set only if `estim` computes it.
 """
-function setstate!(estim::StateEstimator, x̂)
+function setstate!(estim::StateEstimator, x̂, P̂=nothing)
     size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))")
     estim.x̂0 .= x̂ .- estim.x̂op
+    setstate_cov!(estim, P̂)
     return estim
 end
 
+"Set the covariance error estimate `estim.P̂` to `P̂`."
+function setstate_cov!(estim::StateEstimator, P̂)
+    if !isnothing(P̂)
+        size(P̂) == (estim.nx̂, estim.nx̂) || error("P̂ size must be $((estim.nx̂, estim.nx̂))")
+        estim.P̂ .= to_hermitian(P̂)
+    end
+    return nothing
+end
+
 @doc raw"""
     setmodel!(estim::StateEstimator, model=estim.model; <keyword arguments>) -> estim
 

src/estimator/internal_model.jl

@@ -226,6 +226,12 @@ function init_internalmodel(As, Bs, Cs, Ds)
     return Âs, B̂s
 end
 
+"Throw an error if P̂ != nothing."
+function setstate_cov!(estim::InternalModel, P̂)
+    P̂ == nothing || error("InternalModel does not compute an estimation covariance matrix P̂.")
+    return nothing
+end
+
 "Update similar values for [`InternalModel`](@ref) estimator."
 function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ )
     Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)

src/estimator/luenberger.jl

@@ -136,6 +136,12 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0)
     return predict_estimate_obsv!(estim, y0m, d0, u0)
 end
 
+"Throw an error if P̂ != nothing."
+function setstate_cov!(estim::Luenberger, P̂)
+    P̂ == nothing || error("Luenberger does not compute an estimation covariance matrix P̂.")
+    return nothing
+end
+
 "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values."
 function setmodel_estimator!(estim::Luenberger, model, args...)
     if estim.model !== model

src/estimator/mhe/execute.jl

@@ -650,6 +650,12 @@ end
 "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged."
 con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g
 
+"Throw an error if P̂ != nothing."
+function setstate_cov!(estim::MovingHorizonEstimator, P̂)
+    P̂ == nothing || error("MovingHorizonEstimator does not compute an estimation covariance matrix P̂.")
+    return nothing
+end
+
 "Update the augmented model, prediction matrices, constrains and data windows for MHE."
 function setmodel_estimator!(
     estim::MovingHorizonEstimator, model, uop_old, yop_old, dop_old, Q̂, R̂