debug : InternalModel for NonLinModel now works

franckgaga · franckgaga · commit 4d790c15e7e1 · 2023-09-18T12:16:36.000-04:00
diff --git a/docs/src/manual/linmpc.md b/docs/src/manual/linmpc.md
@@ -82,7 +82,7 @@ for details).
 
 [^1]: We could have use an [`InternalModel`](@ref) structure with
     `mpc = LinMPC(InternalModel(model), Hp=15, Hc=2, Mwt=[1, 1], Nwt=[0.1, 0.1])` to avoid
-    state estimator design . It was tested on the example of this page and it gives similar 
+    state estimator design. It was tested on the example of this page and it gives similar
     results.
 
 Before closing the loop, we call [`initstate!`](@ref) with the actual plant inputs and
diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl
@@ -2,7 +2,6 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
     estim::S
     ΔŨ::Vector{Float64}
     ŷ ::Vector{Float64}
-    Ŷs::Vector{Float64}
     Hp::Int
     Hc::Int
     M_Hp::Diagonal{Float64, Vector{Float64}}
@@ -29,12 +28,12 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
     Ps::Matrix{Float64}
     d::Vector{Float64}
     D̂::Vector{Float64}
-    Yop::Vector{Float64}
+    Ŷop::Vector{Float64}
     Dop::Vector{Float64}
     function ExplicitMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, ru) where {S<:StateEstimator}
         model = estim.model
         nu, ny, nd = model.nu, model.ny, model.nd
-        ŷ, Ŷs = zeros(ny), zeros(ny*Hp)
+        ŷ = zeros(ny)
         Cwt = Inf # no slack variable ϵ for ExplicitMPC
         Ewt = 0   # economic costs not supported for ExplicitMPC
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
@@ -52,20 +51,20 @@ struct ExplicitMPC{S<:StateEstimator} <: PredictiveController
         P̃_chol = cholesky(P̃)
         Ks, Ps = init_stochpred(estim, Hp)
         d, D̂ = zeros(nd), zeros(nd*Hp)
-        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
+        Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
         nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim,
-            ΔŨ, ŷ, Ŷs,
+            ΔŨ, ŷ,
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, K, Q, P̃, q̃, p,
             P̃_chol,
             Ks, Ps,
             d, D̂,
-            Yop, Dop,
+            Ŷop, Dop,
         )
         return mpc
     end
diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl
@@ -4,7 +4,6 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
     con::ControllerConstraint
     ΔŨ::Vector{Float64}
     ŷ ::Vector{Float64}
-    Ŷs::Vector{Float64}
     Hp::Int
     Hc::Int
     M_Hp::Diagonal{Float64, Vector{Float64}}
@@ -30,12 +29,12 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
     Ps::Matrix{Float64}
     d::Vector{Float64}
     D̂::Vector{Float64}
-    Yop::Vector{Float64}
+    Ŷop::Vector{Float64}
     Dop::Vector{Float64}
     function LinMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim) where {S<:StateEstimator}
         model = estim.model
         nu, ny, nd = model.nu, model.ny, model.nd
-        ŷ, Ŷs = zeros(ny), zeros(ny*Hp)
+        ŷ = zeros(ny)
         Ewt = 0   # economic costs not supported for LinMPC
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru)
         M_Hp = Diagonal{Float64}(repeat(Mwt, Hp))
@@ -51,19 +50,19 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d, D̂ = zeros(nd), zeros(nd*Hp)
-        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
+        Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
         nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim, optim, con,
-            ΔŨ, ŷ, Ŷs,
+            ΔŨ, ŷ,
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, K, Q, P̃, q̃, p,
             Ks, Ps,
             d, D̂,
-            Yop, Dop,
+            Ŷop, Dop,
         )
         init_optimization!(mpc)
         return mpc
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -6,7 +6,6 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
     con::ControllerConstraint
     ΔŨ::Vector{Float64}
     ŷ ::Vector{Float64}
-    Ŷs::Vector{Float64}
     Hp::Int
     Hc::Int
     M_Hp::Diagonal{Float64, Vector{Float64}}
@@ -33,14 +32,14 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
     Ps::Matrix{Float64}
     d::Vector{Float64}
     D̂::Vector{Float64}
-    Yop::Vector{Float64}
+    Ŷop::Vector{Float64}
     Dop::Vector{Float64}
     function NonLinMPC{S, JEFunc}(
         estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE::JEFunc, ru, optim
     ) where {S<:StateEstimator, JEFunc<:Function}
         model = estim.model
         nu, ny, nd = model.nu, model.ny, model.nd
-        ŷ, Ŷs = zeros(ny), zeros(ny*Hp)
+        ŷ = zeros(ny)
         validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, Ewt)
         M_Hp = Diagonal(convert(Vector{Float64}, repeat(Mwt, Hp)))
         N_Hc = Diagonal(convert(Vector{Float64}, repeat(Nwt, Hc)))
@@ -55,19 +54,19 @@ struct NonLinMPC{S<:StateEstimator, JEfunc<:Function} <: PredictiveController
         P̃, q̃, p = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d, D̂ = zeros(nd), zeros(nd*Hp)
-        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
+        Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
         nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim, optim, con,
