added field :x̂end in dict of getinfo

Author: franckgaga

src/controller/explicitmpc.jl

@@ -20,6 +20,12 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
     J::Matrix{Float64}
     K::Matrix{Float64}
     V::Matrix{Float64}
+    ẽx̂::Matrix{Float64}
+    fx̂::Vector{Float64}
+    gx̂::Matrix{Float64}
+    jx̂::Matrix{Float64}
+    kx̂::Matrix{Float64}
+    vx̂::Matrix{Float64}
     P̃::Hermitian{Float64, Matrix{Float64}}
     q̃::Vector{Float64}
     p::Vector{Float64}
@@ -44,8 +50,8 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
         R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(nu, Hp, Hc)
-        E, F, G, J, K, V = init_predmat(estim, model, Hp, Hc)
-        S̃, Ñ_Hc, Ẽ  = S, N_Hc, E # no slack variable ϵ for ExplicitMPC
+        E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
+        S̃, Ñ_Hc, Ẽ, ẽx̂  = S, N_Hc, E, ex̂ # no slack variable ϵ for ExplicitMPC
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         P̃_chol = cholesky(P̃)
         Ks, Ps = init_stochpred(estim, Hp)
@@ -59,7 +65,7 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y, noR̂u,
             S̃, T, 
-            Ẽ, F, G, J, K, V, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
             P̃_chol,
             Ks, Ps,
             d0, D̂0,

src/controller/linmpc.jl

@@ -22,7 +22,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
     J::Matrix{Float64}
     K::Matrix{Float64}
     V::Matrix{Float64}
-    ex̂::Matrix{Float64}
+    ẽx̂::Matrix{Float64}
     fx̂::Vector{Float64}
     gx̂::Matrix{Float64}
     jx̂::Matrix{Float64}
@@ -51,7 +51,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
-        con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
+        con, S̃, Ñ_Hc, Ẽ, ẽx̂ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d0, D̂0 = zeros(nd), zeros(nd*Hp)
@@ -64,7 +64,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y, noR̂u,
             S̃, T,
-            Ẽ, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
             Ks, Ps,
             d0, D̂0,
             Ŷop, Dop,

src/controller/nonlinmpc.jl

@@ -25,7 +25,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
     J::Matrix{Float64}
     K::Matrix{Float64}
     V::Matrix{Float64}
-    ex̂::Matrix{Float64}
+    ẽx̂::Matrix{Float64}
     fx̂::Vector{Float64}
     gx̂::Matrix{Float64}
     jx̂::Matrix{Float64}
@@ -55,7 +55,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
         noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
-        con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
+        con, S̃, Ñ_Hc, Ẽ, ẽx̂ = init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d0, D̂0 = zeros(nd), zeros(nd*Hp)
@@ -68,7 +68,7 @@ struct NonLinMPC{SE<:StateEstimator, JEfunc<:Function} <: PredictiveController
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y, noR̂u,
             S̃, T,  
-            Ẽ, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
+            Ẽ, F, G, J, K, V, ẽx̂, fx̂, gx̂, jx̂, kx̂, vx̂, P̃, q̃, p,
             Ks, Ps,
             d0, D̂0,
             Ŷop, Dop,
@@ -278,8 +278,8 @@ function init_optimization!(mpc::NonLinMPC)
             if ΔŨtup != last_ΔŨtup_float
                 x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 C = get_tmp(C_cache, ΔŨtup[1])
