added: setmodel! for StateEstimator

Author: franckgaga

docs/src/public/generic_func.md

@@ -37,6 +37,12 @@ initstate!
 setstate!
 ```
 
+## Set Plant Model
+
+```@docs
+setmodel!
+```
+
 ## Quick Simulation
 
 ### Simulate

src/ModelPredictiveControl.jl

@@ -23,7 +23,7 @@ import OSQP, Ipopt
 
 export SimModel, LinModel, NonLinModel
 export DiffSolver, RungeKutta
-export setop!, setstate!, updatestate!, evaloutput, linearize
+export setop!, setstate!, setmodel!, updatestate!, evaloutput, linearize
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export MovingHorizonEstimator

src/controller/execute.jl

@@ -474,3 +474,10 @@ Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
 updatestate!(mpc::PredictiveController, u, ym, d=empty(mpc.estim.x̂)) = updatestate!(mpc.estim,u,ym,d)
 updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym"))
+
+"""
+    setstate!(mpc::PredictiveController, x̂)
+
+Set the estimate at `mpc.estim.x̂`.
+"""
+setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)

src/estimator/execute.jl

@@ -221,3 +221,60 @@ function validate_args(estim::StateEstimator, u, ym, d)
     nym = estim.nym
     size(ym) ≠ (nym,) && throw(DimensionMismatch("ym size $(size(ym)) ≠ meas. output size ($nym,)"))
 end
+
+"""
+    setstate!(estim::StateEstimator, x̂)
+
+Set `estim.x̂` states to values specified by `x̂`. 
+"""
+function setstate!(estim::StateEstimator, x̂)
+    size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))")
+    estim.x̂[:] = x̂
+    return estim
+end
+
+"""
+    setmodel!(estim::StateEstimator, model::LinModel) -> estim
+
+Set `estim.model` state-space matrices and operating points to `model` values.
+
+Not supported by [`Luenberger`](@ref) and [`SteadyKalmanFilter`](@ref) estimators, use the
+time-varying [`KalmanFilter`](@ref) instead. The matrix dimensions and sample time must stay
+the same. The observability and controllability of the new augmented model is not verified.
+"""
+function setmodel!(estim::StateEstimator, model::LinModel)
+    validate_model(estim, model)
+    estim.model.A   .= model.A
+    estim.model.Bu  .= model.Bu
+    estim.model.C   .= model.C
+    estim.model.Bd  .= model.Bd
+    estim.model.Dd  .= model.Dd
+    estim.model.uop .= model.uop
+    estim.model.yop .= model.yop
+    estim.model.dop .= model.dop
+    setmodel_estimator!(estim, model)
+    return estim
+end
+
+"Validate the dimensions and sample time of `model` against `estim.model`."
+function validate_model(estim::StateEstimator, model::LinModel)
+    model.Ts == estim.model.Ts || throw(ArgumentError("model.Ts must be $(estim.model.Ts) s"))
+    model.nu == estim.model.nu || throw(ArgumentError("model.nu must be $(estim.model.nu)"))
+    model.nx == estim.model.nx || throw(ArgumentError("model.nx must be $(estim.model.nx)"))
+    model.ny == estim.model.ny || throw(ArgumentError("model.ny must be $(estim.model.ny)"))
+    model.nd == estim.model.nd || throw(ArgumentError("model.nd must be $(estim.model.nd)"))
+end
+
+"Update the augmented model matrices of `estim` by default."
+function setmodel_estimator!(estim::StateEstimator, model::LinModel)
+    As, Cs_u, Cs_y = estim.As, estim.Cs_u, estim.Cs_y
+    Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y, verify_obsv=false)
+    estim.  .= Â
+    estim.B̂u .= B̂u
+    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, :]
+    return nothing
+end

src/estimator/internal_model.jl

@@ -197,6 +197,9 @@ function init_internalmodel(As, Bs, Cs, Ds)
     return Âs, B̂s
 end
 
+"Do nothing else for `InternalModel` estimator."
+setmodel_estimator!(estim::InternalModel, ::LinModel) = nothing
+
 @doc raw"""
     update_estimate!(estim::InternalModel, u, ym, d=empty(estim.x̂)) -> x̂d
 

src/estimator/kalman.jl

@@ -166,6 +166,10 @@ function SteadyKalmanFilter(model::SM, i_ym, nint_u, nint_ym, Q̂, R̂) where {N
     return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂)
 end
 
+"Throw an error if `setmodel!` is called on a SteadyKalmanFilter"
+function setmodel_estimator!(estim::SteadyKalmanFilter, _ )
+    error("SteadyKalmanFilter does not support setmodel! (use KalmanFilter instead)")
+end
 
 
 @doc raw"""

src/estimator/luenberger.jl

@@ -120,4 +120,7 @@ function update_estimate!(estim::Luenberger, u, ym, d=empty(estim.x̂))
     mul!(x̂next, K̂, v̂, 1, 1)
     estim.x̂ .= x̂next
     return nothing
-end
+end
+
+"Throw an error if `setmodel!` is called on `Luenberger` observer."
+setmodel_estimator!(estim::Luenberger, ::LinModel) = error("Luenberger does not support setmodel!")

src/predictive_control.jl

@@ -25,13 +25,6 @@ include("controller/explicitmpc.jl")
 include("controller/linmpc.jl")
 include("controller/nonlinmpc.jl")
 
-"""
-    setstate!(mpc::PredictiveController, x̂)
-
-Set the estimate at `mpc.estim.x̂`.
-"""
-setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)
-
 function Base.show(io::IO, mpc::PredictiveController)
     Hp, Hc = mpc.Hp, mpc.Hc
     nϵ = isinf(mpc.C) ? 0 : 1

src/state_estim.jl

@@ -20,17 +20,6 @@ abstract type StateEstimator{NT<:Real} end
 
 const IntVectorOrInt = Union{Int, Vector{Int}}
 
-"""
-    setstate!(estim::StateEstimator, x̂)
-
-Set `estim.x̂` states to values specified by `x̂`. 
-"""
-function setstate!(estim::StateEstimator, x̂)
-    size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))")
-    estim.x̂[:] = x̂
-    return estim
-end
-
 function Base.show(io::IO, estim::StateEstimator)
     nu, nd = estim.model.nu, estim.model.nd
     nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu