Merge pull request #55 from JuliaControl/adapt_mhe

Added: `setmodel!` for `MovingHorizonEstimator`

Author: franckgaga

docs/src/public/sim_model.md

@@ -8,7 +8,7 @@ The [`SimModel`](@ref) types represents discrete state-space models that can be
 construct [`StateEstimator`](@ref)s and [`PredictiveController`](@ref)s, or as plant
 simulators by calling [`evaloutput`](@ref) and [`updatestate!`](@ref) methods on
 [`SimModel`](@ref) instances (to test estimator/controller designs). For time simulations,
-the states `x` are stored inside [`SimModel`](@ref) instances. Use [`setstate!`](@ref)
+the states ``\mathbf{x}`` are stored inside [`SimModel`](@ref) instances. Use [`setstate!`](@ref)
 method to manually modify them.
 
 ## SimModel

src/controller/construct.jl

@@ -414,7 +414,7 @@ The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at each contr
 # Extended Help
 !!! details "Extended Help"
     Using the augmented matrices ``\mathbf{Â, B̂_u, Ĉ, B̂_d, D̂_d}`` in `estim` (see 
-    [`augment_model`](@ref)) and the function ``\mathbf{W}(j) = ∑_{i=0}^j \mathbf{Â}^i``,
+    [`augment_model`](@ref)), and the function ``\mathbf{W}(j) = ∑_{i=0}^j \mathbf{Â}^i``,
     the prediction matrices are computed by :
     ```math
     \begin{aligned}
@@ -447,8 +447,7 @@ The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at each contr
         \mathbf{Ĉ W}(0)                 \\
         \mathbf{Ĉ W}(1)                 \\
         \vdots                          \\
-        \mathbf{Ĉ W}(H_p-1)             \end{bmatrix} 
-            \mathbf{\big(x̂_{op} + f̂_{op}\big)} 
+        \mathbf{Ĉ W}(H_p-1)             \end{bmatrix}   \mathbf{\big(x̂_{op} + f̂_{op}\big)} 
     \end{aligned}
     ```
     For the terminal constraints, the matrices are computed with:
@@ -468,7 +467,7 @@ The ``\mathbf{F}`` and ``\mathbf{f_x̂}`` vectors are recalculated at each contr
                     \end{bmatrix} \\
     \mathbf{k_x̂} &= \mathbf{Â}^{H_p} \\
     \mathbf{v_x̂} &= \mathbf{W}(H_p-1)\mathbf{B̂_u} \\
