reduce allocation UnscentedKalmanFilter

Author: franckgaga

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/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/kalman.jl

@@ -192,7 +192,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 +347,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 +369,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 +398,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 +574,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 +599,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 +836,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

src/estimator/luenberger.jl

@@ -118,6 +118,6 @@ function update_estimate!(estim::Luenberger, 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

src/estimator/mhe/execute.jl

@@ -81,7 +81,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<
     # --------- update estimate -----------------------
     estim.Ŵ[1:nŵ*Nk] .= @views estim.Z̃[nx̃+1:nx̃+nŵ*Nk] # update Ŵ with optimum for warm-start
     V̂, X̂ = predict!(V̂, X̂, û, ŷ, estim, model, estim.Z̃)
-    x̂ .= X̂[end-nx̂+1:end]
+    estim.x̂ .= @views X̂[end-nx̂+1:end]
     Nk == estim.He && update_cov!(estim::MovingHorizonEstimator)
     return nothing
 end