Merge pull request #105 from JuliaControl/doc_cov

Preparation for v1.0.0 release

Author: franckgaga

.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/Project.toml

@@ -5,10 +5,14 @@ Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 JuMP = "4076af6c-e467-56ae-b986-b466b2749572"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Logging = "56ddb016-857b-54e1-b83d-db4d58db5568"
+ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 
 [compat]
 ControlSystemsBase = "1"
+DAQP = "0.5"
 Documenter = "1"
+JuMP = "1"
 LinearAlgebra = "1.6"
 Logging = "1.6"
+Plots = "1"

docs/make.jl

@@ -27,6 +27,7 @@ makedocs(
             "Examples" => [
                 "Linear Design" => "manual/linmpc.md",
                 "Nonlinear Design" => "manual/nonlinmpc.md",
+                "ModelingToolkit" =>  "manual/mtk.md",
             ],
         ],
         "Functions" => [

docs/src/index.md

@@ -29,6 +29,7 @@ Pages = [
     "manual/installation.md",
     "manual/linmpc.md",
     "manual/nonlinmpc.md",
+    "manual/mtk.md"
 ]
 ```
 

docs/src/manual/mtk.md

@@ -0,0 +1,140 @@
+# [Manual: ModelingToolkit Integration](@id man_mtk)
+
+```@contents
+Pages = ["mtk.md"]
+```
+
+```@setup 1
+using Logging; errlogger = ConsoleLogger(stderr, Error);
+old_logger = global_logger(); global_logger(errlogger);
+```
+
+## Pendulum Model
+
+This example integrates the simple pendulum model of the [last section](@ref man_nonlin) in the
+[`ModelingToolkit.jl`](https://docs.sciml.ai/ModelingToolkit/stable/) (MTK) framework and
+extracts appropriate `f!` and `h!` functions to construct a [`NonLinModel`](@ref). An
+[`NonLinMPC`](@ref) is designed from this model and simulated to reproduce the results of
+the last section.
+
+!!! danger "Disclaimer"
+    This simple example is not an official interface to `ModelingToolkit.jl`. It is provided
+    as a basic starting template to combine both packages. There is no guarantee that it
+    will work for all corner cases.
+
+We first construct and instantiate the pendulum model:
+
+```@example 1
+using ModelPredictiveControl, ModelingToolkit
+using ModelingToolkit: D_nounits as D, t_nounits as t, varmap_to_vars
+@mtkmodel Pendulum begin
+    @parameters begin
+        g = 9.8
+        L = 0.4
+        K = 1.2
+        m = 0.3
+    end
+    @variables begin
+        θ(t) # state
+        ω(t) # state
+        τ(t) # input
+        y(t) # output
+    end
+    @equations begin
+        D(θ)    ~ ω
+        D(ω)    ~ -g/L*sin(θ) - K/m*ω + τ/m/L^2
+        y       ~ θ * 180 / π
+    end
+end
+@named mtk_model = Pendulum()
+mtk_model = complete(mtk_model)
+```
+
+We than convert the MTK model to an [input-output system](https://docs.sciml.ai/ModelingToolkit/stable/basics/InputOutput/):
+
+```@example 1
+function generate_f_h(model, inputs, outputs)
+    (_, f_ip), dvs, psym, io_sys = ModelingToolkit.generate_control_function(
+        model, inputs, split=false; outputs
+    )
+    if any(ModelingToolkit.is_alg_equation, equations(io_sys)) 
+        error("Systems with algebraic equations are not supported")
+    end
+    h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs = inputs)
+    nx = length(dvs)
+    vx = string.(dvs)
+    par = varmap_to_vars(defaults(io_sys), psym)
+    function f!(ẋ, x, u, _ , _ )
+        f_ip(ẋ, x, u, par, 1)
+        nothing
+    end
+    function h!(y, x, _ , _ )
+        y .= h_(x, 1, par, 1)
+        nothing
+    end
+    return f!, h!, nx, vx
+end
+inputs, outputs = [mtk_model.τ], [mtk_model.y]
+f!, h!, nx, vx = generate_f_h(mtk_model, inputs, outputs)
+nu, ny, Ts = length(inputs), length(outputs), 0.1
+vu, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (°)"]
+nothing # hide
+```
+
+A [`NonLinModel`](@ref) can now be constructed:
+
+```@example 1
+model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
+```
+
+We also instantiate a plant model with a 25 % larger friction coefficient ``K``:
+
+```@example 1
+mtk_model.K = defaults(mtk_model)[mtk_model.K] * 1.25
+f_plant, h_plant, _, _ = generate_f_h(mtk_model, inputs, outputs)
+plant = setname!(NonLinModel(f_plant, h_plant, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
+```
+
+## Controller Design
+
+We can than reproduce the Kalman filter and the controller design of the [last section](@ref man_nonlin):
+
+```@example 1
+α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1]
+estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u)
+Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5]
+nmpc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt=Inf)
+umin, umax = [-1.5], [+1.5]
+nmpc = setconstraint!(nmpc; umin, umax)
+```
+
+The 180° setpoint response is identical:
+
+```@example 1
+using Plots
+N = 35
+using JuMP; unset_time_limit_sec(nmpc.optim) # hide
+res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
+plot(res_ry)
+savefig("plot1_MTK.svg"); nothing # hide
+```
+
+![plot1_MTK](plot1_MTK.svg)
+
+and also the output disturbance rejection:
+
+```@example 1
+res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10])
+plot(res_yd)
+savefig("plot2_MTK.svg"); nothing # hide
+```
+
+![plot2_MTK](plot2_MTK.svg)
+
+## Acknowledgement
+
+Authored by `1-Bart-1` and `baggepinnen`, thanks for the contribution.
+
+```@setup 1
+global_logger(old_logger);
+```

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 parameter `p` can be of any type but use a mutable
+type like a vector of you want to modify it later. 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,12 @@ 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, _ , p)
+    Ts = p
     τ, ω = 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