Merge pull request #128 from JuliaControl/custom_constraints

Custom constraints : add tests

Author: franckgaga

Project.toml

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

README.md

@@ -70,71 +70,66 @@ for more detailed examples.
 
 ## Features
 
-### Legend
-
-- [x] implemented feature  
-- [ ] planned feature
-
 ### Model Predictive Control Features
 
-- [x] linear and nonlinear plant models exploiting multiple dispatch
-- [x] model linearization based on automatic differentiation (exact Jacobians)
-- [x] supported objective function terms:
-  - [x] output setpoint tracking
-  - [x] move suppression
-  - [x] input setpoint tracking
-  - [x] terminal costs
-  - [x] custom economic costs (economic model predictive control)
-- [x] adaptive linear model predictive controller
-  - [x] manual model modification
-  - [x] automatic successive linearization of a nonlinear model
-  - [x] objective function weights and covariance matrices modification
-- [x] explicit predictive controller for problems without constraint
-- [x] online-tunable soft and hard constraints on:
-  - [x] output predictions
-  - [x] manipulated inputs
-  - [x] manipulated inputs increments
-  - [x] terminal states to ensure nominal stability
-- [x] custom economic inequality constraints (soft or hard)
-- [x] supported feedback strategy:
-  - [x] state estimator (see State Estimation features)
-  - [x] internal model structure with a custom stochastic model
-- [x] automatic model augmentation with integrating states for offset-free tracking
-- [x] support for unmeasured model outputs
-- [x] feedforward action with measured disturbances that supports direct transmission
-- [x] custom predictions for:
-  - [x] output setpoints
-  - [x] measured disturbances
-- [x] easy integration with `Plots.jl`
-- [x] optimization based on `JuMP.jl`:
-  - [x] quickly compare multiple optimizers
-  - [x] nonlinear solvers relying on automatic differentiation (exact derivative)
-- [x] additional information about the optimum to ease troubleshooting
-- [x] real-time control loop features:
-  - [x] implementations that carefully limits the allocations
-  - [x] simple soft real-time utilities
+- linear and nonlinear plant models exploiting multiple dispatch
+- model linearization based on automatic differentiation (exact Jacobians)
+- supported objective function terms:
+  - output setpoint tracking
+  - move suppression
+  - input setpoint tracking
+  - terminal costs
+  - custom economic costs (economic model predictive control)
+- adaptive linear model predictive controller
+  - manual model modification
+  - automatic successive linearization of a nonlinear model
+  - objective function weights and covariance matrices modification
+- explicit predictive controller for problems without constraint
+- online-tunable soft and hard constraints on:
+  - output predictions
+  - manipulated inputs
+  - manipulated inputs increments
+  - terminal states to ensure nominal stability
+- custom nonlinear inequality constraints (soft or hard)
+- supported feedback strategy:
+  - state estimator (see State Estimation features)
+  - internal model structure with a custom stochastic model
+- automatic model augmentation with integrating states for offset-free tracking
+- support for unmeasured model outputs
+- feedforward action with measured disturbances that supports direct transmission
+- custom predictions for:
+  - output setpoints
+  - measured disturbances
+- easy integration with `Plots.jl`
+- optimization based on `JuMP.jl`:
+  - quickly compare multiple optimizers
+  - nonlinear solvers relying on automatic differentiation (exact derivative)
+- additional information about the optimum to ease troubleshooting
+- real-time control loop features:
+  - implementations that carefully limits the allocations
+  - simple soft real-time utilities
 
 ### State Estimation Features
 
-- [x] supported state estimators/observers:
-  - [x] steady-state Kalman filter
-  - [x] Kalman filter
-  - [x] Luenberger observer
-  - [x] internal model structure
-  - [x] extended Kalman filter
-  - [x] unscented Kalman filter
-  - [x] moving horizon estimator
-- [x] easily estimate unmeasured disturbances by adding one or more integrators at the:
-  - [x] manipulated inputs
-  - [x] measured outputs
-- [x] bumpless manual to automatic transfer for control with a proper initial estimate
-- [x] estimators in two possible forms:
-  - [x] filter (or current) form to improve accuracy and robustness
-  - [x] predictor (or delayed) form to reduce computational load
-- [x] moving horizon estimator in two formulations:
-  - [x] linear plant models (quadratic optimization)
-  - [x] nonlinear plant models (nonlinear optimization)
-- [x] moving horizon estimator online-tunable soft and hard constraints on:
-  - [x] state estimates
-  - [x] process noise estimates
-  - [x] sensor noise estimates
+- supported state estimators/observers:
+  - steady-state Kalman filter
+  - Kalman filter
+  - Luenberger observer
+  - internal model structure
+  - extended Kalman filter
+  - unscented Kalman filter
+  - moving horizon estimator
+- easily estimate unmeasured disturbances by adding one or more integrators at the:
+  - manipulated inputs
+  - measured outputs
+- bumpless manual to automatic transfer for control with a proper initial estimate
+- estimators in two possible forms:
+  - filter (or current) form to improve accuracy and robustness
+  - predictor (or delayed) form to reduce computational load
+- moving horizon estimator in two formulations:
+  - linear plant models (quadratic optimization)
+  - nonlinear plant models (nonlinear optimization)
+- moving horizon estimator online-tunable soft and hard constraints on:
+  - state estimates
+  - process noise estimates
+  - sensor noise estimates

