tests: add preparestate! when necessary

franckgaga · franckgaga · commit 9ecb633f2b5d · 2024-08-07T12:13:56.000-04:00
diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl
@@ -127,8 +127,10 @@ end
     setstate!(mpc1, [1,2,3,4])
     @test mpc1.estim.x̂0 ≈ [1,2,3,4]
     setstate!(mpc1, [0,0,0,0])
+    preparestate!(mpc1, mpc1.estim())
     updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim())
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]
+    preparestate!(mpc1, mpc1.estim())
     @test_throws ArgumentError updatestate!(mpc1, [0,0])
 end
 
@@ -246,6 +248,7 @@ end
     setstate!(model, [1,1])
     for i=1:20
         y = model()
+        preparestate!(mpc, y)
         u = moveinput!(mpc, [0, 0])
         X_mpc[:,i] = model.x0
         updatestate!(mpc, u, y)
@@ -398,8 +401,10 @@ end
     setstate!(mpc1, [1,2,3,4])
     @test mpc1.estim.x̂0 ≈ [1,2,3,4]
     setstate!(mpc1, [0,0,0,0])
+    preparestate!(mpc1, mpc1.estim())
     updatestate!(mpc1, mpc1.estim.model.uop, mpc1.estim())
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]
+    preparestate!(mpc1, mpc1.estim())
     @test_throws ArgumentError updatestate!(mpc1, [0,0])
 end
 
@@ -596,6 +601,7 @@ end
     setstate!(nmpc1, [1,2,3,4])
     @test nmpc1.estim.x̂0 ≈ [1,2,3,4]
     setstate!(nmpc1, [0,0,0,0])
+    preparestate!(nmpc1, nmpc1.estim())
     updatestate!(nmpc1, nmpc1.estim.model.uop, nmpc1.estim())
     @test nmpc1.estim.x̂0 ≈ [0,0,0,0] atol=1e-6
 end
diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl
@@ -67,14 +67,16 @@ end
 @testset "SteadyKalmanFilter estimator methods" begin
     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)
     @test skalmanfilter1.x̂0 ≈ zeros(4)
     @test evaloutput(skalmanfilter1) ≈ skalmanfilter1() ≈ [50, 30]
     @test evaloutput(skalmanfilter1, Float64[]) ≈ skalmanfilter1(Float64[]) ≈ [50, 30]
     @test initstate!(skalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]]
     linmodel2 = LinModel(append(tf(1, [1, 0]), tf(2, [10, 1])), 1.0)
-    skalmanfilter2 = SteadyKalmanFilter(linmodel2, nint_u=[1, 1])
+    skalmanfilter2 = SteadyKalmanFilter(linmodel2, nint_u=[1, 1], direct=false)
     x = initstate!(skalmanfilter2, [10, 3], [0.5, 6+0.1])
     @test evaloutput(skalmanfilter2) ≈ [0.5, 6+0.1]
     @test updatestate!(skalmanfilter2, [10, 3], [0.5, 6+0.1]) ≈ x
@@ -103,6 +105,7 @@ end
     @test skalmanfilter2() ≈ [51, 32] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     skalmanfilter3 = SteadyKalmanFilter(linmodel3)
+    preparestate!(skalmanfilter3, [0])
     x̂ = updatestate!(skalmanfilter3, [0], [0])
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
@@ -170,7 +173,9 @@ end
 @testset "KalmanFilter estimator methods" begin
     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)
     @test kalmanfilter1.x̂0 ≈ zeros(4)
     @test evaloutput(kalmanfilter1) ≈ kalmanfilter1() ≈ [50, 30]
@@ -201,6 +206,7 @@ end
     @test kalmanfilter2() ≈ [51, 32] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     kalmanfilter3 = KalmanFilter(linmodel3)
+    preparestate!(kalmanfilter3, [0])
     x̂ = updatestate!(kalmanfilter3, [0], [0])
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
@@ -213,6 +219,7 @@ end
     kalmanfilter = KalmanFilter(linmodel, nint_ym=0)
     @test kalmanfilter.Â ≈ [0.5]
     @test evaloutput(kalmanfilter) ≈ [50.0]
+    preparestate!(kalmanfilter, [50.0])
     x̂ = updatestate!(kalmanfilter, [2.0], [50.0])
     @test x̂ ≈ [3.0]
     newlinmodel = LinModel(ss(0.2, 0.3, 1.0, 0, 10.0))
@@ -221,6 +228,7 @@ end
     @test kalmanfilter.Â ≈ [0.2]
     @test evaloutput(kalmanfilter) ≈ [55.0]
     @test kalmanfilter.lastu0 ≈ [2.0 - 3.0]
+    preparestate!(kalmanfilter, [55.0])
     x̂ = updatestate!(kalmanfilter, [3.0], [55.0])
     @test x̂ ≈ [3.0]
     newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0])
@@ -274,7 +282,9 @@ end
 @testset "Luenberger estimator methods" begin
     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)
     @test lo1.x̂0 ≈ zeros(4)
     @test evaloutput(lo1) ≈ lo1() ≈ [50, 30]
@@ -305,6 +315,7 @@ end
     @test lo2() ≈ [51, 32] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     lo3 = Luenberger(linmodel3)
+    preparestate!(lo3, [0])
     x̂ = updatestate!(lo3, [0], [0])
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
@@ -411,6 +422,7 @@ end
 
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     internalmodel3 = InternalModel(linmodel3)
+    preparestate!(internalmodel3, [0])
     x̂ = updatestate!(internalmodel3, [0], [0])
     @test x̂ ≈ [0]
     @test isa(x̂, Vector{Float32})
@@ -422,6 +434,7 @@ end
     internalmodel = InternalModel(linmodel)
     @test internalmodel.Â ≈ [0.5]
     @test evaloutput(internalmodel) ≈ [50.0]
+    preparestate!(internalmodel, [50.0])
     x̂ = updatestate!(internalmodel, [2.0], [50.0])
     @test x̂ ≈ [3.0]
     newlinmodel = LinModel(ss(0.2, 0.3, 1.0, 0, 10.0))
@@ -430,6 +443,7 @@ end
     @test internalmodel.Â ≈ [0.2]
     @test evaloutput(internalmodel) ≈ [55.0]
     @test internalmodel.lastu0 ≈ [2.0 - 3.0]
+    preparestate!(internalmodel, [55.0])
     x̂ = updatestate!(internalmodel, [3.0], [55.0])
     @test x̂ ≈ [3.0]
     newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0])
@@ -500,7 +514,9 @@ end
     h(x,_)   = linmodel1.C*x
     nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), 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
     @test ukf1.x̂0 ≈ zeros(4) atol=1e-9
     @test evaloutput(ukf1) ≈ ukf1() ≈ [50, 30]
@@ -531,6 +547,7 @@ end
     @test ukf2() ≈ [51, 32] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     ukf3 = UnscentedKalmanFilter(linmodel3)
+    preparestate!(ukf3, [0])
     x̂ = updatestate!(ukf3, [0], [0])
     @test x̂ ≈ [0, 0] atol=1e-3
     @test isa(x̂, Vector{Float32})
@@ -542,6 +559,7 @@ end
     ukf1 = UnscentedKalmanFilter(linmodel, nint_ym=0)
     @test ukf1.Â ≈ [0.5]
     @test evaloutput(ukf1) ≈ [50.0]
+    preparestate!(ukf1, [50.0])
     x̂ = updatestate!(ukf1, [2.0], [50.0])
     @test x̂ ≈ [3.0]
     newlinmodel = LinModel(ss(0.2, 0.3, 1.0, 0, 10.0))
@@ -550,6 +568,7 @@ end
     @test ukf1.Â ≈ [0.2]
     @test evaloutput(ukf1) ≈ [55.0]
     @test ukf1.lastu0 ≈ [2.0 - 3.0]
+    preparestate!(ukf1, [55.0])
     x̂ = updatestate!(ukf1, [3.0], [55.0])
     @test x̂ ≈ [3.0]
     newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0])
@@ -627,7 +646,9 @@ end
     h(x,_)   = linmodel1.C*x
     nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), 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
     @test ekf1.x̂0 ≈ zeros(4) atol=1e-9
     @test evaloutput(ekf1) ≈ ekf1() ≈ [50, 30]
