cleanup predict! method for custom constraints

franckgaga · franckgaga · commit 2a0066be5b05 · 2024-11-20T18:01:48.000-05:00
diff --git a/src/controller/execute.jl b/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 ``\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``). 
+- `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``. 
 
 # Examples
 ```jldoctest
@@ -135,8 +135,8 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     info[:Ŷ]    = Ŷ0 + mpc.Yop
     info[:x̂end] = x̂0end + mpc.estim.x̂op
     info[:Ŷs]   = Ŷs
-    info[:R̂y]   = mpc.R̂y0 + mpc.Yop
-    info[:R̂u]   = mpc.R̂u0 + mpc.Uop
+    info[:R̂y]   = mpc.R̂y
+    info[:R̂u]   = mpc.R̂u
     # --- non-Unicode fields ---
     info[:DeltaU] = info[:ΔU]
     info[:epsilon] = info[:ϵ]
@@ -198,14 +198,14 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
         mul!(F, mpc.G, mpc.d0, 1, 1)
         mul!(F, mpc.J, mpc.D̂0, 1, 1)
     end
-    mpc.R̂y0 .= R̂y .- mpc.Yop
-    Cy = F .- mpc.R̂y0
+    mpc.R̂y .= R̂y
+    Cy = F .- (R̂y .- mpc.Yop)
     M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ
     mul!(q̃, M_Hp_Ẽ', Cy)
     r .= dot(Cy, mpc.M_Hp, Cy)
     if ~mpc.noR̂u
-        mpc.R̂u0 .= R̂u .- mpc.Uop
-        Cu = mpc.T_lastu0 .- mpc.R̂u0
+        mpc.R̂u .= R̂u
+        Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) 
         L_Hp_S̃ = mpc.L_Hp*mpc.S̃
         mul!(q̃, L_Hp_S̃', Cu, 1, 1)
         r .+= dot(Cu, mpc.L_Hp, Cu)
@@ -217,7 +217,7 @@ end
 @doc raw"""
     initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
 
-Init `ŷ, F, d0, D̂0, D̂e, R̂y0, R̂u0` vectors when model is not a [`LinModel`](@ref).
+Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` 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)
@@ -229,9 +229,9 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂
         mpc.D̂e[1:model.nd]     .= d
         mpc.D̂e[model.nd+1:end] .= D̂
     end
-    mpc.R̂y0 .= (R̂y .- mpc.Yop)
+    mpc.R̂y .= R̂y
     if ~mpc.noR̂u
-        mpc.R̂u0 .= (R̂u .- mpc.Uop)
+        mpc.R̂u .= R̂u 
     end
     return nothing
 end
@@ -356,57 +356,55 @@ function predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, mod
 end
 
 """
-    obj_nonlinprog!(U0 , Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ)
+    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ŷe, Ue, ΔŨ)
 
 Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
 
