diff --git a/.gitignore b/.gitignore index 14fe61749..57db912da 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,4 @@ lcov.info benchmark/Manifest.toml /.benchmarkci /benchmark/*.json +results_ModelPredictiveControl* diff --git a/benchmark/0_bench_setup.jl b/benchmark/0_bench_setup.jl new file mode 100644 index 000000000..ac563ffb3 --- /dev/null +++ b/benchmark/0_bench_setup.jl @@ -0,0 +1,18 @@ +Ts = 400.0 +sys = [ tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]); + tf(-0.74,[800.0,1]) tf(0.74,[800.0,1]) tf(-0.74,[800.0,1]) ] +function f_lin!(ẋ, x, u, d, p) + mul!(ẋ, p.A, x) + mul!(ẋ, p.Bu, u, 1, 1) + mul!(ẋ, p.Bd, d, 1, 1) + return nothing +end +function h_lin!(y, x, d, p) + mul!(y, p.C, x) + mul!(y, p.Dd, d, 1, 1) + return nothing +end +linmodel = setop!(LinModel(sys, Ts, i_d=[3]), uop=[10, 50], yop=[50, 30], dop=[5]) +nonlinmodel = NonLinModel(f_lin!, h_lin!, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing) +nonlinmodel = setop!(nonlinmodel, uop=[10, 50], yop=[50, 30], dop=[5]) +u, d, y = [10, 50], [5], [50, 30] \ No newline at end of file diff --git a/benchmark/1_bench_sim_model.jl b/benchmark/1_bench_sim_model.jl new file mode 100644 index 000000000..13f5edd1e --- /dev/null +++ b/benchmark/1_bench_sim_model.jl @@ -0,0 +1,27 @@ +## ----------------- Unit tests ---------------------------------------------------------- +const UNIT_MODEL = SUITE["unit tests"]["SimModel"] + +UNIT_MODEL["LinModel"]["updatestate!"] = + @benchmarkable( + updatestate!($linmodel, $u, $d); + ) +UNIT_MODEL["LinModel"]["evaloutput"] = + @benchmarkable( + evaloutput($linmodel, $d); + ) +UNIT_MODEL["NonLinModel"]["updatestate!"] = + @benchmarkable( + updatestate!($nonlinmodel, $u, $d); + ) +UNIT_MODEL["NonLinModel"]["evaloutput"] = + @benchmarkable( + evaloutput($nonlinmodel, $d); + ) +UNIT_MODEL["NonLinModel"]["linearize!"] = + @benchmarkable( + linearize!($linmodel, $nonlinmodel); + ) + +## ----------------- Case studies --------------------------------------------------------- +const CASE_MODEL = SUITE["case studies"]["SimModel"] +# TODO: Add case study benchmarks for SimModel \ No newline at end of file diff --git a/benchmark/2_bench_state_estim.jl b/benchmark/2_bench_state_estim.jl new file mode 100644 index 000000000..f86ce9d99 --- /dev/null +++ b/benchmark/2_bench_state_estim.jl @@ -0,0 +1,285 @@ +## ----------------- Unit tests ----------------------------------------------------------- +const UNIT_ESTIM = SUITE["unit tests"]["StateEstimator"] + +skf = SteadyKalmanFilter(linmodel) +UNIT_ESTIM["SteadyKalmanFilter"]["preparestate!"] = + @benchmarkable( + preparestate!($skf, $y, $d), + ) +UNIT_ESTIM["SteadyKalmanFilter"]["updatestate!"] = + @benchmarkable( + updatestate!($skf, $u, $y, $d), + setup=preparestate!($skf, $y, $d), + ) +UNIT_ESTIM["SteadyKalmanFilter"]["evaloutput"] = + @benchmarkable( + evaloutput($skf, $d), + setup=preparestate!($skf, $y, $d), + ) + +kf = KalmanFilter(linmodel, nint_u=[1, 1], direct=false) +UNIT_ESTIM["KalmanFilter"]["preparestate!"] = + @benchmarkable( + preparestate!($kf, $y, $d), + ) +UNIT_ESTIM["KalmanFilter"]["updatestate!"] = + @benchmarkable( + updatestate!($kf, $u, $y, $d), + setup=preparestate!($kf, $y, $d), + ) +UNIT_ESTIM["KalmanFilter"]["evaloutput"] = + @benchmarkable( + evaloutput($kf, $d), + setup=preparestate!($kf, $y, $d), + ) + +lo = Luenberger(linmodel, nint_u=[1, 1]) +UNIT_ESTIM["Luenberger"]["preparestate!"] = + @benchmarkable( + preparestate!($lo, $y, $d), + ) +UNIT_ESTIM["Luenberger"]["updatestate!"] = + @benchmarkable( + updatestate!($lo, $u, $y, $d), + setup=preparestate!($lo, $y, $d), + ) +UNIT_ESTIM["Luenberger"]["evaloutput"] = + @benchmarkable( + evaloutput($lo, $d), + setup=preparestate!($lo, $y, $d), + ) + +im_lin = InternalModel(linmodel) +im_nonlin = InternalModel(nonlinmodel) +UNIT_ESTIM["InternalModel"]["preparestate!"]["LinModel"] = + @benchmarkable( + preparestate!($im_lin, $y, $d), + ) +UNIT_ESTIM["InternalModel"]["updatestate!"]["LinModel"] = + @benchmarkable( + updatestate!($im_lin, $u, $y, $d), + setup=preparestate!($im_lin, $y, $d), + ) +UNIT_ESTIM["InternalModel"]["evaloutput"]["LinModel"] = + @benchmarkable( + evaloutput($im_lin, $d), + setup=preparestate!($im_lin, $y, $d), + ) +UNIT_ESTIM["InternalModel"]["preparestate!"]["NonLinModel"] = + @benchmarkable( + preparestate!($im_nonlin, $y, $d), + ) +UNIT_ESTIM["InternalModel"]["updatestate!"]["NonLinModel"] = + @benchmarkable( + updatestate!($im_nonlin, $u, $y, $d), + setup=preparestate!($im_nonlin, $y, $d), + ) +UNIT_ESTIM["InternalModel"]["evaloutput"]["NonLinModel"] = + @benchmarkable( + evaloutput($im_nonlin, $d), + setup=preparestate!($im_nonlin, $y, $d), + ) + +ukf_lin = UnscentedKalmanFilter(linmodel) +ukf_nonlin = UnscentedKalmanFilter(nonlinmodel) +UNIT_ESTIM["UnscentedKalmanFilter"]["preparestate!"]["LinModel"] = + @benchmarkable( + preparestate!($ukf_lin, $y, $d), + ) +UNIT_ESTIM["UnscentedKalmanFilter"]["updatestate!"]["LinModel"] = + @benchmarkable( + updatestate!($ukf_lin, $u, $y, $d), + setup=preparestate!($ukf_lin, $y, $d), + ) +UNIT_ESTIM["UnscentedKalmanFilter"]["evaloutput"]["LinModel"] = + @benchmarkable( + evaloutput($ukf_lin, $d), + setup=preparestate!($ukf_lin, $y, $d), + ) +UNIT_ESTIM["UnscentedKalmanFilter"]["preparestate!"]["NonLinModel"] = + @benchmarkable( + preparestate!($ukf_nonlin, $y, $d), + ) +UNIT_ESTIM["UnscentedKalmanFilter"]["updatestate!"]["NonLinModel"] = + @benchmarkable( + updatestate!($ukf_nonlin, $u, $y, $d), + setup=preparestate!($ukf_nonlin, $y, $d), + ) +UNIT_ESTIM["UnscentedKalmanFilter"]["evaloutput"]["NonLinModel"] = + @benchmarkable( + evaloutput($ukf_nonlin, $d), + setup=preparestate!($ukf_nonlin, $y, $d), + ) + +ekf_lin = ExtendedKalmanFilter(linmodel, nint_u=[1, 1], direct=false) +ekf_nonlin = ExtendedKalmanFilter(nonlinmodel, nint_u=[1, 1], direct=false) +UNIT_ESTIM["ExtendedKalmanFilter"]["preparestate!"]["LinModel"] = + @benchmarkable( + preparestate!($ekf_lin, $y, $d), + ) +UNIT_ESTIM["ExtendedKalmanFilter"]["updatestate!"]["LinModel"] = + @benchmarkable( + updatestate!($ekf_lin, $u, $y, $d), + setup=preparestate!($ekf_lin, $y, $d), + ) +UNIT_ESTIM["ExtendedKalmanFilter"]["evaloutput"]["LinModel"] = + @benchmarkable( + evaloutput($ekf_lin, $d), + setup=preparestate!($ekf_lin, $y, $d), + ) +UNIT_ESTIM["ExtendedKalmanFilter"]["preparestate!"]["NonLinModel"] = + @benchmarkable( + preparestate!($ekf_nonlin, $y, $d), + ) +UNIT_ESTIM["ExtendedKalmanFilter"]["updatestate!"]["NonLinModel"] = + @benchmarkable( + updatestate!($ekf_nonlin, $u, $y, $d), + setup=preparestate!($ekf_nonlin, $y, $d), + ) +UNIT_ESTIM["ExtendedKalmanFilter"]["evaloutput"]["NonLinModel"] = + @benchmarkable( + evaloutput($ekf_nonlin, $d), + setup=preparestate!($ekf_nonlin, $y, $d), + ) + +mhe_lin_curr = MovingHorizonEstimator(linmodel, He=10, direct=true) +mhe_lin_pred = MovingHorizonEstimator(linmodel, He=10, direct=false) +mhe_nonlin_curr = MovingHorizonEstimator(nonlinmodel, He=10, direct=true) +mhe_nonlin_pred = MovingHorizonEstimator(nonlinmodel, He=10, direct=false) + +samples, evals, seconds = 5000, 1, 60 +UNIT_ESTIM["MovingHorizonEstimator"]["preparestate!"]["LinModel"]["Current form"] = + @benchmarkable( + preparestate!($mhe_lin_curr, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["LinModel"]["Current form"] = + @benchmarkable( + updatestate!($mhe_lin_curr, $u, $y, $d), + setup=preparestate!($mhe_lin_curr, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["preparestate!"]["LinModel"]["Prediction form"] = + @benchmarkable( + preparestate!($mhe_lin_pred, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["LinModel"]["Prediction form"] = + @benchmarkable( + updatestate!($mhe_lin_pred, $u, $y, $d), + setup=preparestate!($mhe_lin_pred, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["preparestate!"]["NonLinModel"]["Current form"] = + @benchmarkable( + preparestate!($mhe_nonlin_curr, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["NonLinModel"]["Current form"] = + @benchmarkable( + updatestate!($mhe_nonlin_curr, $u, $y, $d), + setup=preparestate!($mhe_nonlin_curr, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["preparestate!"]["NonLinModel"]["Prediction form"] = + @benchmarkable( + preparestate!($mhe_nonlin_pred, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) +UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["NonLinModel"]["Prediction form"] = + @benchmarkable( + updatestate!($mhe_nonlin_pred, $u, $y, $d), + setup=preparestate!($mhe_nonlin_pred, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) + +## ----------------- Case studies --------------------------------------------------- +const CASE_ESTIM = SUITE["case studies"]["StateEstimator"] + +## ----------------- Case study: CSTR ----------------------------------------------------- +G = [ tf(1.90, [18, 1]) tf(1.90, [18, 1]); + tf(-0.74,[8, 1]) tf(0.74, [8, 1]) ] +uop, yop = [20, 20], [50, 30] +model = setop!(LinModel(G, 2.0); uop, yop) +plant = setop!(LinModel(G, 2.0); uop, yop) +plant.A[diagind(plant.A)] .-= 0.1 # plant-model mismatch +function test_mhe(mhe, plant) + plant.x0 .= 0; y = plant() + initstate!(mhe, plant.uop, y) + N = 75; u = [20, 20]; ul = 0 + U, Y, Ŷ = zeros(2, N), zeros(2, N), zeros(2, N) + for i = 1:N + i == 26 && (u = [15, 25]) + i == 51 && (ul = -10) + y = plant() + preparestate!(mhe, y) + ŷ = evaloutput(mhe) + U[:,i], Y[:,i], Ŷ[:,i] = u, y, ŷ + updatestate!(mhe, u, y) + updatestate!(plant, u+[0,ul]) + end + return U, Y, Ŷ +end +He = 10; nint_u = [1, 1]; σQint_u = [1, 2] + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +direct = true +mhe_cstr_osqp_curr = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) +mhe_cstr_osqp_curr = setconstraint!(mhe_cstr_osqp_curr, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +JuMP.unset_time_limit_sec(mhe_cstr_osqp_curr.optim) + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +direct = false +mhe_cstr_osqp_pred = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) +mhe_cstr_osqp_pred = setconstraint!(mhe_cstr_osqp_pred, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +JuMP.unset_time_limit_sec(mhe_cstr_osqp_pred.optim) + +optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +direct = true +mhe_cstr_daqp_curr = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) +mhe_cstr_daqp_curr = setconstraint!(mhe_cstr_daqp_curr, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +JuMP.set_attribute(mhe_cstr_daqp_curr.optim, "eps_prox", 1e-6) # needed to support hessians H≥0 + +optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +direct = false +mhe_cstr_daqp_pred = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) +mhe_cstr_daqp_pred = setconstraint!(mhe_cstr_daqp_pred, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +JuMP.set_attribute(mhe_cstr_daqp_pred.optim, "eps_prox", 1e-6) # needed to support hessians H≥0 + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +direct = true +mhe_cstr_ipopt_curr = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) +mhe_cstr_ipopt_curr = setconstraint!(mhe_cstr_ipopt_curr, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +JuMP.unset_time_limit_sec(mhe_cstr_ipopt_curr.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +direct = false +mhe_cstr_ipopt_pred = MovingHorizonEstimator(model; He, nint_u, σQint_u, optim, direct) +mhe_cstr_ipopt_pred = setconstraint!(mhe_cstr_ipopt_pred, v̂min=[-1, -0.5], v̂max=[+1, +0.5]) +JuMP.unset_time_limit_sec(mhe_cstr_ipopt_pred.optim) + +samples, evals = 500, 1 +CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["OSQP"]["Current form"] = + @benchmarkable(test_mhe($mhe_cstr_osqp_curr, $plant); + samples=samples, evals=evals + ) +CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["OSQP"]["Prediction form"] = + @benchmarkable(test_mhe($mhe_cstr_osqp_pred, $plant); + samples=samples, evals=evals + ) +CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["DAQP"]["Current form"] = + @benchmarkable(test_mhe($mhe_cstr_daqp_curr, $plant); + samples=samples, evals=evals + ) +CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["DAQP"]["Prediction form"] = + @benchmarkable(test_mhe($mhe_cstr_daqp_pred, $plant); + samples=samples, evals=evals + ) +CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["Ipopt"]["Current form"] = + @benchmarkable(test_mhe($mhe_cstr_ipopt_curr, $plant); + samples=samples, evals=evals + ) +CASE_ESTIM["CSTR"]["MovingHorizonEstimator"]["Ipopt"]["Prediction form"] = + @benchmarkable(test_mhe($mhe_cstr_ipopt_pred, $plant); + samples=samples, evals=evals + ) \ No newline at end of file diff --git a/benchmark/3_bench_predictive_control.jl b/benchmark/3_bench_predictive_control.jl new file mode 100644 index 000000000..3ffd1a86e --- /dev/null +++ b/benchmark/3_bench_predictive_control.jl @@ -0,0 +1,486 @@ +# ---------------------- Unit tests ------------------------------------------------------- +const UNIT_MPC = SUITE["unit tests"]["PredictiveController"] + +linmpc_ss = LinMPC( + linmodel, transcription=SingleShooting(), + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) +linmpc_ms = LinMPC( + linmodel, transcription=MultipleShooting(), + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) + +samples, evals, seconds = 500, 1, 60 +UNIT_MPC["LinMPC"]["moveinput!"]["SingleShooting"] = + @benchmarkable( + moveinput!($linmpc_ss, $y, $d), + setup=preparestate!($linmpc_ss, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) +UNIT_MPC["LinMPC"]["moveinput!"]["MultipleShooting"] = + @benchmarkable( + moveinput!($linmpc_ms, $y, $d), + setup=preparestate!($linmpc_ms, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) + +empc = ExplicitMPC(linmodel, Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10) + +UNIT_MPC["ExplicitMPC"]["moveinput!"] = + @benchmarkable( + moveinput!($empc, $y, $d), + setup=preparestate!($empc, $y, $d), + ) + +nmpc_lin_ss = NonLinMPC( + linmodel, transcription=SingleShooting(), + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) +nmpc_lin_ms = NonLinMPC( + linmodel, transcription=MultipleShooting(), + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) +nmpc_nonlin_ss = NonLinMPC( + nonlinmodel, transcription=SingleShooting(), + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) +nmpc_nonlin_ms = NonLinMPC( + nonlinmodel, transcription=MultipleShooting(), + Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10 +) + +samples, evals, seconds = 500, 1, 60 +UNIT_MPC["NonLinMPC"]["moveinput!"]["LinModel"]["SingleShooting"] = + @benchmarkable( + moveinput!($nmpc_lin_ss, $y, $d), + setup=preparestate!($nmpc_lin_ss, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) +UNIT_MPC["NonLinMPC"]["moveinput!"]["LinModel"]["MultipleShooting"] = + @benchmarkable( + moveinput!($nmpc_lin_ms, $y, $d), + setup=preparestate!($nmpc_lin_ms, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) +UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["SingleShooting"] = + @benchmarkable( + moveinput!($nmpc_nonlin_ss, $y, $d), + setup=preparestate!($nmpc_nonlin_ss, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) +UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["MultipleShooting"] = + @benchmarkable( + moveinput!($nmpc_nonlin_ms, $y, $d), + setup=preparestate!($nmpc_nonlin_ms, $y, $d), + samples=samples, evals=evals, seconds=seconds + ) + + +## ---------------------- Case studies ---------------------------------------------------- +const CASE_MPC = SUITE["case studies"]["PredictiveController"] + +## ----------------- Case study: CSTR without feedforward ------------------------ +G = [ tf(1.90, [18, 1]) tf(1.90, [18, 1]); + tf(-0.74,[8, 1]) tf(0.74, [8, 1]) ] +uop, yop = [20, 20], [50, 30] +model = setop!(LinModel(G, 2.0); uop, yop) +plant = setop!(LinModel(G, 2.0); uop, yop) +plant.A[diagind(plant.A)] .-= 0.1 # plant-model mismatch +function test_mpc(mpc, plant) + plant.x0 .= 0; y = plant() + initstate!(mpc, plant.uop, y) + N = 75; ry = [50, 30]; ul = 0 + U, Y, Ry = zeros(2, N), zeros(2, N), zeros(2, N) + for i = 1:N + i == 26 && (ry = [48, 35]) + i == 51 && (ul = -10) + y = plant() + preparestate!(mpc, y) + u = mpc(ry) + U[:,i], Y[:,i], Ry[:,i] = u, y, ry + updatestate!(mpc, u, y) + updatestate!(plant, u+[0,ul]) + end + return U, Y, Ry +end + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +transcription = SingleShooting() +mpc_osqp_ss = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_osqp_ss.optim) + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +transcription = MultipleShooting() +mpc_osqp_ms = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_osqp_ms.optim) + +optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +transcription = SingleShooting() +mpc_daqp_ss = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) + +# # Skip DAQP with MultipleShooting, it is not designed for sparse Hessians. Kind of works +# # with "eps_prox" configured to 1e-6, but not worth it. +# optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +# transcription = MultipleShooting() +# mpc_daqp_ms = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) +# JuMP.set_attribute(mpc_daqp_ms.optim, "eps_prox", 1e-6) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = SingleShooting() +mpc_ipopt_ss = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_ipopt_ss.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = MultipleShooting() +mpc_ipopt_ms = setconstraint!(LinMPC(model; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_ipopt_ms.optim) + +samples, evals = 500, 1 +CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["OSQP"]["SingleShooting"] = + @benchmarkable(test_mpc($mpc_osqp_ss, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["OSQP"]["MultipleShooting"] = + @benchmarkable(test_mpc($mpc_osqp_ms, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["DAQP"]["SingleShooting"] = + @benchmarkable(test_mpc($mpc_daqp_ss, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["Ipopt"]["SingleShooting"] = + @benchmarkable(test_mpc($mpc_ipopt_ss, $plant); + samples=samples, evals=evals +) +CASE_MPC["CSTR"]["LinMPC"]["Without feedforward"]["Ipopt"]["MultipleShooting"] = + @benchmarkable(test_mpc($mpc_ipopt_ms, $plant); + samples=samples, evals=evals + ) + +## ----------------- Case study: CSTR with feedforward ------------------------- +model_d = LinModel([G G[1:2, 2]], 2.0; i_d=[3]) +model_d = setop!(model_d; uop, yop, dop=[20]) +function test_mpc_d(mpc_d, plant) + plant.x0 .= 0; y = plant(); d = [20] + initstate!(mpc_d, plant.uop, y, d) + N = 75; ry = [50, 30]; ul = 0 + U, Y, Ry = zeros(2, N), zeros(2, N), zeros(2, N) + for i = 1:N + i == 26 && (ry = [48, 35]) + i == 51 && (ul = -10) + y, d = plant(), [20+ul] + preparestate!(mpc_d, y, d) + u = mpc_d(ry, d) + U[:,i], Y[:,i], Ry[:,i] = u, y, ry + updatestate!(mpc_d, u, y, d) + updatestate!(plant, u+[0,ul]) + end + return U, Y, Ry +end + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +transcription = SingleShooting() +mpc_d_osqp_ss = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_d_osqp_ss.optim) + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +transcription = MultipleShooting() +mpc_d_osqp_ms = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_d_osqp_ms.optim) + +optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +transcription = SingleShooting() +mpc_d_daqp_ss = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) + +# # Skip DAQP with MultipleShooting, it is not designed for sparse Hessians. Kind of works +# # with "eps_prox" configured to 1e-6, but not worth it. +# optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +# transcription = MultipleShooting() +# mpc_d_daqp_ms = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) +# JuMP.set_attribute(mpc_d_daqp_ms.optim, "eps_prox", 1e-6) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = SingleShooting() +mpc_d_ipopt_ss = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_d_ipopt_ss.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = MultipleShooting() +mpc_d_ipopt_ms = setconstraint!(LinMPC(model_d; optim, transcription), ymin=[45, -Inf]) +JuMP.unset_time_limit_sec(mpc_d_ipopt_ms.optim) + +samples, evals = 500, 1 +CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["OSQP"]["SingleShooting"] = + @benchmarkable(test_mpc_d($mpc_d_osqp_ss, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["OSQP"]["MultipleShooting"] = + @benchmarkable(test_mpc_d($mpc_d_osqp_ms, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["DAQP"]["SingleShooting"] = + @benchmarkable(test_mpc_d($mpc_d_daqp_ss, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["Ipopt"]["SingleShooting"] = + @benchmarkable(test_mpc_d($mpc_d_ipopt_ss, $plant); + samples=samples, evals=evals + ) +CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["Ipopt"]["MultipleShooting"] = + @benchmarkable(test_mpc_d($mpc_d_ipopt_ms, $plant); + samples=samples, evals=evals + ) + + +# ----------------- Case study: Pendulum noneconomic ----------------------------- +function f!(ẋ, x, u, _ , p) + g, L, K, m = p # [m/s²], [m], [kg/s], [kg] + θ, ω = x[1], x[2] # [rad], [rad/s] + τ = u[1] # [Nm] + ẋ[1] = ω + ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2 +end +h!(y, x, _ , _ ) = (y[1] = 180/π*x[1]) # [°] +p = [9.8, 0.4, 1.2, 0.3] +nu = 1; nx = 2; ny = 1; Ts = 0.1 +model = NonLinModel(f!, h!, Ts, nu, nx, ny; p) +σQ = [0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1] +estim = UnscentedKalmanFilter(model; σQ, σR, nint_u, σQint_u) +p_plant = copy(p); p_plant[3] = 1.25*p[3] +plant = NonLinModel(f!, h!, Ts, nu, nx, ny; p=p_plant) +N = 35; u = [0.5]; + +Hp, Hc, Mwt, Nwt, Cwt = 20, 2, [0.5], [2.5], Inf +umin, umax = [-1.5], [+1.5] +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180] + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = SingleShooting() +nmpc_ipopt_ss = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +nmpc_ipopt_ss = setconstraint!(nmpc_ipopt_ss; umin, umax) +JuMP.unset_time_limit_sec(nmpc_ipopt_ss.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = MultipleShooting() +nmpc_ipopt_ms = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +nmpc_ipopt_ms = setconstraint!(nmpc_ipopt_ms; umin, umax) +JuMP.unset_time_limit_sec(nmpc_ipopt_ms.optim) + +optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false) +transcription = SingleShooting() +nmpc_madnlp_ss = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +nmpc_madnlp_ss = setconstraint!(nmpc_madnlp_ss; umin, umax) +JuMP.unset_time_limit_sec(nmpc_madnlp_ss.optim) + +# TODO: does not work well with MadNLP and MultipleShooting, figure out why. Current theory: +# MadNLP LBFGS approximation is less robust than Ipopt version. Re-test when exact Hessians +# will be supported in ModelPredictiveControl.jl. The following attributes kinda work with +# the MadNLP LBFGS approximation but super slow (~1000 times slower than Ipopt): +# optim = JuMP.Model(MadNLP.Optimizer) +# transcription = MultipleShooting() +# nmpc_madnlp_ms = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +# nmpc_madnlp_ms = setconstraint!(nmpc_madnlp_ms; umin, umax) +# JuMP.unset_time_limit_sec(nmpc_madnlp_ms.optim) +# JuMP.set_attribute(nmpc_madnlp_ms.optim, "hessian_approximation", MadNLP.CompactLBFGS) +# MadNLP_QNopt = MadNLP.QuasiNewtonOptions(; max_history=42) +# JuMP.set_attribute(nmpc_madnlp_ms.optim, "quasi_newton_options", MadNLP_QNopt) + +samples, evals, seconds = 50, 1, 15*60 +CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["SingleShooting"] = + @benchmarkable( + sim!($nmpc_ipopt_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["MultipleShooting"] = + @benchmarkable( + sim!($nmpc_ipopt_ms, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["MadNLP"]["SingleShooting"] = + @benchmarkable( + sim!($nmpc_madnlp_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) + +# ----------------- Case study: Pendulum economic -------------------------------- +h2!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; y[2]=x[2]) +nu, nx, ny = 1, 2, 2 +model2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p) +plant2 = NonLinModel(f!, h2!, Ts, nu, nx, ny; p=p_plant) +estim2 = UnscentedKalmanFilter(model2; σQ, σR, nint_u, σQint_u, i_ym=[1]) +function JE(UE, ŶE, _ , p) + Ts = p + τ, ω = @views UE[1:end-1], ŶE[2:2:end-1] + return Ts*dot(τ, ω) +end +p = Ts; Mwt2 = [Mwt; 0.0]; Ewt = 3.5e3 +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180; 0] + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = SingleShooting() +empc_ipopt_ss = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, transcription, p) +empc_ipopt_ss = setconstraint!(empc_ipopt_ss; umin, umax) +JuMP.unset_time_limit_sec(empc_ipopt_ss.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = MultipleShooting() +empc_ipopt_ms = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, transcription, p) +empc_ipopt_ms = setconstraint!(empc_ipopt_ms; umin, umax) +JuMP.unset_time_limit_sec(empc_ipopt_ms.optim) + +optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false) +transcription = SingleShooting() +empc_madnlp_ss = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, transcription, p) +empc_madnlp_ss = setconstraint!(empc_madnlp_ss; umin, umax) +JuMP.unset_time_limit_sec(empc_madnlp_ss.optim) + +# TODO: test EMPC with MadNLP and MultipleShooting, see comment above. + +samples, evals, seconds = 50, 1, 15*60 +CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["SingleShooting"] = + @benchmarkable( + sim!($empc_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["MultipleShooting"] = + @benchmarkable( + sim!($empc_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["MadNLP"]["SingleShooting"] = + @benchmarkable( + sim!($empc_madnlp_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) + +# -------------- Case study: Pendulum custom constraints -------------------------- +function gc!(LHS, Ue, Ŷe, _, p, ϵ) + Pmax = p + i_τ, i_ω = 1, 2 + for i in eachindex(LHS) + τ, ω = Ue[i_τ], Ŷe[i_ω] + P = τ*ω + LHS[i] = P - Pmax - ϵ + i_τ += 1 + i_ω += 2 + end + return nothing +end +Cwt, Pmax, nc = 1e5, 3, Hp+1 +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180; 0] + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = SingleShooting() +nmpc2_ipopt_ss = NonLinMPC(estim2; + Hp, Hc, Nwt=Nwt, Mwt=[0.5, 0], Cwt, gc!, nc, p=Pmax, optim, transcription +) +nmpc2_ipopt_ss = setconstraint!(nmpc2_ipopt_ss; umin, umax) +JuMP.unset_time_limit_sec(nmpc2_ipopt_ss.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = MultipleShooting() +nmpc2_ipopt_ms = NonLinMPC(estim2; + Hp, Hc, Nwt=Nwt, Mwt=[0.5, 0], Cwt, gc!, nc, p=Pmax, optim, transcription +) +nmpc2_ipopt_ms = setconstraint!(nmpc2_ipopt_ms; umin, umax) +JuMP.unset_time_limit_sec(nmpc2_ipopt_ms.optim) + +# TODO: test custom constraints with MadNLP and SingleShooting, see comment above. +# TODO: test custom constraints with MadNLP and MultipleShooting, see comment above. + +samples, evals, seconds = 50, 1, 15*60 +CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["SingleShooting"] = + @benchmarkable( + sim!($nmpc2_ipopt_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) +CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["MultipleShooting"] = + @benchmarkable( + sim!($nmpc2_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0), + samples=samples, evals=evals, seconds=seconds + ) + +# ----------------- Case study: Pendulum successive linearization ------------------------- +linmodel = linearize(model, x=[0, 0], u=[0]) +kf = KalmanFilter(linmodel; σQ, σR, nint_u, σQint_u) +function sim2!(mpc, nlmodel, N, ry, plant, x, 𝕩̂, y_step) + U, Y, Ry = zeros(1, N), zeros(1, N), zeros(1, N) + setstate!(plant, x); setstate!(mpc, 𝕩̂) + initstate!(mpc, [0], plant()) + linmodel = linearize(nlmodel; u=[0], x=𝕩̂[1:2]) + setmodel!(mpc, linmodel) + for i = 1:N + y = plant() + y_step + 𝕩̂ = preparestate!(mpc, y) + u = mpc(ry) + linearize!(linmodel, nlmodel; u, x=𝕩̂[1:2]) + setmodel!(mpc, linmodel) + U[:,i], Y[:,i], Ry[:,i] = u, y, ry + updatestate!(mpc, u, y) + updatestate!(plant, u) + end + U_data, Y_data, Ry_data = U, Y, Ry + return SimResult(mpc, U_data, Y_data; Ry_data) +end +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180]; y_step=[0] + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +transcription = SingleShooting() +mpc3_osqp_ss = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +mpc3_osqp_ss = setconstraint!(mpc3_osqp_ss; umin, umax) +JuMP.unset_time_limit_sec(mpc3_osqp_ss.optim) +JuMP.set_attribute(mpc3_osqp_ss.optim, "polish", true) # needed to +JuMP.set_attribute(mpc3_osqp_ss.optim, "sigma", 1e-9) # needed to + +optim = JuMP.Model(OSQP.Optimizer, add_bridges=false) +transcription = MultipleShooting() +mpc3_osqp_ms = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +mpc3_osqp_ms = setconstraint!(mpc3_osqp_ms; umin, umax) +JuMP.unset_time_limit_sec(mpc3_osqp_ms.optim) + +optim = JuMP.Model(DAQP.Optimizer, add_bridges=false) +transcription = SingleShooting() +mpc3_daqp_ss = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +mpc3_daqp_ss = setconstraint!(mpc3_daqp_ss; umin, umax) + +# skip DAQP with MultipleShooting, it is not designed for sparse Hessians +# did not found any settings that works well here (always reach the iteration limit). + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = SingleShooting() +mpc3_ipopt_ss = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +mpc3_ipopt_ss = setconstraint!(mpc3_ipopt_ss; umin, umax) +JuMP.unset_time_limit_sec(mpc3_ipopt_ss.optim) + +optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false) +transcription = MultipleShooting() +mpc3_ipopt_ms = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription) +mpc3_ipopt_ms = setconstraint!(mpc3_ipopt_ms; umin, umax) +JuMP.unset_time_limit_sec(mpc3_ipopt_ms.optim) + +samples, evals = 500, 1 +CASE_MPC["Pendulum"]["LinMPC"]["Successive linearization"]["OSQP"]["SingleShooting"] = + @benchmarkable( + sim2!($mpc3_osqp_ss, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), + samples=samples, evals=evals + ) +CASE_MPC["Pendulum"]["LinMPC"]["Successive linearization"]["OSQP"]["MultipleShooting"] = + @benchmarkable( + sim2!($mpc3_osqp_ms, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), + samples=samples, evals=evals + ) +CASE_MPC["Pendulum"]["LinMPC"]["Successive linearization"]["DAQP"]["SingleShooting"] = + @benchmarkable( + sim2!($mpc3_daqp_ss, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), + samples=samples, evals=evals + ) +CASE_MPC["Pendulum"]["LinMPC"]["Successive linearization"]["Ipopt"]["SingleShooting"] = + @benchmarkable( + sim2!($mpc3_ipopt_ss, $model, $N, $ry, $plant, $x_0, $x̂_0, $y_step), + samples=samples, evals=evals + ) +CASE_MPC["Pendulum"]["LinMPC"]["Successive linearization"]["Ipopt"]["MultipleShooting"] = + @benchmarkable( + sim2!($mpc3_ipopt_ms, $model, $N, $ ry, $plant, $x_0, $x̂_0, $y_step), + samples=samples, evals=evals + ) diff --git a/benchmark/benchmarks.jl b/benchmark/benchmarks.jl index 345e49a5e..8407e38b3 100644 --- a/benchmark/benchmarks.jl +++ b/benchmark/benchmarks.jl @@ -1,141 +1,13 @@ using BenchmarkTools using ModelPredictiveControl, ControlSystemsBase, LinearAlgebra +using JuMP, OSQP, DAQP, Ipopt, MadNLP -Ts = 400.0 -sys = [ tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]) tf(1.90,[1800.0,1]); - tf(-0.74,[800.0,1]) tf(0.74,[800.0,1]) tf(-0.74,[800.0,1]) ] +const SUITE = BenchmarkGroup(["ModelPredictiveControl"]) -const SUITE = BenchmarkGroup() +SUITE["unit tests"] = BenchmarkGroup(["allocation-free", "allocations", "single call"]) +SUITE["case studies"] = BenchmarkGroup(["performance", "speed" ,"integration"]) -## ================================================================================== -## ================== SimModel benchmarks =========================================== -## ================================================================================== -linmodel = setop!(LinModel(sys, Ts, i_d=[3]), uop=[10, 50], yop=[50, 30], dop=[5]) -function f!(ẋ, x, u, d, p) - mul!(ẋ, p.A, x) - mul!(ẋ, p.Bu, u, 1, 1) - mul!(ẋ, p.Bd, d, 1, 1) - return nothing -end -function h!(y, x, d, p) - mul!(y, p.C, x) - mul!(y, p.Dd, d, 1, 1) - return nothing -end -nonlinmodel = NonLinModel(f!, h!, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing) -nonlinmodel = setop!(nonlinmodel, uop=[10, 50], yop=[50, 30], dop=[5]) -u, d, y = [10, 50], [5], [50, 30] - -SUITE["SimModel"]["allocation"] = BenchmarkGroup(["allocation"]) -SUITE["SimModel"]["allocation"]["LinModel_updatestate!"] = @benchmarkable( - updatestate!($linmodel, $u, $d), - samples=1 -) -SUITE["SimModel"]["allocation"]["LinModel_evaloutput"] = @benchmarkable( - evaloutput($linmodel, $d), - samples=1 -) -SUITE["SimModel"]["allocation"]["NonLinModel_updatestate!"] = @benchmarkable( - updatestate!($nonlinmodel, $u, $d), - samples=1 -) -SUITE["SimModel"]["allocation"]["NonLinModel_evaloutput"] = @benchmarkable( - evaloutput($nonlinmodel, $d), - samples=1 -) - -SUITE["SimModel"]["allocation"]["NonLinModel_linearize!"] = @benchmarkable( - linearize!($linmodel, $nonlinmodel), - samples=1 -) - -## ================================================================================== -## ================== StateEstimator benchmarks ===================================== -## ================================================================================== -skf = SteadyKalmanFilter(linmodel) -SUITE["StateEstimator"]["allocation"] = BenchmarkGroup(["allocation"]) -SUITE["StateEstimator"]["allocation"]["SteadyKalmanFilter_preparestate!"] = @benchmarkable( - preparestate!($skf, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["SteadyKalmanFilter_updatestate!"] = @benchmarkable( - updatestate!($skf, $u, $y, $d), - setup=preparestate!($skf, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["SteadyKalmanFilter_evaloutput"] = @benchmarkable( - evaloutput($skf, $d), - setup=preparestate!($skf, $y, $d), - samples=1 -) - -kf = KalmanFilter(linmodel, nint_u=[1, 1], direct=false) -SUITE["StateEstimator"]["allocation"]["KalmanFilter_preparestate!"] = @benchmarkable( - preparestate!($kf, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["KalmanFilter_updatestate!"] = @benchmarkable( - updatestate!($kf, $u, $y, $d), - setup=preparestate!($kf, $y, $d), - samples=1 -) - -lo = Luenberger(linmodel, nint_u=[1, 1]) -#SUITE["StateEstimator"]["allocation"]["Luenberger_preparestate!"] = @benchmarkable( -# preparestate!($lo, $y, $d), -# samples=1 -#) -SUITE["StateEstimator"]["allocation"]["Luenberger_updatestate!"] = @benchmarkable( - updatestate!($lo, $u, $y, $d), - setup=preparestate!($lo, $y, $d), - samples=1 -) - -im = InternalModel(nonlinmodel) -SUITE["StateEstimator"]["allocation"]["InternalModel_preparestate!"] = @benchmarkable( - preparestate!($im, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["InternalModel_updatestate!"] = @benchmarkable( - updatestate!($im, $u, $y, $d), - setup=preparestate!($im, $y, $d), - samples=1 -) - -ukf = UnscentedKalmanFilter(nonlinmodel) -SUITE["StateEstimator"]["allocation"]["UnscentedKalmanFilter_preparestate!"] = @benchmarkable( - preparestate!($ukf, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["UnscentedKalmanFilter_updatestate!"] = @benchmarkable( - updatestate!($ukf, $u, $y, $d), - setup=preparestate!($ukf, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["UnscentedKalmanFilter_evaloutput"] = @benchmarkable( - evaloutput($ukf, $d), - setup=preparestate!($ukf, $y, $d), - samples=1 -) - -ekf = ExtendedKalmanFilter(linmodel, nint_u=[1, 1], direct=false) -SUITE["StateEstimator"]["allocation"]["ExtendedKalmanFilter_preparestate!"] = @benchmarkable( - preparestate!($ekf, $y, $d), - samples=1 -) -SUITE["StateEstimator"]["allocation"]["ExtendedKalmanFilter_updatestate!"] = @benchmarkable( - updatestate!($ekf, $u, $y, $d), - setup=preparestate!($ekf, $y, $d), - samples=1 -) - -## ================================================================================== -## ================== PredictiveController benchmarks =============================== -## ================================================================================== -empc = ExplicitMPC(linmodel, Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1]) -SUITE["PredictiveController"]["allocation"] = BenchmarkGroup(["allocation"]) -SUITE["PredictiveController"]["allocation"]["ExplicitMPC_moveinput!"] = @benchmarkable( - moveinput!($empc, $y, $d), - setup=preparestate!($empc, $y, $d), - samples=1 -) \ No newline at end of file +include("0_bench_setup.jl") +include("1_bench_sim_model.jl") +include("2_bench_state_estim.jl") +include("3_bench_predictive_control.jl") diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d8ebb5841..5b5d2050a 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -550,8 +550,8 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele and as efficient as possible. All the function outputs and derivatives are cached and updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use - of `Vararg{T,N}` (see the (performance tip)[@extref Julia Be-aware-of-when-Julia-avoids-specializing] - ) and memoization to avoid redundant computations. This is already complex, but it's even + of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing)) + and memoization to avoid redundant computations. This is already complex, but it's even worse knowing that most automatic differentiation tools do not support splatting. - The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined.