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 guarantees 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
R
denoted xR
k ; xk is obtained as the solution to the difference equation (1) without the process noise Wk :
R
=
;
k
xR
f
x
(3)
k +1
k
This difference equation has an initial condition xR
0 . We define the error between Xk and
xR
as
k
k = Xk 0 xR
(4)
k
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
1
0f
xR
k ;k
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
k ;k
@ f (x; k ) +
We make the definition
Xk 0 xR
k
@x
x=xRk
@ f (x; t) Ak =
;
@x
x=xRk
and note that Ak is an n 2 n matrix of the following form:
2
@f1 (x;k )
@x1
6
..
.
@fn (x;k )
..
.
@fn (x;k )
:::
@x1
6
6
4
Ak =
3
@f1 (x;k )
@xn
7
:::
7
7
5
x=xRk
@xn
With this definition,
k+1
f xR
k;k
=
Ak k + GWk
+ Ak
Xk
0 xRk
+ GW k
0f
xR
k ;k
(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
k ; k) +
We define Hk as
@ h (x; k ) @x
x=xRk
X k 0 xR
k
Hk =
@ h (x; k ) @x
x=xRk
Then we can write (2) as
Zk
h(xRk; k) + Hk
Xk 0 xR
k
+ Vk :
Define k as
k
=
=
=
1
Zk 0 h(xR
k ; k)
h(Xk ; k ) + Vk
0 h(xRk; k)
Hk Xk
+ Vk
0 xRk
Hk k + Vk
A Taylor series expansion of a vector function f (x) about the point x0 is the following:
f (x) = f (x0 ) +
@ f (x) (x 0 x0 ) + Higher Order Terms
@ x x=x0
2
(6)
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 xR
k , 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
^ 0j0 = E [ 0 ] = E X0 0 xR
0
i
=0
P0j0 = PX0
Thus,
^ 1j0 = A0^ 0j0 = 0
The error covariance for the estimate ^ 1j0 is
T
P1j0 = A0P0j0 AT
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 X
1j0
P1j0
=
=
At time k
= 1,
E
E
E
1 0
^
j
1 0
10
X1 0 x1
R
0
^
X1 0 X
1j0
^
X
j
^
j
10
0x
0 X^ j
R
10
X1
T 1
T
X1 0 x1
R
0
=
0
1
K1 = P1j0H1T H1 P1j0 H1T + R
=
^ 1j0 + K1 1 0 H1 ^ 1j0
K1 1 ;
3
j
10
0x
R
T 1
(7)
we process the observation Z1 to obtain
^ 1j1
^
X
10
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
h
R
^
X
1j0 + K1 Z1 0 h x1 ; 1
i
i
^
^
X
1j0 + K1 Z1 0 h X1j0 ; 1
^ ; this can be shown using a
Note that P1j1 is approximately the error covariance of X
1j1
derivation similar to (7).
Now, to compute the estimate for k = 2, we could continue to use the reference trajectory 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
R
^
^ , where 3 represents
x1 = X1j1. With this new reference trajectory, we have 31 = X1 0 X
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 X
1j1
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:
^
X
k +1jk
=
^ ;k
f X
k jk
T
Pk+1jk = Ak Pkjk AT
k + GQG
Ak =
@ f (x; t) @x
2
^
x= X
kk
:
j
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:
Hk+1 =
@ h (x; tk ) @x
^
x=X
k+1 k
j
^
X
k +1jk +1
^
=X
k +1jk + Kk +1
Pk+1jk+1 = (I
0
1
Kk+1 = Pk+1jk HkT+1 Hk+1 Pk+1jk HkT+1 + R
h
i
^
Zk+1 0 h X
k +1jk ; k + 1
0 Kk
:
j
+1 Hk +1 ) Pk +1 k
^
^
Note that in the filter update equation for X
k +1jk +1 , the residual Zk +1 0 h Xk +1jk ; k + 1
plays the same role as the innovations in the standard Kalman filter. This residual sequence 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 continuous/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 continuous/discrete Extended Kalman filter by starting with a nominal reference trajectory denoted 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
+
Denote
@ f (x; t) @x
x=xR (t)
X(t) 0 xR (t)
@ f (x; t) A(t) =
;
@x
x=xR (t)
and note that A(t) is an n 2 n matrix of the following form:
2
A(t) =
@f1 (x;t)
@x1
6
6
6
4
3
@f1 (x;t)
@xn
7
:::
..
.
@fn (x;t)
..
.
@fn (x;t)
:::
@x1
7
7
5
x=xR (t)
@xn
With this definition,
_ (t)
f xR (t); t
=
A(t) (t) + GW(t)
+ A(t)
X(t) 0 xR (t)
+ GW(t)
0f
xR (t); 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(x
R
(tk ) ; tk ) +
We define Hk as
@ h (x; tk ) @x
x=xR (tk )
X(tk ) 0 xR (tk )
Hk =
@ h (x; tk ) @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 )
0 xR (tk)
=
Hk X(tk )
=
Hk (tk ) + Vk
+ Vk
0 h(xR (tk ) ; 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) + ^ (tjk ) :
X
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
^ (tj0) = xR (t) ;
X
1
where xR (t) is the solution to the differential equation (10) with an initial condition of
mX (t0 ). The error covariance is
j
j
P (t 0) = (t; t0)P (t0 0) T (t; t0) +
Z t
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
j
j
K1 = P (t1 0) H1T H1 P (t1 0) H1T + R
0
1
^ (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
^ (t j0) + K Z (t ) 0 h X
^ (t j0) ; t
X
1
1
1
1
1
1
1
1
1
:
^ (tj1) for t t t , we could continue to use the reference traNow, to obtain X
1
2
jectory that we used for the interval [t0; t1]; however, we now have a (hopefully) better
^ (t j1). Thus, to get the reference trajectory for
estimate of the state at time t1, namely X
1
^ (t j1). If our
t1 t t2, we find the solution of (10) with initial condition xR (t1 ) = X
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
j
j
P (t 1) = (t; t1)P (t1 1) T (t; t1) +
Z t
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:
^ (tjk ) is the solution of (10) with X
^ (t jk ) as the initial
Predictor Step: For tk t tk+1 , X
k
condition. The corresponding error covariance update is
j
j
T
P (t k ) = (t; tk )P (tk k ) (t; tk ) +
Z t
tk
(t; )GQGT T (t; )d;
where (t; ) is the state transition matrix obtained from
A(t) =
@ f (x; t) @x
^
(tjk )
x=X
:
Filter Step At tk+1 , the filter step is the following:
Hk+1 =
@ h (x; tk+1 ) j
@x
^
(tk +1 jk )
x=X
h
0
1
j
Kk+1 = P (tk+1 k ) HkT+1 Hk+1 P (tk+1 k ) HkT+1 + R
i
^ (t
^
^
X
k +1 jk + 1) = X (tk +1 jk ) + Kk +1 Z (tk +1 ) 0 h X (tk +1 jk )
j
(tk+1 k + 1) = (I
0 Kk
8
+1 Hk +1 ) P
j
(tk+1 k )
: