test: added no-allocations tests for StateEstimator

Author: franckgaga

test/1_test_sim_model.jl

@@ -158,9 +158,9 @@ end
     using DifferentiationInterface
     import FiniteDiff
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    f1(x,u,_,model) = model.A*x + model.Bu*u
-    h1(x,_,model)   = model.C*x
-    nonlinmodel1 = NonLinModel(f1,h1,Ts,2,2,2,solver=nothing,p=linmodel1)
+    f1!(x,u,_,model) = model.A*x + model.Bu*u
+    h1!(x,_,model)   = model.C*x
+    nonlinmodel1 = NonLinModel(f1!,h1!,Ts,2,2,2,solver=nothing,p=linmodel1)
     @test nonlinmodel1.nx == 2
     @test nonlinmodel1.nu == 2
     @test nonlinmodel1.nd == 0
@@ -268,12 +268,12 @@ end
 @testitem "NonLinModel sim methods" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    function f1(xnext, x, u, _ , model)
+    function f1!(xnext, x, u, _ , model)
         mul!(xnext, model.A, x)
         mul!(xnext, model.Bu, u, 1, 1)
     end
-    h1(y, x , _ , model) = mul!(y, model.C, x)
-    nonlinmodel = NonLinModel(f1,h1,Ts,2,2,2,p=linmodel1,solver=nothing)
+    h1!(y, x , _ , model) = mul!(y, model.C, x)
+    nonlinmodel = NonLinModel(f1!,h1!,Ts,2,2,2,p=linmodel1,solver=nothing)
 
     u, d = zeros(2), Float64[]
     @test updatestate!(nonlinmodel, u) ≈ zeros(2) 
@@ -296,9 +296,9 @@ end
     using DifferentiationInterface
     import ForwardDiff, FiniteDiff
     Ts = 1.0
-    f1(x,u,d,_) = x.^5 .+ u.^4 .+ d.^3
-    h1(x,d,_)   = x.^2 .+ d
-    nonlinmodel1 = NonLinModel(f1,h1,Ts,1,1,1,1,solver=nothing)
+    f1!(x,u,d,_) = x.^5 .+ u.^4 .+ d.^3
+    h1!(x,d,_)   = x.^2 .+ d
+    nonlinmodel1 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=nothing)
     x, u, d = [2.0], [3.0], [4.0]
     linmodel1 = linearize(nonlinmodel1; x, u, d)
     @test linmodel1.A  ≈ 5*x.^4
@@ -313,7 +313,7 @@ end
     @test linmodel1.C  ≈ linmodel1b.C
     @test linmodel1.Dd ≈ linmodel1b.Dd  
 
-    nonlinmodel2 = NonLinModel(f1,h1,Ts,1,1,1,1,solver=nothing, jacobian=AutoFiniteDiff()) 
+    nonlinmodel2 = NonLinModel(f1!,h1!,Ts,1,1,1,1,solver=nothing, jacobian=AutoFiniteDiff()) 
     linmodel2 = linearize(nonlinmodel2; x, u, d)
     @test linmodel2.A  ≈ 5*x.^4 atol=1e-3
     @test linmodel2.Bu ≈ 4*u.^3 atol=1e-3
@@ -357,14 +357,15 @@ end
             yl  = linmodel3(d)
             Ynl[i] = ynl[1]
             Yl[i]  = yl[1]
-            linmodel3 = linearize(nonlinmodel3; u, d)
+            linearize!(linmodel3, nonlinmodel3; u, d)
             updatestate!(nonlinmodel3, u, d)
             updatestate!(linmodel3, u, d)
         end
         Ynl, Yl
     end
     @test all(isapprox.(Ynl, Yl, atol=1e-6))
 
+    # test nd==0 also works with AutoFiniteDiff (does not support empty matrices):
     f2!(xnext, x, u, _, _) = (xnext .= x .+ u)
     h2!(y, x, _, _) = (y .= x)
     nonlinmodel4 = NonLinModel(f2!,h2!,Ts,1,1,1,0,solver=nothing,jacobian=AutoFiniteDiff())
@@ -378,7 +379,7 @@ end
     end
     nonlinmodel4 = NonLinModel(f3!, h3!, Ts, 1, 1, 1, 1, solver=nothing)
     linmodel4 = linearize(nonlinmodel4; x, u, d)
-    # see this bug : https://github.com/JuliaLang/julia/issues/51112
+    # return nothing (see this issue : https://github.com/JuliaLang/julia/issues/51112):
     linearize2!(linmodel, model) = (linearize!(linmodel, model); nothing)
     linearize2!(linmodel4, nonlinmodel4)
     @test @allocations(linearize2!(linmodel4, nonlinmodel4)) == 0

