Extended Kalman Filter

The extended Kalman filter (EKF) would be used to handle the target state estimation problems
which occur in non-linear stochastic processes. A general motion model used in discrete
extended Kalman filter for target tracking is [1, 3]:
X (k ) FX (k 1) Gw(k 1)
(1)

z (k ) h( X ( k )) v(k )

(2)

where X (k ) is the state vector, F is the state transition matrix and G is the process noise

w(k ) and the measurement noise v( k ) are zero-mean, mutually

independent, white, Gaussian with covariance Q and R respectively. z (k ) is the measurement
vector at time k and h( X ( k )) is a nonlinear function of the states computed at time k .
gain matrix. The process noise

Linear Kalman filter could be used for target tracking if the states and the measurements are in
Cartesian coordinate system. Radar and IRST provide the measurements in a spherical
coordinate system. In most cases the state vector could be estimated in Cartesian coordinate
system. Eq (2) is nonlinear and it needs to be linearized to fit into the Kalman filter framework
entailing the use of extended Kalman filter (EKF). In target state estimation problems where the
process is non-linear, the non-linear model has to be linearised in order to apply the Kalman filter.
Two methods are available to linearise the non-linear model. First method linearises the nonlinear
model about nominal trajectory in the target state space that is independent of the
measurements. The nominal trajectory is pre-computable. The resultant tracking filter is called
linearised Kalman filter. The second method linearises the non-linear model about an estimated
trajectory that is continuously updated by the target state estimates resulting from the
measurements. The resultant tracking filter is called extended Kalman filter.
Since the process model is linear the time update is similar to Kalman filter. The predicted target
state and associated error covariance matrix are computed as:

and

~
X ( k | k 1) FX ( k 1 | k 1)
P (k | k 1) FP (k 1 | k 1) F T GQG T

(3)
(4)

The predicted measurement is computed as:

~
~
z ( k | k 1) h( X ( k | k 1))

(5)

Now considered the perturbation of the measurement model from the predicted state
~
X ( k | k 1) . The measurement model in eq.(2) could be approximated as:

z m ( k ) h( X ( kk 1)) H ( X ( k | k 1)) X ( k | k 1) v( k )

where

(6)

~
X ( k | k 1) X ( k ) X ( k | k 1)

~
h( X (k | k 1))
H ( X (k | k 1))
X

~
X X ( k | k 1)

h1
x
1

hm
x1

h1
x n

hm

x n

Finite difference method

Calculation of linearized measurement matrix can be accomplished by the finite difference

method. This method is generalized and flexible.

H (k ) H ij
where

hi
x j

hi ( x j x j ) hi ( x j )

x X%( k |k 1)

x j

(16)

i 1, 2,..., length of the measurement vector

j =1,2,...,length of the state vector
x j =perturbation step size

x in each of the unknown variables, the perturbed value hi ( x j x j ) is

computed. The corresponding elements of H ij are given by the finite difference in the function h
For small perturbation

(eq. 2) to changes in that sate. In general a perturbation step size of 10 -7 is considered to be

adequate.
Innovation is computed by subtracting eq. 2 from eq. 6 as:

~
z m (k ) ~
z ( k | k 1) H ( X ( k | k 1))( X ( k | k 1)) v( k )

(7)

The covariance of the innovation is computed as:

~
~
~
S E T H ( X ( k | k 1)) P (k | k 1) H T ( X (k | k 1)) R
The rest of the procedure is similar to Kalman filter i.e.
~
~
K P ( k | k 1) H ( X (k | k 1)) S 1

~
X (k | k ) X (k | k 1) K
~
P ( k | k ) [ I KH ( X ( k | k 1))]P ( k | k 1)

(8)

(9)
(10)
(11)

The extended Kalman filter tracking algorithm is summarized in table-2.

