doc: prediction and defect matrices of MultipleShooting for MovingHorizonEstimator

docs/src/internals/state_estim.md

@@ -14,6 +14,7 @@ ModelPredictiveControl.init_ukf
 ModelPredictiveControl.init_internalmodel
 ModelPredictiveControl.init_ZtoŴ
 ModelPredictiveControl.init_predmat_mhe
+ModelPredictiveControl.init_defectmat_mhe
 ModelPredictiveControl.relaxarrival
 ModelPredictiveControl.relaxX̂
 ModelPredictiveControl.relaxŴ
@@ -44,6 +45,8 @@ ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
 ModelPredictiveControl.set_warmstart_mhe!
 ModelPredictiveControl.predict_mhe!
 ModelPredictiveControl.con_nonlinprog_mhe!
+ModelPredictiveControl.con_nonlinprogeq_mhe!
+ModelPredictiveControl.getstate!
 ```
 
 ## Remove Operating Points

src/controller/transcription.jl

@@ -202,7 +202,7 @@ end
         model::LinModel, estim, transcription::MultipleShooting, Hp, Hc, nb
     ) -> E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 
-Construct the prediction matrices for [`LinModel`](@ref) and [`MultipleShooting`](@ref).
+Construct them for [`LinModel`](@ref) and [`MultipleShooting`](@ref).
 
 They are defined in the Extended Help section.
 

src/estimator/mhe/construct.jl

@@ -16,8 +16,8 @@ the former is always a linear inequality constraint (it's a decision variable).
 """
 struct EstimatorConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     # matrices for the estimated state constraints:
-    Ẽx̂      ::Matrix{NT}
-    Fx̂      ::Vector{NT}
+    ẼX̂      ::Matrix{NT}
+    FX̂      ::Vector{NT}
     GX̂      ::Matrix{NT}
     JX̂      ::Matrix{NT}
     BX̂      ::Vector{NT}
@@ -1110,7 +1110,7 @@ function init_defaultcon_mhe(
     C_ŵmin, C_ŵmax = fill(0.0, nŴ),  fill(0.0, nŴ)
     C_v̂min, C_v̂max = fill(0.0, nYm), fill(0.0, nYm)
     A_x̂min, A_x̂max, ẽx̄ = relaxarrival(ex̄, c_x̂min, c_x̂max, nε)
-    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(EX̂, C_x̂min, C_x̂max, nε)
+    A_X̂min, A_X̂max, ẼX̂ = relaxX̂(EX̂, C_x̂min, C_x̂max, nε)
     A_Ŵmin, A_Ŵmax     = relaxŴ(Tŵ, C_ŵmin, C_ŵmax, nε)
     A_V̂min, A_V̂max, Ẽ  = relaxV̂(E, C_v̂min, C_v̂max , nε)
     Aeq, ẼS = augmentdefect(ES, nε; slackfirst=true)
@@ -1125,10 +1125,10 @@ function init_defaultcon_mhe(
         A_x̂min, A_x̂max, A_X̂min, A_X̂max, A_Ŵmin, A_Ŵmax, A_V̂min, A_V̂max, Aeq
     )
     # dummy vectors (updated just before optimization):
-    Fx̂, FS = zeros(NT, nx̂*He), zeros(NT, nS)
+    FX̂, FS = zeros(NT, nx̂*He), zeros(NT, nS)
     b, beq = zeros(NT, size(A, 1)), zeros(NT, size(Aeq, 1))
     con = EstimatorConstraint{NT, GCfunc}(
-        Ẽx̂, Fx̂, GX̂, JX̂, BX̂,
+        ẼX̂, FX̂, GX̂, JX̂, BX̂,
         ẼS, FS, GS, JS, BS,
         x̂0min, x̂0max, X̂0min, X̂0max, Ŵmin, Ŵmax, V̂min, V̂max,
         Z̃min, Z̃max,
@@ -1179,22 +1179,22 @@ function relaxarrival(ex̄::AbstractMatrix{NT}, c_x̂min, c_x̂max, nε) where N
 end
 
 @doc raw"""
-    relaxX̂(EX̂, C_x̂min, C_x̂max, nε) -> A_X̂min, A_X̂max, Ẽx̂
+    relaxX̂(EX̂, C_x̂min, C_x̂max, nε) -> A_X̂min, A_X̂max, ẼX̂
 
 Augment estimated state constraints with slack variable ε for softening the MHE.
 
 Denoting the MHE decision variable augmented with the slack variable ``\mathbf{Z̃} = 
-[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ_x̂}``
-matrix that appears in estimated states equation ``\mathbf{X̂} = \mathbf{Ẽ_x̂ Z̃ + F_x̂}``. It
+[\begin{smallmatrix} ε \\ \mathbf{Z} \end{smallmatrix}]``, it returns the ``\mathbf{Ẽ_X̂}``
+matrix that appears in estimated states equation ``\mathbf{X̂_0} = \mathbf{Ẽ_X̂ Z̃ + F_X̂}``. It
 also returns the ``\mathbf{A}`` matrices for the inequality constraints:
 ```math
 \begin{bmatrix} 
     \mathbf{A_{X̂_{min}}} \\ 
     \mathbf{A_{X̂_{max}}}
 \end{bmatrix} \mathbf{Z̃} ≤
 \begin{bmatrix}
