added : NonLinModel mpc objective function

Author: franckgaga

example/juMPC.jl

@@ -38,7 +38,7 @@ f2(x,u,d) = A*x + Bu*u + Bd*d
 h2(x,_) = C*x
 
 nonLinModel1 = NonLinModel(f,h,Ts,2,4,2)
-nonLinModel2 = NonLinModel(f2,h2,Ts,2,4,2,1)
+nonLinModel2 = setop!(NonLinModel(f2,h2,Ts,2,4,2,1), uop=[10,10],yop=[50,30],dop=[5])
 
 internalModel1 = InternalModel(linModel1)
 internalModel2 = InternalModel(linModel1,stoch_ym=[tf([1,0],[1,-1],Ts) 0; 0 tf([1,0],[1,-1],Ts)])
@@ -69,7 +69,13 @@ initstate!(uscKalmanFilter1,[0,0],[2,1])
 
 nmpc1 = NonLinMPC(uscKalmanFilter1)
 
-nmpc2 = NonLinMPC(nonLinModel1)
+nmpc2 = NonLinMPC(nonLinModel2, Hp=15, Hc=1, Mwt=[1, 1] , Nwt=[0.1, 0.1], Cwt=1e5)
+
+setconstraint!(nmpc2, c_umin=[0,0], c_umax=[0,0])
+setconstraint!(nmpc2, c_ŷmin=[1,1], c_ŷmax=[1,1])
+setconstraint!(nmpc2, umin=[5, 9.9], umax=[Inf,Inf])
+setconstraint!(nmpc2, ŷmin=[-Inf,-Inf], ŷmax=[55, 35])
+setconstraint!(nmpc2, Δumin=[-Inf,-Inf],Δumax=[+Inf,+Inf])
 
 
 nx = linModel4.nx
@@ -127,11 +133,13 @@ function test_mpc(model, mpc)
     return u_data, y_data, r_data, d_data
 end
 
-@time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
+#@time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
+
+#@time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)
+
+@time u_data, y_data, r_data, d_data = test_mpc(nonLinModel2, nmpc2)
 
-@time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)
 
-#=
 using PlotThemes, Plots
 #theme(:default)
 theme(:dark)
@@ -152,5 +160,5 @@ pd = plot(0:N-1,d_data[1,:],label=raw"$d_1$")
 display(pd)
 display(pu)
 display(py)
-=#
+
 

src/controller/linmpc.jl

@@ -13,6 +13,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
     L_Hp::Diagonal{Float64, Vector{Float64}}
     C::Float64
     R̂u::Vector{Float64}
+    R̂y::Vector{Float64}
     S̃_Hp::Matrix{Bool}
     T_Hp::Matrix{Bool}
     T_Hc::Matrix{Bool}
@@ -41,7 +42,8 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
         L_Hp = Diagonal{Float64}(repeat(Lwt, Hp))
         C = Cwt
         # manipulated input setpoint predictions are constant over Hp :
-        R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[] 
+        R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[]
+        R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
         S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
         con, S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
@@ -55,7 +57,7 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
             estim, optim, con,
             ΔŨ, x̂d, x̂s, ŷ,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, Cwt, R̂u,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, Kd, Q, P̃, q̃,
             Ks, Ps,

src/controller/nonlinmpc.jl

@@ -15,6 +15,7 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
     E::Float64
     JE::JEFunc
     R̂u::Vector{Float64}
+    R̂y::Vector{Float64}
     S̃_Hp::Matrix{Bool}
     T_Hp::Matrix{Bool}
     T_Hc::Matrix{Bool}
@@ -46,6 +47,7 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
         C = Cwt
         # manipulated input setpoint predictions are constant over Hp :
         R̂u = ~iszero(Lwt) ? repeat(ru, Hp) : R̂u = Float64[] 
+        R̂y = zeros(ny* Hp) # dummy R̂y (updated just before optimization)
         S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
         con, S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
@@ -59,7 +61,7 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
             estim, optim, con,
             ΔŨ, x̂d, x̂s, ŷ,
             Hp, Hc, 
-            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u,
+            M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, Kd, Q, P̃, q̃,
             Ks, Ps,
@@ -189,17 +191,47 @@ init_objective!(mpc::NonLinMPC, _ ) = nothing
 
 function obj_nonlinprog(mpc::NonLinMPC, model::LinModel, ΔŨ::NTuple{N, T}) where {T, N}
     ΔŨ = collect(ΔŨ) # convert NTuple to Vector
