Extended Kalman Filter Lecture Notes

EEE 581-Spring 1997

Darryl Morrell

1 Introduction
In this lecture note, we extend the Kalman Filter to non-linear system models to obtain an
approximate filter–the Extended Kalman Filter. We will do this by finding an approximate
error system that is linear, and applying the Kalman filter to this error system. Because
the EKF is obtained using a linear approximation of a non-linear system, it offers no guar-
antees of optimality in a mean squared error sense (or in any other sense). However, for
many systems, the EKF has proven to be a useful method of obtaining good estimates of
the system state.

2 Discrete/Discrete EKF
The system model that we use is the following discrete/discrete model:

Xk+1 = f (Xk ; k ) + GWk (1)

Zk = h (Xk ; k ) + Vk (2)
In this model, Wk is a discrete-time white noise process with mean zero and covariance
matrix Q, Vk is a discrete-time white noise process with mean zero and covariance matrix
R, and Wj , Vk , and X0 are uncorrelated for all j and k .
We develop the Extended Kalman filter by starting with a nominal reference trajectory
denoted xR R
k ; xk is obtained as the solution to the difference equation (1) without the pro-
cess noise Wk :  
xR
k +1 = f x R
k ; k (3)
This difference equation has an initial condition xR0 . We define the error between Xk and
xR
k as
 k = Xk 0 xR
k (4)
We now find an approximate linear model that describes the dynamics of  k :

k+1 = Xk+1 0 xR
k +1
 
= f (Xk ; k ) + GWk 0f xR
k ;k

1
To obtain a linear approximation of this equation, we make a Taylor series expansion1 of
f (Xk ; k ) about the value xR
k and drop all but the constant and linear terms:

   
f (Xk ; k )  f xR Xk 0 xR
@ f (x; k )
k ;k + k
@x x=xRk
We make the definition
@ f (x; t)
Ak = ;
x=xRk @x

and note that Ak is an n 2 n matrix of the following form:

2 3
@f1 (x;k ) @f1 (x;k )
@x1
::: @xn
6 7
Ak =
6
6
.. .. 7
7
4
. . 5
@fn (x;k ) @fn (x;k )
:::
@x1 @xn x=xRk
With this definition,
     
 k+1  f xR
k;k + Ak Xk 0 xRk + GW k 0f xR
k ;k

= Ak  k + GWk (5)
Note that (5) is a linear difference equation.
We follow a similar approach to obtain an (approximate) linear relationship from (2).
We expand h(Xk ; k ) in a Taylor series about the nominal trajectory xR
k:

 
h(Xk ; k )  h(xR X k 0 xR
@ h (x; k )
k ; k) + k
@x x=xRk
We define Hk as
@ h (x; k )
Hk =
@x x=xRk
Then we can write (2) as
 
Zk  h(xRk; k) + Hk Xk 0 xR
k + Vk :

Define  k as
k = Zk 0 h(xR k ; k)
= h(Xk ; k ) + Vk
 
0 h(xRk; k)
 Hk Xk 0 xRk + Vk

= Hk  k + Vk (6)
1
A Taylor series expansion of a vector function f (x) about the point x0 is the following:
@ f (x )
f (x) = f (x0 ) + (x 0 x0 ) + Higher Order Terms
@ x x=x0

2
So we have an (approximate) linear relationship between the quantity  k (which can be
computed from Zk ) and  k .
Assuming that  k obeys the difference equation (5) and the observation equation (6),
we can use a discrete/discrete Kalman filter to compute ^ k+1jk , Pk+1jk ,  ^
k +1jk +1 , and Pk +1jk +1 .
R
Note that the actual error between Xk and xk does not obey the linear equations (5) and
(6). Thus, ^ k+1jk and ^ k+1jk+1 are not optimal MMSE estimates of the actual error; we hope,
however, that they are still good estimates of this error. How do we obtain an estimate
of Xk+1 from ^ k+1jk or ^ k+1jk ? Since ^ k+1jk and ^ k+1jk are estimates of the error between Xk
and xRk , reasonable estimates of Xk +1 are

^
X k +1jk = xR
k +1 +  k +1jk
^

^
X k +1jk +1 = xR
k +1 +  k +1jk +1
^

How do we choose the reference trajectory? A reasonable value for xR

0 would be mX0 ,
the mean of the initial state. Using this value gives
h i
^ 0j0 = E [ 0 ] = E X0 0 xR
0 =0

P0j0 = PX0

Thus,
^ 1j0 = A0^ 0j0 = 0
The error covariance for the estimate ^ 1j0 is

P1j0 = A0P0j0 AT
T
0 + GQG

The estimate for X1 is

^
X 1j0 = xR
1 +  k +1jk
^ = f (mX0 ; 0)
^ :
Note that P1j0 is also approximately the error covariance of the estimate X1j0

  T 
P1j0 = E 1 0 ^ j
10 1 0 ^ j
10
     T 
 E X1 0 x1 R