docs/src/public/predictive_control.md

@@ -12,8 +12,9 @@ assumes by default that the current model mismatch estimation is constant in the
 (same approach as dynamic matrix control, DMC).
 
 !!! info
-    The nomenclature uses capital letters for time series (and matrices) and hats for the
-    predictions (and estimations, for state estimators).
+    The nomenclature uses boldfaces for vectors or matrices, capital boldface letters for
+    vectors representing time series (and also for matrices), and hats for the predictions
+    (and also for the observer estimations).
 
 To be precise, at the ``k``th control period, the vectors that encompass the future measured
 disturbances ``\mathbf{d̂}``, model outputs ``\mathbf{ŷ}`` and setpoints ``\mathbf{r̂_y}``
@@ -31,7 +32,9 @@ over the prediction horizon ``H_p`` are defined as:
     \end{bmatrix}
 ```
 
-The vectors for the manipulated input ``\mathbf{u}`` are shifted by one time step:
+in which ``\mathbf{D̂}``, ``\mathbf{Ŷ}`` and  ``\mathbf{R̂_y}`` are vectors of `nd*Hp`, `ny*Hp`
+and `ny*Hp` elements, respectively. The vectors for the manipulated input ``\mathbf{u}``
+are shifted by one time step:
 
 ```math
     \mathbf{U} = \begin{bmatrix}
@@ -42,10 +45,10 @@ The vectors for the manipulated input ``\mathbf{u}`` are shifted by one time ste
     \end{bmatrix}
 ```
 
-Defining the manipulated input increment as ``\mathbf{Δu}(k+j) =
-\mathbf{u}(k+j) - \mathbf{u}(k+j-1)``, the control horizon ``H_c`` enforces that
-``\mathbf{Δu}(k+j) = \mathbf{0}`` for ``j ≥ H_c``. For this reason, the vector that collects
-them is truncated up to ``k+H_c-1``:
+in which ``\mathbf{U}`` and ``\mathbf{R̂_u}`` are both vectors of `nu*Hp` elements. Defining
+the manipulated input increment as ``\mathbf{Δu}(k+j) = \mathbf{u}(k+j) - \mathbf{u}(k+j-1)``,
+the control horizon ``H_c`` enforces that ``\mathbf{Δu}(k+j) = \mathbf{0}`` for ``j ≥ H_c``.
+For this reason, the vector that collects them is truncated up to ``k+H_c-1``:
 
 ```math
     \mathbf{ΔU} =
@@ -54,6 +57,8 @@ them is truncated up to ``k+H_c-1``:
     \end{bmatrix}
 ```
 
+in which ``\mathbf{ΔU}`` is a vector of `nu*Hc` elements.
+
 ## PredictiveController
 
 ```@docs

docs/src/public/sim_model.md

@@ -11,6 +11,12 @@ simulators by calling [`evaloutput`](@ref) and [`updatestate!`](@ref) methods on
 the states ``\mathbf{x}`` are stored inside [`SimModel`](@ref) instances. Use [`setstate!`](@ref)
 method to manually modify them.
 
