changed: renamed x0 to x̂d in nonlinear eq. constraints

This is clearer: it's the prediction of the deterministic states (in opposition to the state of the stochastic model of the unmeasured disturbances). It also equals the deviation vector of the model state `x0`, but using a "d" and a "hat" is clearer in this specific context.

Author: franckgaga

src/controller/transcription.jl

@@ -1406,14 +1406,14 @@ Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`]
 The method mutates the `geq`, `X̂0`, `Û0` and `K` vectors in argument. The defects of the 
 stochastic states are linear equality constraints (see [`init_defectmat`](@ref)). The
 defects of the deterministic states are computed with:
-with:
 ```math
-\mathbf{ŝ_d}(k+j+1) = \mathbf{f}\Big(\mathbf{x_0}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j)\Big) 
-                      - \mathbf{x_0}(k+j+1)
+\mathbf{ŝ_d}(k+j+1) = \mathbf{f}\Big(\mathbf{x̂_d}(k+j), \mathbf{û_0}(k+j), \mathbf{d̂_0}(k+j)\Big) 
+                      - \mathbf{x̂_d}(k+j+1)
 ```
-for ``j = 0, 1, ... , H_p-1``, and in which the deterministic state ``\mathbf{x_0}`` are
-extracted from the decision variables `Z̃`, ``\mathbf{f}`` is the deterministic state update
-function [`f!`](@ref), the disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref)
+for ``j = 0, 1, ... , H_p-1`` and in which the deterministic state ``\mathbf{x̂_d}`` are the
+first `model.nx` elements of the augmented states ``\mathbf{x̂_0}, and they extracted from
+the decision variables `Z̃`. The function ``\mathbf{f}`` is the deterministic state update
+function [`f!`](@ref). The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref)
 documentation. The defects of the stochastic states are linear equality constraints (see
 [`init_defectmat`](@ref)).
 """
@@ -1431,19 +1431,19 @@ function con_nonlinprogeq!(
     disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃)
     @threadsif f_threads for j=1:Hp
         if j < 2
-            x0_Z̃ = @views mpc.estim.x̂0[1:nx]
+            x̂d_Z̃ = @views mpc.estim.x̂0[1:nx]
             d̂0   = @views mpc.d0[1:nd]
         else
-            x0_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx + nx̂*(j-2))]
+            x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)]
             d̂0   = @views   D̂0[(1 + nd*(j-2)):(nd*(j-1))]
         end
         û0       = @views   Û0[(1 + nu*(j-1)):(nu*j)]
         k        = @views    K[(1 + nk*(j-1)):(nk*j)]
-        x0next   = @views   X̂0[(1 + nx̂*(j-1)):(nx + nx̂*(j-1))]
-        x0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx + nx̂*(j-1))]
+        x̂dnext   = @views   X̂0[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)]
+        x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)]
         sdnext    = @views geq[(1 + nx*(j-1)):(nx*j)]
-        f!(x0next, k, model, x0_Z̃, û0, d̂0, model.p)
-        sdnext .= @. x0next - x0next_Z̃
+        f!(x̂dnext, k, model, x̂d_Z̃, û0, d̂0, model.p)
+        sdnext .= @. x̂dnext - x̂dnext_Z̃
     end
     return geq
 end
@@ -1459,23 +1459,23 @@ Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalColloca
 
 The deterministic state defects are computed with:
 ```math
-\mathbf{s_d}(k+j+1) = \mathbf{x_0}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] 
-                       - \mathbf{x_0}(k+j+1)                                              
+\mathbf{ŝ_d}(k+j+1) = \mathbf{x̂_d}(k+j) + 0.5 T_s [\mathbf{k̇}_1(k+j) + \mathbf{k̇}_2(k+j)] 
+                       - \mathbf{x̂_d}(k+j+1)                                              
 ```
-for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x_0}`` is the deterministic state
+for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x̂_d}`` are the deterministic states
 extracted from the decision variables `Z̃`. The ``\mathbf{k̇}`` coefficients are  evaluated
 from the continuous-time function `model.f!` and:
 ```math
 \begin{aligned}