@@ -658,6 +679,7 @@ end
     @test ekf2() ≈ [51, 32] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     ekf3 = ExtendedKalmanFilter(linmodel3)
+    preparestate!(ekf3, [0])
     x̂ = updatestate!(ekf3, [0], [0])
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
@@ -669,6 +691,7 @@ end
     ekf1 = ExtendedKalmanFilter(linmodel, nint_ym=0)
     @test ekf1.Â ≈ [0.5]
     @test evaloutput(ekf1) ≈ [50.0]
+    preparestate!(ekf1, [50.0])
     x̂ = updatestate!(ekf1, [2.0], [50.0])
     @test x̂ ≈ [3.0]
     newlinmodel = LinModel(ss(0.2, 0.3, 1.0, 0, 10.0))
@@ -677,6 +700,7 @@ end
     @test ekf1.Â ≈ [0.2]
     @test evaloutput(ekf1) ≈ [55.0]
     @test ekf1.lastu0 ≈ [2.0 - 3.0]
+    preparestate!(ekf1, [55.0])
     x̂ = updatestate!(ekf1, [3.0], [55.0])
     @test x̂ ≈ [3.0]
     newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0])
@@ -782,6 +806,7 @@ end
     h(x,d)   = linmodel1.C*x + linmodel1.Dd*d
     nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5])
     mhe1 = MovingHorizonEstimator(nonlinmodel, He=2)
+    preparestate!(mhe1, [50, 30], [5])
     x̂ = updatestate!(mhe1, [10, 50], [50, 30], [5])
     @test x̂ ≈ zeros(6) atol=1e-9
     @test mhe1.x̂0 ≈ zeros(6) atol=1e-9
@@ -824,6 +849,7 @@ end
     @test mhe2([5]) ≈ [51, 32] atol=1e-3
     linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     mhe3 = MovingHorizonEstimator(linmodel3, He=1)
+    preparestate!(mhe3, [0])
     x̂ = updatestate!(mhe3, [0], [0])
     @test x̂ ≈ [0, 0] atol=1e-3
     @test isa(x̂, Vector{Float32})
@@ -840,6 +866,7 @@ end
     optim = Model(Ipopt.Optimizer)
     covestim = ExtendedKalmanFilter(nonlinmodel, 1:2, 0, 0, Q̂, Q̂, R̂)
     mhe5 = MovingHorizonEstimator(nonlinmodel, 1, 1:2, 0, 0, Q̂, Q̂, R̂, Inf; optim, covestim)
+    preparestate!(mhe5, [50, 30], [5])
     x̂ = updatestate!(mhe5, [10, 50], [50, 30], [5])
     @test x̂ ≈ zeros(4) atol=1e-9
     @test mhe5.x̂0 ≈ zeros(4) atol=1e-9
@@ -906,6 +933,7 @@ end
     @test_throws ArgumentError setconstraint!(mhe2, c_v̂min=[-1])
     @test_throws ArgumentError setconstraint!(mhe2, c_v̂max=[+1])
 
+    preparestate!(mhe1, [50, 30])
     updatestate!(mhe1, [10, 50], [50, 30])
     @test_throws ErrorException setconstraint!(mhe1, x̂min=[-Inf,-Inf])
     @test_throws ErrorException setconstraint!(mhe1, x̂max=[+Inf,+Inf])
@@ -938,10 +966,12 @@ end
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe, x̂min=[1,1], x̂max=[100,100])
+    preparestate!(mhe, [50, 30])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
     @test x̂ ≈ [1, 1] atol=5e-2
 
     setconstraint!(mhe, x̂min=[-100,-100], x̂max=[-1,-1])
+    preparestate!(mhe, [50, 30])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
     @test x̂ ≈ [-1, -1] atol=5e-2
 
@@ -950,10 +980,12 @@ end
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe, ŵmin=[1,1], ŵmax=[100,100])
+    preparestate!(mhe, [50, 30])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
     @test mhe.Ŵ ≈ [1,1] atol=5e-2
 
     setconstraint!(mhe, ŵmin=[-100,-100], ŵmax=[-1,-1])
