Improve performance and reduce allocations of LinMPC and KalmanFilter

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.20.0"
+version = "0.20.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
@@ -19,7 +19,7 @@ RecipesBase = "3cdcf5f2-1ef4-517c-9805-6587b60abb01"
 ControlSystemsBase = "1.9"
 ForwardDiff = "0.10"
 Ipopt = "1"
-JuMP = "1"
+JuMP = "1.21"
 LinearAlgebra = "1.6"
 OSQP = "0.8"
 PreallocationTools = "0.4"

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)
+    Cy = F .- mpc.R̂y
+    M_Hp_Ẽ = mpc.M_Hp*mpc.Ẽ
+    mul!(q̃, M_Hp_Ẽ', Cy)
+    p .= dot(Cy, mpc.M_Hp, Cy)
     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)
+        Cu = mpc.T_lastu .- mpc.R̂u
+        L_Hp_S̃ = mpc.L_Hp*mpc.S̃
+        mul!(q̃, L_Hp_S̃', Cu, 1, 1)
+        p .+= dot(Cu, mpc.L_Hp, Cu)
     end
+    lmul!(2, q̃)
     return nothing
 end
 
@@ -277,8 +278,11 @@ function linconstraint!(mpc::PredictiveController, model::LinModel)
     mpc.con.b[(n+1):(n+nx̂)]  .= @. -mpc.con.x̂min + fx̂
     n += nx̂
     mpc.con.b[(n+1):(n+nx̂)]  .= @. +mpc.con.x̂max - fx̂
-    lincon = mpc.optim[:linconstraint]
-    set_normalized_rhs.(lincon, mpc.con.b[mpc.con.i_b])
+    if any(mpc.con.i_b) 
+        lincon = mpc.optim[:linconstraint]
+        set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
+    end
+    return nothing
 end
 
 "Set `b` excluding predicted output constraints when `model` is not a [`LinModel`](@ref)."
@@ -292,8 +296,11 @@ function linconstraint!(mpc::PredictiveController, ::SimModel)
     mpc.con.b[(n+1):(n+nΔŨ)] .= @. -mpc.con.ΔŨmin
     n += nΔŨ
     mpc.con.b[(n+1):(n+nΔŨ)] .= @. +mpc.con.ΔŨmax
-    lincon = mpc.optim[:linconstraint]
-    set_normalized_rhs.(lincon, mpc.con.b[mpc.con.i_b])
+    if any(mpc.con.i_b) 
+        lincon = mpc.optim[:linconstraint]
+        set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b])
+    end
+    return nothing
 end
 
 @doc raw"""

src/controller/linmpc.jl

@@ -254,6 +254,6 @@ end
 
 "For [`LinMPC`](@ref), set the QP linear coefficient `q̃` just before optimization."
 function set_objective_linear_coef!(mpc::LinMPC, ΔŨvar)
-    set_objective_coefficient.(mpc.optim, ΔŨvar, mpc.q̃)
+    set_objective_coefficient(mpc.optim, ΔŨvar, mpc.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 removes 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,9 +236,13 @@ 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̃)
-    estim.H̃.data[1:nZ̃, 1:nZ̃] .= lmul!(2, (ẼZ̃'*M_Nk*ẼZ̃ .+ Ñ_Nk))
+    M_Nk_ẼZ̃ = M_Nk*ẼZ̃
+    @views mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃)
+    @views lmul!(2, estim.q̃[1:nZ̃])
+    estim.p .= dot(FZ̃, M_Nk, FZ̃)
+    estim.H̃.data[1:nZ̃, 1:nZ̃] = Ñ_Nk
+    @views mul!(estim.H̃.data[1:nZ̃, 1:nZ̃], ẼZ̃', M_Nk_ẼZ̃, 1, 1) 
+    @views lmul!(2, estim.H̃.data[1:nZ̃, 1:nZ̃])
     Z̃var_Nk::Vector{VariableRef} = @views optim[:Z̃var][1:nZ̃]
     H̃_Nk = @views estim.H̃[1:nZ̃,1:nZ̃]
     q̃_Nk = @views estim.q̃[1:nZ̃]
@@ -283,8 +287,11 @@ function linconstraint!(estim::MovingHorizonEstimator, model::LinModel)
     estim.con.b[(n+1):(n+nV̂)] .= @. -V̂min + estim.F
     n += nV̂
     estim.con.b[(n+1):(n+nV̂)] .= @. +V̂max - estim.F
-    lincon = estim.optim[:linconstraint]
-    set_normalized_rhs.(lincon, estim.con.b[estim.con.i_b])
+    if any(estim.con.i_b) 
+        lincon = estim.optim[:linconstraint]
+        set_normalized_rhs(lincon, estim.con.b[estim.con.i_b])
+    end
+    return nothing
 end
 
 "Set `b` excluding state and sensor noise bounds if `model` is not a [`LinModel`](@ref)."
@@ -299,8 +306,11 @@ function linconstraint!(estim::MovingHorizonEstimator, ::SimModel)
     estim.con.b[(n+1):(n+nŴ)] .= @. -Ŵmin
     n += nŴ
     estim.con.b[(n+1):(n+nŴ)] .= @. +Ŵmax
-    lincon = estim.optim[:linconstraint]
-    set_normalized_rhs.(lincon, estim.con.b[estim.con.i_b])
+    if any(estim.con.i_b) 
+        lincon = estim.optim[:linconstraint]
+        set_normalized_rhs(lincon, estim.con.b[estim.con.i_b])
+    end
+    return nothing
 end
 
 "Truncate the bounds `Bmin` and `Bmax` to the window size `Nk` if `Nk < He`."