-    - \mathbf{(X̂_{min} - X̂_{op}) + F_x̂} \\
-    + \mathbf{(X̂_{max} - X̂_{op}) - F_x̂}
+    - \mathbf{(X̂_{min} - X̂_{op}) + F_X̂} \\
+    + \mathbf{(X̂_{max} - X̂_{op}) - F_X̂}
 \end{bmatrix}
 ```
 in which ``\mathbf{X̂_{min}, X̂_{max}}`` and ``\mathbf{X̂_{op}}`` vectors respectively contains
@@ -1209,12 +1209,12 @@ function relaxX̂(EX̂::AbstractMatrix{NT}, C_x̂min, C_x̂max, nε) where NT<:R
         # ε impacts estimated process noise constraint calculations:
         A_X̂min, A_X̂max = -[C_x̂min EX̂], [-C_x̂max EX̂]
         # ε has no impact on estimated process noises:
-        Ẽx̂ = [zeros(NT, size(EX̂, 1), 1) EX̂] 
+        ẼX̂ = [zeros(NT, size(EX̂, 1), 1) EX̂] 
     else # Z̃ = Z (only hard constraints)
-        Ẽx̂ = EX̂
+        ẼX̂ = EX̂
         A_X̂min, A_X̂max = -EX̂, EX̂
     end
-    return A_X̂min, A_X̂max, Ẽx̂
+    return A_X̂min, A_X̂max, ẼX̂
 end
 
 @doc raw"""

src/estimator/mhe/execute.jl

@@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0)
     estim.H̃         .= 0
     estim.q̃         .= 0
     estim.r         .= 0
-    estim.con.Fx̂    .= 0
+    estim.con.FX̂    .= 0
     if estim.direct
         # add y0m(-1) to the extended data window (custom NL constraints):
         estim.Yem[1:ny] .= y0m .+ @views yop[estim.i_ym]
@@ -411,8 +411,8 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are:
     \mathbf{q̃}       &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃}                      \\
             r        &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃}
 \end{aligned}
-See [`init_ZtoŴ`](@ref) for the definition of the conversion matrix ``\mathbf{T_ŵ}``.
 ```
