reduce allocations LinMPC and KalmanFilter

Author: franckgaga

src/controller/execute.jl

@@ -173,17 +173,18 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
         mul!(F, mpc.J, mpc.D̂0, 1, 1)
     end
     mpc.R̂y .= R̂y
-    C_y  = similar(F)
-    C_y .= mpc.F .- mpc.R̂y
-    q̃   .= lmul!(2,(mpc.M_Hp*mpc.Ẽ)'*C_y)
-    p   .= dot(C_y, mpc.M_Hp, C_y)
+    C_y = F .- mpc.R̂y
+    M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ
+    mul!(q̃, M_Hp_Ẽ', C_y)
+    p .= dot(C_y, mpc.M_Hp, C_y)
     if ~mpc.noR̂u
         mpc.R̂u .= R̂u
-        C_u  = similar(mpc.T_lastu)
-        C_u .= mpc.T_lastu .- mpc.R̂u
-        mpc.q̃ .+= lmul!(2, (mpc.L_Hp*mpc.S̃)'*C_u)
-        mpc.p .+= dot(C_u, mpc.L_Hp, C_u)
+        C_u = mpc.T_lastu .- mpc.R̂u
+        L_Hp_S̃ = mpc.L_Hp*mpc.S̃
+        mul!(q̃, L_Hp_S̃', C_u, 1, 1)
+        p .+= dot(C_u, mpc.L_Hp, C_u)
     end
+    lmul!(2, q̃)
     return nothing
 end
 

src/estimator/kalman.jl

@@ -818,7 +818,7 @@ 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)
-    mul!(M̂, P̂, Ĉm')
+    mul!(M̂, P̂.data, Ĉm') # the ".data" weirdly remove a type instability in mul!
     rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂)))
     mul!(K̂, Â, M̂)
     ĥ!(ŷ, estim, estim.model, x̂, d)

src/estimator/mhe/execute.jl

@@ -236,8 +236,10 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm]
     M_Nk = [estim.invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk]
     Ñ_Nk = [fill(C, nϵ, nϵ) zeros(nϵ, nx̂+nŴ); zeros(nx̂, nϵ+nx̂+nŴ); zeros(nŴ, nϵ+nx̂) invQ̂_Nk]
-    estim.q̃[1:nZ̃] .= lmul!(2, (M_Nk*ẼZ̃)'*FZ̃)
-    estim.p       .= dot(FZ̃, M_Nk, FZ̃)
+    M_Nk_ẼZ̃ = M_Nk*ẼZ̃
+    mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃)
+    lmul!(2, estim.q̃[1:nZ̃])
+    estim.p .= dot(FZ̃, M_Nk, FZ̃)
     estim.H̃.data[1:nZ̃, 1:nZ̃] .= lmul!(2, (ẼZ̃'*M_Nk*ẼZ̃ .+ Ñ_Nk))
     Z̃var_Nk::Vector{VariableRef} = @views optim[:Z̃var][1:nZ̃]
     H̃_Nk = @views estim.H̃[1:nZ̃,1:nZ̃]