-            ΔŨ, ŷ, Ŷs,
+            ΔŨ, ŷ,
             Hp, Hc, 
             M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u, R̂y,
             S̃_Hp, T_Hp, T_Hc, 
             Ẽ, F, G, J, K, Q, P̃, q̃, p,
             Ks, Ps,
             d, D̂,
-            Yop, Dop,
+            Ŷop, Dop,
         )
         init_optimization!(mpc)
         return mpc
@@ -319,59 +318,6 @@ function setnonlincon!(mpc::NonLinMPC, ::NonLinModel)
     return nothing
 end
 
-# TODO: déplacer les 2 prochaines méthodes dans predictive_control.jl
-
-"""
-    obj_nonlinprog(mpc::PredictiveController, model::LinModel, ΔŨ::Vector{Real})
-
-Objective function for [`NonLinMPC`](@ref) when `model` is a [`LinModel`](@ref).
-"""
-function obj_nonlinprog(mpc::PredictiveController, model::LinModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
-    J = obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃)
-    if !iszero(mpc.E)
-        U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + model.uop)
-        UE = [U; U[(end - model.nu + 1):end]]
-        ŶE = [mpc.ŷ; Ŷ]
-        D̂E = [mpc.d; mpc.D̂]
-        J += mpc.E*mpc.JE(UE, ŶE, D̂E)
-    end
-    return J
-end
-
-"""
-    obj_nonlinprog(mpc::PredictiveController, model::SimModel, ΔŨ::Vector{Real})
-
-Objective function for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
-"""
-function obj_nonlinprog(mpc::PredictiveController, model::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
-    # --- output setpoint tracking term ---
-    êy = mpc.R̂y - Ŷ
-    JR̂y = êy'*mpc.M_Hp*êy  
-    # --- move suppression and slack variable term ---
-    JΔŨ = ΔŨ'*mpc.Ñ_Hc*ΔŨ
-    # --- input over prediction horizon ---
-    if !isempty(mpc.R̂u) || !iszero(mpc.E)
-        U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + model.uop)
-    end
-    # --- input setpoint tracking term ---
-    if !isempty(mpc.R̂u)
-        êu = mpc.R̂u - U
-        JR̂u = êu'*mpc.L_Hp*êu
-    else
-        JR̂u = 0.0
-    end
-    # --- economic term ---
-    if !iszero(mpc.E)
-        UE = [U; U[(end - model.nu + 1):end]]
-        ŶE = [mpc.ŷ; Ŷ]
-        D̂E = [mpc.d; mpc.D̂]
-        E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E)
-    else
-        E_JE = 0.0
-    end
-    return JR̂y + JΔŨ + JR̂u + E_JE
-end
-
 
 """
     con_nonlinprog(mpc::NonLinMPC, ::LinModel, ΔŨ::Vector{Real})
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -517,7 +517,7 @@ of sigma points, and ``\mathbf{X̂}_{k-1}^j(k)``, the vector at the ``j``th colu
     \mathbf{X̄}_{k-1}(k) &= \begin{bmatrix} \mathbf{X̂}_{k-1}^{1}(k) - \mathbf{x̂}_{k-1}(k) & \mathbf{X̂}_{k-1}^{2}(k) - \mathbf{x̂}_{k-1}(k) & \cdots & \mathbf{X̂}_{k-1}^{n_σ}(k) - \mathbf{x̂}_{k-1}(k) \end{bmatrix} \\
     \mathbf{Ȳ^m}(k)     &= \begin{bmatrix} \mathbf{Ŷ^m}^{1}(k)     - \mathbf{ŷ^m}(k)     & \mathbf{Ŷ^m}^{2}(k)     - \mathbf{ŷ^m}(k)     & \cdots & \mathbf{Ŷ^m}^{n_σ}(k)     - \mathbf{ŷ^m}(k)     \end{bmatrix} \\
     \mathbf{M̂}(k)       &= \mathbf{Ȳ^m}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) + \mathbf{R̂} \\
-    \mathbf{K̂}(k)       &= \mathbf{X̄}_{k-1}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) \big[\mathbf{M̂}(k)\big]^{-1} \\
+    \mathbf{K̂}(k)       &= \mathbf{X̄}_{k-1}(k) \mathbf{Ŝ} \mathbf{Ȳ^m}'(k) \mathbf{M̂^{-1}}(k) \\
     \mathbf{x̂}_k(k)     &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k) \big[ \mathbf{y^m}(k) - \mathbf{ŷ^m}(k) \big] \\
     \mathbf{P̂}_k(k)     &= \mathbf{P̂}_{k-1}(k) - \mathbf{K̂}(k) \mathbf{M̂}(k) \mathbf{K̂}'(k) \\
     \mathbf{X̂}_k(k)     &= \bigg[\begin{matrix} \mathbf{x̂}_{k}(k) & \mathbf{x̂}_{k}(k) & \cdots & \mathbf{x̂}_{k}(k) \end{matrix}\bigg] + \bigg[\begin{matrix} \mathbf{0} & \gamma \sqrt{\mathbf{P̂}_{k}(k)} & - \gamma \sqrt{\mathbf{P̂}_{k}(k)} \end{matrix}\bigg] \\
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
diff --git a/src/state_estim.jl b/src/state_estim.jl
