changed nomenclature for extended vectors

Author: franckgaga

src/controller/execute.jl

@@ -31,12 +31,12 @@ See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref).
 - `mpc::PredictiveController` : solve optimization problem of `mpc`.
 - `ry=mpc.estim.model.yop` : current output setpoints ``\mathbf{r_y}(k)``.
 - `d=[]` : current measured disturbances ``\mathbf{d}(k)``.
-- `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}``, constant
-   in the future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``.
-- `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}``, constant
-   in the future by default or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``.
-- `R̂u=mpc.Uop` or *`Rhatu`* : predicted manipulated input setpoints, constant in the future 
-   by default or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for ``j=0`` to ``H_p-1``. 
+- `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}`` (constant
+   in the future by default, or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``).
+- `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}`` (constant
+   in the future by default, or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``).
+- `R̂u=mpc.Uop` or *`Rhatu`* : predicted manipulated input setpoints ``\mathbf{R̂_u}`` (constant
+   in the future by default, or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for ``j=0`` to ``H_p-1``). 
 
 # Examples
 ```jldoctest
@@ -193,8 +193,8 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
-        mpc.D̂E[1:model.nd]     .= d
-        mpc.D̂E[model.nd+1:end] .= D̂
+        mpc.D̂e[1:model.nd]     .= d
+        mpc.D̂e[model.nd+1:end] .= D̂
         mul!(F, mpc.G, mpc.d0, 1, 1)
         mul!(F, mpc.J, mpc.D̂0, 1, 1)
     end
@@ -217,7 +217,7 @@ end
 @doc raw"""
     initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
 
-Init `ŷ, F, d0, D̂0, D̂E, R̂y0, R̂u0` vectors when model is not a [`LinModel`](@ref).
+Init `ŷ, F, d0, D̂0, D̂e, R̂y0, R̂u0` vectors when model is not a [`LinModel`](@ref).
 """
 function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
@@ -226,8 +226,8 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
-        mpc.D̂E[1:model.nd]     .= d
-        mpc.D̂E[model.nd+1:end] .= D̂
+        mpc.D̂e[1:model.nd]     .= d
+        mpc.D̂e[model.nd+1:end] .= D̂
     end
     mpc.R̂y0 .= (R̂y .- mpc.Yop)
     if ~mpc.noR̂u

src/controller/explicitmpc.jl

@@ -30,7 +30,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Ps::Matrix{NT}
     d0::Vector{NT}
     D̂0::Vector{NT}
-    D̂E::Vector{NT}
+    D̂e::Vector{NT}
     Uop::Vector{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
@@ -62,7 +62,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         H̃_chol = cholesky(H̃)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
-        d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
+        d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
@@ -78,7 +78,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             H̃, q̃, r,
             H̃_chol,
             Ks, Ps,
-            d0, D̂0, D̂E,
+            d0, D̂0, D̂e,
             Uop, Yop, Dop,
             buffer
         )

src/controller/linmpc.jl

@@ -39,7 +39,7 @@ struct LinMPC{
     Ps::Matrix{NT}
     d0::Vector{NT}
     D̂0::Vector{NT}
-    D̂E::Vector{NT}
+    D̂e::Vector{NT}
     Uop::Vector{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
@@ -71,7 +71,7 @@ struct LinMPC{
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
-        d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
+        d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
@@ -86,7 +86,7 @@ struct LinMPC{
             Ẽ, F, G, J, K, V, B, 
             H̃, q̃, r,
             Ks, Ps,
-            d0, D̂0, D̂E,
+            d0, D̂0, D̂e,
             Uop, Yop, Dop,
             buffer
         )

src/controller/nonlinmpc.jl

@@ -43,7 +43,7 @@ struct NonLinMPC{
     Ps::Matrix{NT}
     d0::Vector{NT}
     D̂0::Vector{NT}
-    D̂E::Vector{NT}
+    D̂e::Vector{NT}
     Uop::Vector{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
@@ -76,7 +76,7 @@ struct NonLinMPC{
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
-        d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
+        d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
@@ -91,7 +91,7 @@ struct NonLinMPC{
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
             Ks, Ps,
-            d0, D̂0, D̂E,
+            d0, D̂0, D̂e,
             Uop, Yop, Dop,
             buffer
         )
@@ -165,8 +165,8 @@ state estimator :
 - `JE=(_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e},
    \mathbf{D̂_e}, \mathbf{p})``.
 - `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom inequality constraint function 
-   ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)`` (mutating or 
-   not, details in Extended Help).
+   ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``, mutating or 
+   not (details in Extended Help).
 - `nc=0` : number of custom inequality constraints.
 - `p=model.p` : ``J_E`` and ``\mathbf{g_c}`` functions parameter ``\mathbf{p}`` (any type).
 - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive
@@ -346,20 +346,6 @@ function validate_JE(NT, JE)
     return nothing
 end
 
-"Get mutating custom constraint function `gc!` from the provided function in argument."
-function get_mutating_gc(NT, gc)
-    ismutating_gc = validate_gc(NT, gc)
-    gc! = if ismutating_gc
-        gc
-    else
-        function gc!(LHS, Ue, Ŷe, D̂e, p, ϵ)
-            LHS .= gc(Ue, Ŷe, D̂e, p, ϵ)
-            return nothing
-        end
-    end
-    return gc!
-end
-
 """
     validate_gc(NT, gc) -> ismutating
 
@@ -371,30 +357,55 @@ function validate_gc(NT, gc)
         #     LHS,        Ue,         Ŷe,         D̂e,         p,   ϵ
         Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any, NT}
     )
-    println(ismutating)
     #                                      Ue,         Ŷe,         D̂e,         p,   ϵ
     if !(ismutating || hasmethod(gc, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT}))
         error(
             "the custom constraint function has no method with type signature "*
-            "gc(UE::Vector{$(NT)}, ŶE::Vector{$(NT)}, D̂E::Vector{$(NT)}, p::Any, ϵ::$(NT)) "*
+            "gc(Ue::Vector{$(NT)}, Ŷe::Vector{$(NT)}, D̂e::Vector{$(NT)}, p::Any, ϵ::$(NT)) "*
             "or mutating form gc!(LHS::Vector{$(NT)}, Ue::Vector{$(NT)}, Ŷe::Vector{$(NT)}, "*
             "D̂e::Vector{$(NT)}, p::Any, ϵ::$(NT))"
         )
     end
     return ismutating
 end
 
+"Get mutating custom constraint function `gc!` from the provided function in argument."
+function get_mutating_gc(NT, gc)
+    ismutating_gc = validate_gc(NT, gc)
+    gc! = if ismutating_gc
+        gc
+    else
+        println("YO!")
+        function gc!(LHS, Ue, Ŷe, D̂e, p, ϵ)
+            LHS .= gc(Ue, Ŷe, D̂e, p, ϵ)
+            return nothing
+        end
+    end
+    return gc!
+end
+
+function test_custom_functions(JE, gc!, uop; Uop, dop, Dop, ΔŨ, p)
+    Ue = [Uop; uop]
+    D̂e = [dop; Dop]
+    Ŷ0, x̂0next =
+    Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, mpc.ΔŨ)
+
+
+    JE = JE(Uop, Uop, Dop, p)
+
+end
+
 """
     addinfo!(info, mpc::NonLinMPC) -> info
 
 For [`NonLinMPC`](@ref), add `:sol` and the optimal economic cost `:JE`.
 """
 function addinfo!(info, mpc::NonLinMPC)
     U, Ŷ, D̂, ŷ, d = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d]
-    UE = [U; U[(end - mpc.estim.model.nu + 1):end]]
-    ŶE = [ŷ; Ŷ]
-    D̂E = [d; D̂]
-    info[:JE]  = mpc.JE(UE, ŶE, D̂E, mpc.p)
+    Ue = [U; U[(end - mpc.estim.model.nu + 1):end]]
+    Ŷe = [ŷ; Ŷ]
+    D̂e = [d; D̂]
+    info[:JE]  = mpc.JE(Ue, Ŷe, D̂e, mpc.p)
     info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true)
     return info
 end
@@ -571,15 +582,15 @@ 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
+        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)
+        Ue = [U; uend]
+        Ŷe = [ŷ; Ŷ]
+        E_JE = mpc.E*mpc.JE(Ue, Ŷe, D̂e, mpc.p)
     else
         E_JE = 0.0
     end

src/model/nonlinmodel.jl

@@ -179,13 +179,6 @@ function get_mutating_functions(NT, f, h)
     return f!, h!
 end
 
-"Get the types of `f!`, `h!` and `solver` to construct a `NonLinModel`."
-function get_types(
-    ::F, ::H, ::P, ::DS
-) where {F<:Function, H<:Function, P<:Any, DS<:DiffSolver} 
-    return F, H, P, DS
-end
-
 """
     validate_f(NT, f) -> ismutating
 
@@ -194,7 +187,7 @@ Validate `f` function argument signature and return `true` if it is mutating.
 function validate_f(NT, f)
     ismutating = hasmethod(
         f, 
-        #       ẋ/xnext,    x,          u,          d,          p    
+        #       ẋ or xnext, x,          u,          d,          p    
         Tuple{  Vector{NT}, Vector{NT}, Vector{NT}, Vector{NT}, Any}
     )
     #                                     x,          u,          d,          p
@@ -231,6 +224,13 @@ function validate_h(NT, h)
     return ismutating
 end
 
+"Get the types of `f!`, `h!` and `solver` to construct a `NonLinModel`."
+function get_types(
+    ::F, ::H, ::P, ::DS
+) where {F<:Function, H<:Function, P<:Any, DS<:DiffSolver} 
+    return F, H, P, DS
+end
+
 "Do nothing if `model` is a [`NonLinModel`](@ref)."
 steadystate!(::SimModel, _ , _ ) = nothing
 

src/precompile.jl

@@ -112,8 +112,8 @@ preparestate!(nmpc_mhe, [55, 30])
 u = nmpc_mhe([55, 30])
 sim!(nmpc_mhe, 3, [55, 30])
 
-function JE( _ , ŶE, _ , R̂y)
-    Ŷ = ŶE[3:end]
+function JE( _ , Ŷe, _ , R̂y)
+    Ŷ = Ŷe[3:end]
     Ȳ = R̂y - Ŷ
     return dot(Ȳ, Ȳ)
 end

test/test_predictive_control.jl

@@ -491,7 +491,7 @@ 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,p) -> p*UE.*ŶE.*D̂E, p=2)
+    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],2) == 2*[1,2].*[3,4].*[4,6]
     optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0))
@@ -538,7 +538,7 @@ end
     @test info[:Ŷ][end] ≈ r[1] atol=5e-2
     Hp = 1000
     R̂y = fill(r[1], Hp)
-    JE = (_ , ŶE, _ , R̂y) -> sum((ŶE[2:end] - R̂y).^2)
+    JE = (_ , Ŷe, _ , R̂y) -> sum((Ŷe[2:end] - R̂y).^2)
     nmpc = NonLinMPC(linmodel, Mwt=[0], Nwt=[0], Cwt=Inf, Ewt=1, JE=JE, p=R̂y, Hp=Hp, Hc=1)
     preparestate!(nmpc, [10])
     u = moveinput!(nmpc)