bench: adding some PredictiveController benchmarks

.gitignore

@@ -9,3 +9,4 @@ lcov.info
 benchmark/Manifest.toml
 /.benchmarkci
 /benchmark/*.json
+results_ModelPredictiveControl*

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]

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

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
+    )