-The function is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can
+The method is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can
 also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J`
-at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates the `U0` and
-`Ȳ` arguments.
+at specific `Ue`, `Ŷe` and `ΔŨ`, values. It does not mutate any argument.
 """
 function obj_nonlinprog!(
-    U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT}
+    _, _, mpc::PredictiveController, model::LinModel, Ŷe, Ue, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
-    J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
-    E_JE = obj_econ!(U0, Ȳ, mpc, model, Ŷ0, ΔŨ)
-    return J + E_JE
+    JQP  = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
+    E_JE = obj_econ!(Ue, Ŷe, mpc, model)
+    return JQP + E_JE
 end
 
 """
-    obj_nonlinprog!(U0, Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷ0, ΔŨ)
+    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷe, Ue, ΔŨ)
 
-Nonlinear programming objective function when `model` is not a [`LinModel`](@ref). The
+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
-`U0`, `Ȳ` and `Ū` arguments (input over `Hp`, and output and input setpoint tracking error, 
-respectively).
+`Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in
+`Ŷe` and `Ue` arguments).
 """
 function obj_nonlinprog!(
-    U0, Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷ0, ΔŨ::AbstractVector{NT}
+    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷe, Ue, ΔŨ::AbstractVector{NT}
 ) where NT<:Real
+    nu, ny = model.nu, model.ny
     # --- output setpoint tracking term ---
-    Ȳ  .= mpc.R̂y0 .- Ŷ0
+    Ȳ  .= Ŷe[ny+1:end]
+    Ȳ  .= mpc.R̂y .- Ȳ
     JR̂y = dot(Ȳ, mpc.M_Hp, Ȳ)
     # --- move suppression and slack variable term ---
     JΔŨ = dot(ΔŨ, mpc.Ñ_Hc, ΔŨ)
-    # --- input over prediction horizon ---
-    if !mpc.noR̂u || !iszero(mpc.E)
-        U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ mpc.T_lastu0
-    end
     # --- input setpoint tracking term ---
     if !mpc.noR̂u
-        Ū  .= mpc.R̂u0 .- U0
+        Ū  .= Ue[1:end-nu]
+        Ū  .= mpc.R̂u .- Ū
         JR̂u = dot(Ū, mpc.L_Hp, Ū)
     else
         JR̂u = 0.0
     end
     # --- economic term ---
-    E_JE = obj_econ!(U0, Ȳ, mpc, model, Ŷ0, ΔŨ)
+    E_JE = obj_econ!(Ue, Ŷe, mpc, model)
     return JR̂y + JΔŨ + 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) -> ΔŨ
diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl
@@ -9,8 +9,8 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Ñ_Hc::Hermitian{NT, Matrix{NT}}
     L_Hp::Hermitian{NT, Matrix{NT}}
     E::NT
-    R̂u0::Vector{NT}
-    R̂y0::Vector{NT}
+    R̂u::Vector{NT}
+    R̂y::Vector{NT}
     noR̂u::Bool
     S̃::Matrix{NT} 
     T::Matrix{NT}
@@ -49,7 +49,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
         L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
-        R̂y0, R̂u0, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc)
@@ -72,7 +72,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             M_Hp, Ñ_Hc, L_Hp, Ewt,  
-            R̂u0, R̂y0, noR̂u,
+            R̂u, R̂y, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl
@@ -19,8 +19,8 @@ struct LinMPC{
     Ñ_Hc::Hermitian{NT, Matrix{NT}}
     L_Hp::Hermitian{NT, Matrix{NT}}
     E::NT
-    R̂u0::Vector{NT}
-    R̂y0::Vector{NT}
+    R̂u::Vector{NT}
+    R̂y::Vector{NT}
     noR̂u::Bool
     S̃::Matrix{NT} 
     T::Matrix{NT}
@@ -57,7 +57,7 @@ struct LinMPC{
         N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
         L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
-        R̂y0, R̂u0, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
@@ -81,7 +81,7 @@ struct LinMPC{
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             M_Hp, Ñ_Hc, L_Hp, Ewt, 
-            R̂u0, R̂y0, noR̂u,
+            R̂u, R̂y, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B, 
             H̃, q̃, r,
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -23,8 +23,8 @@ struct NonLinMPC{
     E::NT
     JE::JEfunc
     p::P
-    R̂u0::Vector{NT}
-    R̂y0::Vector{NT}
+    R̂u::Vector{NT}
+    R̂y::Vector{NT}
     noR̂u::Bool
     S̃::Matrix{NT}
     T::Matrix{NT}
@@ -62,7 +62,7 @@ struct NonLinMPC{
         N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
         L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
-        R̂y0, R̂u0, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
+        R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
@@ -86,7 +86,7 @@ struct NonLinMPC{
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             M_Hp, Ñ_Hc, L_Hp, Ewt, JE, p,
-            R̂u0, R̂y0, noR̂u,
+            R̂u, R̂y, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
@@ -375,7 +375,6 @@ function get_mutating_gc(NT, gc)
     gc! = if ismutating_gc
         gc
     else
-        println("YO!")
         function gc!(LHS, Ue, Ŷe, D̂e, p, ϵ)
             LHS .= gc(Ue, Ŷe, D̂e, p, ϵ)
             return nothing
@@ -385,14 +384,13 @@ function get_mutating_gc(NT, gc)
 end
 
 function test_custom_functions(JE, gc!, uop; Uop, dop, Dop, ΔŨ, p)
+    # TODO: contunue here (important to guide the users, sim! can be used on NonLinModel
+    # but there is no similar function for the custom functions of NonLinMPC)
     Ue = [Uop; uop]
     D̂e = [dop; Dop]
     Ŷ0, x̂0next =
     Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, mpc.ΔŨ)
-
-
     JE = JE(Uop, Uop, Dop, p)
-
 end
 
 """