-    \mathbf{b_x̂} &= \mathbf{W}(H_p-1) \mathbf{\big(x̂_{op} + f̂_{op}\big)}
+    \mathbf{b_x̂} &= \mathbf{W}(H_p-1)    \mathbf{\big(f̂_{op} - x̂_{op}\big)}
     \end{aligned}
     ```
 """
@@ -525,15 +524,16 @@ function init_predmat(estim::StateEstimator{NT}, model::LinModel, Hp, Hc) where
             jx̂[:  , iCol] = j < Hp ? getpower(Âpow, Hp-j-1)*B̂d : zeros(NT, nx̂, nd)
         end
     end
-    # --- state x̂ and state update f̂op operating points ---
+    # --- state x̂op and state update f̂op operating points ---
     coef_bx̂ = getpower(Âpow_csum, Hp-1)
     coef_B  = Matrix{NT}(undef, ny*Hp, nx̂)
     for j=1:Hp
         iRow = (1:ny) .+ ny*(j-1)
         coef_B[iRow,:] = Ĉ*getpower(Âpow_csum, j-1)
     end
-    bx̂ = coef_bx̂ * (estim.f̂op - estim.x̂op)
-    B  = coef_B  * (estim.f̂op - estim.x̂op)
+    f̂op_n_x̂op = estim.f̂op - estim.x̂op
+    bx̂ = coef_bx̂ * f̂op_n_x̂op
+    B  = coef_B  * f̂op_n_x̂op
     return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 end
 

src/controller/execute.jl

@@ -564,12 +564,12 @@ function setmodel_controller!(mpc::PredictiveController, model::LinModel, x̂op_
         mpc.Yop[(1+ny*i):(ny+ny*i)] .= model.yop
         mpc.Dop[(1+nd*i):(nd+nd*i)] .= model.dop
     end
-    con.U0min .-= mpc.Uop # convert U0 to U with the new operating point
-    con.U0max .-= mpc.Uop # convert U0 to U with the new operating point
-    con.Y0min .-= mpc.Yop # convert Y0 to Y with the new operating point
-    con.Y0max .-= mpc.Yop # convert Y0 to Y with the new operating point
-    con.x̂0min .-= estim.x̂op # convert x̂0 to x̂ with the new operating point
-    con.x̂0max .-= estim.x̂op # convert x̂0 to x̂ with the new operating point
+    con.U0min .-= mpc.Uop # convert U to U0 with the new operating point
+    con.U0max .-= mpc.Uop # convert U to U0 with the new operating point
+    con.Y0min .-= mpc.Yop # convert Y to Y0 with the new operating point
+    con.Y0max .-= mpc.Yop # convert Y to Y0 with the new operating point
+    con.x̂0min .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
+    con.x̂0max .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
     con.A_Ymin .= A_Ymin
     con.A_Ymax .= A_Ymax
     con.A_x̂min .= A_x̂min

src/estimator/execute.jl

@@ -245,7 +245,8 @@ Set model and operating points of `estim` [`StateEstimator`](@ref) to `model` va
 
 Allows model adaptation of estimators based on [`LinModel`](@ref) at runtime ([`NonLinModel`](@ref)
 is not supported). Not supported by [`Luenberger`](@ref) and [`SteadyKalmanFilter`](@ref),
-use the time-varying [`KalmanFilter`](@ref) instead. The matrix dimensions and sample time
+use the time-varying [`KalmanFilter`](@ref) instead.  The [`MovingHorizonEstimator`] model 
+is kept constant over the estimation horizon ``H_e``. The matrix dimensions and sample time
 must stay the same. Note that the observability and controllability of the new augmented
 model is not verified (see Extended Help for details).
 
@@ -272,6 +273,9 @@ julia> setmodel!(kf, LinModel(ss(0.42, 0.5, 1, 0, 4.0))); kf.model.A
 """
 function setmodel!(estim::StateEstimator, model::LinModel)
     validate_model(estim.model, model)
+    uop_old = copy(estim.model.uop)
+    yop_old = copy(estim.model.yop)
+    dop_old = copy(estim.model.dop)
     # --- update model matrices and its operating points ---
     estim.model.A   .= model.A
     estim.model.Bu  .= model.Bu
@@ -285,7 +289,7 @@ function setmodel!(estim::StateEstimator, model::LinModel)
     estim.model.dop .= model.dop
     estim.model.xop .= model.xop
     estim.model.fop .= model.fop
-    setmodel_estimator!(estim, model)
+    setmodel_estimator!(estim, model, uop_old, yop_old, dop_old)
     return estim
 end
 
@@ -300,7 +304,7 @@ end
 validate_model(::SimModel, ::SimModel) = error("Only LinModel is supported for setmodel!")
 
 "Update the augmented model matrices of `estim` by default."
-function setmodel_estimator!(estim::StateEstimator, model::LinModel)
+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, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y, verify_obsv=false)
     # --- update augmented state-space matrices ---

src/estimator/internal_model.jl

@@ -203,7 +203,7 @@ function init_internalmodel(As, Bs, Cs, Ds)
 end
 
 "Update similar values for [`InternalModel`](@ref) estimator."
-function setmodel_estimator!(estim::InternalModel, model::LinModel)
+function setmodel_estimator!(estim::InternalModel, model::LinModel, _ , _ , _)
     Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)
     # --- update augmented state-space matrices ---
     estim.  .= Â
@@ -215,7 +215,7 @@ function setmodel_estimator!(estim::InternalModel, model::LinModel)
     estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point
     estim.x̂op .= x̂op
     estim.f̂op .= f̂op
-    estim.x̂0 .-= estim.x̂op # convert x̂0 to x̂ with the new operating point
+    estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point
 end
 
 @doc raw"""

src/estimator/kalman.jl

@@ -165,7 +165,7 @@ function SteadyKalmanFilter(model::SM, i_ym, nint_u, nint_ym, Q̂, R̂) where {N
 end
 
 "Throw an error if `setmodel!` is called on a SteadyKalmanFilter"
-function setmodel_estimator!(estim::SteadyKalmanFilter, model::LinModel)
+function setmodel_estimator!(::SteadyKalmanFilter, ::LinModel, _ , _ , _)
     error("SteadyKalmanFilter does not support setmodel! (use KalmanFilter instead)")
 end
 

src/estimator/luenberger.jl

@@ -122,4 +122,4 @@ function update_estimate!(estim::Luenberger, u, ym, d=empty(estim.x̂0))
 end
 
 "Throw an error if `setmodel!` is called on `Luenberger` observer."
-setmodel_estimator!(estim::Luenberger, ::LinModel) = error("Luenberger does not support setmodel!")
+setmodel_estimator!(::Luenberger,::LinModel,_,_,_) = error("Luenberger does not support setmodel!")