added : reduce allocations KalmanFilter and ExtendedKalmanFilter

franckgaga · franckgaga · commit 5a119a1922e1 · 2024-08-13T15:17:34.000-04:00
diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl
@@ -146,13 +146,13 @@ end
 Return empty matrices, and `x̂op` & `f̂op` vectors, if `model` is not a [`LinModel`](@ref).
 """
 function augment_model(model::SimModel{NT}, As, args... ; verify_obsv=false) where NT<:Real
-    nu, nx, nd = model.nu, model.nx, model.nd
+    nu, nx, nd, ny = model.nu, model.nx, model.nd, model.ny
     nxs = size(As, 1)
     Â   = zeros(NT, 0, nx+nxs)
     B̂u  = zeros(NT, 0, nu)
-    Ĉ   = zeros(NT, 0, nx+nxs)
+    Ĉ   = zeros(NT, ny, 0)
     B̂d  = zeros(NT, 0, nd)
-    D̂d  = zeros(NT, 0, nd)
+    D̂d  = zeros(NT, ny, 0)
     x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)]
     return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
 end
diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl
@@ -153,8 +153,8 @@ in which ``\mathbf{Ĉ^m, D̂_d^m}`` are the rows of `estim.Ĉ, estim.D̂d`  th
 measured outputs ``\mathbf{y^m}``.
 """
 function init_estimate!(estim::StateEstimator, ::LinModel, y0m, d0, u0)
-    Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d
-    Ĉm, D̂dm = @views Ĉ[estim.i_ym, :], D̂d[estim.i_ym, :] # measured outputs ym only
+    Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d
+    Ĉm, D̂dm = estim.Ĉm, estim.D̂dm
     # TODO: use estim.buffer.x̂ to reduce allocations
     estim.x̂0 .= [I - Â; Ĉm]\[B̂u*u0 + B̂d*d0 + estim.f̂op - estim.x̂op; y0m - D̂dm*d0]
     return nothing
@@ -390,6 +390,8 @@ function setmodel_estimator!(estim::StateEstimator, model, _ , _ , _ , Q̂, R̂)
     estim.Ĉ  .= Ĉ
     estim.B̂d .= B̂d
     estim.D̂d .= D̂d
+    estim.Ĉm  .= @views Ĉ[estim.i_ym, :]
+    estim.D̂dm .= @views D̂d[estim.i_ym, :]
     # --- update state estimate and its operating points ---
     estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point
     estim.x̂op .= x̂op
diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl
@@ -16,11 +16,13 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     Bs::Matrix{NT}
     Cs::Matrix{NT}
     Ds::Matrix{NT}
-    Â ::Matrix{NT}
-    B̂u::Matrix{NT}
-    Ĉ ::Matrix{NT}
-    B̂d::Matrix{NT}
-    D̂d::Matrix{NT}
+    Â   ::Matrix{NT}
+    B̂u  ::Matrix{NT}
+    Ĉ   ::Matrix{NT}
+    B̂d  ::Matrix{NT}
+    D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     Âs::Matrix{NT}
     B̂s::Matrix{NT}
     direct::Bool
@@ -36,6 +38,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         nxs = size(As,1)
         nx̂ = model.nx
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model)
+        Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:]
         Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
         lastu0 = zeros(NT, nu)
         # x̂0 and x̂d are same object (updating x̂d will update x̂0):
@@ -50,7 +53,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Bs, Cs, Ds, 
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             Âs, B̂s,
             direct, corrected,
             buffer
@@ -156,8 +159,8 @@ function matrices_internalmodel(model::LinModel)
 end
 "Return empty matrices, and `x̂op` & `f̂op` vectors, if `model` is not a [`LinModel`](@ref)."
 function matrices_internalmodel(model::SimModel{NT}) where NT<:Real
-    nu, nx, nd = model.nu, model.nx, model.nd
-    Â, B̂u, Ĉ, B̂d, D̂d = zeros(NT,0,nx), zeros(NT,0,nu), zeros(NT,0,nx), zeros(NT,0,nd), zeros(NT,0,nd)
+    nu, nx, nd, ny = model.nu, model.nx, model.nd, model.ny
+    Â, B̂u, Ĉ, B̂d, D̂d = zeros(NT,0,nx), zeros(NT,0,nu), zeros(NT,ny,0), zeros(NT,0,nd), zeros(NT,ny,0)
     x̂op, f̂op = copy(model.xop), copy(model.fop)
     return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
 end
@@ -225,6 +228,8 @@ function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ )
     estim.Ĉ  .= Ĉ
     estim.B̂d .= B̂d
     estim.D̂d .= D̂d
+    estim.Ĉm .= @views Ĉ[estim.i_ym,:]
+    estim.D̂m .= @views D̂d[estim.i_ym,:]
     # --- update state estimate and its operating points ---
     estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point
     estim.x̂op .= x̂op
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -19,6 +19,8 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     Ĉ   ::Matrix{NT}
     B̂d  ::Matrix{NT}
     D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     Q̂::Hermitian{NT, Matrix{NT}}
     R̂::Hermitian{NT, Matrix{NT}}
     K̂::Matrix{NT}
