Preparation for v1.0.0 release

.gitignore

@@ -5,3 +5,4 @@ LocalPreferences.toml
 .vscode/
 *.cov
 docs/Manifest.toml
+lcov.info

Project.toml

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

README.md

@@ -24,7 +24,7 @@ using Pkg; Pkg.add("ModelPredictiveControl")
 To construct model predictive controllers (MPCs), we must first specify a plant model that
 is typically extracted from input-output data using [system identification](https://github.com/baggepinnen/ControlSystemIdentification.jl).
 The model here is linear with one input, two outputs and a large time delay in the first
-channel:
+channel (a transfer function matrix, with $s$ as the Laplace variable):
 
 ```math
 \mathbf{G}(s) = \frac{\mathbf{y}(s)}{\mathbf{u}(s)} = 

docs/src/manual/nonlinmpc.md

@@ -40,34 +40,34 @@ The plant model is nonlinear:
 
 in which ``g`` is the gravitational acceleration in m/s², ``L``, the pendulum length in m,
 ``K``, the friction coefficient at the pivot point in kg/s, and ``m``, the mass attached at
-the end of the pendulum in kg. The [`NonLinModel`](@ref) constructor assumes by default
-that the state function `f` is continuous in time, that is, an ordinary differential
-equation system (like here):
+the end of the pendulum in kg, all bundled in the parameter vector ``\mathbf{p} =
+[\begin{smallmatrix} g & L & K & m \end{smallmatrix}]'``. The [`NonLinModel`](@ref)
+constructor assumes by default that the state function `f` is continuous in time, that is,
+an ordinary differential equation system (like here):
 
 ```@example 1
 using ModelPredictiveControl
-function pendulum(par, x, u)
-    g, L, K, m = par        # [m/s²], [m], [kg/s], [kg]
+function f(x, u, _ , p)
+    g, L, K, m = p          # [m/s²], [m], [kg/s], [kg]
     θ, ω = x[1], x[2]       # [rad], [rad/s]
     τ  = u[1]               # [Nm]
     dθ = ω
     dω = -g/L*sin(θ) - K/m*ω + τ/m/L^2
     return [dθ, dω]
 end
-# declared constants, to avoid type-instability in the f function, for speed:
-const par = (9.8, 0.4, 1.2, 0.3)
-f(x, u, _ ) = pendulum(par, x, u)
-h(x, _ )    = [180/π*x[1]]  # [°]
+h(x, _ , _ ) = [180/π*x[1]] # [°]
+p_model = [9.8, 0.4, 1.2, 0.3]
 nu, nx, ny, Ts = 1, 2, 1, 0.1
 vu, vx, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"]
-model = setname!(NonLinModel(f, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
+model = setname!(NonLinModel(f, h, Ts, nu, nx, ny; p=p_model); u=vu, x=vx, y=vy)
 ```
 
 The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. Note that special
 characters like ``θ`` can be typed in the Julia REPL or VS Code by typing `\theta` and
-pressing the `<TAB>` key. The tuple `par` is constant here to improve the [performance](https://docs.julialang.org/en/v1/manual/performance-tips/#Avoid-untyped-global-variables).
-A 4th order [`RungeKutta`](@ref) method solves the differential equations by default. It is
-good practice to first simulate `model` using [`sim!`](@ref) as a quick sanity check:
+pressing the `<TAB>` key. Note that the model parameter `p` can be any 
+ A 4th order [`RungeKutta`](@ref) method solves the differential
+equations by default. It is good practice to first simulate `model` using [`sim!`](@ref) as
+a quick sanity check:
 
 ```@example 1
 using Plots
@@ -101,9 +101,9 @@ motor torque ``τ``, with an associated standard deviation `σQint_u` of 0.1 N m
 estimator tuning is tested on a plant with a 25 % larger friction coefficient ``K``:
 
 ```@example 1
-const par_plant = (par[1], par[2], 1.25*par[3], par[4])
-f_plant(x, u, _ ) = pendulum(par_plant, x, u)
-plant = setname!(NonLinModel(f_plant, h, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
+p_plant = copy(p_model)
+p_plant[3] = 1.25*p_model[3]
+plant = setname!(NonLinModel(f, h, Ts, nu, nx, ny; p=p_plant); u=vu, x=vx, y=vy)
 res = sim!(estim, N, [0.5], plant=plant, y_noise=[0.5])
 plot(res, plotu=false, plotxwithx̂=true)
 savefig("plot2_NonLinMPC.svg"); nothing # hide
@@ -182,10 +182,10 @@ angle ``θ`` is measured here). As the arguments of [`NonLinMPC`](@ref) economic
 Kalman Filter similar to the previous one (``\mathbf{y^m} = θ`` and ``\mathbf{y^u} = ω``):
 
 ```@example 1
-h2(x, _ ) = [180/π*x[1], x[2]]
+h2(x, _ , _ ) = [180/π*x[1], x[2]]
 nu, nx, ny = 1, 2, 2
-model2 = setname!(NonLinModel(f      , h2, Ts, nu, nx, ny), u=vu, x=vx, y=[vy; vx[2]])
-plant2 = setname!(NonLinModel(f_plant, h2, Ts, nu, nx, ny), u=vu, x=vx, y=[vy; vx[2]])
+model2 = setname!(NonLinModel(f, h2, Ts, nu, nx, ny; p=p_model), u=vu, x=vx, y=[vy; vx[2]])
+plant2 = setname!(NonLinModel(f, h2, Ts, nu, nx, ny; p=p_plant), u=vu, x=vx, y=[vy; vx[2]])
 estim2 = UnscentedKalmanFilter(model2; σQ, σR, nint_u, σQint_u, i_ym=[1])
 ```
 
@@ -194,11 +194,11 @@ output vector of `plant` argument corresponds to the model output vector in `mpc
 We can now define the ``J_E`` function and the `empc` controller:
 
 ```@example 1
-function JE(UE, ŶE, _ )
+function JE(UE, ŶE, _ , Ts)
     τ, ω = UE[1:end-1], ŶE[2:2:end-1]
     return Ts*sum(τ.*ω)
 end
-empc = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=[0.5, 0], Cwt=Inf, Ewt=3.5e3, JE=JE)
+empc = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=[0.5, 0], Cwt=Inf, Ewt=3.5e3, JE=JE, p=Ts)
 empc = setconstraint!(empc; umin, umax)
 ```
 

src/controller/execute.jl

@@ -89,7 +89,7 @@ The function should be called after calling [`moveinput!`](@ref). It returns the
 - `:Ŷs` or *`:Yhats`* : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}``
 - `:R̂y` or *`:Rhaty`* : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}``
 - `:R̂u` or *`:Rhatu`* : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}``
