📐 added: OrthogonalCollocation with gaussradau and gausslegendre schemes

Project.toml

@@ -1,11 +1,12 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "1.16.3"
+version = "1.17.0"
 authors = ["Francis Gagnon"]
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 DifferentiationInterface = "a0c0ee7d-e4b9-4e03-894e-1c5f64a51d63"
+FastGaussQuadrature = "442a2c76-b920-505d-bb47-c5924d526838"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9"
 JuMP = "4076af6c-e467-56ae-b986-b466b2749572"
@@ -33,6 +34,7 @@ ControlSystemsBase = "1.18.2"
 DAQP = "0.6, 0.7.1"
 DifferentiationInterface = "0.7.11"
 Documenter = "1"
+FastGaussQuadrature = "1.1.0"
 FiniteDiff = "2"
 ForwardDiff = "0.10, 1"
 Ipopt = "1"

docs/src/internals/predictive_control.md

@@ -25,6 +25,7 @@ ModelPredictiveControl.relaxterminal
 ModelPredictiveControl.augmentdefect
 ModelPredictiveControl.init_quadprog
 ModelPredictiveControl.init_stochpred
+ModelPredictiveControl.init_orthocolloc
 ModelPredictiveControl.init_matconstraint_mpc
 ModelPredictiveControl.get_nonlinobj_op(::NonLinMPC, ::ModelPredictiveControl.GenericModel)
 ModelPredictiveControl.get_nonlincon_oracle(::NonLinMPC, ::ModelPredictiveControl.GenericModel)

docs/src/internals/state_estim.md

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

docs/src/public/predictive_control.md

