added: 📐 OrthogonalCollocation 📐 for MovingHorizonEstimator

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "2.11.0"
+version = "2.12.0"
 authors = ["Francis Gagnon"]
 
 [deps]

docs/make.jl

@@ -55,6 +55,7 @@ makedocs(
                 "Plant Models" => "internals/sim_model.md",
                 "State Estimators" => "internals/state_estim.md",
                 "Predictive Controllers" => "internals/predictive_control.md",
+                "Miscellaneous" => "internals/misc.md"
             ],
         ],  
         "Index" => "func_index.md"

docs/src/index.md

@@ -58,6 +58,7 @@ Depth = 1
 Pages = [
     joinpath("internals", "sim_model.md"),
     joinpath("internals", "state_estim.md"),
-    joinpath("internals", "predictive_control.md")
+    joinpath("internals", "predictive_control.md"),
+    joinpath("internals", "misc.md")
 ]
 ```

docs/src/internals/misc.md

@@ -0,0 +1,11 @@
+# Functions: Miscellaneous Internals
+
+```@contents
+Pages = ["misc.md"]
+```
+
+## Orthogonal Collocation
+
+```@docs
+ModelPredictiveControl.init_orthocolloc
+```

docs/src/internals/predictive_control.md

@@ -15,7 +15,6 @@ The prediction methodology of this module is mainly based on Maciejowski textboo
 ModelPredictiveControl.move_blocking
 ModelPredictiveControl.init_ZtoΔU   
 ModelPredictiveControl.init_ZtoU
-ModelPredictiveControl.init_orthocolloc
 ModelPredictiveControl.init_predmat
 ModelPredictiveControl.init_defectmat
 ModelPredictiveControl.relaxU
@@ -45,7 +44,6 @@ ModelPredictiveControl.linconstrainteq!
 ModelPredictiveControl.optim_objective!(::PredictiveController)
 ModelPredictiveControl.set_warmstart_mpc!
 ModelPredictiveControl.predict!
-ModelPredictiveControl.disturbedinput!
 ModelPredictiveControl.con_nonlinprog!
 ModelPredictiveControl.con_nonlinprogeq!
 ModelPredictiveControl.getinput!

docs/src/internals/state_estim.md

@@ -69,24 +69,6 @@ ModelPredictiveControl.f̂!
 ModelPredictiveControl.ĥ!
 ```
 
-## Update Quadratic Optimization
-
-```@docs
-ModelPredictiveControl.initpred!(::MovingHorizonEstimator, ::LinModel)
-ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod)
-```
-
-## Solve Optimization Problem
-
-```@docs
-ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
-ModelPredictiveControl.set_warmstart_mhe!
-ModelPredictiveControl.predict_mhe!
-ModelPredictiveControl.con_nonlinprog_mhe!
-ModelPredictiveControl.con_nonlinprogeq_mhe!
-ModelPredictiveControl.getstate!
-```
-
 ## Remove Operating Points
 
 ```@docs
@@ -118,3 +100,22 @@ ModelPredictiveControl.correct_estimate!
 ```@docs
 ModelPredictiveControl.update_estimate!
 ```
+
+## Update Quadratic Optimization
+
+```@docs
+ModelPredictiveControl.initpred!(::MovingHorizonEstimator, ::LinModel)
+ModelPredictiveControl.linconstraint!(::MovingHorizonEstimator, ::LinModel, ::TranscriptionMethod)
+```
+
+## Solve Optimization Problem
+
+```@docs
+ModelPredictiveControl.optim_objective!(::MovingHorizonEstimator)
+ModelPredictiveControl.set_warmstart_mhe!
+ModelPredictiveControl.disturbedinput!
+ModelPredictiveControl.predict_mhe!
+ModelPredictiveControl.con_nonlinprog_mhe!
+ModelPredictiveControl.con_nonlinprogeq_mhe!
+ModelPredictiveControl.getstate!
+```

src/controller/transcription.jl

@@ -484,7 +484,7 @@ end
 @doc raw"""
     init_defectmat(
         model::SimModel, estim::StateEstimator, transcription::OrthogonalCollocation, 
-        Hp, Hc, nb, Co=nothing, λo=nothing
+        Hp, Hc, _ , Co, λo
     ) -> ES, GS, JS, KS, VS, BS
 
 Init the matrices for computing the continuity constraints and stochastic state defects.
@@ -533,8 +533,9 @@ The matrices ``\mathbf{E_S}`` and ``\mathbf{K_S}`` are defined in the Extended H
         \mathbf{0}    & \mathbf{0}                                                                                                  \end{bmatrix}
     \end{aligned}
     ```