-                predict!(x̂, Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, x̂, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_float = ΔŨtup
             end
             return obj_nonlinprog(mpc, model, Ŷ, ΔŨ)
@@ -290,8 +290,8 @@ function init_optimization!(mpc::NonLinMPC)
             if ΔŨtup != last_ΔŨtup_dual
                 x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 C = get_tmp(C_cache, ΔŨtup[1])
-                predict!(x̂, Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, x̂, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_dual = ΔŨtup
             end
             return obj_nonlinprog(mpc, model, Ŷ, ΔŨ)
@@ -302,8 +302,8 @@ function init_optimization!(mpc::NonLinMPC)
                 x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
                 ΔŨ = collect(ΔŨtup)
-                predict!(x̂, Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, x̂, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                C = con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_float = ΔŨtup
             end
             return C[i]
@@ -314,8 +314,8 @@ function init_optimization!(mpc::NonLinMPC)
                 x̂ = get_tmp(x̂_cache, ΔŨtup[1])
                 Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
                 ΔŨ = collect(ΔŨtup)
-                predict!(x̂, Ŷ, mpc, model, ΔŨ)
-                con_nonlinprog!(C, mpc, model, x̂, Ŷ, ΔŨ)
+                Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ)
+                C = con_nonlinprog!(C, mpc, model, x̂end, Ŷ, ΔŨ)
                 last_ΔŨtup_dual = ΔŨtup
             end
             return C[i]
@@ -377,25 +377,25 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
 end
 
 """
-    con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂, Ŷ, ΔŨ)
+    con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
 
 Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
 """
-function con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂, Ŷ, ΔŨ)
+function con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, x̂end, Ŷ, ΔŨ)
     ny, nx̂, Hp = model.ny, mpc.estim.nx̂, mpc.Hp
     i_end_Ymin, i_end_Ymax = 1Hp*ny      , 2Hp*ny
     i_end_x̂min, i_end_x̂max = 2Hp*ny + 1nx̂, 2Hp*ny + 2nx̂
     if !isinf(mpc.C) # constraint softening activated :
         ϵ = ΔŨ[end]
         C[           1:i_end_Ymin] = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
         C[i_end_Ymin+1:i_end_Ymax] = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
-        C[i_end_Ymax+1:i_end_x̂min] = (mpc.con.x̂min - x̂) - ϵ*mpc.con.c_x̂min
-        C[i_end_x̂min+1:i_end_x̂max] = (x̂ - mpc.con.x̂max) - ϵ*mpc.con.c_x̂max
+        C[i_end_Ymax+1:i_end_x̂min] = (mpc.con.x̂min - x̂end) - ϵ*mpc.con.c_x̂min
+        C[i_end_x̂min+1:i_end_x̂max] = (x̂end - mpc.con.x̂max) - ϵ*mpc.con.c_x̂max
     else # no constraint softening :
-        C[           1:i_end_Ymin] = (mpc.con.Ymin - Ŷ)
-        C[i_end_Ymin+1:i_end_Ymax] = (Ŷ - mpc.con.Ymax)
-        C[i_end_Ymax+1:i_end_x̂min] = mpc.con.x̂min - x̂
-        C[i_end_x̂min+1:i_end_x̂max] = x̂ - mpc.con.x̂max 
+        C[           1:i_end_Ymin] = mpc.con.Ymin - Ŷ
+        C[i_end_Ymin+1:i_end_Ymax] = Ŷ - mpc.con.Ymax
+        C[i_end_Ymax+1:i_end_x̂min] = mpc.con.x̂min - x̂end
+        C[i_end_x̂min+1:i_end_x̂max] = x̂end - mpc.con.x̂max
     end
     C[isinf.(C)] .= 0 # replace ±Inf with 0 to avoid INVALID_MODEL error
     return C

src/predictive_control.jl