+!!! info
+    The nomenclature in this page introduces the model manipulated input ``\mathbf{u}``,
+    measured disturbances ``\mathbf{d}``, state ``\mathbf{x}`` and output ``\mathbf{y}``,
+    four vectors of `nu`, `nd`, `nx` and `ny` elements, respectively. The ``\mathbf{z}``
+    vector combines the elements of ``\mathbf{u}`` and ``\mathbf{d}``.
+
 ## SimModel
 
 ```@docs

docs/src/public/state_estim.md

@@ -28,8 +28,10 @@ solving the MPC problem with [`moveinput!`](@ref), for when the estimations are
 [^1]: also denoted ``\mathbf{x̂}_{k|k}`` [elsewhere](https://en.wikipedia.org/wiki/Kalman_filter).
 
 !!! info
-    All the estimators support measured ``\mathbf{y^m}`` and unmeasured ``\mathbf{y^u}``
-    model outputs, where ``\mathbf{y}`` refers to all of them.
+    The nomenclature in this page introduces the estimated state ``\mathbf{x̂}`` and output
+    ``\mathbf{ŷ}`` vectors of respectively `nx̂` and `ny` elements. Also, all the estimators
+    support measured ``\mathbf{y^m}`` (`nym` elements) and unmeasured ``\mathbf{y^u}``
+    (`nyu` elements) model output, where ``\mathbf{y}`` refers to all of them.
 
 ## StateEstimator
 

src/controller/execute.jl

@@ -120,8 +120,8 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     Ȳ, Ū        = similar(mpc.Yop), similar(mpc.Uop)
     Ŷe, Ue      = Vector{NT}(undef, nŶe), Vector{NT}(undef, nUe)
     Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, mpc.ΔŨ)
-    Ŷe, Ue    = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, mpc.ΔŨ)
-    J         = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷe, Ue, mpc.ΔŨ)
+    Ue, Ŷe    = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, mpc.ΔŨ)
+    J         = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, mpc.ΔŨ)
     U  = Ū
     U .= @views Ue[1:end-model.nu]
     Ŷ  = Ȳ
@@ -362,28 +362,28 @@ function predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, mod
 end
 
 """
-    extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ) -> Ŷe, Ue
+    extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) -> Ŷe, Ue
 
-Compute the extended predictions `Ŷe` and `Ue` for the nonlinear optimization.
+Compute the extended vectors `Ue` and `Ŷe` and  for the nonlinear optimization.
 
-The function mutates `Ŷe`, `Ue` and `Ū` in arguments, without assuming any initial values.
+The function mutates `Ue`, `Ŷe` and `Ū` in arguments, without assuming any initial values.
 """
-function extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
+function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
     ny, nu = model.ny, model.nu
-    # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
-    Ŷe[1:ny]     .= mpc.ŷ
-    Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
     # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
     U0 = Ū
     U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ mpc.T_lastu0
     Ue[1:end-nu] .= U0 .+ mpc.Uop
     # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
     Ue[end-nu+1:end] .= @views Ue[end-2nu+1:end-nu]
-    return Ŷe, Ue
+    # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
+    Ŷe[1:ny]     .= mpc.ŷ
+    Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
+    return Ue, Ŷe
 end
 
 """
-    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ŷe, Ue, ΔŨ)
+    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ)
 
 Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
 
@@ -392,23 +392,23 @@ also be called on any [`PredictiveController`](@ref)s to evaluate the objective
 at specific `Ue`, `Ŷe` and `ΔŨ`, values. It does not mutate any argument.
 """
 function obj_nonlinprog!(
-    _, _, mpc::PredictiveController, model::LinModel, Ŷe, Ue, ΔŨ::AbstractVector{NT}
+    _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
     JQP  = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
-    E_JE = obj_econ!(Ue, Ŷe, mpc, model)
+    E_JE = obj_econ(mpc, model, Ue, Ŷe)
     return JQP + E_JE
 end
 
 """
-    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷe, Ue, ΔŨ)
+    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ)
 
 Nonlinear programming objective method when `model` is not a [`LinModel`](@ref). The
 function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates
 `Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in
 `Ŷe` and `Ue` arguments).
 """
 function obj_nonlinprog!(
-    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷe, Ue, ΔŨ::AbstractVector{NT}
+    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
 ) where NT<:Real
     nu, ny = model.nu, model.ny
     # --- output setpoint tracking term ---
@@ -426,12 +426,12 @@ function obj_nonlinprog!(
         JR̂u = 0.0
     end
     # --- economic term ---
-    E_JE = obj_econ!(Ue, Ŷe, mpc, model)
+    E_JE = obj_econ(mpc, model, Ue, Ŷe)
     return JR̂y + JΔŨ + JR̂u + E_JE
 end
 
 "By default, the economic term is zero."
-obj_econ!( _ , _ , ::PredictiveController, ::SimModel) = 0.0
+obj_econ(::PredictiveController, ::SimModel, _ , _ ) = 0.0
 
 @doc raw"""
     optim_objective!(mpc::PredictiveController) -> ΔŨ

src/controller/linmpc.jl

@@ -175,6 +175,7 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil
     | ``H_p``              | prediction horizon (integer)                             | `()`             |
     | ``H_c``              | control horizon (integer)                                | `()`             |
     | ``\mathbf{ΔU}``      | manipulated input increments over ``H_c``                | `(nu*Hc,)`       |
+    | ``\mathbf{D̂}``       | predicted measured disturbances over ``H_p``             | `(nd*Hp,)`       |
     | ``\mathbf{Ŷ}``       | predicted outputs over ``H_p``                           | `(ny*Hp,)`       |
     | ``\mathbf{U}``       | manipulated inputs over ``H_p``                          | `(nu*Hp,)`       |
     | ``\mathbf{R̂_y}``     | predicted output setpoints over ``H_p``                  | `(ny*Hp,)`       |