-    Note that if `estim` is an [`InternalModel`](@ref), the state vector is not augmented so
-    the rows of ``\mathbf{E_S}`` and ``\mathbf{K_S}`` with an even index are removed. 
+    Note that if `estim` is an [`InternalModel`](@ref), the state vector is not augmented 
+    hence the rows of ``\mathbf{E_S}`` and ``\mathbf{K_S}`` related to the stochastic states
+    are removed (2nd row of blocks, 4th row of blocks, and so on). 
 """
 function init_defectmat(
     model::NonLinModel, estim::StateEstimator, transcription::OrthogonalCollocation, 
@@ -1009,7 +1010,7 @@ end
 @doc raw"""
     set_warmstart_mpc!(mpc::PredictiveController, ::OrthogonalCollocation, Z̃var) -> Z̃s
 
-Set and return the warm-start value of `Z̃var` for [`OrthogonalCollocation`](@ref).
+Do the same but for [`OrthogonalCollocation`](@ref).
 
 It warm-starts the solver at:
 ```math
@@ -1065,7 +1066,7 @@ end
 @doc raw"""
     set_warmstart_mpc!(mpc::PredictiveController, ::TranscriptionMethod, Z̃var) -> Z̃s
 
-Set and return the warm-start value of `Z̃var` for other [`TranscriptionMethod`](@ref).
+Do the same but for other [`TranscriptionMethod`](@ref).
 
 It warm-starts the solver at:
 ```math
@@ -1356,9 +1357,9 @@ function con_nonlinprogeq!(
         k        = @views    K[(1 + nk*(j-1)):(nk*j)]
         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)]
+        ŝdnext    = @views geq[(1 + nx*(j-1)):(nx*j)]
         f!(x̂dnext, k, model, x̂d_Z̃, û0, d̂0, model.p)
-        sdnext .= @. x̂dnext - x̂dnext_Z̃
+        ŝdnext .= @. x̂dnext - x̂dnext_Z̃
     end
     return geq
 end
@@ -1378,8 +1379,8 @@ The deterministic state defects are computed with:
                        - \mathbf{x̂_d}(k+j+1)                                              
 ```
 for ``j = 0, 1, ... , H_p-1``, and in which ``\mathbf{x̂_d}`` are the deterministic states
-extracted from the decision variable `Z̃`. The ``\mathbf{k̇}`` coefficients are  evaluated
-from the continuous-time function `model.f!` and:
+extracted from the decision variable `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̂_d}(k+j),   \mathbf{û_0}(k+j),   \mathbf{d̂_0}(k+j),   \mathbf{p}\Big) \\
@@ -1398,7 +1399,7 @@ function con_nonlinprogeq!(
     Hp, Hc = mpc.Hp, mpc.Hc
     nΔU, nX̂ = nu*Hc, nx̂*Hp
     f_threads = transcription.f_threads
-    Ts, p = model.Ts, model.p
+    Ts = model.Ts
     nk = get_nk(model, transcription)
     D̂0 = mpc.D̂0
     X̂0_Z̃ = @views Z̃[(nΔU+1):(nΔU+nX̂)]
@@ -1411,27 +1412,27 @@ function con_nonlinprogeq!(
             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)]
         d̂0next   = @views   D̂0[(1 + nd*(j-1)):(nd*j)]
         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)]