@@ -475,31 +473,33 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     model = mpc.estim.model
     nu, ny, nx̂, Hp = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp
     ng, nΔŨ, nU, nŶ = length(mpc.con.i_g), length(mpc.ΔŨ), Hp*nu, Hp*ny
+    nUe, nŶe = nU + nu, nŶ + ny
     Nc = nΔŨ + 3
     ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nΔŨ), Nc)
-    Ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nŶ), Nc)
-    U0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nU), Nc)
-    g_cache::DiffCache{Vector{JNT}, Vector{JNT}}      = DiffCache(zeros(JNT, ng), Nc)
-    x̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nx̂), Nc)
-    x̂0next_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc)
-    u0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nu), Nc)
-    û0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nu), Nc)
-    Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}}      = DiffCache(zeros(JNT, nŶ), Nc)
-    Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}}      = DiffCache(zeros(JNT, nU), Nc)
+    Ŷe_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nŶe), Nc)
+    Ue_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nUe), Nc)
+    Ȳ_cache::DiffCache{Vector{JNT}, Vector{JNT}}      = DiffCache(zeros(JNT, nŶ),  Nc)
+    Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}}      = DiffCache(zeros(JNT, nU),  Nc)
+    x̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nx̂),  Nc)
+    x̂0next_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂),  Nc)
+    u0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nu),  Nc)
+    û0_cache::DiffCache{Vector{JNT}, Vector{JNT}}     = DiffCache(zeros(JNT, nu),  Nc)
+    g_cache::DiffCache{Vector{JNT}, Vector{JNT}}      = DiffCache(zeros(JNT, ng),  Nc)
     function Jfunc(ΔŨtup::T...) where T<:Real
         ΔŨ1 = ΔŨtup[begin]
         ΔŨ, g = get_tmp(ΔŨ_cache, ΔŨ1), get_tmp(g_cache, ΔŨ1) 
         for i in eachindex(ΔŨtup)
             ΔŨ[i] = ΔŨtup[i] # ΔŨ .= ΔŨtup seems to produce a type instability
-        end 
-        Ŷ0 = get_tmp(Ŷ0_cache, ΔŨ1)
+        end
+        Ŷe, Ue = get_tmp(Ŷe_cache, ΔŨ1), get_tmp(Ue_cache, ΔŨ1)
+        Ȳ,  Ū  = get_tmp(Ȳ_cache, ΔŨ1),  get_tmp(Ū_cache, ΔŨ1)
         x̂0, x̂0next = get_tmp(x̂0_cache, ΔŨ1), get_tmp(x̂0next_cache, ΔŨ1)
-        u0, û0 = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1)
-        Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
+        u0, û0     = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1)
+        Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
+        Ŷe, Ue    = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
         g = get_tmp(g_cache, ΔŨ1)
         g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, ΔŨ)
-        U0, Ȳ, Ū = get_tmp(U0_cache, ΔŨ1), get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1)
-        return obj_nonlinprog!(U0, Ȳ, Ū, mpc, model, Ŷ0, ΔŨ)::T
+        return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷe, Ue, ΔŨ)::T
     end
     function gfunc_i(i, ΔŨtup::NTuple{N, T}) where {N, T<:Real}
         ΔŨ1 = ΔŨtup[begin]
