debug : sim! call on ExplicitMPC now works

Author: franckgaga

README.md

@@ -49,7 +49,7 @@ Our goal is controlling the first output $y_1$, but the second one $y_2$ should
 
 ```julia
 mpc = LinMPC(model, Mwt=[1, 0], Nwt=[0.1])
-mpc = setconstraint!(mpc, ŷmax=[Inf, 35])
+mpc = setconstraint!(mpc, ymax=[Inf, 35])
 ```
 
 The keyword arguments `Mwt` and `Nwt` are the output setpoint tracking and move suppression
@@ -60,7 +60,7 @@ The result is displayed with [`Plots.jl`](https://github.com/JuliaPlots/Plots.jl
 using Plots
 ry = [5, 0]
 res = sim!(mpc, 40, ry)
-plot(res, plotry=true, plotŷmax=true)
+plot(res, plotry=true, plotymax=true)
 ```
 
 ![StepChangeResponse](/docs/src/assets/readme_result.svg)

docs/src/internals/predictive_control.md

@@ -1,5 +1,9 @@
 # Functions: PredictiveController Internals
 
+```@contents
+Pages = ["predictive_control.md"]
+```
+
 The prediction methodology of this module is mainly based on Maciejowski textbook [^1].
 
 [^1]: Maciejowski, J. 2000, "Predictive control : with constraints", 1st ed., Prentice Hall,

docs/src/internals/sim_model.md

@@ -1,5 +1,9 @@
 # Functions: SimModel Internals
 
+```@contents
+Pages = ["sim_model.md"]
+```
+
 ```@docs
 ModelPredictiveControl.steadystate
 ModelPredictiveControl.f

docs/src/internals/state_estim.md

@@ -1,5 +1,9 @@
 # Functions: StateEstimator Internals
 
+```@contents
+Pages = ["state_estim.md"]
+```
+
 ## Estimator Initialization
 
 ```@docs

docs/src/manual/installation.md

@@ -6,6 +6,15 @@ To install the `ModelPredictiveControl` package, run this command in the Julia R
 using Pkg; Pkg.add("ModelPredictiveControl")
 ```
 
+Doing so will install the package to default Julia environnement, that is, accessible
+anywhere. To facilitate sharing of code and reproducibility of results, it is recommended to
+install packages in a project environnement. To generate a new project named `MyProject`
+with this package in the current working directory, write this in the REPL:
+
+```julia
+using Pkg; Pkg.generate("MyProject"); Pkg.activate("."); Pkg.add("ModelPredictiveControl")
+```
+
 Note that that the construction of linear models typically requires `ss` or `tf` functions,
 it is thus recommended to load the package with:
 

docs/src/manual/linmpc.md

@@ -66,16 +66,17 @@ We design our [`LinMPC`](@ref) controllers by including the linear level constra
 [`setconstraint!`](@ref) (`±Inf` values should be used when there is no bound):
 
 ```@example 1
-mpc = setconstraint!(LinMPC(model, Hp=15, Hc=2), ŷmin=[45, -Inf])
+mpc = setconstraint!(LinMPC(model, Hp=15, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1]), ymin=[45, -Inf])
 ```
 
-in which `Hp` and `Hc` keyword arguments are the predictive and control horizons
-respectively. By default, [`LinMPC`](@ref) controllers use [`OSQP`](https://osqp.org/) to
-solve the problem, soft constraints on output predictions ``\mathbf{ŷ}`` to ensure
-feasibility, and a [`SteadyKalmanFilter`](@ref) to estimate the plant states. An attentive
-reader will also notice that the Kalman filter estimates two additional states compared to
-the plant model. These are the integrating states for the unmeasured plant disturbances, and
-they are automatically added the model outputs if feasible (see [`SteadyKalmanFilter`](@ref)
+in which `Hp`, `Hc` keyword arguments are respectively the predictive and control horizons,
+and `Mwt` and `Nwt`, the output setpoint tracking and move suppression weights. By default,
+[`LinMPC`](@ref) controllers use [`OSQP`](https://osqp.org/) to solve the problem, soft
+constraints on output predictions ``\mathbf{ŷ}`` to ensure feasibility, and a
+[`SteadyKalmanFilter`](@ref) to estimate the plant states. An attentive reader will also
+notice that the Kalman filter estimates two additional states compared to the plant model.
+These are the integrating states for the unmeasured plant disturbances, and they are
+automatically added to the model outputs by default if feasible (see [`SteadyKalmanFilter`](@ref)
 for details).
 
 Before closing the loop, we call [`initstate!`](@ref) with the actual plant inputs and
@@ -157,7 +158,7 @@ real-life control problems. Constructing a [`LinMPC`](@ref) with `DAQP` and inpu
 using JuMP, DAQP
 daqp  = Model(DAQP.Optimizer)
 estim = SteadyKalmanFilter(model, nint_u=[1, 1], nint_ym=[0, 0])
-mpc2  = setconstraint!(LinMPC(estim, Hp=15, Hc=2, optim=daqp), ŷmin=[45, -Inf])
+mpc2  = setconstraint!(LinMPC(estim, Hp=15, Hc=2, optim=daqp), ymin=[45, -Inf])
 ```
 
 leads to similar computational times, but it does accelerate the rejection of the load
@@ -169,4 +170,3 @@ initstate!(mpc2, model.uop, model())
 u_data2, y_data2, ry_data2 = test_mpc(mpc2, model)
 plot_data(t_data, u_data2, y_data2, ry_data2)
 ```
-

src/controller/explicitmpc.jl

@@ -24,6 +24,7 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
     P̃::Hermitian{Float64, Matrix{Float64}}
     q̃::Vector{Float64}
     p::Vector{Float64}
+    P̃_chol::Cholesky{Float64, Matrix{Float64}}
     Ks::Matrix{Float64}
     Ps::Matrix{Float64}
     d::Vector{Float64}
@@ -48,6 +49,7 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
         E, F, G, J, K, Q = init_predmat(estim, model, Hp, Hc)
         _ , S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
+        P̃_chol = cholesky(P̃)
         Ks, Ps = init_stochpred(estim, Hp)
         d, D̂ = zeros(nd), zeros(nd*Hp)
         Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
@@ -60,6 +62,7 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, K, Q, P̃, q̃, p,
+            P̃_chol,
             Ks, Ps,
             d, D̂,
             Yop, Dop,
@@ -177,8 +180,7 @@ linconstraint!(::ExplicitMPC, ::LinModel) = nothing
 Analytically solve the optimization problem for [`ExplicitMPC`](@ref).
 """
 function optim_objective!(mpc::ExplicitMPC)
-    mpc.ΔŨ[:] = -mpc.P̃\mpc.q̃
-    return mpc.ΔŨ
+    return ldiv!(mpc.ΔŨ, mpc.P̃_chol, -mpc.q̃)
 end
 
 "For [`ExplicitMPC`](@ref), return an empty summary."

src/controller/nonlinmpc.jl

@@ -285,13 +285,13 @@ function init_optimization!(mpc::NonLinMPC)
     @NLobjective(optim, Min, Jfunc(ΔŨvar...))
     if nC ≠ 0
         n = 0
-        for i in eachindex(con.Ŷmin)
-            sym = Symbol("C_Ŷmin_$i")
+        for i in eachindex(con.Ymin)
+            sym = Symbol("C_Ymin_$i")
             register(optim, sym, nvar, Cfunc[n + i], autodiff=true)
         end
-        n = lastindex(con.Ŷmin)
-        for i in eachindex(con.Ŷmax)
-            sym = Symbol("C_Ŷmax_$i")
+        n = lastindex(con.Ymin)
+        for i in eachindex(con.Ymax)
+            sym = Symbol("C_Ymax_$i")
             register(optim, sym, nvar, Cfunc[n + i], autodiff=true)
         end
     end
@@ -308,12 +308,12 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
     ΔŨvar = mpc.optim[:ΔŨvar]
     con = mpc.con
     map(con -> delete(optim, con), all_nonlinear_constraints(optim))
-    for i in findall(.!isinf.(con.Ŷmin))
-        f_sym = Symbol("C_Ŷmin_$(i)")
+    for i in findall(.!isinf.(con.Ymin))
+        f_sym = Symbol("C_Ymin_$(i)")
         add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
     end
-    for i in findall(.!isinf.(con.Ŷmax))
-        f_sym = Symbol("C_Ŷmax_$(i)")
+    for i in findall(.!isinf.(con.Ymax))
+        f_sym = Symbol("C_Ymax_$(i)")
         add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
     end
     return nothing
@@ -389,15 +389,15 @@ Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](
 function con_nonlinprog(mpc::NonLinMPC, ::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
     if !isinf(mpc.C) # constraint softening activated :
         ϵ = ΔŨ[end]
-        C_Ŷmin = (mpc.con.Ŷmin - Ŷ) - ϵ*mpc.con.c_Ŷmin
-        C_Ŷmax = (Ŷ - mpc.con.Ŷmax) - ϵ*mpc.con.c_Ŷmax
+        C_Ymin = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
+        C_Ymax = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
     else # no constraint softening :
-        C_Ŷmin = (mpc.con.Ŷmin - Ŷ)
-        C_Ŷmax = (Ŷ - mpc.con.Ŷmax)
+        C_Ymin = (mpc.con.Ymin - Ŷ)
+        C_Ymax = (Ŷ - mpc.con.Ymax)
     end
     # replace -Inf with 0 to avoid INVALID_MODEL error :
-    C_Ŷmin[isinf.(C_Ŷmin)] .= 0
-    C_Ŷmax[isinf.(C_Ŷmax)] .= 0
-    C = [C_Ŷmin; C_Ŷmax]
+    C_Ymin[isinf.(C_Ymin)] .= 0
+    C_Ymax[isinf.(C_Ymax)] .= 0
+    C = [C_Ymin; C_Ymax]
     return C
 end

src/plot_sim.jl

@@ -131,11 +131,11 @@ identical to [`sim!(::StateEstimator, ::Int)`](@ref).
 ```julia-repl
 julia> model = LinModel([tf(3, [30, 1]); tf(2, [5, 1])], 4);
 
-julia> mpc = setconstraint!(LinMPC(model, Mwt=[0, 1], Nwt=[0.01], Hp=30), ŷmin=[0, -Inf]);
+julia> mpc = setconstraint!(LinMPC(model, Mwt=[0, 1], Nwt=[0.01], Hp=30), ymin=[0, -Inf]);
 
 julia> res = sim!(mpc, 25, [0, 0], y_noise=[0.1], y_step=[-10, 0]);
 
-julia> using Plots; plot(res, plotry=true, plotŷ=true, plotŷmin=true, plotu=true)
+julia> using Plots; plot(res, plotry=true, plotŷ=true, plotymin=true, plotu=true)
 
 ```
 """
@@ -407,8 +407,8 @@ end
 @recipe function plot(
     res::SimResult{<:PredictiveController}; 
     plotry      = true,
-    plotŷmin    = true,
-    plotŷmax    = true,
+    plotymin    = true,
+    plotymax    = true,
     plotŷ       = false,
     plotu       = true,
     plotru      = true,
@@ -433,6 +433,9 @@ end
     (plotx̂ ||  plotxwithx̂) && (layout_mat = [layout_mat (nx̂, 1)])
     layout := layout_mat
     xguide --> "Time (s)"
+    # --- constraints ---
+    Umin, Umax = getUcon(mpc, nu)
+    Ymin, Ymax = getYcon(mpc, ny)
     # --- outputs y ---
     subplot_base = 0
     for i in 1:ny
@@ -468,7 +471,7 @@ end
                 t, res.Ry_data[i, :]
             end
         end
-        if plotŷmin && !isinf(mpc.con.Ŷmin[i])
+        if plotymin && !isinf(Ymin[i])
             @series begin
                 yguide    --> "\$y_$i\$"
                 color     --> 4
@@ -477,10 +480,10 @@ end
                 linewidth --> 1.5
                 label     --> "\$\\mathbf{\\hat{y}_{min}}\$"
                 legend    --> true
-                t, fill(mpc.con.Ŷmin[i], length(t))
+                t, fill(Ymin[i], length(t))
             end
         end
-        if plotŷmax && !isinf(mpc.con.Ŷmax[i])
+        if plotymax && !isinf(Ymax[i])
             @series begin
                 yguide    --> "\$y_$i\$"
                 color     --> 5
@@ -489,7 +492,7 @@ end
                 linewidth --> 1.5
                 label     --> "\$\\mathbf{\\hat{y}_{max}}\$"
                 legend    --> true
-                t, fill(mpc.con.Ŷmax[i], length(t))
+                t, fill(Ymax[i], length(t))
             end
         end
     end
@@ -518,7 +521,7 @@ end
                     t, res.Ry_data[i, :]
                 end
             end
-            if plotumin && !isinf(mpc.con.Umin[i])
+            if plotumin && !isinf(Umin[i])
                 @series begin
                     yguide    --> "\$u_$i\$"
                     color     --> 4
@@ -527,10 +530,10 @@ end
                     linewidth --> 1.5
                     label     --> "\$\\mathbf{u_{min}}\$"
                     legend    --> true
-                    t, fill(mpc.con.Umin[i], length(t))
+                    t, fill(Umin[i], length(t))
                 end
             end
-            if plotumax && !isinf(mpc.con.Umax[i])
+            if plotumax && !isinf(Umax[i])
                 @series begin
                     yguide    --> "\$u_$i\$"
                     color     --> 5
@@ -539,7 +542,7 @@ end
                     linewidth --> 1.5
                     label     --> "\$\\mathbf{u_{max}}\$"
                     legend    --> true
-                    t, fill(mpc.con.Umax[i], length(t))
+                    t, fill(Umax[i], length(t))
                 end
             end
         end
@@ -590,4 +593,10 @@ end
             end
         end
     end
-end
+end
+
+getUcon(mpc::PredictiveController, _ ) = mpc.con.Umin, mpc.con.Umax
+getUcon(mpc::ExplicitMPC, nu) = fill(-Inf, mpc.Hp*nu), fill(+Inf, mpc.Hp*nu)
+
+getYcon(mpc::PredictiveController, _ ) = mpc.con.Ymin, mpc.con.Ymax
+getYcon(mpc::ExplicitMPC, ny) = fill(-Inf, mpc.Hp*ny), fill(+Inf, mpc.Hp*ny)

src/precompile_calls.jl

@@ -4,43 +4,43 @@ Ts = 4.0
 model = setop!(LinModel(sys, Ts), uop=[10, 10], yop=[50, 30])
 y = model()
 
-mpc_im = setconstraint!(LinMPC(InternalModel(model)), ŷmin=[45, -Inf])
+mpc_im = setconstraint!(LinMPC(InternalModel(model)), ymin=[45, -Inf])
 initstate!(mpc_im, model.uop, y)
 mpc_im.estim([50, 30])
 u = mpc_im([55, 30], ym=y)
 sim!(mpc_im, 3)
 
-mpc_kf = setconstraint!(LinMPC(KalmanFilter(model)), ŷmin=[45, -Inf])
+mpc_kf = setconstraint!(LinMPC(KalmanFilter(model)), ymin=[45, -Inf])
 initstate!(mpc_kf, model.uop, model())
 mpc_kf.estim()
 u = mpc_kf([55, 30])
 sim!(mpc_kf, 3, [55, 30])
 
-mpc_lo = setconstraint!(LinMPC(Luenberger(model)), ŷmin=[45, -Inf])
+mpc_lo = setconstraint!(LinMPC(Luenberger(model)), ymin=[45, -Inf])
 initstate!(mpc_lo, model.uop, model())
 mpc_lo.estim()
 u = mpc_lo([55, 30])
 sim!(mpc_lo, 3, [55, 30])
 
-mpc_ukf = setconstraint!(LinMPC(UnscentedKalmanFilter(model)), ŷmin=[45, -Inf])
+mpc_ukf = setconstraint!(LinMPC(UnscentedKalmanFilter(model)), ymin=[45, -Inf])
 initstate!(mpc_ukf, model.uop, model())
 mpc_ukf.estim()
 u = mpc_ukf([55, 3])
 sim!(mpc_ukf, 3, [55, 30])
 
-mpc_ekf = setconstraint!(LinMPC(ExtendedKalmanFilter(model)), ŷmin=[45, -Inf])
+mpc_ekf = setconstraint!(LinMPC(ExtendedKalmanFilter(model)), ymin=[45, -Inf])
 initstate!(mpc_ekf, model.uop, model())
 mpc_ekf.estim()
 u = mpc_ekf([55, 30])
 sim!(mpc_ekf, 3, [55, 30])
 
-mpc_skf = setconstraint!(LinMPC(SteadyKalmanFilter(model)), ŷmin=[45, -Inf])
+mpc_skf = setconstraint!(LinMPC(SteadyKalmanFilter(model)), ymin=[45, -Inf])
 initstate!(mpc_skf, model.uop, model())
 mpc_skf.estim()
 u = mpc_skf([55, 30])
 sim!(mpc_skf, 3, [55, 30])
 
-nmpc_skf = setconstraint!(NonLinMPC(SteadyKalmanFilter(model), Cwt=Inf), ŷmin=[45, -Inf])
+nmpc_skf = setconstraint!(NonLinMPC(SteadyKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf])
 initstate!(nmpc_skf, model.uop, model())
 nmpc_skf.estim()
 u = nmpc_skf([55, 30])
@@ -59,17 +59,17 @@ h(x,_) = model.C*x
 
 nlmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2), uop=[10, 10], yop=[50, 30])
 y = nlmodel()
-nmpc_im = setconstraint!(NonLinMPC(InternalModel(nlmodel), Cwt=Inf), ŷmin=[45, -Inf])
+nmpc_im = setconstraint!(NonLinMPC(InternalModel(nlmodel), Cwt=Inf), ymin=[45, -Inf])
 initstate!(nmpc_im, nlmodel.uop, y)
 u = nmpc_im([55, 30], ym=y)
 sim!(nmpc_im, 3, [55, 30])
 
-nmpc_ukf = setconstraint!(NonLinMPC(UnscentedKalmanFilter(nlmodel), Cwt=Inf), ŷmin=[45, -Inf])
+nmpc_ukf = setconstraint!(NonLinMPC(UnscentedKalmanFilter(nlmodel), Cwt=Inf), ymin=[45, -Inf])
 initstate!(nmpc_ukf, nlmodel.uop, y)
 u = nmpc_ukf([55, 30])
 sim!(nmpc_ukf, 3, [55, 30])
 
-nmpc_ekf = setconstraint!(NonLinMPC(ExtendedKalmanFilter(model), Cwt=Inf), ŷmin=[45, -Inf])
+nmpc_ekf = setconstraint!(NonLinMPC(ExtendedKalmanFilter(model), Cwt=Inf), ymin=[45, -Inf])
 initstate!(nmpc_ekf, model.uop, model())
 u = nmpc_ekf([55, 30])
 sim!(nmpc_ekf, 3, [55, 30])