bench: added skf bench

Author: franckgaga

benchmark/benchmarks.jl

@@ -1,10 +1,14 @@
 using BenchmarkTools
-
 using ModelPredictiveControl, ControlSystemsBase, LinearAlgebra
+
 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])   ] 
-linmodel = setop!(LinModel(sys, Ts, i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])
+
+const SUITE = BenchmarkGroup()
+
+## ================== 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)
@@ -17,12 +21,9 @@ function h!(y, x, d, p)
     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 = [10, 50], [5]
+nonlinmodel = setop!(nonlinmodel, uop=[10, 50], yop=[50, 30], dop=[5])
+u, d, y = [10, 50], [5], [50, 30]
 
-const SUITE = BenchmarkGroup()
-
-## ================== SimModel benchmarks =========================================
 SUITE["SimModel"]["allocation"] = BenchmarkGroup(["allocation"])
 SUITE["SimModel"]["allocation"]["LinModel_updatestate!"] = @benchmarkable(
     updatestate!($linmodel, $u, $d),
@@ -47,3 +48,15 @@ SUITE["SimModel"]["allocation"]["NonLinModel_linearize!"] = @benchmarkable(
 )
 
 ## ================== 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_update!"] = @benchmarkable(
+    updatestate!($skf, $u, $y, $d),
+    setup=preparestate!($skf, $y, $d),
+    samples=1
+)