@@ -508,10 +508,13 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
             for i in eachindex(ΔŨtup)
                 ΔŨ[i] = ΔŨtup[i] # ΔŨ .= ΔŨtup seems to produce a type instability
             end
-            Ŷ0 = get_tmp(Ŷ0_cache, ΔŨ1)
+            Ŷe, Ue = get_tmp(Ŷe_cache, ΔŨ1), get_tmp(Ue_cache, ΔŨ1)
+            Ȳ,  Ū  = get_tmp(Ȳ_cache, ΔŨ1),  get_tmp(Ū_cache, ΔŨ1)
             x̂0, x̂0next = get_tmp(x̂0_cache, ΔŨ1), get_tmp(x̂0next_cache, ΔŨ1)
-            u0, û0 = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1)
-            Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
+            u0, û0     = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1)
+            Ŷ0, x̂0end = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
+            Ŷe, Ue    = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
+            g = get_tmp(g_cache, ΔŨ1)
             g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, ΔŨ)
         end
         return g[i]::T
@@ -520,6 +523,27 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     return Jfunc, gfunc
 end
 
+"""
+    extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ) -> Ŷe, Ue
+
+Compute the extended predictions `Ŷe` and `Ue` for the nonlinear optimization.
+
+The function mutates `Ŷe`, `Ue` and `Ū` in arguments, without assuming any initial values.
+"""
+function extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
+    ny, nu = model.ny, model.nu
+    # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
+    Ŷe[1:ny]     .= mpc.ŷ
+    Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
+    # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
+    U0 = Ū
+    U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ 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 Ŷe, Ue
+end
+
 "Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`."
 function setnonlincon!(
     mpc::NonLinMPC, ::NonLinModel, optim::JuMP.GenericModel{JNT}
@@ -579,20 +603,8 @@ end
 "No nonlinear constraints if `model` is a [`LinModel`](@ref), return `g` unchanged."
 con_nonlinprog!(g, ::NonLinMPC, ::LinModel, _ , _ , _ ) = g
 
-"Evaluate the economic term of the objective function for [`NonLinMPC`](@ref)."
-function obj_econ!(U0, Ȳ, mpc::NonLinMPC, model::SimModel, Ŷ0, ΔŨ)
-    if !iszero(mpc.E)
-        ny, Hp, ŷ, D̂e = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂e
-        U   = U0
-        U .+= mpc.Uop
-        uend = @views U[(end-model.nu+1):end]
-        Ŷ  = Ȳ
-        Ŷ .= Ŷ0 .+ mpc.Yop
-        Ue = [U; uend]
-        Ŷe = [ŷ; Ŷ]
-        E_JE = mpc.E*mpc.JE(Ue, Ŷe, D̂e, mpc.p)
-    else
-        E_JE = 0.0
-    end
+"Evaluate the economic term `E*JE` of the objective function for [`NonLinMPC`](@ref)."
+function obj_econ!(Ue, Ŷe, mpc::NonLinMPC, model::SimModel)
+    E_JE = iszero(mpc.E) ? 0.0 : mpc.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p)
     return E_JE
 end
diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl
@@ -479,7 +479,7 @@ end
     nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing)
     nmpc1 = NonLinMPC(nonlinmodel, Hp=15)
     @test isa(nmpc1.estim, UnscentedKalmanFilter)
-    @test size(nmpc1.R̂y0, 1) == 15*nmpc1.estim.model.ny
+    @test size(nmpc1.R̂y, 1) == 15*nmpc1.estim.model.ny
     nmpc2 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=Inf)
     @test size(nmpc2.Ẽ, 2) == 4*nonlinmodel.nu
     nmpc3 = NonLinMPC(nonlinmodel, Hp=15, Hc=4, Cwt=1e6)