0 ^
X
10 j 0x R
1 X1 0 x1 R
0 ^
X
10 j 0x R
1
  T 

= E X1 0 X
^
1j0 X1 0 X^ j10 (7)

At time k = 1, we process the observation Z1 to obtain

 0
 1
K1 = P1j0H1T H1 P1j0 H1T + R

 
^ 1j1 = ^ 1j0 + K1  1 0 H1 ^ 1j0
= K1  1 ;

3
where we have used the fact that ^ 1j0 = 0.

P1j1 = (I 0K H )P j : 1 1 10

^
The estimate X 1j1 is obtained as

^
X 1j1 = xR
1 +  1j1
^

= xR
1 + K1  1
h  i

1j0 + K1 Z1 0 h x1 ; 1
^ R
= X
h  i

1j0 + K1 Z1 0 h X1j0 ; 1
= ^
X ^

Note that P1j1 is approximately the error covariance of X ^ ; this can be shown using a
1j1
derivation similar to (7).
Now, to compute the estimate for k = 2, we could continue to use the reference tra-
jectory that we used for k = 1; however, we now have a (hopefully) better estimate of
^ . Thus, to get the reference trajectory for k = 2, we set
the state at time 1, namely X 1j1

x1 = X1j1. With this new reference trajectory, we have  31 = X1 0 X

R ^ ^ , where 3 represents
1j1 1
3
the error with the new reference trajectory. Assuming that  1 has a mean of zero 2 , the
estimate of  31 is ^3 1j1 = 0 and
^ 2j1 = A1^3 1j1 = 0
Thus,  
^
X 2j1 = xR
2 +  2j1
^ = ^ ;1
f X1j1

The approximate error covariance for this estimate is

P2j1 = A1P1j1 A1T + GQGT

At k = 2, we process the observation Z2 in the same way as we processed the observation

Z1 . Further prediction and filter updates follow this same pattern.
To summarize, the Extended Kalman Filter equations are the following:
Predictor Update:
 
^ ^ ;k
X k +1jk = f Xk jk

Pk+1jk = Ak Pkjk AT
k + GQG
T

@ f ( x ; t)
Ak = :
@x ^
x= X kk j

2
This assumption would be true if the system model were linear; since the system model is not linear,
we hope that the assumption is approximately true and that the mean of 31 is approximately zero.

4
Filter Update:

@ h (x; tk )
Hk+1 =
@x ^
x=X k+1 k j

 0
 1
Kk+1 = Pk+1jk HkT+1 Hk+1 Pk+1jk HkT+1 + R
h  i
^
X k +1jk +1
^
=X k +1jk + Kk +1 Zk+1 0 h X
^
k +1jk ; k + 1 :

Pk+1jk+1 = (I 0 Kk j
+1 Hk +1 ) Pk +1 k

 

k +1jk +1 , the residual Zk +1 0 h Xk +1jk ; k + 1

^
Note that in the filter update equation for X ^

plays the same role as the innovations in the standard Kalman filter. This residual se-
quence can be monitored to verify correct filter operation, much in the same way that the
innovation sequence can be monitored in the standard Kalman filter.

3 Continuous/Discrete EKF
The development of the continuous/discrete EKF is very similar to the development
of the discrete/discrete EKF. The system model that we use is the following continu-
ous/discrete model:
X_ (t) = f (X(t); t) + GW(t) (8)
Z (tk ) = h (X(tk ); tk ) + Vk (9)
In this model, W(t) is a continuous-time white noise process with mean zero and intensity
Q, Vk is a discrete-time white noise process with covariance R, and W(t), Vk , and X(t0)
are uncorrelated for all t and k .
As in the derivation of the discrete/discrete Kalman filter, we develop the continu-
ous/discrete Extended Kalman filter by starting with a nominal reference trajectory de-
noted xR (t); xR (t) is obtained as the solution to the differential equation (8) without the
process noise W(t):  
x_ R (t) = f xR (t); t (10)
This differential equation has some initial condition xR (t0). We denote the error between
X(t) and xR (t) as
 (t) = X(t) 0 xR (t) (11)
We now find an approximate linear model that describes the dynamics of  (t) by
taking the derivative of (11):

_ (t) = X_ (t) 0 x_ R (t)

 
= f (X(t); t) + GW(t) 0 f xR (t); t

5
To obtain a linear approximation of this equation, we make a Taylor series expansion of
f (X(t); t) about the value xR (t) and drop all but the constant and linear terms:
   
f (X(t); t)  f xR (t); t X(t) 0 xR (t)
@ f (x; t)
+
@x x=xR (t)
Denote
@ f (x; t)
A(t) = ;
x=xR (t) @x

and note that A(t) is an n 2 n matrix of the following form:

2 3
@f1 (x;t) @f1 (x;t)
@x1
::: @xn
6 7
A(t) =
6
6
.. .. 7
7
4
. . 5
@fn (x;t) @fn (x;t)
:::
@x1 @xn x=xR (t)
With this definition,
     
