doc: describing the internal of the various prediction and transcription methods

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.9.0"
+version = "1.9.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

README.md

@@ -115,6 +115,7 @@ for more detailed examples.
 - supported transcription methods of the optimization problem:
   - direct single shooting
   - direct multiple shooting
+  - trapezoidal collocation
 - additional information about the optimum to ease troubleshooting
 - real-time control loop features:
   - implementations that carefully limits the allocations

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

@@ -75,8 +75,7 @@ order hold) between the samples, but linear interpolation will be added soon.
 
 This transcription computes the predictions by calling the continuous-time model in the
 equality constraint function and by using the implicit trapezoidal rule. It can handle
-moderately stiff systems and is A-stable. However, it may not be as efficient as more
-advanced collocation methods for highly stiff systems. Note that the built-in [`StateEstimator`](@ref)
+moderately stiff systems and is A-stable. Note that the built-in [`StateEstimator`](@ref)
 will still use the `solver` provided at the construction of the [`NonLinModel`](@ref) to
 estimate the plant states, not the trapezoidal rule (see `supersample` option of 
 [`RungeKutta`](@ref) for stiff systems). See Extended Help for more details.
@@ -89,7 +88,8 @@ transcription method.
     Note that the stochastic model of the unmeasured disturbances is strictly discrete-time,
     as described in [`ModelPredictiveControl.init_estimstoch`](@ref). Collocation methods
     require continuous-time dynamics. Because of this, the stochastic states are transcribed
