test: new ExtendedKalmanFilter tests with AutoFiniteDiff

Author: franckgaga

src/estimator/kalman.jl

@@ -956,10 +956,9 @@ end
 
 Construct an extended Kalman Filter with the [`SimModel`](@ref) `model`.
 
-Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model and the
-keyword arguments are identical to [`UnscentedKalmanFilter`](@ref), except for `α`, `β` and 
-`κ` which do not apply to the extended Kalman Filter. The Jacobians of the augmented model 
-``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic
+Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model is
+identical to [`UnscentedKalmanFilter`](@ref). By default, the Jacobians of the augmented
+model ``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic
 differentiation. This estimator is allocation-free if `model` simulations do not allocate.
 !!! warning
     See the Extended Help of [`linearize`](@ref) function if you get an error like:    

test/2_test_state_estim.jl

@@ -378,9 +378,9 @@ end
     @test internalmodel2.nxs == 1
     @test internalmodel2.nx̂ == 4
 
-    f(x,u,d,_) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
-    h(x,d,_)   = linmodel2.C*x + linmodel2.Dd*d
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2, solver=nothing)
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2, solver=nothing, p=linmodel2)
     internalmodel3 = InternalModel(nonlinmodel)
     @test internalmodel3.nym == 2
     @test internalmodel3.nyu == 0
@@ -499,9 +499,9 @@ end
 @testitem "UnscentedKalmanFilter construction" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys,Ts,i_d=[3])
-    f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
-    h(x,d,_)   = linmodel1.C*x + linmodel1.Du*d
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Du*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1)
 
     ukf1 = UnscentedKalmanFilter(linmodel1)
     @test ukf1.nym == 2
@@ -557,9 +557,10 @@ end
 @testitem "UnscentedKalmanFilter estimator methods" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u
-    h(x,_,_)   = linmodel1.C*x
-    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
+    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)
+    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
@@ -630,9 +631,9 @@ end
     setmodel!(ukf1, Q̂=[1e-3], R̂=[1e-6])
     @test ukf1.Q̂ ≈ [1e-3]
     @test ukf1.R̂ ≈ [1e-6]
-    f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
-    h(x,d,_)   = linmodel.C*x + linmodel.Du*d
-    nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1)
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Du*d
+    nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing, p=linmodel)
     ukf2 = UnscentedKalmanFilter(nonlinmodel, nint_ym=0)
     setmodel!(ukf2, Q̂=[1e-3], R̂=[1e-6])
     @test ukf2.Q̂ ≈ [1e-3]
@@ -642,10 +643,12 @@ end
 
 @testitem "ExtendedKalmanFilter construction" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    using DifferentiationInterface
+    import FiniteDiff
     linmodel1 = LinModel(sys,Ts,i_d=[3])
-    f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
-    h(x,d,_)   = linmodel1.C*x + linmodel1.Du*d
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Du*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1)
 
     ekf1 = ExtendedKalmanFilter(linmodel1)
     @test ekf1.nym == 2
@@ -689,17 +692,23 @@ end
     @test ekf8.Q̂ ≈ I(6)
     @test ekf8.R̂ ≈ I(2)
 
+    ekf9 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff())
+    @test ekf9.jacobian === AutoFiniteDiff()
+
     linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
     ekf8 = ExtendedKalmanFilter(linmodel2)
     @test isa(ekf8, ExtendedKalmanFilter{Float32})
 end
 
 @testitem "ExtendedKalmanFilter estimator methods" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    using DifferentiationInterface
+    import FiniteDiff
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
-    f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u
-    h(x,_,_)   = linmodel1.C*x
-    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
+    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)
+    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
@@ -741,6 +750,11 @@ end
     x̂ = updatestate!(ekf3, [0], [0])
     @test x̂ ≈ [0, 0]
     @test isa(x̂, Vector{Float32})
+    ekf4 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff())
+    preparestate!(ekf4, [50, 30])
+    @test updatestate!(ekf4, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9
+    preparestate!(ekf4, [50, 30])
+    @test evaloutput(ekf4) ≈ ekf4() ≈ [50, 30]
 end
 
 @testitem "ExtendedKalmanFilter set model" setup=[SetupMPCtests] begin
@@ -1008,9 +1022,10 @@ end
 @testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])
-    f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
-    h(x,d,_)   = linmodel.C*x + linmodel.Dd*d
-    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5])
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing)
+    nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5])
     mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1)
     preparestate!(mhe, [50, 30], [5])
     updatestate!(mhe, [10, 50], [50, 30], [5])
@@ -1085,9 +1100,10 @@ end
     setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38))
     @test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38)))
 
-    f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u
-    h(x,d,_)   = linmodel1.C*x 
-    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
+    f(x,u,d,model) = model.A*x + model.Bu*u
+    h(x,d,model)   = model.C*x 
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel1, solver=nothing)
+    nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
 
     mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3)
     setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10))
@@ -1181,9 +1197,10 @@ end
     info = getinfo(mhe)
     @test info[:V̂] ≈ [-1,-1] atol=5e-2
 
-    f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u
-    h(x,_,_)   = linmodel1.C*x
-    nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
+    f(x,u,_,model) = model.A*x + model.Bu*u
+    h(x,_,model)   = model.C*x
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel1, solver=nothing)
+    nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
     mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
 
     setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[100,100])
@@ -1266,9 +1283,9 @@ end
     setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6])
     @test mhe.Q̂ ≈ [1e-3]
     @test mhe.R̂ ≈ [1e-6]
-    f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
-    h(x,d,_)   = linmodel.C*x + linmodel.Du*d
-    nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1)
+    f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
+    h(x,d,model)   = model.C*x + model.Du*d
+    nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing)
     mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
     setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6])
     @test mhe2.Q̂ ≈ [1e-3]
@@ -1307,9 +1324,9 @@ end
         updatestate!(kf,  [11, 50], y, [25])
     end
     @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3
-    f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
-    h = (x,d,_)   -> linmodel1.C*x + linmodel1.Dd*d
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
+    f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
+    h = (x,d,model)   -> model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing)
     nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20])
     mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false)
     ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=false)
@@ -1356,9 +1373,9 @@ end
 @testitem "MovingHorizonEstimator LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin
     using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt
     linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20])
-    f = (x,u,d,_) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
-    h = (x,d,_)   -> linmodel.C*x + linmodel.Dd*d
-    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
+    f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
+    h = (x,d,model)   -> model.C*x + model.Dd*d
+    nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing)
     nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20])
     optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb" => "yes"))
     mhe_lin = MovingHorizonEstimator(linmodel, He=5, nint_ym=0, direct=true, optim=optim)