changed: D0 length does not change with mhe.direct

The vector will always include one additionnal data point compared to the other data windows.

Author: franckgaga

src/estimator/mhe/construct.jl

@@ -157,8 +157,7 @@ struct MovingHorizonEstimator{
         Z̃ = zeros(NT, nZ̃)
         X̂op = repeat(x̂op, He)
         X̂0, Y0m = zeros(NT, nx̂*He), zeros(NT, nym*He)
-        nD0 = direct ? nd*(He+1) : nd*He
-        U0, D0  = zeros(NT, nu*He), zeros(NT, nD0) 
+        U0, D0  = zeros(NT, nu*He), zeros(NT, nd*(He+1)) 
         Ŵ = zeros(NT, nx̂*He)
         buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk, He, nε)
         x̂0arr_old = zeros(NT, nx̂)
@@ -1038,14 +1037,14 @@ produces an estimator in the current form, while the prediction form is obtained
                                         &= \mathbf{E Z + F}
 \end{aligned}
 ```
-in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values of
-the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured outputs
-``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}`` comprises one 
-additional measured disturbance if ``p=0``, that is, it includes the deviation vectors
-``\mathbf{d_0}(k-j+1)`` from ``j=N_k+1-p`` to ``1``. The constant ``\mathbf{B}`` is the
-contribution for non-zero state ``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}``
-operating points (for linearization, see [`augment_model`](@ref) and [`linearize`](@ref)).
-The method also returns the matrices for the estimation error at arrival:
+in which ``\mathbf{U_0}`` and ``\mathbf{Y_0^m}`` respectively include the deviation values
+of the manipulated inputs ``\mathbf{u_0}(k-j+p)`` from ``j=N_k`` to ``1`` and measured
+outputs ``\mathbf{y_0^m}(k-j+1)`` from ``j=N_k`` to ``1``. The vector ``\mathbf{D_0}``
+includes the the measured disturbance deviation values ``\mathbf{d_0}(k-j)`` from from
+``j=N_k`` to ``0``. The constant ``\mathbf{B}`` is the contribution for non-zero state
+``\mathbf{x̂_{op}}`` and state update ``\mathbf{f̂_{op}}`` operating points (for linearization,
+see [`augment_model`](@ref) and [`linearize`](@ref)). The method also returns the matrices
+for the estimation error at arrival:
 ```math
     \mathbf{x̄} = \mathbf{x̂_0^†}(k-N_k+p) - \mathbf{x̂_0}(k-N_k+p) = \mathbf{e_x̄ Z + f_x̄}
 ```
@@ -1108,10 +1107,10 @@ see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstrain
         \vdots                                      & \vdots                                        & \ddots & \vdots       \\
         \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_u}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_u}    & \cdots & \mathbf{0}   \end{bmatrix} \\
     \mathbf{J} &= - \begin{bmatrix}
-        \mathbf{D̂_d^m}                              & \mathbf{0}                                    & \cdots & \mathbf{0}   \\ 
-        \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d}      & \mathbf{D̂_d^m}                                & \cdots & \mathbf{0}   \\ 
-        \vdots                                      & \vdots                                        & \ddots & \vdots       \\
-        \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d}    & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\
+        \mathbf{0}  & \mathbf{D̂_d^m}                              & \mathbf{0}                                    & \cdots & \mathbf{0}   \\ 
+        \mathbf{0}  & \mathbf{Ĉ^m}\mathbf{Â}^{0}\mathbf{B̂_d}      & \mathbf{D̂_d^m}                                & \cdots & \mathbf{0}   \\ 
+        \vdots      & \vdots                                      & \vdots                                        & \ddots & \vdots       \\
+        \mathbf{0}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-2}\mathbf{B̂_d}  & \mathbf{Ĉ^m}\mathbf{Â}^{H_e-3}\mathbf{B̂_d}    & \cdots & \mathbf{D̂_d^m} \end{bmatrix} \\
     \mathbf{B} &= - \begin{bmatrix}
         \mathbf{0}                           \\  
         \mathbf{Ĉ^m S}(0)                    \\
@@ -1138,8 +1137,8 @@ see [`initpred!(::MovingHorizonEstimator, ::LinModel)`](@ref) and [`linconstrain
         \vdots                              & \vdots                            & \ddots & \vdots                       \\
         \mathbf{Â}^{H_e-1}\mathbf{B̂_d}      & \mathbf{Â}^{H_e-2}\mathbf{B̂_d}    & \cdots & \mathbf{Â}^{0}\mathbf{B̂_d}   \end{bmatrix} \ , \quad
     \mathbf{J_x̂} = \begin{cases}
