Merge pull request #52 from JuliaControl/adapt_linmpc

Added: adaptatation of `LinMPC` model through  `setmodel!`

Author: franckgaga

docs/src/public/generic_func.md

@@ -37,6 +37,12 @@ initstate!
 setstate!
 ```
 
+## Set Plant Model
+
+```@docs
+setmodel!
+```
+
 ## Quick Simulation
 
 ### Simulate

src/ModelPredictiveControl.jl

@@ -23,7 +23,7 @@ import OSQP, Ipopt
 
 export SimModel, LinModel, NonLinModel
 export DiffSolver, RungeKutta
-export setop!, setstate!, updatestate!, evaloutput, linearize
+export setop!, setstate!, setmodel!, updatestate!, evaloutput, linearize
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
 export MovingHorizonEstimator

src/controller/construct.jl

@@ -548,7 +548,7 @@ function init_quadprog(::LinModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:
 end
 "Return empty matrices if `model` is not a [`LinModel`](@ref)."
 function init_quadprog(::SimModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:Real}
-    H̃ = Hermitian(zeros(NT, 0, 0))
+    H̃ = Hermitian(zeros(NT, 0, 0), :L)
     q̃ = zeros(NT, 0)
     p = zeros(NT, 1)            # dummy value (updated just before optimization)
     return H̃, q̃, p
@@ -679,7 +679,7 @@ function relaxΔU(::SimModel{NT}, C, C_Δumin, C_Δumax, ΔUmin, ΔUmax, N_Hc) w
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]]
         A_ϵ = [zeros(NT, 1, length(ΔUmin)) NT[1.0]]
         A_ΔŨmin, A_ΔŨmax = -[I  C_Δumin; A_ϵ], [I -C_Δumax; A_ϵ]
-        Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1);zeros(NT, 1, nΔU) C])
+        Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1);zeros(NT, 1, nΔU) C], :L)
     else # ΔŨ = ΔU (only hard constraints)
         ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
         I_Hc = Matrix{NT}(I, nΔU, nΔU)

src/controller/execute.jl

@@ -474,3 +474,10 @@ Call [`updatestate!`](@ref) on `mpc.estim` [`StateEstimator`](@ref).
 """
 updatestate!(mpc::PredictiveController, u, ym, d=empty(mpc.estim.x̂)) = updatestate!(mpc.estim,u,ym,d)
 updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym"))
+
+"""
+    setstate!(mpc::PredictiveController, x̂)
+
+Set the estimate at `mpc.estim.x̂`.
+"""
+setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)

src/controller/explicitmpc.jl

@@ -42,7 +42,9 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         Ewt = 0   # economic costs not supported for ExplicitMPC
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
         # Matrix() call is needed to convert `Diagonal` to normal `Matrix`
-        M_Hp, N_Hc, L_Hp = Hermitian(Matrix(M_Hp)), Hermitian(Matrix(N_Hc)), Hermitian(Matrix(L_Hp))
+        M_Hp = Hermitian(Matrix(M_Hp), :L) 
+        N_Hc = Hermitian(Matrix(N_Hc), :L)
+        L_Hp = Hermitian(Matrix(L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)

src/controller/linmpc.jl

@@ -50,7 +50,9 @@ struct LinMPC{
         Ewt = 0   # economic costs not supported for LinMPC
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
         # Matrix() call is needed to convert `Diagonal` to normal `Matrix`
-        M_Hp, N_Hc, L_Hp = Hermitian(Matrix(M_Hp)), Hermitian(Matrix(N_Hc)), Hermitian(Matrix(L_Hp))
+        M_Hp = Hermitian(Matrix(M_Hp), :L) 
+        N_Hc = Hermitian(Matrix(N_Hc), :L)
+        L_Hp = Hermitian(Matrix(L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)

src/controller/nonlinmpc.jl

@@ -51,7 +51,9 @@ struct NonLinMPC{
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
         # Matrix() call is needed to convert `Diagonal` to normal `Matrix`
-        M_Hp, N_Hc, L_Hp = Hermitian(Matrix(M_Hp)), Hermitian(Matrix(N_Hc)), Hermitian(Matrix(L_Hp))
+        M_Hp = Hermitian(Matrix(M_Hp), :L) 
+        N_Hc = Hermitian(Matrix(N_Hc), :L)
+        L_Hp = Hermitian(Matrix(L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         noR̂u = iszero(L_Hp)

src/estimator/execute.jl

@@ -221,3 +221,60 @@ function validate_args(estim::StateEstimator, u, ym, d)
     nym = estim.nym
     size(ym) ≠ (nym,) && throw(DimensionMismatch("ym size $(size(ym)) ≠ meas. output size ($nym,)"))
 end
+
+"""
+    setstate!(estim::StateEstimator, x̂)
+
+Set `estim.x̂` states to values specified by `x̂`. 
+"""
+function setstate!(estim::StateEstimator, x̂)
+    size(x̂) == (estim.nx̂,) || error("x̂ size must be $((estim.nx̂,))")
+    estim.x̂[:] = x̂
+    return estim
+end
+
+"""
+    setmodel!(estim::StateEstimator, model::LinModel) -> estim
+
+Set `estim.model` state-space matrices and operating points to `model` values.
+
+Not supported by [`Luenberger`](@ref) and [`SteadyKalmanFilter`](@ref) estimators, use the
+time-varying [`KalmanFilter`](@ref) instead. The matrix dimensions and sample time must stay
+the same. The observability and controllability of the new augmented model is not verified.
+"""
+function setmodel!(estim::StateEstimator, model::LinModel)
+    validate_model(estim, model)
+    estim.model.A   .= model.A
+    estim.model.Bu  .= model.Bu
+    estim.model.C   .= model.C
+    estim.model.Bd  .= model.Bd
+    estim.model.Dd  .= model.Dd
+    estim.model.uop .= model.uop
+    estim.model.yop .= model.yop
+    estim.model.dop .= model.dop
+    setmodel_estimator!(estim, model)
+    return estim
+end
+
+"Validate the dimensions and sample time of `model` against `estim.model`."
+function validate_model(estim::StateEstimator, model::LinModel)
+    model.Ts == estim.model.Ts || throw(ArgumentError("model.Ts must be $(estim.model.Ts) s"))
+    model.nu == estim.model.nu || throw(ArgumentError("model.nu must be $(estim.model.nu)"))
+    model.nx == estim.model.nx || throw(ArgumentError("model.nx must be $(estim.model.nx)"))
+    model.ny == estim.model.ny || throw(ArgumentError("model.ny must be $(estim.model.ny)"))
+    model.nd == estim.model.nd || throw(ArgumentError("model.nd must be $(estim.model.nd)"))
+end
+
+"Update the augmented model matrices of `estim` by default."
+function setmodel_estimator!(estim::StateEstimator, model::LinModel)
+    As, Cs_u, Cs_y = estim.As, estim.Cs_u, estim.Cs_y
+    Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y, verify_obsv=false)
+    estim.  .= Â
+    estim.B̂u .= B̂u
+    estim.Ĉ  .= Ĉ
+    estim.B̂d .= B̂d
+    estim.D̂d .= D̂d
+    estim.Ĉm  .= @views Ĉ[estim.i_ym, :]
+    estim.D̂dm .= @views D̂d[estim.i_ym, :]
+    return nothing
+end

src/estimator/internal_model.jl

@@ -197,6 +197,9 @@ function init_internalmodel(As, Bs, Cs, Ds)
     return Âs, B̂s
 end
 
+"Do nothing else for `InternalModel` estimator."
+setmodel_estimator!(estim::InternalModel, ::LinModel) = nothing
+
 @doc raw"""
     update_estimate!(estim::InternalModel, u, ym, d=empty(estim.x̂)) -> x̂d
 

src/estimator/kalman.jl

@@ -166,6 +166,10 @@ function SteadyKalmanFilter(model::SM, i_ym, nint_u, nint_ym, Q̂, R̂) where {N
     return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂)
 end
 
+"Throw an error if `setmodel!` is called on a SteadyKalmanFilter"
+function setmodel_estimator!(estim::SteadyKalmanFilter, _ )
+    error("SteadyKalmanFilter does not support setmodel! (use KalmanFilter instead)")
+end
 
 
 @doc raw"""
@@ -192,7 +196,7 @@ function update_estimate!(estim::SteadyKalmanFilter, u, ym, d=empty(estim.x̂))
     mul!(x̂next, B̂u, u, 1, 1)
     mul!(x̂next, B̂d, d, 1, 1)
     mul!(x̂next, K̂, v̂, 1, 1)
-    x̂ .= x̂next
+    estim.x̂ .= x̂next
     return nothing
 end
 
@@ -347,6 +351,7 @@ function update_estimate!(estim::KalmanFilter, u, ym, d)
     return update_estimate_kf!(estim, u, ym, d, estim.Â, estim.Ĉm, estim.P̂, estim.x̂)
 end
 
+
 struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     model::SM
     lastu0::Vector{NT}
@@ -368,12 +373,13 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     B̂d::Matrix{NT}
     D̂d::Matrix{NT}
     P̂0::Hermitian{NT, Matrix{NT}}
-    Q̂ ::Hermitian{NT, Matrix{NT}}
-    R̂ ::Hermitian{NT, Matrix{NT}}
+    Q̂::Hermitian{NT, Matrix{NT}}
+    R̂::Hermitian{NT, Matrix{NT}}
     K̂::Matrix{NT}
     M̂::Hermitian{NT, Matrix{NT}}
-    X̂ ::Matrix{NT}
+    X̂::Matrix{NT}
     Ŷm::Matrix{NT}
+    sqrtP̂::LowerTriangular{NT, Matrix{NT}}
     nσ::Int 
     γ::NT
     m̂::Vector{NT}
@@ -396,14 +402,15 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
         K̂ = zeros(NT, nx̂, nym)
         M̂ = Hermitian(zeros(NT, nym, nym), :L)
         X̂, Ŷm = zeros(NT, nx̂, nσ), zeros(NT, nym, nσ)
+        sqrtP̂ = LowerTriangular(zeros(NT, nx̂, nx̂))
         return new{NT, SM}(
             model,
             lastu0, x̂, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs_u, Cs_y, nint_u, nint_ym,
             Â, B̂u, Ĉ, B̂d, D̂d,
             P̂0, Q̂, R̂,
-            K̂, M̂, X̂, Ŷm,
+            K̂, M̂, X̂, Ŷm, sqrtP̂,
             nσ, γ, m̂, Ŝ
         )
     end
@@ -571,21 +578,23 @@ noise, respectively.
 """
 function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:Real
     x̂, P̂, Q̂, R̂, K̂, M̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂, estim.M̂
-    X̂, Ŷm = estim.X̂, estim.Ŷm
     nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
+    X̂, Ŷm = estim.X̂, estim.Ŷm
+    sqrtP̂ = estim.sqrtP̂
     # --- initialize matrices ---
-    X̂_next = Matrix{NT}(undef, nx̂, nσ)
+    x̂next = Vector{NT}(undef, nx̂)
     û  = Vector{NT}(undef, estim.model.nu)
     ŷm = Vector{NT}(undef, nym)
     ŷ  = Vector{NT}(undef, estim.model.ny)
-    sqrt_P̂ = LowerTriangular{NT, Matrix{NT}}(Matrix{NT}(undef, nx̂, nx̂))
     # --- correction step ---
-    sqrt_P̂ .= cholesky(P̂).L
-    γ_sqrt_P̂ = lmul!(γ, sqrt_P̂)
+    P̂_chol  = sqrtP̂.data
+    P̂_chol .= P̂
+    cholesky!(Hermitian(P̂_chol, :L)) # also modifies sqrtP̂
+    γ_sqrtP̂ = lmul!(γ, sqrtP̂) 
     X̂ .= x̂
-    X̂[:, 2:nx̂+1]   .+= γ_sqrt_P̂
-    X̂[:, nx̂+2:end] .-= γ_sqrt_P̂
+    X̂[:, 2:nx̂+1]   .+= γ_sqrtP̂
+    X̂[:, nx̂+2:end] .-= γ_sqrtP̂
     for j in axes(Ŷm, 2)
         @views ĥ!(ŷ, estim, estim.model, X̂[:, j], d)
         @views Ŷm[:, j] .= ŷ[estim.i_ym]
@@ -594,27 +603,36 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     X̄, Ȳm = X̂, Ŷm
     X̄  .= X̂  .- x̂
     Ȳm .= Ŷm .- ŷm
-    M̂ .= Hermitian(Ȳm * Ŝ * Ȳm' .+ R̂)
+    M̂.data .= Ȳm * Ŝ * Ȳm' .+ R̂
     mul!(K̂, X̄, lmul!(Ŝ, Ȳm'))
     rdiv!(K̂, cholesky(M̂))
     v̂ = ŷm
     v̂ .= ym .- ŷm
-    x̂_cor = x̂ + K̂ * v̂
-    P̂_cor = P̂ - Hermitian(K̂ * M̂ * K̂', :L)
+    x̂cor  = x̂next
+    x̂cor .= x̂
+    mul!(x̂cor, K̂, v̂, 1, 1)
+    P̂cor = Hermitian(P̂ .- K̂ * M̂ * K̂', :L)
     # --- prediction step ---
-    X̂_cor, sqrt_P̂_cor = X̂, sqrt_P̂
-    sqrt_P̂_cor .= cholesky(P̂_cor).L
-    γ_sqrt_P̂_cor = lmul!(γ, sqrt_P̂_cor)
-    X̂_cor .= x̂_cor
-    X̂_cor[:, 2:nx̂+1]   .+= γ_sqrt_P̂_cor
-    X̂_cor[:, nx̂+2:end] .-= γ_sqrt_P̂_cor
-    for j in axes(X̂_next, 2)
-        @views f̂!(X̂_next[:, j], û, estim, estim.model, X̂_cor[:, j], u, d)
+    X̂cor, sqrtP̂cor = X̂, sqrtP̂
+    P̂cor_chol  = sqrtP̂cor.data
+    P̂cor_chol .= P̂cor
+    cholesky!(Hermitian(P̂cor_chol, :L)) # also modifies sqrtP̂cor
+    γ_sqrtP̂cor = lmul!(γ, sqrtP̂cor)
+    X̂cor .= x̂cor
+    X̂cor[:, 2:nx̂+1]   .+= γ_sqrtP̂cor
+    X̂cor[:, nx̂+2:end] .-= γ_sqrtP̂cor
+    X̂next = X̂cor
+    for j in axes(X̂next, 2)
+        @views x̂cor .= X̂cor[:, j]
+        @views f̂!(X̂next[:, j], û, estim, estim.model, x̂cor, u, d)
     end
-    x̂_next = mul!(x̂, X̂_next, m̂)
-    X̄_next = X̂_next
-    X̄_next .= X̂_next .- x̂_next
-    P̂ .= Hermitian(X̄_next * Ŝ * X̄_next' .+ Q̂)
+    x̂next .= mul!(x̂, X̂next, m̂)
+    X̄next  = X̂next
+    X̄next .= X̂next .- x̂next
+    P̂next  = P̂cor
+    P̂next.data .= X̄next * Ŝ * X̄next' .+ Q̂
+    estim.x̂ .= x̂next
+    estim.P̂ .= P̂next
     return nothing
 end
 
@@ -822,17 +840,17 @@ function update_estimate_kf!(estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂
     Q̂, R̂, M̂, K̂ = estim.Q̂, estim.R̂, estim.M̂, estim.K̂
     nx̂, nu, ny = estim.nx̂, estim.model.nu, estim.model.ny
     x̂next, û, ŷ = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu), Vector{NT}(undef, ny)
-    P̂next = similar(P̂)
     mul!(M̂, P̂.data, Ĉm') # the ".data" weirdly removes a type instability in mul!
-    rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂)))
+    rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂, :L)))
     mul!(K̂, Â, M̂)
     ĥ!(ŷ, estim, estim.model, x̂, d)
     ŷm = @views ŷ[estim.i_ym]
     v̂  = ŷm
     v̂ .= ym .- ŷm
     f̂!(x̂next, û, estim, estim.model, x̂, u, d)
     mul!(x̂next, K̂, v̂, 1, 1)
+    P̂next = Hermitian(Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂, :L)
     estim.x̂ .= x̂next
-    P̂ .= Hermitian(Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂)
+    estim.P̂ .= P̂next
     return nothing
 end