test/2_test_state_estim.jl

@@ -65,11 +65,14 @@ end
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     skalmanfilter1 = SteadyKalmanFilter(linmodel1, nint_ym=[1, 1])
-    preparestate!(skalmanfilter1, [50, 30])
-    @test updatestate!(skalmanfilter1, [10, 50], [50, 30]) ≈ zeros(4)
-    preparestate!(skalmanfilter1, [50, 30])
-    @test updatestate!(skalmanfilter1, [10, 50], [50, 30], Float64[]) ≈ zeros(4)
+    u, y, d = [10, 50], [50, 30], Float64[]
+    preparestate!(skalmanfilter1, y)
+    @test updatestate!(skalmanfilter1, u, y) ≈ zeros(4)
+    preparestate!(skalmanfilter1, y)
+    @test updatestate!(skalmanfilter1, u, y, d) ≈ zeros(4)
     @test skalmanfilter1.x̂0 ≈ zeros(4)
+    @test @allocations(preparestate!(skalmanfilter1, y)) == 0
+    @test @allocations(updatestate!(skalmanfilter1, u, y)) == 0
     preparestate!(skalmanfilter1, [50, 30])
     @test evaloutput(skalmanfilter1) ≈ skalmanfilter1() ≈ [50, 30]
     @test evaloutput(skalmanfilter1, Float64[]) ≈ skalmanfilter1(Float64[]) ≈ [50, 30]
@@ -194,11 +197,14 @@ end
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     kalmanfilter1 = KalmanFilter(linmodel1)
-    preparestate!(kalmanfilter1, [50, 30])
-    @test updatestate!(kalmanfilter1, [10, 50], [50, 30]) ≈ zeros(4)
-    preparestate!(kalmanfilter1, [50, 30])
-    @test updatestate!(kalmanfilter1, [10, 50], [50, 30], Float64[]) ≈ zeros(4)
+    u, y, d = [10, 50], [50, 30], Float64[]
+    preparestate!(kalmanfilter1, y)
+    @test updatestate!(kalmanfilter1, u, y) ≈ zeros(4)
+    preparestate!(kalmanfilter1, y)
+    @test updatestate!(kalmanfilter1, u, y, d) ≈ zeros(4)
     @test kalmanfilter1.x̂0 ≈ zeros(4)
+    @test @allocations(preparestate!(kalmanfilter1, y)) == 0
+    @test @allocations(updatestate!(kalmanfilter1, u, y)) == 0
     preparestate!(kalmanfilter1, [50, 30])
     @test evaloutput(kalmanfilter1) ≈ kalmanfilter1() ≈ [50, 30]
     @test evaloutput(kalmanfilter1, Float64[]) ≈ kalmanfilter1(Float64[]) ≈ [50, 30]
@@ -311,11 +317,14 @@ end
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])
     lo1 = Luenberger(linmodel1, nint_ym=[1, 1])
-    preparestate!(lo1, [50, 30])
-    @test updatestate!(lo1, [10, 50], [50, 30]) ≈ zeros(4)
-    preparestate!(lo1, [50, 30])
-    @test updatestate!(lo1, [10, 50], [50, 30], Float64[]) ≈ zeros(4)
+    u, y, d = [10, 50], [50, 30], Float64[]
+    preparestate!(lo1, y)
+    @test updatestate!(lo1, u, y) ≈ zeros(4)
+    preparestate!(lo1, y)
+    @test updatestate!(lo1, u, y, d) ≈ zeros(4)
     @test lo1.x̂0 ≈ zeros(4)
+    @test @allocations(preparestate!(lo1, y)) == 0
+    @test @allocations(updatestate!(lo1, u, y)) == 0
     preparestate!(lo1, [50, 30])
     @test evaloutput(lo1) ≈ lo1() ≈ [50, 30]
     @test evaloutput(lo1, Float64[]) ≈ lo1(Float64[]) ≈ [50, 30]
@@ -436,12 +445,15 @@ end
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]) , uop=[10,50], yop=[50,30])
     internalmodel1 = InternalModel(linmodel1)
