changed: replaced f̂_input! with disturbedinput!

Author: franckgaga

docs/src/internals/state_estim.md

@@ -27,7 +27,6 @@ ModelPredictiveControl.get_nonlincon_oracle(::MovingHorizonEstimator, ::ModelPre
 ```@docs
 ModelPredictiveControl.f̂!
 ModelPredictiveControl.ĥ!
-ModelPredictiveControl.f̂_input!
 ```
 
 ## Update Quadratic Optimization

src/controller/execute.jl

@@ -312,7 +312,36 @@ function predictstoch!(Ŷs, mpc::PredictiveController, estim::InternalModel)
     return nothing
 end
 "Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)."
-predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
+predictstoch!(Ŷs, ::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
+
+"""
+    disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0) -> nothing
+
+Fill disturbed inputs of the augmented model `Û0` in-place with stochastic states in `X̂0`
+
+Both `Û0` and `U0` variables include deviation vectors from ``k+0`` to ``k+H_p-1``. The
+predicted states `X̂0` include deviation vectors from ``k+1`` to ``k+H_p-1`` (the current one
+is stored in `estim.x̂0`).
+
+This function is used for the collocation methods that directly call the state derivative 
+function `estim.model.f!` with the manipulated inputs augmented with the estimated 
+disturbances at model input (see [`init_estimstoch`](@ref)). It's also necessary to prefill
+the `Û0` vector before anything since both `û0` and `û0next` are needed at each stage with
+hold order `h>0`, thus potential race conditions with multi-threading.
+"""
+function disturbedinput!(Û0, mpc::PredictiveController, estim::StateEstimator, U0, X̂0)
+    nu, nx, nx̂ = estim.model.nu, estim.model.nx, estim.nx̂
+    Cs_u = estim.Cs_u
+    Û0 .= U0                            
+    for j=0:mpc.Hp-1
+        xs = @views j < 1 ? estim.x̂0[(nx+1):(nx̂)] : X̂0[(nx+1+nx̂*(j-1)):(nx̂*j)] 
+        û0 = @views Û0[(1+nu*j):(nu*(j+1))]
+        mul!(û0, Cs_u, xs, 1, 1)                    # û0 = u0 + Cs_u*xs               
+    end
+    return nothing
+end
+"No input disturbances for [`InternalModel`](@ref), hence do `Û0 .= U0`."
+disturbedinput!(Û0, ::PredictiveController, ::InternalModel, U0, _) = (Û0 .= U0; nothing)
 
 @doc raw"""
     linconstraint_custom!(mpc::PredictiveController, model::SimModel)

src/controller/transcription.jl

@@ -1397,7 +1397,7 @@ deterministic and stochastic states extracted from the decision variables `Z̃`.
 \end{aligned}
 ```
 in which ``h`` is the hold order `transcription.h` and the disturbed input ``\mathbf{û_0}``
-is defined in [`f̂_input!`](@ref).
+is defined in [`f̂!`](@ref) documentation.
 """
 function con_nonlinprogeq!(
     geq, X̂0, Û0, K̇, 
@@ -1412,11 +1412,7 @@ function con_nonlinprogeq!(
     nk = get_nk(model, transcription)
     D̂0 = mpc.D̂0
     X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)]
-    for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed):
-        x̂0_Z̃ =   @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] 
-        u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))],  Û0[(1 + nu*j):(nu*(j+1))]
-        f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0)
-    end
+    disturbedinput!(Û0, 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̂]
@@ -1503,9 +1499,9 @@ and disturbances are piecewise constant or linear:
 \mathbf{d̂}_i(k+j) &= (1-τ_i)\mathbf{d̂_0}(k+j) + τ_i\mathbf{d̂_0}(k+j+1)                      
 \end{aligned}
 ```
-The disturbed input ``\mathbf{û_0}(k+j)`` is defined in [`f̂_input!`](@ref). The defects for
-the stochastic states ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref)
-method, and the ones for the continuity constraint of the deterministic states are:
+The disturbed input ``\mathbf{û_0}`` is defined in [`f̂!`](@ref). The stochastic state 
+defects ``\mathbf{s_s}`` are computed as the [`TrapezoidalCollocation`](@ref) method, and
+the ones for the continuity constraint of the deterministic states are:
 ```math
 \mathbf{s_c}(k+j+1) 
     = \mathbf{C_o} \begin{bmatrix}                                          
@@ -1535,11 +1531,7 @@ function con_nonlinprogeq!(
     D̂0 = mpc.D̂0
     X̂0_Z̃, K_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)], Z̃[(nΔU+nX̂+1):(nΔU+nX̂+nk*Hp)]
     D̂temp = mpc.buffer.D̂
-    for j=0:Hp-1 # prefilling Û0 to avoid race-condition (both û0 and û0next are needed):
-        x̂0_Z̃ =   @views j < 1 ? mpc.estim.x̂0[1:nx̂] : X̂0_Z̃[(1 + nx̂*(j-1)):(nx̂*j)] 
-        u0, û0 = @views U0[(1 + nu*j):(nu*(j+1))],  Û0[(1 + nu*j):(nu*(j+1))]
-        f̂_input!(û0, mpc.estim, model, x̂0_Z̃, u0)
-    end
+    disturbedinput!(Û0, 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̂]

src/estimator/execute.jl

@@ -121,25 +121,6 @@ function fs!(x̂0next, estim::StateEstimator, model::SimModel, x̂0)
     return nothing
 end
 
-@doc raw"""
-    f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0) -> nothing
-
-Compute the disturbed input ``\mathbf{û_0}`` of the augmented model from `x̂0` and `u0`.
-
-It mutates `û0` in place with the following equation:
-```math
-\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k)
-```
-where ``\mathbf{C_{s_u}}`` is defined in [`init_estimstoch`](@ref), and ``\mathbf{x_s}`` is
-extracted from `x̂0` as the last `estim.nxs` elements.
-"""
-function f̂_input!(û0, estim::StateEstimator, model::SimModel, x̂0, u0)
-    xs = @views x̂0[model.nx+1:end]
-    mul!(û0, estim.Cs_u, xs)      # ys_u = Cs_u*xs
-    û0 .+= u0                     # û0 = u0 + ys_u  
-    return nothing
-end
-
 @doc raw"""
     ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing
 

src/estimator/internal_model.jl

@@ -189,13 +189,6 @@ Does nothing since [`InternalModel`](@ref) does not augment the state vector.
 """
 fs!( _ , ::InternalModel, ::SimModel, _ ) = nothing
 
-@doc raw"""
-    f̂_input!(û0, estim::InternalModel, model::SimModel, x̂0, u0) -> nothing
-
-Compute `û0 .= u0` since [`InternalModel`](@ref) does not augment the state vector.
-"""
-f̂_input!(û0, ::InternalModel, ::SimModel, _ , u0) = (û0 .= u0; nothing)
-
 @doc raw"""
     ĥ!(ŷ0, estim::InternalModel, model::NonLinModel, x̂0, d0)