Table-1 Extended Kalman filter algorithm
Step 1: initialization
X (0) X 0
: initial state vector
(0) P
P
: initial state error covariance
0
Step 2: time update
~
: predicted state
X ( k | k 1) FX ( k 1 | k 1)
~
T
T

P ( k | k 1) FP ( k 1 | k 1) F GQG : predicted state error covariance

Step 3: measurement update

 h1
x
1

H (k )
hm
x1

H (k ) H ij

or

h1
x n

hm

x n

: linearised measurement matrix

hi
x j
~

~
X X ( k |k 1)

hi ( x j x j ) hi ( x j )

x X%( k |k 1)

x j

z (k ) HX (k | k 1)
~
S HP ( k | k 1) H T R
~
K P ( k | k 1) H T S 1
~
X (k | k ) X (k | k 1) K
~
P (k | k ) [ I KH ]P ( k | k 1)

: innovation
: innovation covariance
: Kalman gain
: estimated state
: estimated state error covariance

2.2.6 Numerical Simulation

Ground truth target trajectory with position, velocity and acceleration components in each of the
three Cartesian coordinates x -, y -and z -axis using the 3DOF kinematic model are simulated
to test the performance of the algorithms. The following parameters are considered in the
simulation.

T = 0.25sec.
2
6
Process noise variance w 10
Sampling interval

Measurement noise variance in:

range:

2
rng
10 2

2
6
azimuth: azi 10

elevation: ele 10
2

Duration of simulation: 125sec.

Initial sate vector:

X x

State transition matrix F 0

0

z T 100

Process noise gain matrix G 0

0

0 ,

20

0.5 100

where 0
0

50
T
1
0

0.3 10 1 0.01
T 2 / 2

T
1

T 3 / 6
2
where T / 2
T

The simulated data for 120sec in polar coordinate is shown in Fig-1.

azi.(rad)

1.1
1
0.9
0.8

20

40

60

80

100

120

rng(m)

ele.(rad)

0.06
noisy
true

0.04
0.02
0

20

40

20

40

60

80

100

120

80

100

120

10000
5000
0

60
time in sec.

Fig-2.3a Measurements in polar coordinates

The initial sate vector to initialize the tracking filter is:

X 0 0.9 X t
: initial estimated state vector at scan number one
where X
0

X t : true state vector at scan number one

The initial state error covariance matrix to initialize the tracking filter is:

diag X X
P
0
t
0

Processes and measurement noise covariance matrices are:

1
Q w2 0
0

0
1
0

0
0

2
azi

R 0
0

2
ele

2
rng

Measurement matrix has to be computed each iteration using partial differential equation method
or finite difference method. In this work finite difference method is used, since it is very simple and
easy adoptable. The tracking filter performance in terms of PFE, MAE, NIS, NEES, RMSPE,
RMSVE and RMSAE are shown in Table1 & Table-2. The PFE is within the acceptable range
except in acceleration at z-axis, since there is dynamics in that direction. Even though, PFE is
accepted as performance metric, it fails to provide accurate results when there is constant or
oscillating around. It is due to the norm of the true value at the denominator. This difficulty will be
over come by using MAE. Mean NIS is equivalent to the size of the measurement vector that
shows the filter consistency. It is learned that mean NEES never satisfy the criteria.

Table-1 Percentage fit error and mean absolute error in position, velocity and acceleration
in x-, y- and z-axis
x-axis
y-axis
z-axis
pos.
vel.
acc.
pos.
vel.
acc.
pos.
vel.
acc.
PFE
0.04
0.49
2.87
0.04
0.66
4.49
0.63
2.80
20.51
MAE 0.983 0.116 0.007 1.409 0.179 0.008 0.525 0.037 0.002
Table- Normalized estimation error square, normalized innovation square, root mean
square position error, root mean square velocity error and root mean square acceleration
error
NEES NIS
RMSPE RMSVE RMSAE
7.05
2.99
1.381
0.305
0.011
Initially RSSPE, RSSVE and RSSAE go to minimum after few iterations i.e. after learning the filter
as shown in Fig-2. Absolute error in positions, velocities and accelerations in x-, y- and z-axis are
shown in Fig-3. The NESS and NIS with their theoretical bounds are shown in Fig-4. It is learnt
that NEES never satisfies the criteria. The NIS is within the bounds shows the filter consistency.