@@ -198,7 +198,7 @@ function setconstraint!(
     model, con, optim = mpc.estim.model, mpc.con, mpc.optim
     nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc
     notSolvedYet = (termination_status(optim) == OPTIMIZE_NOT_CALLED)
-    C, E, ex̂ = mpc.C, mpc.Ẽ[:, 1:nu*Hc], mpc.ex̂
+    C, E, ex̂ = mpc.C, mpc.Ẽ[:, 1:nu*Hc], mpc.ẽx̂[:, 1:nu*Hc]
     isnothing(Umin)     && !isnothing(umin)     && (Umin    = repeat(umin,    Hp))
     isnothing(Umax)     && !isnothing(umax)     && (Umax    = repeat(umax,    Hp))
     isnothing(ΔUmin)    && !isnothing(Δumin)    && (ΔUmin   = repeat(Δumin,   Hc))
@@ -386,18 +386,19 @@ Get additional information about `mpc` controller optimum to ease troubleshootin
 The function should be called after calling [`moveinput!`](@ref). It returns the dictionary
 `info` with the following fields:
 
-- `:ΔU` : optimal manipulated input increments over `Hc` ``(\mathbf{ΔU})``
-- `:ϵ`  : optimal slack variable ``(ϵ)``
-- `:J`  : objective value optimum ``(J)``
-- `:U`  : optimal manipulated inputs over `Hp` ``(\mathbf{U})``
-- `:u`  : current optimal manipulated input ``(\mathbf{u})``
-- `:d`  : current measured disturbance ``(\mathbf{d})``
-- `:D̂`  : predicted measured disturbances over `Hp` ``(\mathbf{D̂})``
-- `:ŷ`  : current estimated output ``(\mathbf{ŷ})``
-- `:Ŷ`  : optimal predicted outputs over `Hp` ``(\mathbf{Ŷ})``
-- `:Ŷs` : predicted stochastic output over `Hp` of [`InternalModel`](@ref) ``(\mathbf{Ŷ_s})``
-- `:R̂y` : predicted output setpoint over `Hp` ``(\mathbf{R̂_y})``
-- `:R̂u` : predicted manipulated input setpoint over `Hp` ``(\mathbf{R̂_u})``
+- `:ΔU`  : optimal manipulated input increments over `Hc` ``(\mathbf{ΔU})``
+- `:ϵ`   : optimal slack variable ``(ϵ)``
+- `:J`   : objective value optimum ``(J)``
+- `:U`   : optimal manipulated inputs over `Hp` ``(\mathbf{U})``
+- `:u`   : current optimal manipulated input ``(\mathbf{u})``
+- `:d`   : current measured disturbance ``(\mathbf{d})``
+- `:D̂`   : predicted measured disturbances over `Hp` ``(\mathbf{D̂})``
+- `:ŷ`   : current estimated output ``(\mathbf{ŷ})``
+- `:Ŷ`   : optimal predicted outputs over `Hp` ``(\mathbf{Ŷ})``
+- `:x̂end : optimal terminal states ``(\mathbf{x̂}_{k-1}(k+H_p))``
+- `:Ŷs`  : predicted stochastic output over `Hp` of [`InternalModel`](@ref) ``(\mathbf{Ŷ_s})``
+- `:R̂y`  : predicted output setpoint over `Hp` ``(\mathbf{R̂_y})``
+- `:R̂u`  : predicted manipulated input setpoint over `Hp` ``(\mathbf{R̂_u})``
 
 For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer
 solution summary that can be printed. Lastly, the optimal economic cost `:JE` is also
@@ -416,17 +417,18 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3)
 """
 function getinfo(mpc::PredictiveController)
     info = Dict{Symbol, InfoDictType}()
-    x̂, Ŷ = similar(mpc.estim.x̂), similar(mpc.Ŷop)
-    predict!(x̂, Ŷ, mpc, mpc.estim.model, mpc.ΔŨ)
-    info[:ΔU]  = mpc.ΔŨ[1:mpc.Hc*mpc.estim.model.nu]
-    info[:ϵ]   = isinf(mpc.C) ? NaN : mpc.ΔŨ[end]
-    info[:J]   = obj_nonlinprog(mpc, mpc.estim.model, Ŷ, mpc.ΔŨ) + mpc.p[]
-    info[:U]   = mpc.S̃*mpc.ΔŨ + mpc.T*(mpc.estim.lastu0 + mpc.estim.model.uop)
-    info[:u]   = info[:U][1:mpc.estim.model.nu]
-    info[:d]   = mpc.d0 + mpc.estim.model.dop
-    info[:D̂]   = mpc.D̂0 + mpc.Dop
-    info[:ŷ]   = mpc.ŷ
-    info[:Ŷ]   = Ŷ
+    Ŷ, x̂    = similar(mpc.Ŷop), similar(mpc.estim.x̂)
+    Ŷ, x̂end = predict!(Ŷ, x̂, mpc, mpc.estim.model, mpc.ΔŨ)
+    info[:ΔU]   = mpc.ΔŨ[1:mpc.Hc*mpc.estim.model.nu]
+    info[:ϵ]    = isinf(mpc.C) ? NaN : mpc.ΔŨ[end]
+    info[:J]    = obj_nonlinprog(mpc, mpc.estim.model, Ŷ, mpc.ΔŨ) + mpc.p[]
+    info[:U]    = mpc.S̃*mpc.ΔŨ + mpc.T*(mpc.estim.lastu0 + mpc.estim.model.uop)
+    info[:u]    = info[:U][1:mpc.estim.model.nu]
+    info[:d]    = mpc.d0 + mpc.estim.model.dop
+    info[:D̂]    = mpc.D̂0 + mpc.Dop
+    info[:ŷ]    = mpc.ŷ
+    info[:Ŷ]    = Ŷ
+    info[:x̂end] = x̂end
     info[:Ŷs]  = mpc.Ŷop - repeat(mpc.estim.model.yop, mpc.Hp) # Ŷop = Ŷs + Yop
     info[:R̂y]  = mpc.R̂y
     info[:R̂u]  = mpc.R̂u
@@ -592,25 +594,27 @@ end
 predictstoch!(::PredictiveController, ::StateEstimator, _ , _ ) = nothing
 
 @doc raw"""
-    predict!( _ , Ŷ, mpc::PredictiveController, model::LinModel, ΔŨ)
+    predict!(Ŷ, x̂, mpc::PredictiveController, model::LinModel, ΔŨ) -> Ŷ, x̂end
 
-Compute the outputs predictions ``\mathbf{Ŷ}`` when `model` is a [`LinModel`](@ref).
+Compute the predictions `Ŷ` and terminal states `x̂end` if model is a [`LinModel`](@ref).
+
+The method mutates `Ŷ` and `x̂` vector arguments. The `x̂end` vector is used for
+the terminal constraints applied on ``\mathbf{x̂}_{k-1}(k+H_p)``.
 """
-function predict!( _ , Ŷ, mpc::PredictiveController, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
-    mul!(Ŷ, mpc.Ẽ, ΔŨ) + mpc.F # in-place operations to reduce allocations
-    return nothing
+function predict!(Ŷ, x̂, mpc::PredictiveController, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
+     # in-place operations to reduce allocations :
+    mul!(Ŷ, mpc.Ẽ, ΔŨ) + mpc.F
+    mul!(x̂, mpc.ẽx̂, ΔŨ) + mpc.fx̂
+    x̂end = x̂
+    return Ŷ, x̂end
 end
 
 @doc raw"""
-    predict!(x̂, Ŷ, mpc::PredictiveController, model::SimModel, ΔŨ)
-
-Compute ``\mathbf{x̂}_{k-1}(k+H_p)``, ``\mathbf{Ŷ}`` if `model` is not a [`LinModel`](@ref). 
+    predict!(Ŷ, x̂, mpc::PredictiveController, model::SimModel, ΔŨ) -> Ŷ, x̂end
 
-The method mutates `x̂` and `Ŷ` vector arguments. After a `predict!` call, `x̂` will contain
-the states at the end of the prediction horizon ``\mathbf{x̂}_{k-1}(k+H_p)`` (used for
-terminal constraints). 
+Compute both vectors if `model` is not a [`LinModel`](@ref).
 """
-function predict!(x̂, Ŷ, mpc::PredictiveController, model::SimModel, ΔŨ::Vector{T}) where {T<:Real}
+function predict!(Ŷ, x̂, mpc::PredictiveController, model::SimModel, ΔŨ::Vector{T}) where {T<:Real}
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     u0::Vector{T} = copy(mpc.estim.lastu0)
     d0::Vector{T} = copy(mpc.d0)
@@ -624,7 +628,8 @@ function predict!(x̂, Ŷ, mpc::PredictiveController, model::SimModel, ΔŨ::V
         Ŷ[(1 + ny*(j-1)):(ny*j)] = ĥ(mpc.estim, x̂, d0)
     end
     Ŷ[:] = Ŷ + mpc.Ŷop # Ŷop = Ŷs + Yop, and Ŷs=0 if mpc.estim is not an InternalModel
-    return nothing
+    x̂end = x̂
+    return Ŷ, x̂end
 end
 
 @doc raw"""
@@ -980,7 +985,7 @@ function init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
     A_Umin, A_Umax, S̃ = relaxU(C, c_Umin, c_Umax, S)
     A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, Ñ_Hc = relaxΔU(C, c_ΔUmin, c_ΔUmax, ΔUmin, ΔUmax, N_Hc)
     A_Ymin, A_Ymax, Ẽ = relaxŶ(model, C, c_Ymin, c_Ymax, E)
-    A_x̂min, A_x̂max = relaxterminal(model, C, c_x̂min, c_x̂max, ex̂)
+    A_x̂min, A_x̂max, ẽx̂ = relaxterminal(model, C, c_x̂min, c_x̂max, ex̂)
     i_Umin,  i_Umax  = .!isinf.(Umin),  .!isinf.(Umax)
     i_ΔŨmin, i_ΔŨmax = .!isinf.(ΔŨmin), .!isinf.(ΔŨmax)
     i_Ymin,  i_Ymax  = .!isinf.(Ymin),  .!isinf.(Ymax)
@@ -996,7 +1001,7 @@ function init_defaultcon(estim, Hp, Hc, C, S, N_Hc, E, ex̂)
         A_Umin  , A_Umax, A_ΔŨmin, A_ΔŨmax  , A_Ymin, A_Ymax, A_x̂min, A_x̂max,
         A       , b     , i_b    , c_Ymin   , c_Ymax, c_x̂min, c_x̂max,
     )
-    return con, S̃, Ñ_Hc, Ẽ
+    return con, S̃, Ñ_Hc, Ẽ, ẽx̂
 end
 
 "Repeat predictive controller constraints over prediction `Hp` and control `Hc` horizons."
@@ -1105,7 +1110,7 @@ function relaxŶ(::LinModel, C, c_Ymin, c_Ymax, E)
     if !isinf(C) # ΔŨ = [ΔU; ϵ]
         # ϵ impacts predicted output constraint calculations:
         A_Ymin, A_Ymax = -[E  c_Ymin], [E -c_Ymax] 
-        # ϵ has not impact on output predictions
+        # ϵ has no impact on output predictions
         Ẽ = [E zeros(size(E, 1), 1)] 
     else # ΔŨ = ΔU (only hard constraints)
         Ẽ = E
@@ -1117,7 +1122,7 @@ end
 "Return empty matrices if model is not a [`LinModel`](@ref)"
 function relaxŶ(::SimModel, C, c_Ymin, c_Ymax, E)
     Ẽ = !isinf(C) ? [E zeros(0, 1)] : E
-    A_Ymin, A_Ymax = Ẽ, Ẽ 
+    A_Ymin, A_Ymax = -Ẽ,  Ẽ 
     return A_Ymin, A_Ymax, Ẽ
 end
 
@@ -1128,16 +1133,23 @@ TBW
 """
 function relaxterminal(::LinModel, C, c_x̂min, c_x̂max, ex̂)
     if !isinf(C) # ΔŨ = [ΔU; ϵ]
-        # ϵ impacts predicted terminal state calculations:
-        A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max] 
+        # ϵ impacts terminal constraint calculations:
+        A_x̂min, A_x̂max = -[ex̂ c_x̂min], [ex̂ -c_x̂max]
+        # ϵ has no impact on terminal state predictions
+        ẽx̂ = [ex̂ zeros(size(ex̂, 1), 1)] 
     else # ΔŨ = ΔU (only hard constraints)
+        ẽx̂ = ex̂
         A_x̂min, A_x̂max = -ex̂,  ex̂
     end
-    return A_x̂min, A_x̂max
+    return A_x̂min, A_x̂max, ẽx̂
 end
 
 "Return empty matrices if model is not a [`LinModel`](@ref)"
-relaxterminal(::SimModel, C, c_x̂min, c_x̂max, ex̂) = -ex̂,  ex̂
+function relaxterminal(::SimModel, C, c_x̂min, c_x̂max, ex̂) 
+    ẽx̂ = !isinf(C) ? [ex̂ zeros(0, 1)] : ex̂
+    A_x̂min, A_x̂max = -ẽx̂,  ẽx̂
+    return A_x̂min, A_x̂max, ẽx̂
+end
 
 
 @doc raw"""