changed: D0 length does not change anymore with mhe.direct

src/controller/nonlinmpc.jl

@@ -273,10 +273,16 @@ NonLinMPC controller with a sample time Ts = 10.0 s:
     `model` is a [`LinModel`](@ref)).
 
     The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the
-    extended vectors ``\mathbf{U_e}`` (`nu*Hp+nu` elements), ``\mathbf{Ŷ_e}`` (`ny+ny*Hp`
-    elements) and  ``\mathbf{D̂_e}`` (`nd+nd*Hp` elements) as arguments. They all include the
-    values from ``k`` to ``k + H_p`` (inclusively). They also receives the slack ``ϵ``
-    (scalar), which is always zero if `Cwt=Inf`.
+    extended vectors ``\mathbf{U_e}``, ``\mathbf{Ŷ_e}`` and  ``\mathbf{D̂_e}`` as arguments. 
+    They also receives the slack ``ϵ`` (scalar), which is always zero if `Cwt=Inf`. The 
+    following table details the vector sizes and the time steps of the first and last data
+    point in them:
+
+    | VECTOR           | SIZE           | FIRST TIME STEP | LAST TIME STEP |
+    | :--------------- | :------------- | :-------------- | :------------- |
+    | ``\mathbf{U_e}`` | `(nu*(Hp+1),)` | ``k + 0``       | ``k + H_p``    |
+    | ``\mathbf{Ŷ_e}`` | `(ny*(Hp+1),)` | ``k + 0``       | ``k + H_p``    |
+    | ``\mathbf{D̂_e}`` | `(nd*(Hp+1),)` | ``k + 0``       | ``k + H_p``    |
 
     More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e.
     ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that

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)]      .= ŵ
@@ -757,7 +751,7 @@ function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, m
     nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym
     nx̃ = nε + nx̂
     x̂0 = @views Z̃[nx̃-nx̂+1:nx̃]
-    if estim.direct
+    if estim.direct # p = 0
         ŷ0next = ŷ0
         d0 = @views estim.D0[1:nd]
         for j=1:Nk
@@ -773,11 +767,11 @@ function predict_mhe!(V̂, X̂0, û0, k, ŷ0, estim::MovingHorizonEstimator, m
             V̂[(1 + nym*(j-1)):(nym*j)] .= y0nextm .- ŷ0nextm
             x̂0, d0 = x̂0next, d0next
         end        
-    else
+    else        # p = 1
         for j=1:Nk
             y0m = @views estim.Y0m[(1 + nym * (j-1)):(nym*j)]
-            d0  = @views estim.D0[ (1 + nd  * (j-1)):(nd*j)]
             u0  = @views estim.U0[ (1 + nu  * (j-1)):(nu*j)]
+            d0  = @views estim.D0[ (1 + nd*j):(nd*(j+1))] # 1st one is d(k-Nk), not used
             ŵ   = @views Z̃[(1 + nx̃ + nŵ*(j-1)):(nx̃ + nŵ*j)]
             ĥ!(ŷ0, estim, model, x̂0, d0)
             ŷ0m = @views ŷ0[estim.i_ym]