+        ŝdnext   = @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, x̂d_Z̃, û0, d̂0, p)
+            model.f!(k̇1, x̂d_Z̃, û0, d̂0, model.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, x̂dnext_Z̃, û0, d̂0next, p)
+            model.f!(k̇2, x̂dnext_Z̃, û0, d̂0next, model.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, x̂dnext_Z̃, û0next, d̂0next, p)
+            model.f!(k̇2, x̂dnext_Z̃, û0next, d̂0next, model.p)
         end
-        sdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2)
+        ŝdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2)
     end
     return geq
 end
@@ -1449,7 +1450,7 @@ Nonlinear equality constrains for [`NonLinModel`](@ref) and [`OrthogonalCollocat
 The defects between the deterministic state derivative at the ``n_o`` collocation points and
 the model dynamics are computed by:
 ```math
-\mathbf{s_k}(k+j)                                                                                 
+\mathbf{ŝ_k}(k+j)                                                                                 
     = \mathbf{M_o} \begin{bmatrix}                                          
         \mathbf{k}_1(k+j) - \mathbf{x̂_d}(k+j)                       \\
         \mathbf{k}_2(k+j) - \mathbf{x̂_d}(k+j)                       \\
@@ -1461,27 +1462,11 @@ the model dynamics are computed by:
         \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̂_d}`` vectors are the
