lastu0 instead of lastu in StateEstimator

franckgaga · franckgaga · commit 35d90aa310d0 · 2023-04-26T16:56:32.000-04:00
diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl
@@ -233,7 +233,7 @@ function write_info!(mpc::LinMPC, ΔŨ, J, ŷs, Ŷs)
     mpc.info.ΔŨ = ΔŨ
     mpc.info.ϵ = isinf(mpc.C) ? NaN : ΔŨ[end]
     mpc.info.J = J
-    mpc.info.U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*mpc.estim.lastu
+    mpc.info.U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + mpc.estim.model.uop)
     mpc.info.u = mpc.info.U[1:mpc.estim.model.nu]
     mpc.info.ŷ = mpc.ŷ
     mpc.info.Ŷ = mpc.Ẽ*ΔŨ + mpc.F
diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl
@@ -1,6 +1,6 @@
 struct InternalModel{M<:SimModel} <: StateEstimator
     model::M
-    lastu::Vector{Float64}
+    lastu0::Vector{Float64}
     x̂::Vector{Float64}
     x̂d::Vector{Float64}
     x̂s::Vector{Float64}
@@ -16,7 +16,7 @@ struct InternalModel{M<:SimModel} <: StateEstimator
     Âs::Matrix{Float64}
     B̂s::Matrix{Float64}
     function InternalModel{M}(model::M, i_ym, Asm, Bsm, Csm, Dsm) where {M<:SimModel}
-        ny = model.ny
+        nu, ny = model.nu, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         if isa(model, LinModel)
             poles = eigvals(model.A)
@@ -38,12 +38,12 @@ struct InternalModel{M<:SimModel} <: StateEstimator
         nxs = size(As,1)
         Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
         i_ym = collect(i_ym)
