rename constant scalar QP from p to r

To avoid possible confusion with MHE `p` constant

Author: franckgaga

src/controller/construct.jl

@@ -603,15 +603,15 @@ Init the quadratic programming Hessian `H̃` for MPC.
 
 The matrix appear in the quadratic general form:
 ```math
-    J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + p 
+    J = \min_{\mathbf{ΔŨ}} \frac{1}{2}\mathbf{(ΔŨ)'H̃(ΔŨ)} + \mathbf{q̃'(ΔŨ)} + r 
 ```
 The Hessian matrix is constant if the model and weights are linear and time invariant (LTI): 
 ```math
     \mathbf{H̃} = 2 (  \mathbf{Ẽ}'\mathbf{M}_{H_p}\mathbf{Ẽ} + \mathbf{Ñ}_{H_c} 
                     + \mathbf{S̃}'\mathbf{L}_{H_p}\mathbf{S̃} )
 ```
-The vector ``\mathbf{q̃}`` and scalar ``p`` need recalculation each control period ``k``, see
-[`initpred!`](@ref). ``p`` does not impact the minima position. It is thus useless at
+The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control period ``k``, see
+[`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at
 optimization but required to evaluate the minimal ``J`` value.
 """
 function init_quadprog(::LinModel, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)

src/controller/execute.jl

@@ -164,7 +164,7 @@ end
 @doc raw"""
     initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u) -> nothing
 
-Init linear model prediction matrices `F, q̃, p` and current estimated output `ŷ`.
+Init linear model prediction matrices `F, q̃, r` and current estimated output `ŷ`.
 
 See [`init_predmat`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
 They are computed with these equations using in-place operations:
@@ -176,14 +176,14 @@ They are computed with these equations using in-place operations:
     \mathbf{C_u}     &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}})          \\
     \mathbf{q̃}       &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} 
                             + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}]              \\
-    p                &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} 
+    r                &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} 
                             + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u}
 \end{aligned}
 ```
 """
 function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
-    ŷ, F, q̃, p = mpc.ŷ, mpc.F, mpc.q̃, mpc.p
+    ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
     ŷ .= evalŷ(mpc.estim, d)
     predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
     F .+= mpc.B
@@ -201,13 +201,13 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     Cy = F .- mpc.R̂y0
     M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ
     mul!(q̃, M_Hp_Ẽ', Cy)
-    p .= dot(Cy, mpc.M_Hp, 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
         L_Hp_S̃ = mpc.L_Hp*mpc.S̃
         mul!(q̃, L_Hp_S̃', Cu, 1, 1)
-        p .+= dot(Cu, mpc.L_Hp, Cu)
+        r .+= dot(Cu, mpc.L_Hp, Cu)
     end
     lmul!(2, q̃)
     return nothing
@@ -367,9 +367,9 @@ at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates t
 function obj_nonlinprog!(
     U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
-    J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.p[]
+    J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
     if !iszero(mpc.E)
-        ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E
+        ŷ, D̂E = mpc.ŷ, mpc.D̂E
         U = U0
         U  .+= mpc.Uop
         uend = @views U[(end-model.nu+1):end]

src/controller/explicitmpc.jl

@@ -24,7 +24,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     B::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     H̃_chol::Cholesky{NT, Matrix{NT}}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
@@ -57,7 +57,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         S̃, Ñ_Hc, Ẽ  = S, N_Hc, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
-        q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1)
+        q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         H̃_chol = cholesky(H̃)
         Ks, Ps = init_stochpred(estim, Hp)
         # dummy vals (updated just before optimization):
@@ -73,7 +73,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
-            H̃, q̃, p,
+            H̃, q̃, r,
             H̃_chol,
             Ks, Ps,
             d0, D̂0, D̂E,

src/controller/linmpc.jl

@@ -34,7 +34,7 @@ struct LinMPC{
     B::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
     d0::Vector{NT}
@@ -67,7 +67,7 @@ struct LinMPC{
         )
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
-        q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1)
+        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)
@@ -82,7 +82,7 @@ struct LinMPC{
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B, 
-            H̃, q̃, p,
+            H̃, q̃, r,
             Ks, Ps,
             d0, D̂0, D̂E,
             Uop, Yop, Dop,

src/controller/nonlinmpc.jl

@@ -36,7 +36,7 @@ struct NonLinMPC{
     B::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     Ks::Matrix{NT}
     Ps::Matrix{NT}
     d0::Vector{NT}
@@ -68,7 +68,7 @@ struct NonLinMPC{
         )
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
-        q̃, p = zeros(NT, size(H̃, 1)), zeros(NT, 1)
+        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)
@@ -83,7 +83,7 @@ struct NonLinMPC{
             R̂u0, R̂y0, noR̂u,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
-            H̃, q̃, p,
+            H̃, q̃, r,
             Ks, Ps,
             d0, D̂0, D̂E,
             Uop, Yop, Dop,

src/estimator/mhe/construct.jl

@@ -87,7 +87,7 @@ struct MovingHorizonEstimator{
     fx̄::Vector{NT}
     H̃::Hermitian{NT, Matrix{NT}}
     q̃::Vector{NT}
-    p::Vector{NT}
+    r::Vector{NT}
     P̂_0::Hermitian{NT, Matrix{NT}}
     Q̂::Hermitian{NT, Matrix{NT}}
     R̂::Hermitian{NT, Matrix{NT}}
@@ -128,9 +128,9 @@ struct MovingHorizonEstimator{
         invP̄ = Hermitian(inv(P̂_0), :L)
         invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L)
         invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L)
-        p = direct ? 0 : 1
+        r = direct ? 0 : 1
         E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe(
-            model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, p
+            model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, r
         )
         # dummy values (updated just before optimization):
         F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He)
@@ -139,7 +139,7 @@ struct MovingHorizonEstimator{
         )
         nZ̃ = size(Ẽ, 2)
         # dummy values, updated before optimization:
-        H̃, q̃, p = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1)
+        H̃, q̃, r = Hermitian(zeros(NT, nZ̃, nZ̃), :L), zeros(NT, nZ̃), zeros(NT, 1)
         Z̃ = zeros(NT, nZ̃)
         X̂op = repeat(x̂op, He)
         X̂0, Y0m = zeros(NT, nx̂*He), zeros(NT, nym*He)
@@ -159,7 +159,7 @@ struct MovingHorizonEstimator{
             As, Cs_u, Cs_y, nint_u, nint_ym,
             Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
             Ẽ, F, G, J, B, ẽx̄, fx̄,
-            H̃, q̃, p,
+            H̃, q̃, r,
             P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt,
             X̂op, X̂0, Y0m, U0, D0, Ŵ, 
             x̂0arr_old, P̂arr_old, Nk,

src/estimator/mhe/execute.jl

@@ -9,7 +9,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0)
     estim.Nk        .= 0
     estim.H̃         .= 0
     estim.q̃         .= 0
-    estim.p         .= 0
+    estim.r         .= 0
     if estim.direct
         # add u0(-1) and d0(-1) to the data windows:
         estim.U0[1:estim.model.nu] .= u0
@@ -211,15 +211,15 @@ end
 @doc raw"""
     initpred!(estim::MovingHorizonEstimator, model::LinModel) -> nothing
 
-Init quadratic optimization matrices `F, fx̄, H̃, q̃, p` for [`MovingHorizonEstimator`](@ref).
+Init quadratic optimization matrices `F, fx̄, H̃, q̃, r` for [`MovingHorizonEstimator`](@ref).
 
 See [`init_predmat_mhe`](@ref) for the definition of the vectors ``\mathbf{F, f_x̄}``. It
 also inits `estim.optim` objective function, expressed as the quadratic general form:
 ```math
-    J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + p 
+    J = \min_{\mathbf{Z̃}} \frac{1}{2}\mathbf{Z̃' H̃ Z̃} + \mathbf{q̃' Z̃} + r 
 ```
 in which ``\mathbf{Z̃} = [\begin{smallmatrix} ϵ \\ \mathbf{Z} \end{smallmatrix}]``. Note that
-``p`` is useless at optimization but required to evaluate the objective minima ``J``. The 
+``r`` is useless at optimization but required to evaluate the objective minima ``J``. The 
 Hessian ``\mathbf{H̃}`` matrix of the quadratic general form is not constant here because
 of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are:
 ```math
@@ -232,7 +232,7 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are:
     \mathbf{Ñ}_{N_k} &= \mathrm{diag}(C,  \mathbf{0},  \mathbf{Q̂}_{N_k}^{-1})               \\
     \mathbf{H̃}       &= 2(\mathbf{Ẽ_Z̃}' \mathbf{M}_{N_k} \mathbf{Ẽ_Z̃} + \mathbf{Ñ}_{N_k})   \\
     \mathbf{q̃}       &= 2(\mathbf{M}_{N_k} \mathbf{Ẽ_Z̃})' \mathbf{F_Z̃}                      \\
-            p        &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃}
+            r        &= \mathbf{F_Z̃}' \mathbf{M}_{N_k} \mathbf{F_Z̃}
 \end{aligned}
 ```
 """
@@ -257,7 +257,7 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     M_Nk_ẼZ̃ = M_Nk*ẼZ̃
     @views mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃)
     @views lmul!(2, estim.q̃[1:nZ̃])
-    estim.p .= dot(FZ̃, M_Nk, FZ̃)
+    estim.r .= dot(FZ̃, M_Nk, FZ̃)
     estim.H̃.data[1:nZ̃, 1:nZ̃] .= Ñ_Nk
     @views mul!(estim.H̃.data[1:nZ̃, 1:nZ̃], ẼZ̃', M_Nk_ẼZ̃, 1, 1) 
     @views lmul!(2, estim.H̃.data[1:nZ̃, 1:nZ̃])
@@ -460,7 +460,7 @@ It can be called on a [`MovingHorizonEstimator`](@ref) object to evaluate the ob
 function at specific `Z̃` and `V̂` values.
 """
 function obj_nonlinprog!( _ , estim::MovingHorizonEstimator, ::LinModel, _ , Z̃)
-    return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.p[]
+    return obj_quadprog(Z̃, estim.H̃, estim.q̃) + estim.r[]
 end
 
 """