-        [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0} \end{smallmatrix}]     & p=0                                   \\
-                             \mathbf{J_x̂^†}                                     & p=1                                   \end{cases}   \\
+        [\begin{smallmatrix} \mathbf{J_x̂^†} & \mathbf{0}      \end{smallmatrix}]   & p=0                                \\
+        [\begin{smallmatrix} \mathbf{0}     & \mathbf{J_x̂^†}  \end{smallmatrix}]   & p=1                                \end{cases}   \\
     \mathbf{B_x̂} &= \begin{bmatrix}
         \mathbf{S}(0)                    \\
         \mathbf{S}(1)                    \\
@@ -1218,22 +1217,20 @@ function init_predmat_mhe(
     # --- measured disturbances D ---
     nĈm_Âpow_B̂d = reduce(vcat, getpower(nĈm_Âpow3D, i)*B̂d for i=0:He-1)
     nĈm_Âpow_B̂d = [-D̂dm; nĈm_Âpow_B̂d]
-    J = zeros(NT, nym*He, nd*(He+1-p))
-    col_begin = iszero(p) ? 1    : 0
-    col_end   = iszero(p) ? He+1 : He
-    i=0
-    for j=col_begin:col_end-1
+    J = zeros(NT, nym*He, nd*(He+1))
+    i = 0
+    for j=1:He
         iRow = (1 + i*nym):(nym*He)
         iCol = (1:nd) .+ j*nd
         J[iRow, iCol] = nĈm_Âpow_B̂d[1:length(iRow) ,:]
         i+=1
     end
     iszero(p) && @views (J[:, 1:nd] = nĈm_Âpow_B̂d[nym+1:end, :])
     Âpow_B̂d = reduce(vcat, getpower(Âpow3D, i)*B̂d for i=0:He-1)
-    Jx̂ = zeros(NT, nx̂*He, nd*(He+1-p))
+    Jx̂ = zeros(NT, nx̂*He, nd*(He+1))
     for j=0:He-1
         iRow = (1 + j*nx̂):(nx̂*He)
-        iCol = (1:nd) .+ j*nd
+        iCol = (1:nd) .+ j*nd .+ p
         Jx̂[iRow, iCol] = Âpow_B̂d[1:length(iRow) ,:]
     end
     # --- state x̂op and state update f̂op operating points ---

src/estimator/mhe/execute.jl

@@ -10,13 +10,10 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0)
     estim.H̃         .= 0
     estim.q̃         .= 0
     estim.r         .= 0
-    if estim.direct
-        # add u0(-1) and d0(-1) to the data windows:
-        estim.U0[1:estim.model.nu] .= u0
-        estim.D0[1:estim.model.nd] .= d0
-    end
+    estim.direct && estim.U0[1:estim.model.nu] .= u0  # add u0(-1) to the data windows
+    estim.D0[1:estim.model.nd] .= d0 # add d0(-1) to the data windows
     estim.lastu0 .= u0
-    # estim.cov.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0)
+    # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0)
     invert_cov!(estim, estim.cov.P̂_0)
     estim.P̂arr_old  .= estim.cov.P̂_0
     estim.x̂0arr_old .= 0
@@ -323,10 +320,7 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last
         estim.Nk .= estim.He
     else
         estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)]  .= y0m
-        if nd > 0
-            # D0 include 1 additional measured disturbance if direct==true (p==0):
-            estim.D0[(1 + nd*(Nk-p)):(nd*Nk+1-p)] .= d0 
-        end  
+        nd > 0 && (estim.D0[(1 + nd*Nk):(nd*(Nk+1))] .= d0)
         estim.U0[(1 + nu*(Nk-1)):(nu*Nk)]     .= u0
         estim.X̂0[(1 + nx̂*(Nk-1)):(nx̂*Nk)]     .= x̂0
         estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)]      .= ŵ