added: support for parameter p in economic cost JE of NonLinMPC

They are identical to the plant model parameter by default.

Author: franckgaga

docs/src/manual/nonlinmpc.md

@@ -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

@@ -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/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,9 +48,9 @@ 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)
@@ -77,11 +79,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 +111,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 +126,10 @@ 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.
 
 !!! 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 +152,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).
@@ -201,11 +204,12 @@ function NonLinMPC(
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     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 +224,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
 
 
@@ -265,14 +270,17 @@ function NonLinMPC(
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
     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
 
 """
@@ -285,7 +293,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 +465,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

src/precompile.jl

@@ -74,10 +74,13 @@ exmpc.estim()
 u = exmpc([55, 30])
 sim!(exmpc, 3, [55, 30])
 
-f(x,u,_,_) = model.A*x + model.Bu*u
-h(x,_,_) = model.C*x
+f(x,u,_,model) = model.A*x + model.Bu*u
+h(x,_,model) = model.C*x
 
-nlmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10, 10], yop=[50, 30])
+nlmodel = setop!(
+    NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=model), 
+    uop=[10, 10], yop=[50, 30]
+)
 y = nlmodel()
 nmpc_im = setconstraint!(NonLinMPC(InternalModel(nlmodel), Hp=10, Cwt=Inf), ymin=[45, -Inf])
 initstate!(nmpc_im, nlmodel.uop, y)
@@ -86,7 +89,9 @@ nmpc_im.estim()
 u = nmpc_im([55, 30])
 sim!(nmpc_im, 3, [55, 30])
 
-nmpc_ukf = setconstraint!(NonLinMPC(UnscentedKalmanFilter(nlmodel), Hp=10, Cwt=1e3), ymin=[45, -Inf])
+nmpc_ukf = setconstraint!(
+    NonLinMPC(UnscentedKalmanFilter(nlmodel), Hp=10, Cwt=1e3), ymin=[45, -Inf]
+)
 initstate!(nmpc_ukf, nlmodel.uop, y)
 preparestate!(nmpc_ukf, [55, 30])
 u = nmpc_ukf([55, 30])
@@ -98,19 +103,24 @@ preparestate!(nmpc_ekf, [55, 30])
 u = nmpc_ekf([55, 30])
 sim!(nmpc_ekf, 3, [55, 30])
 
-nmpc_mhe = setconstraint!(NonLinMPC(MovingHorizonEstimator(nlmodel, He=2), Hp=10, Cwt=Inf), ymin=[45, -Inf])
+nmpc_mhe = setconstraint!(
+    NonLinMPC(MovingHorizonEstimator(nlmodel, He=2), Hp=10, Cwt=Inf), ymin=[45, -Inf]
+)
 setconstraint!(nmpc_mhe.estim, x̂min=[-50,-50,-50,-50], x̂max=[50,50,50,50])
 initstate!(nmpc_mhe, nlmodel.uop, y)
 preparestate!(nmpc_mhe, [55, 30])
 u = nmpc_mhe([55, 30])
 sim!(nmpc_mhe, 3, [55, 30])
 
-function JE( _ , ŶE, _ )
-    Ŷ  = ŶE[3:end]
-    Eŷ = repeat([55; 30], 10) - Ŷ
-    return dot(Eŷ, I, Eŷ)
+function JE( _ , ŶE, _ , R̂y)
+    Ŷ = ŶE[3:end]
+    Ȳ = R̂y - Ŷ
+    return dot(Ȳ, Ȳ)
 end
-empc = setconstraint!(NonLinMPC(nlmodel, Mwt=[0, 0], Hp=10, Cwt=Inf, Ewt=1, JE=JE), ymin=[45, -Inf])
+R̂y = repeat([55; 30], 10)
+empc = setconstraint!(
+    NonLinMPC(nlmodel, Mwt=[0, 0], Hp=10, Cwt=Inf, Ewt=1, JE=JE, p=R̂y), ymin=[45, -Inf]
+)
 preparestate!(empc, [55, 30])
 u = empc()
 sim!(empc, 3)

test/test_predictive_control.jl

@@ -491,9 +491,9 @@ end
     @test nmpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
     nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1])
     @test nmpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
-    nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(UE,ŶE,D̂E) -> UE.*ŶE.*D̂E)
+    nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(UE,ŶE,D̂E,p) -> p*UE.*ŶE.*D̂E, p=2)
     @test nmpc7.E == 1e-3
-    @test nmpc7.JE([1,2],[3,4],[4,6]) == [12, 48]
+    @test nmpc7.JE([1,2],[3,4],[4,6],2) == 2*[1,2].*[3.4].*[4,6]
     optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0))
     nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim)
     @test solver_name(nmpc8.optim) == "Ipopt"