-\mathbf{k̇}_1(k+j) &= \mathbf{f}\Big(\mathbf{x_0}(k+j),   \mathbf{û_0}(k+j),   \mathbf{d̂_0}(k+j),   \mathbf{p}\Big) \\
-\mathbf{k̇}_2(k+j) &= \mathbf{f}\Big(\mathbf{x_0}(k+j+1), \mathbf{û_0}(k+j+h), \mathbf{d̂_0}(k+j+1), \mathbf{p}\Big) 
+\mathbf{k̇}_1(k+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j),   \mathbf{û_0}(k+j),   \mathbf{d̂_0}(k+j),   \mathbf{p}\Big) \\
+\mathbf{k̇}_2(k+j) &= \mathbf{f}\Big(\mathbf{x̂_d}(k+j+1), \mathbf{û_0}(k+j+h), \mathbf{d̂_0}(k+j+1), \mathbf{p}\Big) 
 \end{aligned}
 ```
 in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}``
 is defined in [`f̂!`](@ref) documentation.
 """
 function con_nonlinprogeq!(
-    geq, X̂0, Û0, K̇, 
+    geq, _ , Û0, K̇, 
     mpc::PredictiveController, model::NonLinModel, transcription::TrapezoidalCollocation, 
     U0, Z̃
 )
@@ -1490,35 +1490,33 @@ function con_nonlinprogeq!(
     disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃)
     @threadsif f_threads for j=1:Hp
         if j < 2
-            x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂]
+            x̂d_Z̃ = @views mpc.estim.x̂0[1:nx]
             d̂0   = @views mpc.d0[1:nd]
         else
-            x̂0_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] 
+            x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)]
             d̂0   = @views   D̂0[(1 + nd*(j-2)):(nd*(j-1))]
         end
         k̇        = @views    K̇[(1 + nk*(j-1)):(nk*j)]
         d̂0next   = @views   D̂0[(1 + nd*(j-1)):(nd*j)]
-        x̂0next_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)]  
-        sdnext   = @views  geq[(1 + nx*(j-1)     ):(nx*(j-1) + nx)]
-        x0_Z̃     = @views  x̂0_Z̃[1:nx]
-        x0next_Z̃ = @views x̂0next_Z̃[1:nx]
+        x̂dnext_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)]  
+        sdnext   = @views  geq[(1 + nx*(j-1)):(nx*(j-1) + nx)]
         k̇1, k̇2   = @views k̇[1:nx], k̇[nx+1:2*nx]
         û0 = @views Û0[(1 + nu*(j-1)):(nu*j)]
         if f_threads || h < 1 || j < 2
             # we need to recompute k1 with multi-threading, even with h==1, since the 
             # last iteration (j-1) may not be executed (iterations are re-orderable)
-            model.f!(k̇1, x0_Z̃, û0, d̂0, p)
+            model.f!(k̇1, x̂d_Z̃, û0, d̂0, p)
         else
             k̇1 .= @views K̇[(1 + nk*(j-1)-nx):(nk*(j-1))] # k2 of of the last iter. j-1
         end
         if h < 1
-            model.f!(k̇2, x0next_Z̃, û0, d̂0next, p)
+            model.f!(k̇2, x̂dnext_Z̃, û0, d̂0next, p)
         else
             # j = Hp special case: u(k+Hp-1) = u(k+Hp) since Hc≤Hp implies Δu(k+Hp) = 0:
             û0next = @views j ≥ Hp ? û0 : Û0[(1 + nu*j):(nu*(j+1))]
-            model.f!(k̇2, x0next_Z̃, û0next, d̂0next, p)
+            model.f!(k̇2, x̂dnext_Z̃, û0next, d̂0next, p)
         end
-        sdnext .= @. x0_Z̃ - x0next_Z̃ + 0.5*Ts*(k̇1 + k̇2)
+        sdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2)
     end
     return geq
 end
@@ -1538,20 +1536,21 @@ the model dynamics are computed by:
 ```math
 \mathbf{s_k}(k+j)                                                                                 
     = \mathbf{M_o} \begin{bmatrix}                                          
-        \mathbf{k}_1(k+j) - \mathbf{x_0}(k+j)                       \\
-        \mathbf{k}_2(k+j) - \mathbf{x_0}(k+j)                       \\
+        \mathbf{k}_1(k+j) - \mathbf{x̂_d}(k+j)                       \\
+        \mathbf{k}_2(k+j) - \mathbf{x̂_d}(k+j)                       \\
         \vdots                                                      \\
-        \mathbf{k}_{n_o}(k+j) - \mathbf{x_0}(k+j)                   \\ \end{bmatrix}                                                                                     
+        \mathbf{k}_{n_o}(k+j) - \mathbf{x̂_d}(k+j)                   \end{bmatrix}                                                                                     
     - \begin{bmatrix}
         \mathbf{k̇}_1(k+j)                                           \\
         \mathbf{k̇}_2(k+j)                                           \\
         \vdots                                                      \\
         \mathbf{k̇}_{n_o}(k+j)                                       \end{bmatrix}
 ```
 for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` vectors are