_ (t)  f xR (t); t + A(t) X(t) 0 xR (t) + GW(t) 0f xR (t); t
= A(t) (t) + GW(t) (12)

Note that (12) is now a linear differential equation.

We follow a similar approach to obtain an (approximate) linear relationship from (9).
We expand h(X(tk ); tk ) in a Taylor series about the nominal trajectory xR (tk ):
 
h(X(tk ); tk )  h(xR (tk ) ; tk ) + X(tk ) 0 xR (tk )
@ h (x; tk )
@x x=xR (tk )
We define Hk as
@ h (x; tk )
Hk =
@x x=xR (tk )
Then we can write (9) as
 
Z (tk )  h(xR (tk ) ; tk ) + Hk X(tk ) 0 xR (tk ) + Vk :

Define  (tk ) as

 (tk ) = Z (tk ) 0 h(xR (tk ) ; tk )

 
 h(xR (tk ) ; tk ) + Hk X(tk ) 0 xR (tk ) + Vk 0 h(xR (tk ) ; tk )
 
= Hk X(tk ) 0 xR (tk) + Vk

= Hk  (tk ) + Vk (13)

So we have an (approximate) linear relationship between the quantity  (tk ) (which can
be computed from Zk ) and (tk ).

6
 We can now use a continuous/discrete Kalman filter to compute ^ (tjk ) and P (tjk )
using the linear dynamics equation (12) and the linear observation equation (13). An
^ (tjk ) is obtained as
estimate of X(t) from 

^ (tjk ) = xR (t) + 
X ^ (tjk ) :
X

How do we choose the reference trajectory? Let us first consider the interval t0  t 
t1; a reasonable value for xR (t0) would be mX (t0) = E [X (t0)]. Using this value gives

E [ (t0)] = 0

j
P (t0 0) = PX0

Thus, for t0  t  t , ^ (tj0) = 0. The estimate for X (t) is

1

^ (tj0) = xR (t) ;
X

where xR (t) is the solution to the differential equation (10) with an initial condition of
mX (t0 ). The error covariance is
Z t

j j
P (t 0) = (t; t0)P (t0 0) T (t; t0) +
t0
(t;  )GQGT T (t;  )d;

where (t;  ) is the state transition matrix of the linear system model (12).
At time t1, we process the observation Z (t1 ) to obtain
 0
 1
j
K1 = P (t1 0) H1T H1 P (t1 0) H1T + R j
 
^ (t1 j1) = ^ (t1j0) + K1  (t1) 0 H1 ^ (t1 j0)
= K1  (t1);

where we have used the fact that ^ (t1 j0) = 0.

j
P (t1 1) = (I 0 K H ) P (t j0) :
1 1 1

^ (t j1) is obtained as
The estimate X 1

  
^ (t j1) = xR (t ) + K  (t ) = X
X ^ (t j0) + K Z (t ) 0 h X
^ (t j0) ; t :
1 1 1 1 1 1 1 1 1

Now, to obtain X^ (tj1) for t  t  t , we could continue to use the reference tra-
1 2
jectory that we used for the interval [t0; t1]; however, we now have a (hopefully) better
estimate of the state at time t1, namely X^ (t j1). Thus, to get the reference trajectory for
1

t1  t  t2, we find the solution of (10) with initial condition xR (t1 ) = X^ (t j1). If our
1

7
dynamics and observation models for the linearized error system were exact, we would
expect E [ (t)] = 0; making the assumption that the mean of the error is small, we get

^ (tj1) = xR (t) ;
X

where xR (t) is the solution of the differential equation (10) with an initial condition of
^ (t j1). The error covariance for this estimate is
xR (t1) = X 1

Z t

j j
P (t 1) = (t; t1)P (t1 1) T (t; t1) +
t1
(t;  )GQGT T (t;  )d:

At t2, we process the observation Z (t2) in the same way as we processed the observation
Z (t1 ).
To summarize, the Extended Kalman Filter equations are the following:

Predictor Step: For tk  t  tk+1 , X

^ (tjk ) is the solution of (10) with X
^ (t jk ) as the initial
k
condition. The corresponding error covariance update is
Z t

j j T
P (t k ) = (t; tk )P (tk k )  (t; tk ) +
tk
(t;  )GQGT T (t;  )d;

where (t;  ) is the state transition matrix obtained from

@ f (x; t)
A(t) = :
@x ^
x=X (tjk )

Filter Step At tk+1 , the filter step is the following:

@ h (x; tk+1 )
Hk+1 =
@x ^
x=X (tk +1 jk )

 0
 1
j
Kk+1 = P (tk+1 k ) HkT+1 Hk+1 P (tk+1 k ) HkT+1 + R j
h  i
k +1 jk + 1) = X (tk +1 jk ) + Kk +1 Z (tk +1 ) 0 h X (tk +1 jk )
^ (t
X ^ ^ :

j
(tk+1 k + 1) = (I 0 Kk +1 Hk +1 ) P j
(tk+1 k )