-        lastu = copy(model.uop)
+        lastu0 = zeros(nu)
         x̂d = x̂ = copy(model.x) # x̂ and x̂d are same object (updating x̂d will update x̂)
         x̂s = zeros(nxs)
         return new(
             model, 
-            lastu, x̂, x̂d, x̂s, 
+            lastu0, x̂, x̂d, x̂s, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Bs, Cs, Ds, 
             Âs, B̂s
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -1,6 +1,6 @@
 struct SteadyKalmanFilter <: StateEstimator
     model::LinModel
-    lastu::Vector{Float64}
+    lastu0::Vector{Float64}
     x̂::Vector{Float64}
     i_ym::Vector{Int}
     nx̂::Int
@@ -21,7 +21,7 @@ struct SteadyKalmanFilter <: StateEstimator
     R̂::Hermitian{Float64, Matrix{Float64}}
     K::Matrix{Float64}
     function SteadyKalmanFilter(model, i_ym, nint_ym, Asm, Csm, Q̂, R̂)
-        nx, ny = model.nx, model.ny
+        nu, nx, ny = model.nu, model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
@@ -39,13 +39,13 @@ struct SteadyKalmanFilter <: StateEstimator
             end
         end
         i_ym = collect(i_ym)
-        lastu = copy(model.uop)
+        lastu0 = zeros(nu)
         x̂ = [copy(model.x); zeros(nxs)]
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)
         return new(
             model, 
-            lastu, x̂, 
+            lastu0, x̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs, nint_ym,
             Â, B̂u, B̂d, Ĉ, D̂d, 
@@ -170,7 +170,7 @@ end
 
 struct KalmanFilter <: StateEstimator
     model::LinModel
-    lastu::Vector{Float64}
+    lastu0::Vector{Float64}
     x̂::Vector{Float64}
     P̂::Hermitian{Float64, Matrix{Float64}}
     i_ym::Vector{Int}
@@ -192,7 +192,7 @@ struct KalmanFilter <: StateEstimator
     Q̂::Hermitian{Float64, Matrix{Float64}}
     R̂::Hermitian{Float64, Matrix{Float64}}
     function KalmanFilter(model, i_ym, nint_ym, Asm, Csm, P̂0, Q̂, R̂)
-        nx, ny = model.nx, model.ny
+        nu, nx, ny = model.nu, model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
@@ -201,15 +201,15 @@ struct KalmanFilter <: StateEstimator
         Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         i_ym = collect(i_ym)
-        lastu = copy(model.uop)
+        lastu0 = zeros(nu)
         x̂ = [copy(model.x); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)
         P̂ = copy(P̂0)
         return new(
             model, 
-            lastu, x̂, P̂, 
+            lastu0, x̂, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs, nint_ym,
             Â, B̂u, B̂d, Ĉ, D̂d, 
@@ -322,7 +322,7 @@ end
 
 struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
     model::M
-    lastu::Vector{Float64}
+    lastu0::Vector{Float64}
     x̂::Vector{Float64}
     P̂::Hermitian{Float64, Matrix{Float64}}
     i_ym::Vector{Int}
@@ -343,23 +343,23 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
     function UnscentedKalmanFilter{M}(
         model::M, i_ym, nint_ym, Asm, Csm, P̂0, Q̂, R̂, α, β, κ
     ) where {M<:SimModel}
-        nx, ny = model.nx, model.ny
+        nu, nx, ny = model.nu, model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
         nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
         i_ym = collect(i_ym)
-        lastu = copy(model.uop)
+        lastu0 = zeros(nu)
         x̂ = [copy(model.x); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)
         P̂ = copy(P̂0)
         return new(
             model,
-            lastu, x̂, P̂, 
+            lastu0, x̂, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs, nint_ym,
             P̂0, Q̂, R̂,
diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl
@@ -1,6 +1,6 @@
 struct Luenberger <: StateEstimator
     model::LinModel
-    lastu::Vector{Float64}
+    lastu0::Vector{Float64}
     x̂::Vector{Float64}
     i_ym::Vector{Int}
     nx̂::Int
@@ -19,7 +19,7 @@ struct Luenberger <: StateEstimator
     D̂dm ::Matrix{Float64}
     K::Matrix{Float64}
     function Luenberger(model, i_ym, nint_ym, Asm, Csm, L)
-        nx, ny = model.nx, model.ny
+        nu, nx, ny = model.nu, model.nx, model.ny
         nym, nyu = length(i_ym), ny - length(i_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
@@ -29,11 +29,11 @@ struct Luenberger <: StateEstimator
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
         K = L
         i_ym = collect(i_ym)
-        lastu = copy(model.uop)
+        lastu0 = zeros(nu)
         x̂ = [copy(model.x); zeros(nxs)]
         return new(
             model, 
-            lastu, x̂,
+            lastu0, x̂,
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs, nint_ym,
             Â, B̂u, B̂d, Ĉ, D̂d, 
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
@@ -263,7 +263,7 @@ function moveinput!(
     ΔŨ, J = optim_objective!(mpc, p)
     write_info!(mpc, ΔŨ, J, ŷs, Ŷs)
     Δu = ΔŨ[1:mpc.estim.model.nu] # receding horizon principle: only Δu(k) is used (1st one)
-    u = mpc.estim.lastu + Δu
+    u = mpc.estim.lastu0 + mpc.estim.model.uop + Δu
     return u
 end
 
@@ -344,23 +344,23 @@ end
 
 
 @doc raw"""
-    initpred!(mpc, model::LinModel, d, D̂, Ŷs, R̂y, x̂d, lastu)
+    initpred!(mpc, model::LinModel, d, D̂, Ŷs, R̂y)
 
 Init linear model prediction matrices `F`, `q̃` and `p`.
 
 See [`init_deterpred`](@ref) and [`init_quadprog`](@ref) for the definition of the matrices.
 """
 function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, Ŷs, R̂y)
-    mpc.F[:] = mpc.Kd*mpc.x̂d + mpc.Q*(mpc.estim.lastu - model.uop) + Ŷs + mpc.Yop
+    mpc.F[:] = mpc.Kd*mpc.x̂d + mpc.Q*mpc.estim.lastu0 + Ŷs + mpc.Yop
     if model.nd ≠ 0
         mpc.F .+= mpc.G*(d - model.dop) + mpc.J*(D̂ - mpc.Dop)
     end
     Ẑ = mpc.F - R̂y
     mpc.q̃[:] = 2(mpc.M_Hp*mpc.Ẽ)'*Ẑ
     p = Ẑ'*mpc.M_Hp*Ẑ
     if ~isempty(mpc.R̂u)
-        V̂ = (mpc.T_Hp*mpc.estim.lastu - mpc.R̂u)
-        mpc.q̃ .+= 2(mpc.L_Hp*mpc.T_Hp)'*V̂
+        V̂ = mpc.T_Hp*(mpc.estim.lastu0 + model.uop) - mpc.R̂u
+        mpc.q̃[:] = mpc.q̃ + 2(mpc.L_Hp*mpc.T_Hp)'*V̂
         p += V̂'*mpc.L_Hp*V̂
     end
     #d0 = zeros(model.nd, 0)         # only used for NonLinModel objects
@@ -369,7 +369,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, Ŷs, R̂
 end
 
 @doc raw"""
-    initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs, _ , _ , _ )
+    initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs, _ )
 
 Init `F`, `d0` and `D̂0` prediction matrices for [`NonLinModel`](@ref).
 
@@ -387,14 +387,14 @@ function initpred!(mpc::PredictiveController, model::NonLinModel, d, D̂, Ŷs ,
 end
 
 @doc raw"""
-    linconstraint!(mpc::PredictiveController, ::LinModel, lastu)
+    linconstraint!(mpc::PredictiveController, model::LinModel)
 
 Calc `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
 """
-function linconstaint!(mpc::PredictiveController, ::LinModel)
+function linconstaint!(mpc::PredictiveController, model::LinModel)
     mpc.con.b[:] = [
-        -mpc.con.Umin + mpc.T_Hc*mpc.estim.lastu
-        +mpc.con.Umax - mpc.T_Hc*mpc.estim.lastu
+        -mpc.con.Umin + mpc.T_Hc*(mpc.estim.lastu0 + model.uop)
+        +mpc.con.Umax - mpc.T_Hc*(mpc.estim.lastu0 + model.uop)
         -mpc.con.ΔŨmin
         +mpc.con.ΔŨmax 
         -mpc.con.Ŷmin + mpc.F
@@ -403,14 +403,14 @@ function linconstaint!(mpc::PredictiveController, ::LinModel)
 end
 
 @doc raw"""
-    linconstraint!(mpc::PredictiveController, ::NonLinModel, lastu)
+    linconstraint!(mpc::PredictiveController, model::NonLinModel)
 
 Calc `b` without predicted output ``\mathbf{Ŷ}`` constraints for [`NonLinModel`](@ref). 
 """
-function linconstaint!(mpc::PredictiveController, ::NonLinModel)
+function linconstaint!(mpc::PredictiveController, model::NonLinModel)
     mpc.con.b[:] = [
-        -mpc.con.Umin + mpc.T_Hc*mpc.estim.lastu
-        +mpc.con.Umax - mpc.T_Hc*mpc.estim.lastu
+        -mpc.con.Umin + mpc.T_Hc*(mpc.estim.lastu0 + model.uop)
+        +mpc.con.Umax - mpc.T_Hc*(mpc.estim.lastu0 + model.uop)
         -mpc.con.ΔŨmin
         +mpc.con.ΔŨmax 
     ]
diff --git a/src/state_estim.jl b/src/state_estim.jl
@@ -48,14 +48,14 @@ end
 
 Remove operating points on inputs `u`, measured outputs `ym` and disturbances `d`.
 
-Also store current inputs `u` in `estim.lastu`. This field is used for
-[`PredictiveController`](@ref) computations.
+Also store current inputs without operating points `u0` in `estim.lastu0`. This field is 
+used for [`PredictiveController`](@ref) computations.
 """
 function remove_op!(estim::StateEstimator, u, d, ym)
     u0  = u  - estim.model.uop
     d0  = d  - estim.model.dop
     ym0 = ym - estim.model.yop[estim.i_ym]
-    estim.lastu[:] = u
+    estim.lastu0[:] = u0
     return u0, d0, ym0
 end
 
@@ -220,8 +220,8 @@ julia> x̂ = initstate!(estim, [1], [3 - 0.1])
 """
 function initstate!(estim::StateEstimator, u, ym, d=Float64[])
     model = estim.model
-    # --- init lastu, used in PredictiveController ---
-    estim.lastu[:] = u
+    # --- init lastu0 for PredictiveControllers ---
+    estim.lastu0[:] = u - model.uop
     # --- deterministic model states ---
     x̂d = init_deterstate(model, estim, u, d)
     # --- stochastic model states (integrators) ---