-    separately using a [`MultipleShooting`](@ref) method.
+    separately using a [`MultipleShooting`](@ref) method. See [`con_nonlinprogeq!`](@ref)
+    for more details.
 """
 struct TrapezoidalCollocation <: CollocationMethod
     nc::Int
@@ -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,14 @@ 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) functions is called recursively in a `for` loop:
+```math
+\begin{aligned}
+\mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k) \Big) \\
+\mathbf{ŷ_0}(k)   &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k) \Big)
+\end{aligned}
+```
 """
 function predict!(
     Ŷ0, x̂0end, X̂0, Û0, K0,
@@ -1173,17 +1186,22 @@ 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 augmented output function [`ĥ!`](@ref) 
+is called multiple times in a `for` loop:
+```math
+\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0^†}(k+j), \mathbf{d_0}(k) \Big)
+```
+in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variable `Z̃`.
 """
 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; ϵ]
@@ -1208,7 +1226,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, ϵ
@@ -1285,7 +1303,7 @@ function con_nonlinprog!(
     return g
 end
 
-"""
+@doc raw"""
     con_nonlinprogeq!(
         geq, X̂0, Û0, K0
         mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, 
@@ -1294,7 +1312,14 @@ 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 nonlinear 
+equality constraints `geq` only includes the augmented state defects, computed with:
+```math
+\mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) 
+                    - \mathbf{x̂_0^†}(k+1)
+```
+in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variables 
+`Z̃`, and ``\mathbf{f̂}``, the augmented state function defined in [`f̂!`](@ref).
 """
 function con_nonlinprogeq!(
     geq, X̂0, Û0, K0, 
@@ -1333,21 +1358,33 @@ end
 
 Nonlinear equality constrains for [`NonLinModel`](@ref) and [`TrapezoidalCollocation`](@ref).
 
-The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The deterministic
-and stochastic states are handled separately since collocation methods require 
-continuous-time state-space models, and the stochastic model of the unmeasured disturbances
-is discrete-time. Also note that operating points in `model` are typically zeros for 
-[`NonLinModel`](@ref), but they are handled rigorously here if it's not the case. It should
-be noted that linearization of continuous-time dynamics at non-equilibrium points leads to:
+The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. 
+
+The nonlinear equality constraints `geq` only includes the state defects. The deterministic
+and stochastic states are handled separately since collocation methods require continuous-
+time state-space models, and the stochastic model of the unmeasured disturbances
+is discrete-time. The deterministic and stochastic defects are respectively computed with:
 ```math
-    \mathbf{ẋ_0}(t)   ≈ \mathbf{A x_0}(t) + \mathbf{B_u u_0}(t) + \mathbf{B_d d_0}(t)
+\begin{aligned}
+\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0^†}(k+1) 
+                      + 0.5 T_s (\mathbf{k}_1 + \mathbf{k}_2) \\
+\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s^†}(k+1)
+\end{aligned}
 ```
-as opposed to, for discrete-time models:
+in which ``\mathbf{x_0^†}`` and ``\mathbf{x_s^†}`` are the deterministic and stochastic
+states extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are 
+evaluated from the continuous-time function `model.f!` and:
 ```math
-    \mathbf{x_0}(k+1) ≈ \mathbf{A x_0}(k) + \mathbf{B_u u_0}(k) + \mathbf{B_d d_0}(k) 
-                         + \mathbf{f_{op}} - \mathbf{x_{op}}
+\begin{aligned}
+\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k),     \mathbf{û_0}(k), \mathbf{d_0}(k)  \Big) \\
+\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0^†}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big) 
+\end{aligned}
 ```
-hence no need to add `model.fop` and subtract `model.xop` in the collocation equations.
+and the input of the augmented model is:
+```math
+\mathbf{û_0}(k) = \mathbf{u_0}(k) + \mathbf{C_{s_u} x_s}(k)
+``` 
+the ``\mathbf{A_s, C_{s_u}}`` matrices are defined in [`init_estimstoch`](@ref) doc.
 """
 function con_nonlinprogeq!(
     geq, X̂0, Û0, K0, 
@@ -1378,17 +1415,18 @@ function con_nonlinprogeq!(
         sdnext, ssnext      = @views ŝnext[1:nx], ŝnext[nx+1:end]
         mul!(û0, Cs_u, xs)      # ys_u = Cs_u*xs
         û0 .+= u0               # û0 = u0 + ys_u
-        ẋ0, ẋ0next = @views k0[1:nx], k0[nx+1:2*nx]
-        # no need to handle model.fop and model.xop, see docstring:
-        model.f!(ẋ0, xd, û0, d0, p)
-        model.f!(ẋ0next, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u
+        k1, k2 = @views k0[1:nx], k0[nx+1:2*nx]
+        model.f!(k1, xd, û0, d0, p)
+        model.f!(k2, xdnext_Z̃, û0, d0next, p) # assuming ZOH on manipulated inputs u
         xsnext = @views x̂0next[nx+1:end]
         mul!(xsnext, As, xs)
-        sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(ẋ0 + ẋ0next)
+        sdnext .= @. xd - xdnext_Z̃ + (Ts/2)*(k1 + k2)
         ssnext .= @. xsnext - xsnext_Z̃
         x̂0 = x̂0next_Z̃ # using states in Z̃ for next iteration (allow parallel for)
         d0 = d0next
     end
     return geq
 end
+
+"No eq. 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/execute.jl

@@ -22,7 +22,7 @@ By introducing an augmented state vector ``\mathbf{x̂_0}`` like in [`augment_mo
 the function returns the next state of the augmented model, as deviation vectors:
 ```math
 \begin{aligned}
-    \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big)
+    \mathbf{x̂_0}(k+1) &= \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big) \\
     \mathbf{ŷ_0}(k)   &= \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k)\Big) 
 \end{aligned}
 ```
@@ -65,10 +65,23 @@ function f̂!(x̂0next, û0, k0, estim::StateEstimator, model::SimModel, x̂0,
     return f̂!(x̂0next, û0, k0, model, estim.As, estim.Cs_u, estim.f̂op, estim.x̂op, x̂0, u0, d0)
 end
 
-"""
+@doc raw"""
     f̂!(x̂0next, _ , _ , estim::StateEstimator, model::LinModel, x̂0, u0, d0) -> nothing
 
 Use the augmented model matrices and operating points if `model` is a [`LinModel`](@ref).
+
+# Extended Help
+!!! details "Extended Help"
+
+    This method computes:
+    ```math
+    \begin{aligned}
+    \mathbf{x̂_0}(k+1) &= \mathbf{Â x̂_0}(k) + \mathbf{B̂_u u_0}(k) + \mathbf{B̂_d d_0}(k)
+                         + \mathbf{f̂_{op}} - \mathbf{x̂_{op}}                                \\
+    \mathbf{ŷ_0}(k)   &= \mathbf{Ĉ x̂_0}(k) + \mathbf{D̂_d d_0}(k)
+    \end{aligned}
+    ```
+    with the augmented matrices constructed by [`augment_model`](@ref).
 """
 function f̂!(x̂0next, _ , _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0)
     mul!(x̂0next, estim.Â,  x̂0)

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̂)