+See [`init_ZtoŴ`](@ref) for the definition of the conversion matrix ``\mathbf{T_ŵ}``.
 """
 function initpred!(estim::MovingHorizonEstimator{NT}, model::LinModel) where NT<:Real
     invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He
@@ -918,14 +918,14 @@ function setmodel_estimator!(
         He, estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, estim.x̂op, estim.f̂op, 
         estim.direct
     )
-    A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(EX̂, con.C_x̂min, con.C_x̂max, nε)   
+    A_X̂min, A_X̂max, ẼX̂ = relaxX̂(EX̂, con.C_x̂min, con.C_x̂max, nε)   
     A_V̂min, A_V̂max, Ẽ  = relaxV̂(E, con.C_v̂min, con.C_v̂max, nε) 
     estim.Ẽ .= Ẽ
     estim.G .= G
     estim.J .= J
     estim.B .= B
     # --- linear inequality constraints ---
-    con.Ẽx̂ .= Ẽx̂
+    con.ẼX̂ .= ẼX̂
     con.GX̂ .= GX̂
     con.JX̂ .= JX̂
     con.BX̂ .= BX̂

src/estimator/mhe/transcription.jl

@@ -41,11 +41,11 @@ from ``j=N_k-1`` to ``0``, also in deviation form, are computed with:
 ```math
 \begin{aligned}
     \mathbf{X̂_0}  &= \mathbf{E_X̂ Z + G_X̂ U_0 + J_X̂ D_0 + B_X̂} \\
-                  &= \mathbf{E_X̂ Z + F_x̂}
+                  &= \mathbf{E_X̂ Z + F_X̂}
 \end{aligned}
 ```
 The matrices ``\mathbf{E, G, J, B, E_X̂, G_X̂, J_X̂, B_X̂}`` are defined in the Extended Help 
-section. The vectors ``\mathbf{F, F_x̂, f_x̄}`` are recalculated at each discrete time step, 
+section. The vectors ``\mathbf{F, F_X̂, f_x̄}`` are recalculated at each discrete time step, 
 see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstraint!(::MovingHorizonEstimator, ::LinModel)`](@ref).
 
 # Extended Help
@@ -244,15 +244,70 @@ function init_predmat_mhe(
     return E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂
 end
 
-"""
+@doc raw"""
     init_predmat_mhe(
         model::LinModel, transcription::MultipleShooting, 
         He, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
     ) -> E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂
 
 Construct them for [`LinModel`](@ref) and [`MultipleShooting`](@ref).
 
-TBW
+The ``\mathbf{e_x̄}`` is identical to the [`SingleShooting`](@ref) transcription. The other
+matrices are defined in the Extended Help section.
+
+# Extended Help
+!!! details "Extended Help"
+    The matrices are compute by (notice the minus signs after the equalities):
+    ```math
+    \begin{aligned}
+    \mathbf{E} &= - \begin{bmatrix} 
+        \mathbf{E^x̂} & \mathbf{E^ŵ}                                                          \end{bmatrix} \\
+    \mathbf{G} &=   \mathbf{0}                                                               \\
+    \mathbf{J} &= - \begin{bmatrix}
+        \mathbf{0}  & \mathbf{D̂_d^m}    & \mathbf{0}     & \cdots & \mathbf{0}               \\ 
+        \mathbf{0}  & \mathbf{0}        & \mathbf{D̂_d^m} & \cdots & \mathbf{0}               \\ 
+        \vdots      & \vdots            & \vdots         & \ddots & \vdots                   \\
+        \mathbf{0}  & \mathbf{0}        & \mathbf{0}     & \cdots & \mathbf{D̂_d^m}           \end{bmatrix} \\
+    \mathbf{B} &=   \mathbf{0}
+    \end{aligned}
+    ```
+    The ``\mathbf{E^ŵ}`` matrix is an appropriately size ``\mathbf{0}`` matrix and, for 
+    ``p=0``, we have:
+    ```math
+    \mathbf{E^x̂} = \begin{bmatrix}
+        \mathbf{0}      & \mathbf{Ĉ^m}      & \mathbf{0}     & \cdots       & \mathbf{0}     \\ 
+        \mathbf{0}      & \mathbf{0}        & \mathbf{Ĉ^m}   & \cdots       & \mathbf{0}     \\ 
+        \vdots          & \vdots            & \vdots         & \ddots       & \vdots         \\
+        \mathbf{0}      & \mathbf{0}        & \mathbf{0}     & \cdots       & \mathbf{Ĉ^m}   \end{bmatrix}
+    ```
+    or, for ``p=1``:
+    ```math
+    \mathbf{E^x̂} = \begin{bmatrix}
+        \mathbf{Ĉ^m}   & \mathbf{0}         & \cdots        & \mathbf{0}   & \mathbf{0}      \\ 
+        \mathbf{0}     & \mathbf{Ĉ^m}       & \cdots        & \mathbf{0}   & \mathbf{0}      \\ 
+        \vdots         & \vdots             & \ddots        & \vdots       & \vdots          \\
+        \mathbf{0}     & \mathbf{0}         & \cdots        & \mathbf{Ĉ^m} & \mathbf{0}      \end{bmatrix}
+    ```
+    The matrices for the estimated states are computed by:
+    ```math
+    \begin{aligned}
+    \mathbf{E_X̂} &= \begin{bmatrix} 
+        \mathbf{E_X̂^x̂} & \mathbf{E_X̂^ŵ}                                                      \end{bmatrix} \\
+    \mathbf{G_X̂} &= \mathbf{0}                                                               \\
+    \mathbf{J_X̂} &= \mathbf{0}                                                               \\
+    \mathbf{B_X̂} &= \mathbf{0}
+    \end{aligned}
+    ```
+    The ``\mathbf{E_X̂^ŵ}`` matrix is an appropriately size ``\mathbf{0}`` matrix and:
+    ```math
+    \mathbf{E_X̂^x̂} = \begin{bmatrix}
+        \mathbf{0}      & \mathbf{I}        & \mathbf{0}     & \cdots       & \mathbf{0}     \\ 
+        \mathbf{0}      & \mathbf{0}        & \mathbf{I}     & \cdots       & \mathbf{0}     \\ 
+        \vdots          & \vdots            & \vdots         & \ddots       & \vdots         \\
+        \mathbf{0}      & \mathbf{0}        & \mathbf{0}     & \cdots       & \mathbf{I}     \end{bmatrix}
+    ```
+    The appropriate rows and columns on these matrices are selected using the slicing
+    operator `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning).
 """
 function init_predmat_mhe(
     model::LinModel{NT}, ::MultipleShooting, 
@@ -333,13 +388,64 @@ function init_predmat_mhe(
     return E, G, J, B, ex̄, EX̂, GX̂, JX̂, BX̂
 end
 
-"""
+@doc raw"""
     init_defectmat_mhe(
         model::LinModel, transcription::MultipleShooting, 
         He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, direct
     ) -> ES, GS, JS, BS
 
-TBW
+Init the matrices for computing the defects over the predicted states.
+
+With a [`MultipleShooting`](@ref) transcription, the decision vector ``\mathbf{Z}`` contains
+the arrival state estimate ``\mathbf{x̂_0}(k-N_k+p)``, the stage states ``\mathbf{X̂_0}`` 
+(both defined as deviation vector from ``\mathbf{x̂_{op}}``) and the estimated process noises
+``\mathbf{Ŵ}``. Knowing this, an equation similar to the prediction matrices (see 
+[`init_predmat_mhe`](@ref)) computes the defects of the estimated states over ``H_e``:
+```math
+\begin{aligned}
+    \mathbf{Ŝ}  &= \mathbf{E_S Z} + \mathbf{G_S U_0}  + \mathbf{J_S D_0} + \mathbf{B_S}       \\
+                &= \mathbf{E_S Z} + \mathbf{F_S}
+\end{aligned}
+```   
+They are forced to be ``\mathbf{Ŝ = 0}`` using the optimization equality constraints. The
+matrices ``\mathbf{E_S, G_S, J_S, B_S}`` are defined in the Extended Help section.
+
+# Extended Help
+!!! details "Extended Help"
+    The defect matrices are computed with:
+    ```math
+    \begin{aligned}
+    \mathbf{E_S} &= \begin{bmatrix}
+        \mathbf{E_S^x̂} & \mathbf{E_S^ŵ}                                                             \end{bmatrix} \\
+    \mathbf{E_S^x̂} &= \begin{bmatrix}
+        \mathbf{Â}     & \mathbf{-I}    & \mathbf{0}    & \cdots    & \mathbf{0} & \mathbf{0}       \\
+        \mathbf{0}     & \mathbf{Â}     & \mathbf{-I}   & \cdots    & \mathbf{0} & \mathbf{0}       \\
+        \vdots         & \vdots         & \vdots        & \ddots    & \vdots     & \vdots           \\
+        \mathbf{0}     & \mathbf{0}     & \mathbf{0}    & \cdots    & \mathbf{Â} & \mathbf{-I}      \end{bmatrix} \\
+    \mathbf{E_S^ŵ} &= \begin{bmatrix}
+        \mathbf{I}     &  \mathbf{0}    & \cdots        & \mathbf{0}                                \\
+        \mathbf{0}     &  \mathbf{I}    & \cdots        & \mathbf{0}                                \\
+        \vdots         &  \vdots        & \ddots        & \vdots                                    \\
+        \mathbf{0}     &  \mathbf{0}    & \cdots        & \mathbf{I}                                \end{bmatrix} \\
+    \mathbf{G_S} &= \begin{bmatrix}
+        \mathbf{B̂_u}   &  \mathbf{0}    & \cdots        & \mathbf{0}                                \\
+        \mathbf{0}     &  \mathbf{B̂_u}  & \cdots        & \mathbf{0}                                \\
+        \vdots         &  \vdots        & \ddots        & \vdots                                    \\
+        \mathbf{0}     &  \mathbf{0}    & \cdots        & \mathbf{B̂_u}                              \end{bmatrix} \\
+    \mathbf{J_S^†} &= \begin{bmatrix}
+        \mathbf{B̂_d}   & \mathbf{0}     & \cdots        & \mathbf{0}                                \\
+        \mathbf{0}     & \mathbf{B̂_d}   & \cdots        & \mathbf{0}                                \\
+        \vdots         & \vdots         & \ddots        & \vdots                                    \\
+        \mathbf{0}     & \mathbf{0}     & \cdots        & \mathbf{B̂_d}                              \end{bmatrix} \ , \quad
+    \mathbf{J_S} &= \begin{cases}
+        [\begin{smallmatrix} \mathbf{J_S^†} & \mathbf{0}      \end{smallmatrix}]   & p=0            \\
+        [\begin{smallmatrix} \mathbf{0}     & \mathbf{J_S^†}  \end{smallmatrix}]   & p=1            \end{cases}   \\
+    \mathbf{B_S} &= \begin{bmatrix}
+        \mathbf{f̂_{op} - x̂_{op}} \\ \mathbf{f̂_{op} - x̂_{op}} \\ \vdots \\ \mathbf{f̂_{op} - x̂_{op}}  \end{bmatrix}
+    \end{aligned}
+    ```
+    The appropriate rows and columns on these matrices are selected using the slicing
+    operator `A[i_rows, i_cols]` when ``N_k < H_e`` (at the beginning).
 """
 function init_defectmat_mhe(
     model::LinModel{NT}, ::MultipleShooting, He, Â, B̂u, B̂d, x̂op, f̂op, direct
@@ -637,7 +743,7 @@ end
 
 Set `b` vector for the linear model inequality constraints (``\mathbf{A Z̃ ≤ b}``) of MHE.
 
-Also init ``\mathbf{F_x̂ = G_X̂ U_0 + J_X̂ D_0 + B_X̂}`` vector for the state constraints, see 
+Also init ``\mathbf{F_X̂ = G_X̂ U_0 + J_X̂ D_0 + B_X̂}`` vector for the state constraints, see 
 [`init_predmat_mhe`](@ref).
 """
 function linconstraint!(
@@ -651,20 +757,20 @@ function linconstraint!(
         BX̂     = estim.con.BX̂[1:nX̂]
         GX̂, U0 = estim.con.GX̂[1:nX̂, 1:nU], estim.U0[1:nU]
         JX̂, D0 = estim.con.JX̂[1:nX̂, 1:nD], estim.D0[1:nD]
-        Fx̂     = @views estim.con.Fx̂[1:nX̂]
+        FX̂     = @views estim.con.FX̂[1:nX̂]
     else
         BX̂     = estim.con.BX̂
         GX̂, U0 = estim.con.GX̂, estim.U0
         JX̂, D0 = estim.con.JX̂, estim.D0
-        Fx̂     = estim.con.Fx̂
+        FX̂     = estim.con.FX̂
     end
     X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, nx̂)
     Ŵmin, Ŵmax   = trunc_bounds(estim, estim.con.Ŵmin,  estim.con.Ŵmax,  nŵ)
     V̂min, V̂max   = trunc_bounds(estim, estim.con.V̂min,  estim.con.V̂max,  nym)
-    # --- update Fx̂ vectors for MHE state constraints ---
-    Fx̂ .= BX̂
-    mul!(Fx̂, GX̂, U0, 1, 1)
-    model.nd > 0 && mul!(Fx̂, JX̂, D0, 1, 1)
+    # --- update FX̂ vectors for MHE state constraints ---
+    FX̂ .= BX̂
+    mul!(FX̂, GX̂, U0, 1, 1)
+    model.nd > 0 && mul!(FX̂, JX̂, D0, 1, 1)
     # --- update b vector for linear inequality constraints ---
     nX̂_He, nŴ_He, nV̂_He = length(X̂0min), length(Ŵmin), length(V̂min)
     nx̂ = length(estim.con.x̂0min)
@@ -673,9 +779,9 @@ function linconstraint!(
     n += nx̂
     estim.con.b[(n+1):(n+nx̂)] .= @. +estim.con.x̂0max
     n += nx̂
-    estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.Fx̂
+    estim.con.b[(n+1):(n+nX̂_He)] .= @. -X̂0min + estim.con.FX̂
     n += nX̂_He
-    estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.Fx̂
+    estim.con.b[(n+1):(n+nX̂_He)] .= @. +X̂0max - estim.con.FX̂
     n += nX̂_He
     estim.con.b[(n+1):(n+nŴ_He)] .= @. -Ŵmin
     n += nŴ_He
@@ -934,7 +1040,7 @@ noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k
 ```math
 \begin{aligned}
 \mathbf{V̂}   &= \mathbf{Ẽ Z̃}   + \mathbf{F}     \\
-\mathbf{X̂_0} &= \mathbf{Ẽ_x̂ Z̃} + \mathbf{F_x̂}
+\mathbf{X̂_0} &= \mathbf{Ẽ_X̂ Z̃} + \mathbf{F_X̂}
 \end{aligned}
 ```
 """
@@ -949,16 +1055,16 @@ function predict_mhe!(
         nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk
         nZ̃ = nε + estim.nx̂ + nŴ
         Ẽ,  F  = estim.Ẽ[1:nYm, 1:nZ̃],     estim.F[1:nYm]
-        Ẽx̂, Fx̂ = estim.con.Ẽx̂[1:nX̂, 1:nZ̃], estim.con.Fx̂[1:nX̂]
+        ẼX̂, FX̂ = estim.con.ẼX̂[1:nX̂, 1:nZ̃], estim.con.FX̂[1:nX̂]
         Z̃ = Z̃[1:nZ̃]
         V̂_res, X̂0_res = @views V̂[1:nYm], X̂0[1:nX̂]
     else
         Ẽ, F = estim.Ẽ, estim.F
-        Ẽx̂, Fx̂ = estim.con.Ẽx̂, estim.con.Fx̂
+        ẼX̂, FX̂ = estim.con.ẼX̂, estim.con.FX̂
         V̂_res, X̂0_res = V̂, X̂0
     end
     V̂_res  .= mul!(V̂_res, Ẽ, Z̃) .+ F
-    X̂0_res .= mul!(X̂0_res, Ẽx̂, Z̃) .+ Fx̂
+    X̂0_res .= mul!(X̂0_res, ẼX̂, Z̃) .+ FX̂
     return V̂, X̂0
 end