RSSPE

10
5
0

20

40

60

80

100

120

20

40

60

80

100

120

20

40

80

100

120

RSSVE

6
4
2
0

RSSAE

0.06
0.04
0.02
0

60
time in sec.

Fig-2 Root mean square position, velocity and acceleration errors

AExpos.

6
4
2
0

20

40

60

80

100

120

20

40

60

80

100

120

20

40

80

100

120

AEypos.

6
4
2
0

AEzpos.

1.5
1
0.5
0

60
time in sec.

Fig-3a Absolute errors in positions of x-, y- and z-axis

AExvel.

2
1
0

20

40

60

80

100

120

20

40

60

80

100

120

20

40

80

100

120

AEyvel.

6
4
2
0

AEzvel.

0.1
0.05
0

60
time in sec.

Fig-3b Absolute errors in velocites of x-, y- and z-axis

AExacc.

0.04
0.02
0
0

20

40

60

80

100

120

20

40

60

80

100

120

20

40

80

100

120

AEyacc.

0.04
0.02
0

0
-3

AEzacc.

x 10

2
0

60
time in sec.

Fig-3c Absolute errors in accelerations of x-, y- and z-axis

14

NEES

12
10
8
6
4
4

20

40

20

40

60

80

100

120

80

100

120

NIS

3.5
3
2.5
2

60
time in sec.

Fig-4 Normalized estimation error square and normalized innovation square

The state errors with their theoretical bounds are shown in Fig-5. All these errors are within the
bounds shows the filter robustness.

SExpos.

10
0
-10

20

40

60

80

100

20

40

60

80
100
upper bound

120

SEypos.

10
0
-10

SEzpos.

low er bound

120

SEzpos.

0
-5

20

40

60
time in sec.

80

100

120

SExvel.

Fig-5a State error in positions of x-, y- and z-axis with theoretical bounds

2
0
-2
0

20

40

60

80

100

120

20

40

60

80

100

120

20

40

80

100

120

SEyvel.

5
0
-5

SEzvel.

0.2
0
-0.2

60
time in sec.

Fig-5b State error in velocities of x-, y- and z-axis with theoretical bounds

SExacc.

0.1
0
-0.1

20

40

60

80

100

120

20

40

60

80

100

120

20

40

80

100

120

SEyacc.

0.1
0
-0.1

SEzacc.

0.01
0
-0.01

60
time in sec.

Fig-5c State error in accelerations of x-, y- and z-axis with theoretical bounds
The innovation sequences with theoretical bounds are shown in Fig-6. The innovations are within
the bounds shows the filter consistency. The autocorrelation of innovation sequences with
theoretical bounds are shown in Fig-7. Autocorrelations are within the bounds shows that the
innovation sequence is white. This indicate that filter extract the information present in the signal.

ISazi.

0.01
0
-0.01

20

40

60

80

100

20

40

60

80

100

120

ISele.

0.01
0
-0.01

ISrng

50

low er bound

120

upper bound
ISrng.

0
-50

20

40

60
time in sec.

80

100

120

Fig-6 Innovation sequence with theoretical bounds

ACazi.

1
0.5
0
-0.5

50

100

150

200

250

300

350

400

450

500

50

100

150

200

250

300

350

400

450

500

ACele.

1
0.5
0
-0.5

low er bound

ACrng.

upper bound

0.5

ACrng.

0
-0.5

50

100

150

200

250
300
time lag

350

400

450

500

Fig-7 Autocorrelation of innovation sequence with theoretical bounds

10