-extracted from the decision variable `Z̃`. The ``\mathbf{x_0}`` vectors are the
-deterministic state extracted from `Z̃`. The ``\mathbf{k̇}_i`` derivative for the ``i``th 
-collocation point is computed from the continuous-time function `model.f!` and:
+extracted from the decision variable `Z̃`. The ``\mathbf{x̂_d}`` vectors are the
+deterministic states extracted from ``\mathbf{X̂_̂0}`` in the decision vector `Z̃`. The
+``\mathbf{k̇}_i`` derivative for the ``i``th collocation point is computed from the
+continuous-time function `model.f!` and:
 ```math
 \mathbf{k̇}_i(k+j) =  \mathbf{f}\Big(\mathbf{k}_i(k+j), \mathbf{û_i}(k+j), \mathbf{d̂}_i(k+j), \mathbf{p}\Big)
 ```
@@ -1575,10 +1574,10 @@ ones for the continuity constraint of the deterministic states are:
         \mathbf{k}_2(k+j)                                           \\
         \vdots                                                      \\
         \mathbf{k}_{n_o}(k+j)                                       \end{bmatrix}       
-    + λ_o \mathbf{x_0}(k+j) - \mathbf{x_0}(k+j+1)
+    + λ_o \mathbf{x̂_d}(k+j) - \mathbf{x̂_d}(k+j+1)
 ```
 for ``j = 0, 1, ... , H_p-1``. The differentiation matrix ``\mathbf{M_o}``, the continuity
-matrix ``\mathbf{C_o}`` and the coefficient ``λ_o`` are introduced in [`init_orthocolloc`](@ref).
+matrix ``\mathbf{C_o}`` and the coefficient ``λ_o`` are introduced in [`init_orthocolloc`](@ref). 
 """
 function con_nonlinprogeq!(
     geq, _ , Û0, K̇,  
@@ -1600,25 +1599,23 @@ function con_nonlinprogeq!(
     disturbedinput!(Û0, mpc, mpc.estim, U0, X̂0_Z̃)
     @threadsif f_threads for j=1:Hp
         if j < 2
-            x̂0_Z̃ = @views mpc.estim.x̂0[1:nx̂]
+            x̂d_Z̃ = @views mpc.estim.x̂0[1:nx]
             d̂0   = @views mpc.d0[1:nd]
         else
-            x̂0_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-1))] 
+            x̂d_Z̃ = @views X̂0_Z̃[(1 + nx̂*(j-2)):(nx̂*(j-2) + nx)]
             d̂0   = @views   D̂0[(1 + nd*(j-2)):(nd*(j-1))]
         end
         k̇        = @views     K̇[(1 + nk*(j-1)):(nk*j)]
         k_Z̃      = @views   K_Z̃[(1 + nk*(j-1)):(nk*j)] 
         d̂0next   = @views    D̂0[(1 + nd*(j-1)):(nd*j)]
-        x̂0next_Z̃ = @views  X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)]
+        x̂dnext_Z̃ = @views  X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*(j-1) + nx)]  
         scnext   = @views   geq[(1 + nx_nk*(j-1)     ):(nx_nk*(j-1) + nx)]
         sk       = @views   geq[(1 + nx_nk*(j-1) + nx):(nx_nk*j         )]
-        x0_Z̃     = @views     x̂0_Z̃[1:nx]
-        x0next_Z̃ = @views x̂0next_Z̃[1:nx]
         # ----------------- collocation constraint defects -----------------------------
         û0 = @views Û0[(1 + nu*(j-1)):(nu*j)]
         Δk = k̇
         for i=1:no
-            Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x0_Z̃
+            Δk[(1 + (i-1)*nx):(i*nx)] = @views k_Z̃[(1 + (i-1)*nx):(i*nx)] .- x̂d_Z̃
         end
         mul!(sk, Mo, Δk)
         d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)]
@@ -1640,7 +1637,7 @@ function con_nonlinprogeq!(
         end
         sk .-= k̇
         # ----------------- continuity constraint defects ------------------------------
-        scnext .= mul!(scnext, Co, k_Z̃) .+ (λo.*x0_Z̃) .- x0next_Z̃
+        scnext .= mul!(scnext, Co, k_Z̃) .+ (λo.*x̂d_Z̃) .- x̂dnext_Z̃
     end
     return geq
 end