-- `:x̂end` or *`:xhatend`* : optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)``
+- `:x̂end` or *`:xhatend`* : optimal terminal states, ``\mathbf{x̂}_i(k+H_p)``
 - `:J`   : objective value optimum, ``J``
 - `:U`   : optimal manipulated inputs over ``H_p``, ``\mathbf{U}``
 - `:u`   : current optimal manipulated input, ``\mathbf{u}(k)``
@@ -369,19 +369,7 @@ function obj_nonlinprog!(
     U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
     J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
-    if !iszero(mpc.E)
-        ŷ, D̂E = mpc.ŷ, mpc.D̂E
-        U = U0
-        U  .+= mpc.Uop
-        uend = @views U[(end-model.nu+1):end]
-        Ŷ  = Ȳ
-        Ŷ .= Ŷ0 .+ mpc.Yop
-        UE = [U; uend]
-        ŶE = [ŷ; Ŷ]
-        E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
-    else
-        E_JE = 0.0
-    end
+    E_JE = obj_econ!(U0, Ȳ, mpc, model, Ŷ0, ΔŨ)
     return J + E_JE
 end
 
@@ -413,22 +401,13 @@ function obj_nonlinprog!(
         JR̂u = 0.0
     end
     # --- economic term ---
-    if !iszero(mpc.E)
-        ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E
-        U = U0
-        U  .+= mpc.Uop
-        uend = @views U[(end-model.nu+1):end]
-        Ŷ  = Ȳ
-        Ŷ .= Ŷ0 .+ mpc.Yop
-        UE = [U; uend]
-        ŶE = [ŷ; Ŷ]
-        E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
-    else
-        E_JE = 0.0
-    end
+    E_JE = obj_econ!(U0, Ȳ, mpc, model, Ŷ0, ΔŨ)
     return JR̂y + JΔŨ + JR̂u + E_JE
 end
 
+"By default, the economic term is zero."
+obj_econ!( _ , _ , ::PredictiveController, ::SimModel, _ , _ ) = 0.0
+
 @doc raw"""
     optim_objective!(mpc::PredictiveController) -> ΔŨ
 

src/controller/explicitmpc.jl

@@ -167,7 +167,7 @@ function ExplicitMPC(
     return ExplicitMPC{NT, SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
 end
 
-setconstraint!(::ExplicitMPC,kwargs...) = error("ExplicitMPC does not support constraints.")
+setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.")
 
 function Base.show(io::IO, mpc::ExplicitMPC)
     Hp, Hc = mpc.Hp, mpc.Hc

src/controller/nonlinmpc.jl

@@ -4,7 +4,8 @@ struct NonLinMPC{
     NT<:Real, 
     SE<:StateEstimator, 
     JM<:JuMP.GenericModel, 
-    JEfunc<:Function
+    JEfunc<:Function,
+    P<:Any
 } <: PredictiveController{NT}
     estim::SE
     # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
@@ -21,6 +22,7 @@ struct NonLinMPC{
     L_Hp::Hermitian{NT, Matrix{NT}}
     E::NT
     JE::JEfunc
+    p::P
     R̂u0::Vector{NT}
     R̂y0::Vector{NT}
     noR̂u::Bool
@@ -46,12 +48,13 @@ struct NonLinMPC{
     Yop::Vector{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
-    function NonLinMPC{NT, SE, JM, JEFunc}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, optim::JM
-    ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function}
+    function NonLinMPC{NT, SE, JM, JEFunc, P}(
+        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, p::P, optim::JM
+    ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function, P<:Any}
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
+        validate_JE(NT, JE)
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
         # convert `Diagonal` to normal `Matrix` if required:
         M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
@@ -77,11 +80,11 @@ struct NonLinMPC{
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
         buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
-        mpc = new{NT, SE, JM, JEFunc}(
+        mpc = new{NT, SE, JM, JEFunc, P}(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
-            M_Hp, Ñ_Hc, L_Hp, Ewt, JE, 
+            M_Hp, Ñ_Hc, L_Hp, Ewt, JE, p,
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
@@ -109,12 +112,12 @@ controller minimizes the following objective function at each discrete time ``k`
                        + \mathbf{(ΔU)}'      \mathbf{N}_{H_c} \mathbf{(ΔU)}        \\&
                        + \mathbf{(R̂_u - U)}' \mathbf{L}_{H_p} \mathbf{(R̂_u - U)} 
                        + C ϵ^2  
-                       + E J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E)
+                       + E J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p})
 \end{aligned}
 ```
 See [`LinMPC`](@ref) for the variable definitions. The custom economic function ``J_E`` can
 penalizes solutions with high economic costs. Setting all the weights to 0 except ``E`` 
-creates a pure economic model predictive controller (EMPC). The arguments of ``J_E`` are 
+creates a pure economic model predictive controller (EMPC). The arguments of ``J_E`` include
 the manipulated inputs, the predicted outputs and measured disturbances from ``k`` to 
 ``k+H_p`` inclusively:
 ```math
@@ -124,10 +127,11 @@ the manipulated inputs, the predicted outputs and measured disturbances from ``k
 ```
 since ``H_c ≤ H_p`` implies that ``\mathbf{Δu}(k+H_p) = \mathbf{0}`` or ``\mathbf{u}(k+H_p)=
 \mathbf{u}(k+H_p-1)``. The vector ``\mathbf{D̂}`` includes the predicted measured disturbance
-over ``H_p``.
+over ``H_p``. The argument ``\mathbf{p}`` is a custom parameter object of any type but use a
+mutable one if you want to modify it later e.g.: a vector.
 
 !!! tip
-    Replace any of the 3 arguments with `_` if not needed (see `JE` default value below).
+    Replace any of the 4 arguments with `_` if not needed (see `JE` default value below).
 
 This method uses the default state estimator :
 
@@ -150,7 +154,8 @@ This method uses the default state estimator :
 - `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``.
 - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
 - `Ewt=0.0` : economic costs weight ``E`` (scalar). 
-- `JE=(_,_,_)->0.0` : economic function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E)``.
+- `JE=(_,_,_,_)->0.0` : economic function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p})``.
+- `p=model.p` : ``J_E`` function parameter ``\mathbf{p}`` (any type).
 - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive
    controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
    (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer).
@@ -159,7 +164,7 @@ This method uses the default state estimator :
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);
+julia> model = NonLinModel((x,u,_,_)->0.5x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing);
 
 julia> mpc = NonLinMPC(model, Hp=20, Hc=1, Cwt=1e6)
 NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and:
@@ -200,12 +205,13 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::Function = (_,_,_) -> 0.0,
+    JE::Function = (_,_,_,_) -> 0.0,
+    p = model.p,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
     estim = UnscentedKalmanFilter(model; kwargs...)
-    NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, M_Hp, N_Hc, L_Hp, optim)
+    NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, p, M_Hp, N_Hc, L_Hp, optim)
 end
 
 function NonLinMPC(
@@ -220,12 +226,13 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::Function = (_,_,_) -> 0.0,
+    JE::Function = (_,_,_,_) -> 0.0,
+    p = model.p,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
     estim = SteadyKalmanFilter(model; kwargs...)
-    NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, M_Hp, N_Hc, L_Hp, optim)
+    NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, p, M_Hp, N_Hc, L_Hp, optim)
 end
 
 
@@ -236,7 +243,7 @@ Use custom state estimator `estim` to construct `NonLinMPC`.
 
 # Examples
 ```jldoctest
-julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1, solver=nothing);
+julia> model = NonLinModel((x,u,_,_)->0.5x+u, (x,_,_)->2x, 10.0, 1, 1, 1, solver=nothing);
 
 julia> estim = UnscentedKalmanFilter(model, σQint_ym=[0.05]);
 
@@ -264,15 +271,33 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::JEFunc = (_,_,_) -> 0.0,
+    JE::JEFunc = (_,_,_,_) -> 0.0,
+    p::P = estim.model.p,
     optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
-) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEFunc<:Function}
+) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEFunc<:Function, P<:Any}
     nk = estimate_delays(estim.model)
     if Hp ≤ nk
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    return NonLinMPC{NT, SE, JM, JEFunc}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, optim)
+    return NonLinMPC{NT, SE, JM, JEFunc, P}(
+        estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, p, optim
+    )
+end
+
+"""
+    validate_JE(NT, JE) -> nothing
+
+Validate `JE` function argument signature 
+"""
+function validate_JE(NT, JE)
+    if !hasmethod(JE, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any})
+        error(
+            "the economic function has no method with type signature "*
+            "JE(UE::Vector{$(NT)}, ŶE::Vector{$(NT)}, D̂E::Vector{$(NT)}, p::Any)"
+        )
+    end
+    return nothing
 end
 
 """
@@ -285,7 +310,7 @@ function addinfo!(info, mpc::NonLinMPC)
     UE = [U; U[(end - mpc.estim.model.nu + 1):end]]
     ŶE = [ŷ; Ŷ]
     D̂E = [d; D̂]
-    info[:JE]  = mpc.JE(UE, ŶE, D̂E)
+    info[:JE]  = mpc.JE(UE, ŶE, D̂E, mpc.p)
     info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true)
     return info
 end
@@ -457,4 +482,22 @@ function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, ΔŨ)
 end
 
 "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged."
-con_nonlinprog!(g, ::NonLinMPC, ::LinModel, _ , _ , _ ) = g
+con_nonlinprog!(g, ::NonLinMPC, ::LinModel, _ , _ , _ ) = g
+
+"Evaluate the economic term of the objective function for [`NonLinMPC`](@ref)."
+function obj_econ!(U0, Ȳ, mpc::NonLinMPC, model::SimModel, Ŷ0, ΔŨ)
+    if !iszero(mpc.E)
+        ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E
+        U   = U0
+        U .+= mpc.Uop
+        uend = @views U[(end-model.nu+1):end]
+        Ŷ  = Ȳ
+        Ŷ .= Ŷ0 .+ mpc.Yop
+        UE = [U; uend]
+        ŶE = [ŷ; Ŷ]
+        E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E, mpc.p)
+    else
+        E_JE = 0.0
+    end
+    return E_JE
+end