+    preparestate!(mhe, [50, 30])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
     @test mhe.Ŵ ≈ [-1,-1] atol=5e-2
 
@@ -962,11 +994,13 @@ end
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe, v̂min=[1,1], v̂max=[100,100])
+    preparestate!(mhe, [50, 30])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
     info = getinfo(mhe)
     @test info[:V̂] ≈ [1,1] atol=5e-2
 
     setconstraint!(mhe, v̂min=[-100,-100], v̂max=[-1,-1])
+    preparestate!(mhe, [50, 30])
     x̂ = updatestate!(mhe, [10, 50], [50, 30])
     info = getinfo(mhe)
     @test info[:V̂] ≈ [-1,-1] atol=5e-2
@@ -981,10 +1015,12 @@ end
     setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe2, x̂min=[1,1], x̂max=[100,100])
+    preparestate!(mhe2, [50, 30])
     x̂ = updatestate!(mhe2, [10, 50], [50, 30])
     @test x̂ ≈ [1, 1] atol=5e-2
 
     setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[-1,-1])
+    preparestate!(mhe2, [50, 30])
     x̂ = updatestate!(mhe2, [10, 50], [50, 30])
     @test x̂ ≈ [-1, -1] atol=5e-2
 
@@ -993,10 +1029,12 @@ end
     setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe2, ŵmin=[1,1], ŵmax=[100,100])
+    preparestate!(mhe2, [50, 30])
     x̂ = updatestate!(mhe2, [10, 50], [50, 30])
     @test mhe2.Ŵ ≈ [1,1] atol=5e-2
 
     setconstraint!(mhe2, ŵmin=[-100,-100], ŵmax=[-1,-1])
+    preparestate!(mhe2, [50, 30])
     x̂ = updatestate!(mhe2, [10, 50], [50, 30])
     @test mhe2.Ŵ ≈ [-1,-1] atol=5e-2
 
@@ -1005,11 +1043,13 @@ end
     setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[100,100])
 
     setconstraint!(mhe2, v̂min=[1,1], v̂max=[100,100])
+    preparestate!(mhe2, [50, 30])
     x̂ = updatestate!(mhe2, [10, 50], [50, 30])
     info = getinfo(mhe2)
     @test info[:V̂] ≈ [1,1] atol=5e-2
 
     setconstraint!(mhe2, v̂min=[-100,-100], v̂max=[-1,-1])
+    preparestate!(mhe2, [50, 30])
     x̂ = updatestate!(mhe2, [10, 50], [50, 30])
     info = getinfo(mhe2)
     @test info[:V̂] ≈ [-1,-1] atol=5e-2
@@ -1041,7 +1081,9 @@ end
     for i in 1:6
         X̂0_mhe[:,i] = mhe.x̂0
         X̂0_ukf[:,i] = ukf.x̂0
+        preparestate!(mhe, [50, 31], [25])
         updatestate!(mhe, [11, 50], [50, 31], [25])
+        preparestate!(ukf, [50, 31], [25])
         updatestate!(ukf, [11, 50], [50, 31], [25])
     end
     @test X̂0_mhe ≈ X̂0_ukf atol=1e-3
@@ -1054,6 +1096,7 @@ end
     setconstraint!(mhe, x̂min=[-1000], x̂max=[1000])
     @test mhe.Â ≈ [0.5]
     @test evaloutput(mhe) ≈ [50.0]
+    preparestate!(mhe, [50.0])
     x̂ = updatestate!(mhe, [2.0], [50.0])
     @test x̂ ≈ [3.0]
     newlinmodel = LinModel(ss(0.2, 0.3, 1.0, 0, 10.0))
@@ -1064,6 +1107,7 @@ end
     @test mhe.lastu0 ≈ [2.0 - 3.0]
     @test mhe.U0 ≈ [2.0 - 3.0]
     @test mhe.Y0m ≈ [50.0 - 55.0]
+    preparestate!(mhe, [55.0])
     x̂ = updatestate!(mhe, [3.0], [55.0])
     @test x̂ ≈ [3.0]
     newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0])