@@ -34,6 +36,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         validate_kfcov(nym, nx̂, Q̂, R̂)
         K̂ = try
             Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6
@@ -59,7 +62,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             Q̂, R̂,
             K̂,
             direct, corrected,
@@ -233,7 +236,7 @@ end
 
 "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)."
 function correct_estimate_obsv!(estim::StateEstimator, y0m, d0, K̂)
-    Ĉm, D̂dm = @views estim.Ĉ[estim.i_ym, :], estim.D̂d[estim.i_ym, :]
+    Ĉm, D̂dm = estim.Ĉm, estim.D̂dm
     ŷ0m = @views estim.buffer.ŷ[estim.i_ym]
     # in-place operations to reduce allocations:
     mul!(ŷ0m, Ĉm, estim.x̂0) 
@@ -281,6 +284,8 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     Ĉ   ::Matrix{NT}
     B̂d  ::Matrix{NT}
     D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     P̂_0::Hermitian{NT, Matrix{NT}}
     Q̂::Hermitian{NT, Matrix{NT}}
     R̂::Hermitian{NT, Matrix{NT}}
@@ -297,6 +302,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
         lastu0 = zeros(NT, nu)
         x̂0  = [zeros(NT, model.nx); zeros(NT, nxs)]
@@ -311,7 +317,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             P̂_0, Q̂, R̂,
             K̂,
             direct, corrected,
@@ -408,8 +414,7 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation cova
 ``\mathbf{P̂}_{k}(k)``.
 """
 function correct_estimate!(estim::KalmanFilter, y0m, d0)
-    Ĉm = @views estim.Ĉ[estim.i_ym, :]
-    return correct_estimate_kf!(estim, y0m, d0, Ĉm)
+    return correct_estimate_kf!(estim, y0m, d0, estim.Ĉm)
 end
 
 
@@ -447,11 +452,10 @@ provided below, see [^2] for details.
      <https://en.wikipedia.org/wiki/Kalman_filter>, Accessed 2024-08-08.
 """
 function update_estimate!(estim::KalmanFilter, y0m, d0, u0)
-    Ĉm = @views estim.Ĉ[estim.i_ym, :]
     if !estim.direct
-        correct_estimate_kf!(estim, y0m, d0, Ĉm)
+        correct_estimate_kf!(estim, y0m, d0, estim.Ĉm)
     end
-    return predict_estimate_kf!(estim, y0m, d0, u0, Ĉm, estim.Â)
+    return predict_estimate_kf!(estim, u0, d0, estim.Â)
 end
 
 
@@ -472,11 +476,13 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     Cs_y::Matrix{NT}
     nint_u ::Vector{Int}
     nint_ym::Vector{Int}
-    Â ::Matrix{NT}
-    B̂u::Matrix{NT}
-    Ĉ ::Matrix{NT}
-    B̂d::Matrix{NT}
-    D̂d::Matrix{NT}
+    Â   ::Matrix{NT}
+    B̂u  ::Matrix{NT}
+    Ĉ   ::Matrix{NT}
+    B̂d  ::Matrix{NT}
+    D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     P̂_0::Hermitian{NT, Matrix{NT}}
     Q̂::Hermitian{NT, Matrix{NT}}
     R̂::Hermitian{NT, Matrix{NT}}
@@ -502,6 +508,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
         nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ)
         lastu0 = zeros(NT, nu)
@@ -520,7 +527,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             P̂_0, Q̂, R̂,
             K̂, 
             M̂, X̂0, X̄0, Ŷ0m, Ȳ0m,
@@ -822,11 +829,13 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     Cs_y::Matrix{NT}
     nint_u ::Vector{Int}
     nint_ym::Vector{Int}
-    Â ::Matrix{NT}
-    B̂u::Matrix{NT}
-    Ĉ ::Matrix{NT}
-    B̂d::Matrix{NT}
-    D̂d::Matrix{NT}
+    Â   ::Matrix{NT}
+    B̂u  ::Matrix{NT}
+    Ĉ   ::Matrix{NT}
+    B̂d  ::Matrix{NT}
+    D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     P̂_0::Hermitian{NT, Matrix{NT}}
     Q̂::Hermitian{NT, Matrix{NT}}
     R̂::Hermitian{NT, Matrix{NT}}
@@ -845,6 +854,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
         lastu0 = zeros(NT, nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
@@ -861,7 +871,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             P̂_0, Q̂, R̂,
             K̂,
             F̂_û, Ĥ,
@@ -1008,8 +1018,6 @@ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT
         ForwardDiff.jacobian!(Ĥ, ĥAD!, ŷ0, x̂0)
         Ĥm = @views Ĥ[estim.i_ym, :]
         correct_estimate_kf!(estim, y0m, d0, Ĥm)
-    else
-        Ĥm = @views Ĥ[estim.i_ym, :]
     end
     x̂0corr = estim.x̂0
     # concatenate x̂0next and û0 vectors to allows û0 vector with dual numbers for AD:
@@ -1020,7 +1028,7 @@ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT
     )
     ForwardDiff.jacobian!(estim.F̂_û, f̂AD!, x̂0nextû, x̂0corr)
     F̂  = @views estim.F̂_û[1:estim.nx̂, :]
