From 4aa95def67e92044116cef6e0c57a31a88879db1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 3 Apr 2024 18:11:13 -0400 Subject: [PATCH 1/6] reduce allocations `LinMPC` and `KalmanFilter` --- src/controller/execute.jl | 17 +++++++++-------- src/estimator/kalman.jl | 2 +- src/estimator/mhe/execute.jl | 6 ++++-- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 4c096c7eb..4f7a442bf 100644 --- a/src/controller/execute.jl +++ b/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 diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 3d09d95b3..1de21da84 100644 --- a/src/estimator/kalman.jl +++ b/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) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 27900e92c..fce2db48a 100644 --- a/src/estimator/mhe/execute.jl +++ b/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̃] From e833f67d731107f74c9b53c1a88af052a8d5a277 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 3 Apr 2024 20:16:21 -0400 Subject: [PATCH 2/6] minor modification --- src/controller/execute.jl | 12 ++++++------ src/estimator/kalman.jl | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 4f7a442bf..741558da8 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -173,16 +173,16 @@ 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 = F .- mpc.R̂y + Cy = 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) + mul!(q̃, M_Hp_Ẽ', Cy) + p .= dot(Cy, mpc.M_Hp, Cy) if ~mpc.noR̂u mpc.R̂u .= R̂u - C_u = mpc.T_lastu .- mpc.R̂u + Cu = 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) + mul!(q̃, L_Hp_S̃', Cu, 1, 1) + p .+= dot(Cu, mpc.L_Hp, Cu) end lmul!(2, q̃) return nothing diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 1de21da84..0a7cc9b7a 100644 --- a/src/estimator/kalman.jl +++ b/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̂.data, Ĉm') # the ".data" weirdly remove a type instability in mul! + 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) From e265c8ffaf8174c63d0be20337374d0e0c24cf4c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Apr 2024 14:05:03 -0400 Subject: [PATCH 3/6] added: drastically improve `LinMPC` performance with new `JuMP` batch update methods --- Project.toml | 2 +- src/controller/execute.jl | 2 +- src/controller/linmpc.jl | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Project.toml b/Project.toml index 350e40d1e..72f5ac9e9 100644 --- a/Project.toml +++ b/Project.toml @@ -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" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 741558da8..f795ea551 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -279,7 +279,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel) 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]) + set_normalized_rhs(lincon, mpc.con.b[mpc.con.i_b]) end "Set `b` excluding predicted output constraints when `model` is not a [`LinModel`](@ref)." diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl index 23bf23060..e50780319 100644 --- a/src/controller/linmpc.jl +++ b/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 \ No newline at end of file From 9d90d709a8cb384dd5bff57a8d7bf18e662d619e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Apr 2024 14:32:21 -0400 Subject: [PATCH 4/6] bump version --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 72f5ac9e9..ec56b7f5e 100644 --- a/Project.toml +++ b/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" From 5f4a05bc668f7ce1f31c59971bac11e67184b7ee Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Apr 2024 15:46:42 -0400 Subject: [PATCH 5/6] debug tests --- src/controller/execute.jl | 14 ++++++++++---- src/estimator/mhe/execute.jl | 14 ++++++++++---- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index f795ea551..f8673213f 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -278,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)." @@ -293,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""" diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index fce2db48a..d4ba7d64c 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -285,8 +285,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)." @@ -301,8 +304,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`." From ccecabe4ece5eaf3b6a1d4c455ffd6dc881c07aa Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 4 Apr 2024 16:15:30 -0400 Subject: [PATCH 6/6] debug MHE --- src/estimator/mhe/execute.jl | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index d4ba7d64c..c4d34cf98 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -237,10 +237,12 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) 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] M_Nk_ẼZ̃ = M_Nk*ẼZ̃ - mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃) - lmul!(2, estim.q̃[1:nZ̃]) + @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̃] .= lmul!(2, (ẼZ̃'*M_Nk*ẼZ̃ .+ Ñ_Nk)) + 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̃]