doc: details on internal predict!, con_nonlinprog! and con_nonlinprogeq!

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -41,5 +41,8 @@ ModelPredictiveControl.linconstrainteq!
 ```@docs
 ModelPredictiveControl.optim_objective!(::PredictiveController)
 ModelPredictiveControl.set_warmstart!
+ModelPredictiveControl.predict!
+ModelPredictiveControl.con_nonlinprog!
+ModelPredictiveControl.con_nonlinprogeq!
 ModelPredictiveControl.getinput!
 ```

docs/src/internals/state_estim.md

@@ -40,6 +40,8 @@ ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel)
 ```@docs
 ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
 ModelPredictiveControl.set_warmstart_mhe!
+ModelPredictiveControl.predict_mhe!
+ModelPredictiveControl.con_nonlinprog_mhe!
 ```
 
 ## Remove Operating Points

src/controller/transcription.jl

@@ -1119,8 +1119,14 @@ getU0!(U0, mpc::PredictiveController, Z̃) = (mul!(U0, mpc.P̃u, Z̃) .+ mpc.Tu_
 Compute the predictions `Ŷ0`, terminal states `x̂0end` if model is a [`LinModel`](@ref).
 
 The method mutates `Ŷ0` and `x̂0end` vector arguments. The `x̂end` vector is used for
-the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``. The computations are
-identical for any [`TranscriptionMethod`](@ref) if the model is linear.
+the terminal constraints applied on ``\mathbf{x̂_0}(k+H_p)``. The computations are
+identical for any [`TranscriptionMethod`](@ref) if the model is linear:
+```math
+\begin{aligned}
+\mathbf{Ŷ_0}        &= \mathbf{Ẽ Z̃}   + \mathbf{F} \\
+\mathbf{x̂_0}(k+H_p) &= \mathbf{ẽ_x̂ Z̃} + \mathbf{f_x̂}
+\end{aligned}
+```
 """
 function predict!(
     Ŷ0, x̂0end, _, _, _,
@@ -1142,7 +1148,8 @@ end
 
 Compute vectors if `model` is a [`NonLinModel`](@ref) and for [`SingleShooting`](@ref).
     
-The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments.
+The method mutates `Ŷ0`, `x̂0end`, `X̂0`, `Û0` and `K0` arguments. The augmented model of
+[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop.
 """
 function predict!(
     Ŷ0, x̂0end, X̂0, Û0, K0,
@@ -1174,17 +1181,18 @@ end
     predict!(
         Ŷ0, x̂0end, _ , _ , _ , 
         mpc::PredictiveController, model::NonLinModel, transcription::TranscriptionMethod,
-        U0, Z̃
+        _ , Z̃
     ) -> Ŷ0, x̂0end
 
 Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@ref).
     
-The method mutates `Ŷ0` and `x̂0end` arguments.
+The method mutates `Ŷ0` and `x̂0end` arguments. The states `X̂0` are extracted from the 
+decisions variables `Z̃` and the augmented output function [`ĥ!`](@ref) is applied on them.
 """
 function predict!(
     Ŷ0, x̂0end, _, _, _,
     mpc::PredictiveController, model::NonLinModel, ::TranscriptionMethod,
-    U0, Z̃
+    _ , Z̃
 )
     nu, ny, nd, nx̂, Hp, Hc = model.nu, model.ny, model.nd, mpc.estim.nx̂, mpc.Hp, mpc.Hc
     X̂0 = @views Z̃[(nu*Hc+1):(nu*Hc+nx̂*Hp)] # Z̃ = [ΔU; X̂0; ϵ]
@@ -1209,7 +1217,7 @@ end
 Nonlinear constrains when `model` is a [`LinModel`](@ref).
 
 The method mutates the `g` vectors in argument and returns it. Only the custom constraints
-are include in the `g` vector.
+`gc` are include in the `g` vector.
 """
 function con_nonlinprog!(
     g, ::PredictiveController, ::LinModel, ::TranscriptionMethod, _ , _ , gc, ϵ
@@ -1286,7 +1294,7 @@ function con_nonlinprog!(
     return g
 end
 
-"""
+@doc raw"""
     con_nonlinprogeq!(
         geq, X̂0, Û0, K0
         mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, 
@@ -1295,7 +1303,12 @@ end
 
 Nonlinear equality constrains for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref).
 
-The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument.
+The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The defects are
+updated with:
+```math
+\mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) 
+                    + \text{TODO}
+```
 """
 function con_nonlinprogeq!(
     geq, X̂0, Û0, K0, 
@@ -1394,4 +1407,6 @@ function con_nonlinprogeq!(
     end
     return geq
 end
+
+"No equality constraints for other cases e.g. [`SingleShooting`](@ref), returns `geq` unchanged."
 con_nonlinprogeq!(geq,_,_,_,::PredictiveController,::SimModel,::TranscriptionMethod,_,_)=geq

src/estimator/construct.jl

@@ -356,11 +356,14 @@ end
 """
     default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
 
-One integrator on each measured output by default if `model` is not a  [`LinModel`](@ref).
+One integrator on each measured output by default for other cases e.g. [`NonLinModel`](@ref).
 
-Theres is no verification the augmented model remains observable. If the integrator quantity
-per manipulated input `nint_u ≠ 0`, the method returns zero integrator on each measured
-output.
+If the integrator quantity per manipulated input `nint_u ≠ 0`, the method returns zero 
+integrator on each measured output.
+
+!!! warning
+    Theres is no verification the augmented model remains observable. The resulting 
+    [`StateEstimator`](@ref) object should be assessed separately with e.g.: [`sim!`](@ref). 
 """
 function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
     validate_ym(model, i_ym)

src/estimator/mhe/execute.jl

@@ -115,7 +115,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real
     info = Dict{Symbol, Any}()
     V̂,  X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk])
     û0, k0, ŷ0 = buffer.û, buffer.k, buffer.ŷ
-    V̂,  X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃)
+    V̂,  X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃)
     x̂0arr  = @views estim.Z̃[nx̃-nx̂+1:nx̃]
     x̄ = estim.x̂0arr_old - x̂0arr
     X̂0 = [x̂0arr; X̂0]
@@ -418,7 +418,7 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real
     # --------- update estimate -----------------------
     û0, ŷ0, k0 = buffer.û, buffer.ŷ, buffer.k
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
-    V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃)
+    V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, estim.Z̃)
     x̂0next    = @views X̂0[end-nx̂+1:end] 
     estim.x̂0 .= x̂0next
     return estim.Z̃
@@ -461,7 +461,7 @@ function set_warmstart_mhe!(V̂, X̂0, estim::MovingHorizonEstimator{NT}, Z̃var
     # --- process noise estimates Ŵ ---
     Z̃s[nx̃+1:end] = estim.Ŵ
     # verify definiteness of objective function:
-    V̂, X̂0 = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s)
+    V̂, X̂0 = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃s)
     Js = obj_nonlinprog!(x̄, estim, model, V̂, Z̃s)
     if !isfinite(Js)
         Z̃s[nx̃+1:end] = 0
@@ -578,16 +578,22 @@ function obj_nonlinprog!(
     return dot(x̄, invP̄, x̄) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) + Jϵ
 end
 
-"""
-    predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0
+@doc raw"""
+    predict_mhe!(V̂, X̂0, _, _, _, estim::MovingHorizonEstimator, model::LinModel, Z̃) -> V̂, X̂0
 
 Compute the `V̂` vector and `X̂0` vectors for the `MovingHorizonEstimator` and `LinModel`.
 
-The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The vector `V̂` is the
-estimated sensor noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from 
-``k-N_k+2`` to ``k+1``.
+The function mutates `V̂` and `X̂0` vector arguments. The vector `V̂` is the estimated sensor
+noises from ``k-N_k+1`` to ``k``. The `X̂0` vector is estimated states from ``k-N_k+2`` to 
+``k+1``. The computations are (by truncating the matrices when `N_k < H_e`):
+```math
+\begin{aligned}
+\mathbf{V̂}   &= \mathbf{Ẽ Z̃}   + \mathbf{F}     \\
+\mathbf{X̂_0} &= \mathbf{Ẽ_x̂ Z̃} + \mathbf{F_x̂}
+\end{aligned}
+```
 """
-function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) 
+function predict_mhe!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinModel, Z̃) 
     nϵ, Nk = estim.nϵ, estim.Nk[]
     nX̂, nŴ, nYm = estim.nx̂*Nk, estim.nx̂*Nk, estim.nym*Nk
     nZ̃ = nϵ + estim.nx̂ + nŴ
@@ -596,8 +602,16 @@ function predict!(V̂, X̂0, _ , _ , _ , estim::MovingHorizonEstimator, ::LinMod
     return V̂, X̂0
 end
 
-"Compute the two vectors when `model` is not a `LinModel`."
-function predict!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃)
+@doc raw"""
+    predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃) -> V̂, X̂0
+
+Compute the vectors when `model` is *not* a [`LinModel`](@ref).
+
+The function mutates `V̂`, `X̂0`, `û0` and `ŷ0` vector arguments. The augmented model of
+[`f̂!`](@ref) and [`ĥ!`](@ref) is called recursively in a `for` loop from ``j=1`` to ``N_k``,
+and by adding the estimated process noise ``\mathbf{ŵ}``.
+"""
+function predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim::MovingHorizonEstimator, model::SimModel, Z̃)
     nϵ, Nk = estim.nϵ, estim.Nk[]
     nu, nd, nx̂, nŵ, nym = model.nu, model.nd, estim.nx̂, estim.nx̂, estim.nym
     nx̃ = nϵ + nx̂
@@ -646,18 +660,18 @@ The method mutates all the arguments before `estim` argument.
 """
 function update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim::MovingHorizonEstimator, Z̃)
     model = estim.model
-    V̂, X̂0  = predict!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃)
+    V̂, X̂0  = predict_mhe!(V̂, X̂0, û0, k0, ŷ0, estim, model, Z̃)
     ϵ = getϵ(estim, Z̃)
-    g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ)
+    g = con_nonlinprog_mhe!(g, estim, model, X̂0, V̂, ϵ)
     return nothing
 end
 
 """
-    con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ)
+    con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ) -> g
 
-Nonlinear constrains for [`MovingHorizonEstimator`](@ref).
+Compute nonlinear constrains `g` in-place for [`MovingHorizonEstimator`](@ref).
 """
-function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ)
+function con_nonlinprog_mhe!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ)
     nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[]
     nV̂con, nV̂ = length(estim.con.V̂min),  estim.nym*estim.Nk[]
     for i in eachindex(g)
@@ -684,7 +698,7 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂
 end
 
 "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged."
-con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g
+con_nonlinprog_mhe!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g
 
 "Throw an error if P̂ != nothing."
 function setstate_cov!(::MovingHorizonEstimator, P̂)