Skip to content

Commit 4c53e18

Browse files
committed
test: new ExtendedKalmanFilter tests with AutoFiniteDiff
1 parent 740258f commit 4c53e18

File tree

2 files changed

+56
-40
lines changed

2 files changed

+56
-40
lines changed

src/estimator/kalman.jl

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -956,10 +956,9 @@ end
956956
957957
Construct an extended Kalman Filter with the [`SimModel`](@ref) `model`.
958958
959-
Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model and the
960-
keyword arguments are identical to [`UnscentedKalmanFilter`](@ref), except for `α`, `β` and
961-
`κ` which do not apply to the extended Kalman Filter. The Jacobians of the augmented model
962-
``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic
959+
Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model is
960+
identical to [`UnscentedKalmanFilter`](@ref). By default, the Jacobians of the augmented
961+
model ``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic
963962
differentiation. This estimator is allocation-free if `model` simulations do not allocate.
964963
!!! warning
965964
See the Extended Help of [`linearize`](@ref) function if you get an error like:

test/2_test_state_estim.jl

Lines changed: 53 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -378,9 +378,9 @@ end
378378
@test internalmodel2.nxs == 1
379379
@test internalmodel2.nx̂ == 4
380380

381-
f(x,u,d,_) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d
382-
h(x,d,_) = linmodel2.C*x + linmodel2.Dd*d
383-
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2, solver=nothing)
381+
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
382+
h(x,d,model) = model.C*x + model.Dd*d
383+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2, solver=nothing, p=linmodel2)
384384
internalmodel3 = InternalModel(nonlinmodel)
385385
@test internalmodel3.nym == 2
386386
@test internalmodel3.nyu == 0
@@ -499,9 +499,9 @@ end
499499
@testitem "UnscentedKalmanFilter construction" setup=[SetupMPCtests] begin
500500
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
501501
linmodel1 = LinModel(sys,Ts,i_d=[3])
502-
f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
503-
h(x,d,_) = linmodel1.C*x + linmodel1.Du*d
504-
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
502+
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
503+
h(x,d,model) = model.C*x + model.Du*d
504+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1)
505505

506506
ukf1 = UnscentedKalmanFilter(linmodel1)
507507
@test ukf1.nym == 2
@@ -557,9 +557,10 @@ end
557557
@testitem "UnscentedKalmanFilter estimator methods" setup=[SetupMPCtests] begin
558558
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
559559
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
560-
f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u
561-
h(x,_,_) = linmodel1.C*x
562-
nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
560+
f(x,u,_,model) = model.A*x + model.Bu*u
561+
h(x,_,model) = model.C*x
562+
nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1)
563+
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
563564
ukf1 = UnscentedKalmanFilter(nonlinmodel)
564565
preparestate!(ukf1, [50, 30])
565566
@test updatestate!(ukf1, [10, 50], [50, 30]) zeros(4) atol=1e-9
@@ -630,9 +631,9 @@ end
630631
setmodel!(ukf1, Q̂=[1e-3], R̂=[1e-6])
631632
@test ukf1. [1e-3]
632633
@test ukf1. [1e-6]
633-
f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
634-
h(x,d,_) = linmodel.C*x + linmodel.Du*d
635-
nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1)
634+
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
635+
h(x,d,model) = model.C*x + model.Du*d
636+
nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing, p=linmodel)
636637
ukf2 = UnscentedKalmanFilter(nonlinmodel, nint_ym=0)
637638
setmodel!(ukf2, Q̂=[1e-3], R̂=[1e-6])
638639
@test ukf2. [1e-3]
@@ -642,10 +643,12 @@ end
642643

643644
@testitem "ExtendedKalmanFilter construction" setup=[SetupMPCtests] begin
644645
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
646+
using DifferentiationInterface
647+
import FiniteDiff
645648
linmodel1 = LinModel(sys,Ts,i_d=[3])
646-
f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
647-
h(x,d,_) = linmodel1.C*x + linmodel1.Du*d
648-
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
649+
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
650+
h(x,d,model) = model.C*x + model.Du*d
651+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1)
649652

650653
ekf1 = ExtendedKalmanFilter(linmodel1)
651654
@test ekf1.nym == 2
@@ -689,17 +692,23 @@ end
689692
@test ekf8. I(6)
690693
@test ekf8. I(2)
691694

695+
ekf9 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff())
696+
@test ekf9.jacobian === AutoFiniteDiff()
697+
692698
linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0)
693699
ekf8 = ExtendedKalmanFilter(linmodel2)
694700
@test isa(ekf8, ExtendedKalmanFilter{Float32})
695701
end
696702

