removed : warm starting with unconstrained solution

Author: franckgaga

docs/src/internals/state_estim.md

@@ -3,7 +3,7 @@
 ## Estimator Initialization
 
 ```@docs
-ModelPredictiveControl.init_estimstoch
+ModelPredictiveControl.init_integrators
 ModelPredictiveControl.augment_model
 ModelPredictiveControl.init_ukf
 ModelPredictiveControl.init_internalmodel

src/estimator/internal_model.jl

@@ -32,7 +32,6 @@ struct InternalModel{M<:SimModel} <: StateEstimator
         nx̂ = model.nx
         nxs = size(As,1)
         Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds)
-        i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂d = x̂ = zeros(model.nx) # x̂ and x̂d are same object (updating x̂d will update x̂)
         x̂s = zeros(nxs)

src/estimator/kalman.jl

@@ -21,17 +21,12 @@ struct SteadyKalmanFilter <: StateEstimator
     R̂::Hermitian{Float64, Matrix{Float64}}
     K::Matrix{Float64}
     function SteadyKalmanFilter(model, i_ym, nint_ym, Q̂, R̂)
-        nu, nx, ny = model.nu, model.nx, model.ny
-        nym, nyu = length(i_ym), ny - length(i_ym)
-        Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_ym)
-        nxs = size(Asm,1)
-        nx̂ = nx + nxs
+        nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
+        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         validate_kfcov(nym, nx̂, Q̂, R̂)
-        As, _ , Cs  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         K = try
             Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6
-            R̂_kalman = zeros(eltype(R̂), ny, ny)
+            R̂_kalman = zeros(eltype(R̂), model.ny, model.ny)
             R̂_kalman[i_ym, i_ym] = R̂
             kalman(Discrete, Â, Ĉ, Q̂_kalman, R̂_kalman)[:, i_ym] 
         catch my_error
@@ -44,8 +39,7 @@ struct SteadyKalmanFilter <: StateEstimator
             end
         end
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
-        i_ym = collect(i_ym)
-        lastu0 = zeros(nu)
+        lastu0 = zeros(model.nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
         Q̂ = Hermitian(Q̂, :L)
         R̂ = Hermitian(R̂, :L)
@@ -123,7 +117,7 @@ as state feedback. The method [`default_nint`](@ref) computes the default value
 - Use 1 integrator if the disturbances on the output are typically "step-like"
 - Use 2 integrators if the disturbances on the output are typically "ramp-like" 
 
-The function [`init_estimstoch`](@ref) builds the stochastic model from `nint_ym`.
+The function [`init_integrators`](@ref) builds the stochastic model from `nint_ym`.
 
 !!! tip
     Increasing `σQ_int` values increases the integral action "gain".
@@ -198,17 +192,11 @@ struct KalmanFilter <: StateEstimator
     Q̂::Hermitian{Float64, Matrix{Float64}}
     R̂::Hermitian{Float64, Matrix{Float64}}
     function KalmanFilter(model, i_ym, nint_ym, P̂0, Q̂, R̂)
-        nu, nx, ny = model.nu, model.nx, model.ny
-        nym, nyu = length(i_ym), ny - length(i_ym)
-        Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_ym)
-        nxs = size(Asm,1)
-        nx̂ = nx + nxs
+        nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
+        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
-        As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
-        i_ym = collect(i_ym)
-        lastu0 = zeros(nu)
+        lastu0 = zeros(model.nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)
@@ -335,17 +323,11 @@ struct UnscentedKalmanFilter{M<:SimModel} <: StateEstimator
     function UnscentedKalmanFilter{M}(
         model::M, i_ym, nint_ym, P̂0, Q̂, R̂, α, β, κ
     ) where {M<:SimModel}
-        nu, nx, ny = model.nu, model.nx, model.ny
-        nym, nyu = length(i_ym), ny - length(i_ym)
-        Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_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, [])
+        nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
         augment_model(model, As, Cs) # verify observability for LinModel
+        validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
-        i_ym = collect(i_ym)
-        lastu0 = zeros(nu)
+        lastu0 = zeros(model.nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)
@@ -555,16 +537,10 @@ struct ExtendedKalmanFilter{M<:SimModel} <: StateEstimator
     Q̂::Hermitian{Float64, Matrix{Float64}}
     R̂::Hermitian{Float64, Matrix{Float64}}
     function ExtendedKalmanFilter{M}(model::M, i_ym, nint_ym, P̂0, Q̂, R̂) where {M<:SimModel}
-        nu, nx, ny = model.nu, model.nx, model.ny
-        nym, nyu = length(i_ym), ny - length(i_ym)
-        Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_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, [])
+        nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
         augment_model(model, As, Cs) # verify observability for LinModel
-        i_ym = collect(i_ym)
-        lastu0 = zeros(nu)
+        validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
+        lastu0 = zeros(model.nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
         Q̂ = Hermitian(Q̂, :L)

src/estimator/luenberger.jl

@@ -19,21 +19,15 @@ struct Luenberger <: StateEstimator
     D̂dm ::Matrix{Float64}
     K::Matrix{Float64}
     function Luenberger(model, i_ym, nint_ym, p̂)
-        nu, nx, ny = model.nu, model.nx, model.ny
-        nym, nyu = length(i_ym), ny - length(i_ym)
-        Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_ym)
-        nxs = size(Asm,1)
-        nx̂ = nx + nxs
-        As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        Â , B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
+        nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
+        Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
         K = try
             place(Â, Ĉ, p̂, :o)[:, i_ym]
         catch
             error("Cannot compute the Luenberger gain L with specified poles p̂.")
         end
         Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] # measured outputs ym only
-        i_ym = collect(i_ym)
-        lastu0 = zeros(nu)
+        lastu0 = zeros(model.nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
         return new(
             model, 

src/predictive_control.jl

@@ -336,17 +336,12 @@ Set the estimate at `mpc.estim.x̂`.
 setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)
 
 @doc raw"""
-    initstate!(mpc::PredictiveController, u, ym, d=Float64[]) -> x̂
+    initstate!(mpc::PredictiveController, u, ym, d=Float64[])
 
-Init `mpc.ΔŨ` for warm-starting and the states of `mpc.estim` [`StateEstimator`](@ref).
-
-Before calling [`initstate!(mpc.estim, u, ym, d)`](@ref), it warm-starts ``\mathbf{ΔŨ}``:
-- If `model` is a [`LinModel`](@ref), the vector is filled with the analytical minimum ``J``
-  of the unconstrained problem.
-- Else, the vector is filled with zeros.
+Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero.
 """
 function initstate!(mpc::PredictiveController, u, ym, d=Float64[])
-    mpc.ΔŨ[:] = unconstrained_solution(mpc, mpc.estim.model)
+    mpc.ΔŨ .= 0
     return initstate!(mpc.estim, u, ym, d)
 end
 

src/state_estim.jl

@@ -69,6 +69,23 @@ function remove_op!(estim::StateEstimator, u, d, ym)
     return u0, d0, ym0
 end
 
+"""
+    init_estimstoch(model, i_ym, nint_ym)
+
+Init stochastic model matrices from integrator specifications for state estimation.
+"""
+function init_estimstoch(model, i_ym, nint_ym)
+    validate_ym(model, i_ym)
+    nx, ny = model.nx, model.ny
+    nym, nyu = length(i_ym), ny - length(i_ym)
+    Asm, Csm, nint_ym = init_integrators(i_ym, nint_ym)
+    nxs = size(Asm,1)
+    nx̂ = nx + nxs
+    As, _ , Cs  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
+    return nym, nyu, nxs, nx̂, As, Cs, nint_ym
+end
+
+"Validate the specified measured output indices `i_ym`."
 function validate_ym(model::SimModel, i_ym)
     if length(unique(i_ym)) ≠ length(i_ym) || maximum(i_ym) > model.ny
         error("Measured output indices i_ym should contains valid and unique indices")
@@ -91,7 +108,7 @@ function stoch_ym2y(model::SimModel, i_ym, Asm, Bsm, Csm, Dsm)
 end
 
 @doc raw"""
-    init_estimstoch(i_ym, nint_ym::Vector{Int}) -> Asm, Csm, nint_ym
+    init_integrators(i_ym, nint_ym::Vector{Int}) -> Asm, Csm, nint_ym
 
 Calc stochastic model matrices from output integrators specifications for state estimation.
 
@@ -107,7 +124,7 @@ be added for each measured output ``\mathbf{y^m}``. The argument generates the `
 where ``\mathbf{e}(k)`` is a conceptual and unknown zero mean white noise. 
 ``\mathbf{B_s^m}`` is not used for closed-loop state estimators thus ignored.
 """
-function init_estimstoch(i_ym, nint_ym)
+function init_integrators(i_ym, nint_ym)
     if nint_ym == 0 # alias for no output integrator at all
         nint_ym = fill(0, length(i_ym))
     end
@@ -134,7 +151,7 @@ function init_estimstoch(i_ym, nint_ym)
     return Asm, Csm, nint_ym
 end
 
-function init_estimstoch_u(nu, nint_u)
+function init_integrators_u(nu, nint_u)
     if nint_u == 0 # alias for no output integrator at all
         nint_u = fill(0, length(nu))
     end
@@ -167,7 +184,7 @@ end
 Augment [`LinModel`](@ref) state-space matrices with the stochastic ones `As` and `Cs`.
 
 If ``\mathbf{x}`` are `model.x` states, and ``\mathbf{x_s}``, the states defined at
-[`init_estimstoch`](@ref), we define an augmented state vector ``\mathbf{x̂} = 
+[`init_integrators`](@ref), we define an augmented state vector ``\mathbf{x̂} = 
 [ \begin{smallmatrix} \mathbf{x} \\ \mathbf{x_s} \end{smallmatrix} ]``. The method
 returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`:
 ```math
@@ -187,7 +204,7 @@ function augment_model(model::LinModel, As, Cs; verify_obsv=true)
     B̂d  = [model.Bd; zeros(nxs,nd)]
     D̂d  = model.Dd
     # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny:
-    if verify_obsv && !observability(Â, Ĉ)[begin]
+    if verify_obsv && !observability(Â, Ĉ)[:isobservable]
         error("The augmented model is unobservable. You may try to use 0 "*
               "integrator on model integrating outputs with nint_ym parameter.")
     end
@@ -221,12 +238,11 @@ function default_nint(model::LinModel, i_ym::IntRangeOrVector = 1:model.ny)
     nint_ym = fill(0, length(i_ym))
     for i in eachindex(i_ym)
         nint_ym[i]  = 1
-        Asm, Csm    = init_estimstoch(i_ym, nint_ym)
+        Asm, Csm    = init_integrators(i_ym, nint_ym)
         As , _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
         Â  , _ , Ĉ  = augment_model(model, As, Cs, verify_obsv=false)
         # observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny
-        isobservable, = observability(Â, Ĉ)
-        isobservable || (nint_ym[i] = 0)
+        observability(Â, Ĉ)[:isobservable] || (nint_ym[i] = 0)
     end
     return nint_ym
 end