+    Jqp = obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃)
     U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + model.uop)
     UE = [U; U[end-model.nu+1:end]]
     ŶE = [mpc.ŷ; mpc.Ẽ*ΔŨ + mpc.F]
-    mpc.D̂0 + mpc.Dop
     D̂E = [mpc.d0 + model.dop; mpc.D̂0 + mpc.Dop]
-    return obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃) + mpc.JE(UE, ŶE, D̂E)
+    return Jqp + mpc.JE(UE, ŶE, D̂E)
 end
 
 
 function obj_nonlinprog(mpc::NonLinMPC, model::SimModel, ΔŨ::NTuple{N, T}) where {T,N}
-    J = 0.0
-    #println("yoSimModel")
-    return J
+    ΔŨ = collect(ΔŨ) # convert NTuple to Vector
+    U0 = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0)
+    # --- output setpoint tracking term ---
+    Ŷd0 = Vector{T}(undef, model.ny*mpc.Hp)
+    x̂d = mpc.x̂d
+    d0 = mpc.d0
+    for i=1:mpc.Hp
+        u0 = U0[(1 + model.nu*(i-1)):(model.nu*i)]
+        x̂d = model.f(x̂d, u0, d0)
+        d0 = mpc.D̂0[(1 + model.nd*(i-1)):(model.nd*i)]
+        ŷd0 = model.h(x̂d, d0)
+        Ŷd0[(1 + model.ny*(i-1)):(model.ny*i)] = ŷd0
+    end
+    Ŷ = Ŷd0 + mpc.F
+    êy = mpc.R̂y - Ŷ
+    JR̂y = êy'*mpc.M_Hp*êy  
+    # --- move suppression term ---
+    JΔŨ = ΔŨ'*mpc.Ñ_Hc*ΔŨ 
+    # --- input setpoint tracking term ---
+    U = U0 + mpc.Uop
+    if !isempty(mpc.R̂u)
+        êu = mpc.R̂u - U
+        JR̂u = êu'*mpc.L_Hp*ê
+    else
+        JR̂u = 0.0
+    end
+    # --- slack variable term ---
+    Jϵ = !isinf(mpc.C) ? mpc.C*ΔŨ[end] : 0.0
+    # --- economic term ---
+    UE = [U; U[end-model.nu+1:end]]
+    ŶE = [mpc.ŷ; Ŷ]
+    D̂E = [mpc.d0 + model.dop; mpc.D̂0 + mpc.Dop]
+    return JR̂y + JΔŨ + JR̂u + Jϵ + mpc.JE(UE, ŶE, D̂E)
 end

src/predictive_control.jl

@@ -342,6 +342,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, Ŷs, R̂
         mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
         mpc.F[:] = mpc.F + mpc.G*mpc.d0 + mpc.J*mpc.D̂0
     end
+    mpc.R̂y[:] = R̂y
     Ẑ = mpc.F - R̂y
     mpc.q̃[:] = 2(mpc.M_Hp*mpc.Ẽ)'*Ẑ
     p = Ẑ'*mpc.M_Hp*Ẑ
@@ -355,20 +356,21 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, Ŷs, R̂
 end
 
 @doc raw"""
-    initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs, _ )
+    initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs, R̂y )
 
 Init `F`, `d0` and `D̂0` prediction matrices for [`NonLinModel`](@ref).
 
-For [`NonLinModel`](@ref), the constant matrix `F` is ``\mathbf{F = Ŷ_s + Y_{op}}``, thus it
+For [`NonLinModel`](@ref), the constant matrix ``\mathbf{F = Ŷ_s + Y_{op}}``, thus it
 incorporates the stochastic predictions and the output operating point ``\mathbf{y_{op}}`` 
-repeated over ``H_p``. `d0` and `D̂0` are the measured disturbances and the predictions 
+repeated over ``H_p``. `d0` and `D̂0` are the measured disturbances and its predictions 
 without the operating points ``\mathbf{d_{op}}``.
 """
-function initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs , _ )
+function initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs , R̂y )
     mpc.F[:] = Ŷs + mpc.Yop
     if model.nd ≠ 0
         mpc.d0[:], mpc.D̂0[:] = d - model.dop, D̂ - mpc.Dop
     end
+    mpc.R̂y[:] = R̂y
     p = 0.0 # only used for LinModel objects
     return p
 end