697703
@testitem "ExtendedKalmanFilter estimator methods" setup=[SetupMPCtests] begin
698704
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
705+
using DifferentiationInterface
706+
import FiniteDiff
699707
linmodel1 = LinModel(sys,Ts,i_u=[1,2])
700-
f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u
701-
h(x,_,_) = linmodel1.C*x
702-
nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
708+
f(x,u,_,model) = model.A*x + model.Bu*u
709+
h(x,_,model) = model.C*x
710+
nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1)
711+
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
703712
ekf1 = ExtendedKalmanFilter(nonlinmodel)
704713
preparestate!(ekf1, [50, 30])
705714
@test updatestate!(ekf1, [10, 50], [50, 30]) zeros(4) atol=1e-9
@@ -741,6 +750,11 @@ end
741750
= updatestate!(ekf3, [0], [0])
742751
@test [0, 0]
743752
@test isa(x̂, Vector{Float32})
753+
ekf4 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff())
754+
preparestate!(ekf4, [50, 30])
755+
@test updatestate!(ekf4, [10, 50], [50, 30]) zeros(4) atol=1e-9
756+
preparestate!(ekf4, [50, 30])
757+
@test evaloutput(ekf4) ekf4() [50, 30]
744758
end
745759

746760
@testitem "ExtendedKalmanFilter set model" setup=[SetupMPCtests] begin
@@ -1008,9 +1022,10 @@ end
10081022
@testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin
10091023
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
10101024
linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5])
1011-
f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
1012-
h(x,d,_) = linmodel.C*x + linmodel.Dd*d
1013-
nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5])
1025+
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
1026+
h(x,d,model) = model.C*x + model.Dd*d
1027+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing)
1028+
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5])
10141029
mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1)
10151030
preparestate!(mhe, [50, 30], [5])
10161031
updatestate!(mhe, [10, 50], [50, 30], [5])
@@ -1085,9 +1100,10 @@ end
10851100
setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38))
10861101
@test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38)))
10871102

1088-
f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u
1089-
h(x,d,_) = linmodel1.C*x
1090-
nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
1103+
f(x,u,d,model) = model.A*x + model.Bu*u
1104+
h(x,d,model) = model.C*x
1105+
nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel1, solver=nothing)
1106+
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
10911107

10921108
mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3)
10931109
setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10))
@@ -1181,9 +1197,10 @@ end
11811197
info = getinfo(mhe)
11821198
@test info[:V̂] [-1,-1] atol=5e-2
11831199

1184-
f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u
1185-
h(x,_,_) = linmodel1.C*x
1186-
nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30])
1200+
f(x,u,_,model) = model.A*x + model.Bu*u
1201+
h(x,_,model) = model.C*x
1202+
nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel1, solver=nothing)
1203+
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30])
11871204
mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
11881205

11891206
setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[100,100])
@@ -1266,9 +1283,9 @@ end
12661283
setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6])
12671284
@test mhe. [1e-3]
12681285
@test mhe. [1e-6]
1269-
f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
1270-
h(x,d,_) = linmodel.C*x + linmodel.Du*d
1271-
nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1)
1286+
f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d
1287+
h(x,d,model) = model.C*x + model.Du*d
1288+
nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing)
12721289
mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0)
12731290
setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6])
12741291
@test mhe2. [1e-3]
@@ -1307,9 +1324,9 @@ end
13071324
updatestate!(kf, [11, 50], y, [25])
13081325
end
13091326
@test X̂_mhe X̂_kf atol=1e-3 rtol=1e-3
1310-
f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d
1311-
h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d
1312-
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
1327+
f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
1328+
h = (x,d,model) -> model.C*x + model.Dd*d
1329+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing)
13131330
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20])
13141331
mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false)
13151332
ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=false)
@@ -1356,9 +1373,9 @@ end
13561373
@testitem "MovingHorizonEstimator LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin
13571374
using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt
13581375
linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20])
1359-
f = (x,u,d,_) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d
1360-
h = (x,d,_) -> linmodel.C*x + linmodel.Dd*d
1361-
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing)
1376+
f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d
1377+
h = (x,d,model) -> model.C*x + model.Dd*d
1378+
nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing)
13621379
nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20])
13631380
optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb" => "yes"))
13641381
mhe_lin = MovingHorizonEstimator(linmodel, He=5, nint_ym=0, direct=true, optim=optim)

0 commit comments

Comments
 (0)