-    return predict_estimate_kf!(estim, y0m, d0, u0, Ĥm, F̂)
+    return predict_estimate_kf!(estim, u0, d0, F̂)
 end
 
 "Set `estim.P̂` to `estim.P̂_0` for the time-varying Kalman Filters."
@@ -1079,30 +1087,32 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter},
     I_minus_K̂_Ĉm = estim.buffer.Q̂
     mul!(I_minus_K̂_Ĉm, K̂, Ĉm)
     lmul!(-1, I_minus_K̂_Ĉm)
-    I_minus_K̂_Ĉm[diagind(I_minus_K̂_Ĉm)] .+= 1 # compute I - K̂*Ĉm in-place
+    for i=diagind(I_minus_K̂_Ĉm)
+        I_minus_K̂_Ĉm[i] += 1 # compute I - K̂*Ĉm in-place
+    end
     P̂corr = estim.buffer.P̂
     mul!(P̂corr, I_minus_K̂_Ĉm, P̂)
     estim.P̂ .= Hermitian(P̂corr, :L)
     return nothing
 end
 
 """
-    predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, u0, Ĉm, Â)
+    predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â)
 
 Predict time-varying/extended Kalman Filter estimates with augmented `Ĉm` and `Â` matrices.
 
 Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter`](@ref).
 They predict the state `x̂` and covariance `P̂` with the same equations. See 
 [`update_estimate`](@ref) methods for the equations.
 """
-function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, u0, Ĉm, Â)
+function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â)
     x̂0corr, P̂corr = estim.x̂0, estim.P̂
     Q̂ = estim.Q̂
     x̂0next, û0 = estim.buffer.x̂, estim.buffer.û
     # in-place operations to reduce allocations:
     f̂!(x̂0next, û0, estim, estim.model, x̂0corr, u0, d0)
     P̂corr_Âᵀ = estim.buffer.P̂
-    mul!(P̂corr_Âᵀ, P̂corr, Â')
+    mul!(P̂corr_Âᵀ, P̂corr.data, Â') # the ".data" weirdly removes a type instability in mul!
     Â_P̂corr_Âᵀ = estim.buffer.Q̂
     mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ)
     P̂next  = estim.buffer.P̂
diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl
@@ -19,6 +19,8 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     Ĉ   ::Matrix{NT}
     B̂d  ::Matrix{NT}
     D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     K̂::Matrix{NT}
     direct::Bool
     corrected::Vector{Bool}
@@ -33,6 +35,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         K̂ = try
             ControlSystemsBase.place(Â, Ĉ, poles, :o; direct)[:, i_ym]
         catch
@@ -47,7 +50,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
             lastu0, x̂op, f̂op, x̂0,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             K̂,
             direct, corrected,
             buffer
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -76,6 +76,8 @@ struct MovingHorizonEstimator{
     Ĉ   ::Matrix{NT}
     B̂d  ::Matrix{NT}
     D̂d  ::Matrix{NT}
+    Ĉm  ::Matrix{NT}
+    D̂dm ::Matrix{NT}
     Ẽ ::Matrix{NT}
     F ::Vector{NT}
     G ::Matrix{NT}
@@ -120,6 +122,7 @@ struct MovingHorizonEstimator{
         nxs = size(As, 1)
         nx̂  = model.nx + nxs
         Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
+        Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0)
         lastu0 = zeros(NT, nu)
         x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
@@ -129,7 +132,7 @@ struct MovingHorizonEstimator{
         invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L)
         invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L)
         E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe(
-            model, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
+            model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op
         )
         # dummy values (updated just before optimization):
         F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He)
@@ -155,7 +158,7 @@ struct MovingHorizonEstimator{
             He, nϵ,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
-            Â, B̂u, Ĉ, B̂d, D̂d,
+            Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             Ẽ, F, G, J, B, ẽx̄, fx̄,
             H̃, q̃, p,
             P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt,
@@ -891,7 +894,7 @@ end
 
 @doc raw"""
     init_predmat_mhe(
-        model::LinModel, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
+        model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op
     ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂
 
 Construct the MHE prediction matrices for [`LinModel`](@ref) `model`.
@@ -989,11 +992,10 @@ calculated with:
     All these matrices are truncated when ``N_k < H_e`` (at the beginning).
 """
 function init_predmat_mhe(
-    model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
+    model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op
 ) where {NT<:Real}
     nu, nd = model.nu, model.nd
     nym, nx̂ = length(i_ym), size(Â, 2)
-    Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:] # measured outputs ym only
     nŵ = nx̂
     # --- pre-compute matrix powers ---
     # Apow 3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,He+1] = A^He
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