-    preparestate!(internalmodel1, [50, 30] .+ 1)
-    @test updatestate!(internalmodel1, [10, 50], [50, 30] .+ 1) ≈ zeros(2)
-    preparestate!(internalmodel1, [50, 30] .+ 1)
-    @test updatestate!(internalmodel1, [10, 50], [50, 30] .+ 1, Float64[]) ≈ zeros(2)
+    u, y, d = [10, 50], [50, 30] .+ 1, Float64[]
+    preparestate!(internalmodel1, y)
+    @test updatestate!(internalmodel1, u, y) ≈ zeros(2)
+    preparestate!(internalmodel1, y)
+    @test updatestate!(internalmodel1, u, y, d) ≈ zeros(2)
     @test internalmodel1.x̂d ≈ internalmodel1.x̂0 ≈ zeros(2)
     @test internalmodel1.x̂s ≈ ones(2)
+    @test @allocations(preparestate!(internalmodel1, y)) == 0
+    @test @allocations(updatestate!(internalmodel1, u, y)) == 0
     preparestate!(internalmodel1, [51, 31])
     @test evaloutput(internalmodel1, Float64[]) ≈ [51,31]
     @test initstate!(internalmodel1, [10, 50], [50, 30]) ≈ zeros(2)
@@ -557,16 +569,26 @@ end
 @testitem "UnscentedKalmanFilter estimator methods" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    f(x,u,_,model) = model.A*x + model.Bu*u
-    h(x,_,model)   = model.C*x
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1)
+    function f!(xnext, x,u,_,model)
+        mul!(xnext, model.A, x)
+        mul!(xnext, model.Bu, u, 1, 1)
+        return nothing
+    end
+    function h!(y, x,_,model)
+        mul!(y, model.C, x)
+        return nothing
+    end
+    nonlinmodel = NonLinModel(f!, h!, Ts, 2, 2, 2, solver=nothing, p=linmodel1)
     nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
     ukf1 = UnscentedKalmanFilter(nonlinmodel)
-    preparestate!(ukf1, [50, 30])
-    @test updatestate!(ukf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9
-    preparestate!(ukf1, [50, 30])
-    @test updatestate!(ukf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9
+    u, y, d = [10, 50], [50, 30], Float64[]
+    preparestate!(ukf1, y)
+    @test updatestate!(ukf1, u, y) ≈ zeros(4) atol=1e-9
+    preparestate!(ukf1, y)
+    @test updatestate!(ukf1, u, y, d) ≈ zeros(4) atol=1e-9
     @test ukf1.x̂0 ≈ zeros(4) atol=1e-9
+    @test @allocations(preparestate!(ukf1, y)) == 0
+    @test @allocations(updatestate!(ukf1, u, y)) == 0
     preparestate!(ukf1, [50, 30])
     @test evaloutput(ukf1) ≈ ukf1() ≈ [50, 30]
     @test evaloutput(ukf1, Float64[]) ≈ ukf1(Float64[]) ≈ [50, 30]
@@ -705,16 +727,26 @@ end
     using DifferentiationInterface
     import FiniteDiff
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    f(x,u,_,model) = model.A*x + model.Bu*u
-    h(x,_,model)   = model.C*x
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1)
+    function f!(xnext, x,u,_,model)
+        mul!(xnext, model.A, x)
+        mul!(xnext, model.Bu, u, 1, 1)
+        return nothing
+    end
+    function h!(y, x,_,model)
+        mul!(y, model.C, x)
+        return nothing
+    end
+    nonlinmodel = NonLinModel(f!, h!, Ts, 2, 2, 2, solver=nothing, p=linmodel1)
     nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
     ekf1 = ExtendedKalmanFilter(nonlinmodel)
-    preparestate!(ekf1, [50, 30])
-    @test updatestate!(ekf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9
-    preparestate!(ekf1, [50, 30])
-    @test updatestate!(ekf1, [10, 50], [50, 30], Float64[]) ≈ zeros(4) atol=1e-9
+    u, y, d = [10, 50], [50, 30], Float64[]
+    preparestate!(ekf1, y)
+    @test updatestate!(ekf1, u, y) ≈ zeros(4) atol=1e-9
+    preparestate!(ekf1, y)
+    @test updatestate!(ekf1, u, y, d) ≈ zeros(4) atol=1e-9
     @test ekf1.x̂0 ≈ zeros(4) atol=1e-9
+    @test @allocations(preparestate!(ekf1, y)) == 0
+    @test @allocations(updatestate!(ekf1, u, y)) == 0
     preparestate!(ekf1, [50, 30])
     @test evaloutput(ekf1) ≈ ekf1() ≈ [50, 30]
     @test evaloutput(ekf1, Float64[]) ≈ ekf1(Float64[]) ≈ [50, 30]