@@ -120,4 +120,10 @@ MultipleShooting
 
 ```@docs
 TrapezoidalCollocation
-```
+```
+
+### OrthogonalCollocation
+
+```@docs
+OrthogonalCollocation
+```

src/ModelPredictiveControl.jl

@@ -37,6 +37,8 @@ import JuMP: @variable, @operator, @constraint, @objective
 
 import OSQP, Ipopt
 
+import FastGaussQuadrature
+
 export SimModel, LinModel, NonLinModel
 export DiffSolver, RungeKutta, ForwardEuler
 export setop!, setname!
@@ -48,7 +50,8 @@ export MovingHorizonEstimator
 export ManualEstimator
 export default_nint, initstate!
 export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput!
-export TranscriptionMethod, SingleShooting, MultipleShooting, TrapezoidalCollocation
+export TranscriptionMethod, SingleShooting, MultipleShooting
+export TrapezoidalCollocation, OrthogonalCollocation
 export SimResult, getinfo, sim!
 
 include("general.jl")

src/controller/construct.jl

@@ -692,6 +692,105 @@ function validate_args(mpc::PredictiveController, ry, d, lastu, D̂, R̂y, R̂u)
     size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)"))
 end
 
+@doc raw"""
+    init_ZtoΔU(estim::StateEstimator, transcription::TranscriptionMethod, Hp, Hc) -> PΔu
+
+Init decision variables to input increments over ``H_c`` conversion matrix `PΔu`.
+
+The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{ΔU}``, the input
+increments over ``H_c``, is computed by:
+```math
+\mathbf{ΔU} = \mathbf{P_{Δu}} \mathbf{Z}
+```
+
+in which ``\mathbf{P_{Δu}}`` is defined in the Extended Help section.
+
+# Extended Help
+!!! details "Extended Help"
+    Following the decision variable definition of the [`TranscriptionMethod`](@ref), the
+    conversion matrix ``\mathbf{P_{Δu}}``, we have:
+    - ``\mathbf{P_{Δu}} = \mathbf{I}`` if `transcription` is a [`SingleShooting`](@ref)
+    - ``\mathbf{P_{Δu}} = [\begin{smallmatrix}\mathbf{I} & \mathbf{0} \end{smallmatrix}]`` otherwise.
+    The matrix is store as as `SparseMatrixCSC` to support both cases efficiently.
+"""
+function init_ZtoΔU(
+    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc
+) where {NT<:Real}
+    I_nu_Hc = sparse(Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc))
+    nZ = get_nZ(estim, transcription, Hp, Hc)
+    nΔU = estim.model.nu*Hc
+    PΔu = [I_nu_Hc spzeros(NT, nΔU, nZ - nΔU)]
+    return PΔu
+end
+
+@doc raw"""
+    init_ZtoU(estim, transcription, Hp, Hc, nb) -> Pu, Tu
+
+Init decision variables to inputs over ``H_p`` conversion matrices.
+
+The conversion from the decision variables ``\mathbf{Z}`` to ``\mathbf{U}``, the manipulated
+inputs over ``H_p``, is computed by:
+```math
+\mathbf{U} = \mathbf{P_u} \mathbf{Z} + \mathbf{T_u} \mathbf{u}(k-1)
+```
+The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended Help section.
+
+# Extended Help
+!!! details "Extended Help"
+    With ``n_i``, the ``i``th element of the ``\mathbf{n_b}`` vector defined in [`move_blocking`](@ref)
+    documentation, we introduce the ``\mathbf{Q}(n_i)`` matrix of size `(nu*ni, nu)`:
+    ```math
+    \mathbf{Q}(n_i) =       \begin{bmatrix}
+        \mathbf{I}          \\
+        \mathbf{I}          \\
+        \vdots              \\
+        \mathbf{I}          \end{bmatrix}            
+    ```
+    The ``\mathbf{U}`` vector and the conversion matrices are defined as:
+    ```math
+    \mathbf{U} = \begin{bmatrix}
+        \mathbf{u}(k + 0)                                                                   \\
+        \mathbf{u}(k + 1)                                                                   \\
+        \vdots                                                                              \\
+        \mathbf{u}(k + H_p - 1)                                                             \end{bmatrix} , \quad
+    \mathbf{P_u^†} = \begin{bmatrix}
+        \mathbf{Q}(n_1)         & \mathbf{0}            & \cdots    & \mathbf{0}            \\
+        \mathbf{Q}(n_2)         & \mathbf{Q}(n_2)       & \cdots    & \mathbf{0}            \\
+        \vdots                  & \vdots                & \ddots    & \vdots                \\
+        \mathbf{Q}(n_{H_c})     & \mathbf{Q}(n_{H_c})   & \cdots    & \mathbf{Q}(n_{H_c})   \end{bmatrix} , \quad
+    \mathbf{T_u} = \begin{bmatrix}
+        \mathbf{I}                                                                          \\
+        \mathbf{I}                                                                          \\
+        \vdots                                                                              \\
+        \mathbf{I}                                                                          \end{bmatrix}
+    ```
+    and, depending on the transcription method, we have:
+    - ``\mathbf{P_u} = \mathbf{P_u^†}`` if `transcription` is a [`SingleShooting`](@ref)
+    - ``\mathbf{P_u} = [\begin{smallmatrix}\mathbf{P_u^†} & \mathbf{0} \end{smallmatrix}]``
+      if `transcription` is a [`MultipleShooting`](@ref)
+    The conversion matrices are stored as `SparseMatrixCSC` since it was benchmarked that it
+    is generally more performant than normal dense matrices, even for small `nu`, `Hp` and 
+    `Hc` values. Using `Bool` element type and `BitMatrix` is also slower.
+"""
+function init_ZtoU(
+    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb
+) where {NT<:Real}
+    nu = estim.model.nu
+    I_nu = sparse(Matrix{NT}(I, nu, nu))
+    PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc)
+    for i=1:Hc
+        ni    = nb[i]
+        Q_ni  = repeat(I_nu, ni, 1)
+        iRows = (1:nu*ni) .+ @views nu*sum(nb[1:i-1])
+        PuDagger[iRows, :] = [repeat(Q_ni, 1, i) spzeros(nu*ni, nu*(Hc-i))]
+    end
+    PuDagger = sparse(PuDagger)
+    nZ = get_nZ(estim, transcription, Hp, Hc)
+    Pu = [PuDagger spzeros(NT, nu*Hp, nZ - nu*Hc)]
+    Tu = repeat(I_nu, Hp)
+    return Pu, Tu
+end
+
 @doc raw"""
     init_quadprog(
         model::LinModel, transcriptions::TranscriptionMethod, weights::ControllerWeights, 

src/controller/execute.jl

@@ -142,15 +142,15 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     info = Dict{Symbol, Any}()
     ΔŨ     = Vector{NT}(undef, nΔŨ)
     x̂0end  = similar(mpc.estim.x̂0)
-    K0     = Vector{NT}(undef, nK) 
+    K      = Vector{NT}(undef, nK) 
     Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe)
     U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop)
     Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0)
     U,  Ŷ  = buffer.U, buffer.Ŷ
     D̂      = buffer.D̂
     U0 = getU0!(U0, mpc, Z̃)
     ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃)
-    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, K0, mpc, model, transcription, U0, Z̃)
+    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, K, mpc, model, transcription, U0, Z̃)
     Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
     U .= U0 .+ mpc.Uop
     Ŷ .= Ŷ0 .+ mpc.Yop

src/controller/legacy.jl

@@ -23,21 +23,21 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     J::Vector{JNT}                   = zeros(JNT, 1)
     ΔŨ::Vector{JNT}                  = zeros(JNT, nΔŨ)
     x̂0end::Vector{JNT}               = zeros(JNT, nx̂)
-    K0::Vector{JNT}                  = zeros(JNT, nK)
+    K::Vector{JNT}                  = zeros(JNT, nK)
     Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe)
     U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU),  zeros(JNT, nŶ)
     Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU),  zeros(JNT, nX̂)
     gc::Vector{JNT}, g::Vector{JNT}  = zeros(JNT, nc),  zeros(JNT, ng)
     geq::Vector{JNT}                 = zeros(JNT, neq)
     # ---------------------- objective function ------------------------------------------- 
-    function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq)
-        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+    function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq)
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃)
         return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
     end
     Z̃_∇J = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
     ∇J_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
-        Cache(Û0), Cache(K0), Cache(X̂0), 
+        Cache(Û0), Cache(K), Cache(X̂0), 
         Cache(gc), Cache(g), Cache(geq),
     )
     ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict)
@@ -64,14 +64,14 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
         end
     end
     # --------------------- inequality constraint functions -------------------------------
-    function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq)
-        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+    function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, geq)
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃)
         return g
     end
     Z̃_∇g = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
     ∇g_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), 
-        Cache(Û0), Cache(K0), Cache(X̂0), 
+        Cache(Û0), Cache(K), Cache(X̂0), 
         Cache(gc), Cache(geq),
     )
     # temporarily enable all the inequality constraints for sparsity detection:
@@ -109,14 +109,14 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
         ∇g_funcs![i] = ∇gfuncs_i!
     end
     # --------------------- equality constraint functions ---------------------------------
-    function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) 
-        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
+    function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g) 
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃)
         return geq
     end
     Z̃_∇geq = fill(myNaN, nZ̃)    # NaN to force update_predictions! at first call
     ∇geq_context = (
         Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0),
-        Cache(Û0), Cache(K0),   Cache(X̂0),
+        Cache(Û0), Cache(K),   Cache(X̂0),
         Cache(gc), Cache(g)
     )
     ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac, Z̃_∇geq, ∇geq_context...; strict)