-deterministic states extracted from ``\mathbf{X̂_0}`` also in `Z̃`, and they correspond to the
-states at the beginning of the interval ``τ_0=0``. 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)
-```
-Based on the normalized time ``τ_i ∈ [0, 1]`` and hold order `transcription.h`, the inputs
-and disturbances are piecewise constant or linear:
-```math
-\begin{aligned}
-\mathbf{û}_i(k+j) &=                                                                        \begin{cases}
-                     \mathbf{û_0}(k+j)                                    &  h = 0          \\
-                     (1-τ_i)\mathbf{û_0}(k+j) + τ_i\mathbf{û_0}(k+j+1)    &  h = 1          \end{cases} \\
-\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}`` is defined in [`f̂!`](@ref). The defects for the
-continuity constraints and stochastic states are linear equality constraints (see
-[`init_defectmat`](@ref)).
+for ``j = 0, 1, ... , H_p-1``, and knowing that the ``\mathbf{k}_i(k+j)`` and 
+``\mathbf{x̂_d}(k+j)`` vectors are extracted from the decision variables in `Z̃`. The
+``\mathbf{k̇}_i`` vectors are evaluated from the continuous-time function `model.f`, as
+described in [`init_orthocolloc`](@ref). The defects for the continuity constraints and the
+stochastic states are linear equality constraints (see [`init_defectmat`](@ref)).
 """
 function con_nonlinprogeq!(
     geq, _ , Û0, K̇,  
@@ -1492,7 +1477,6 @@ function con_nonlinprogeq!(
     Hp, Hc = mpc.Hp, mpc.Hc
     nΔU, nX̂ = nu*Hc, nx̂*Hp
     f_threads = transcription.f_threads
-    p = model.p
     Mo, no, τ =  mpc.Mo, transcription.no, transcription.τ
     nk = get_nk(model, transcription)
     D̂0 = mpc.D̂0
@@ -1507,17 +1491,17 @@ function con_nonlinprogeq!(
             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)]
         k_Z̃      = @views   K_Z̃[(1 + nk*(j-1)):(nk*j)] 
         d̂0next   = @views    D̂0[(1 + nd*(j-1)):(nd*j)]
-        sk       = @views   geq[(1 + nk*(j-1)):(nk*j)]
+        ŝk       = @views   geq[(1 + nk*(j-1)):(nk*j)]
         # ----------------- 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)] .- x̂d_Z̃
         end
-        mul!(sk, Mo, Δk)
+        mul!(ŝk, Mo, Δk)
         d̂i = @views D̂temp[(1 + nd*(j-1)):(nd*j)]
         if h > 0
             ûi = similar(û0) # TODO: remove this allocation
@@ -1527,15 +1511,15 @@ function con_nonlinprogeq!(
             ki_Z̃ = @views k_Z̃[(1 + (i-1)*nx):(i*nx)]
             d̂i  .= (1-τ[i]).*d̂0 .+ τ[i].*d̂0next
             if h < 1
-                model.f!(k̇i, ki_Z̃, û0, d̂i, p)
+                model.f!(k̇i, ki_Z̃, û0, d̂i, model.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))]
                 ûi .= (1-τ[i]).*û0 .+ τ[i].*û0next
-                model.f!(k̇i, ki_Z̃, ûi, d̂i, p)
+                model.f!(k̇i, ki_Z̃, ûi, d̂i, model.p)
             end
         end
-        sk .-= k̇
+        ŝk .-= k̇
     end
     return geq
 end

src/estimator/construct.jl

@@ -8,6 +8,7 @@ struct StateEstimatorBuffer{NT<:Real}
     Ŵ ::Vector{NT}
     X̂ ::Vector{NT}
     Ŷ ::Vector{NT}
+    D ::Vector{NT}
     P̂ ::Matrix{NT}
     Q̂ ::Matrix{NT}
     R̂ ::Matrix{NT}
@@ -34,8 +35,8 @@ function StateEstimatorBuffer{NT}(
     He::Int=0, nŵ::Int=nx̂, nε::Int=0,
     transcription::TranscriptionMethod = SingleShooting()
 ) where NT <: Real
-    nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nŵ)
-    nV̂, nŴ, nX̂, nŶ = nym*He, nŵ*He, nx̂*He, ny*He
+    nZ̃ = nε + get_nZ_mhe(transcription, He, nx̂, nk, nŵ)
+    nV̂, nŴ, nX̂, nŶ, nD = nym*He, nŵ*He, nx̂*He, ny*He, nd*(He+1)
     u  = Vector{NT}(undef, nu)
     û  = Vector{NT}(undef, nu)
     k  = Vector{NT}(undef, nk)
@@ -45,6 +46,7 @@ function StateEstimatorBuffer{NT}(
     Ŵ  = Vector{NT}(undef, nŴ)
     X̂  = Vector{NT}(undef, nX̂)
     Ŷ  = Vector{NT}(undef, nŶ)
+    D  = Vector{NT}(undef, nD)
     P̂  = Matrix{NT}(undef, nx̂, nx̂)
     Q̂  = Matrix{NT}(undef, nx̂, nx̂)
     R̂  = Matrix{NT}(undef, nym, nym)
@@ -53,7 +55,9 @@ function StateEstimatorBuffer{NT}(
     ŷ  = Vector{NT}(undef, ny)
     d  = Vector{NT}(undef, nd)
     empty = Vector{NT}(undef, 0)
-    return StateEstimatorBuffer{NT}(u, û, k, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty)
+    return StateEstimatorBuffer{NT}(
+        u, û, k, x̂, Z̃, V̂, Ŵ, X̂, Ŷ, D, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty
+    )
 end
 
 "Include all the covariance matrices for the Kalman filters and moving horizon estimator."

src/estimator/mhe.jl

@@ -8,7 +8,7 @@ get_other_dims(estim::MovingHorizonEstimator) = (estim.He, estim.nε)
 "Print optimizer and other information for `MovingHorizonEstimator`."
 function print_details(io::IO, estim::MovingHorizonEstimator)
     println(io, "├ optimizer: $(JuMP.solver_name(estim.optim)) ")
-    println(io, "├ transcription: $(nameof(typeof(estim.transcription)))")
+    println(io, "├ transcription: $(transcription_str(estim.transcription))")
     print_backends(io, estim, estim.model)
     println(io, "├ arrival covariance: $(nameof(typeof(estim.covestim))) ")
     println(io, "├ direct: $(estim.direct)")