89

6.0 EXTENDED KALMAN FILTER

6.1 Introduction
One of the underlying assumptions of the Kalman filter is that it is
designed to estimate the states of a linear system based on measurements that
are a linear function of the states. Unfortunately, in many of the situations
where we would like to use a Kalman filter, we have a non-linear system model
and/or a non-linear measurement equation. Specifically, the system model is a
non-linear function of the states and/or the measurements are non-linear
functions of the states. Usually, the non-linearities dont extend to the system
disturbances and measurement noise.
Because of the attractiveness of the Kalman filter, designers have
developed a set of mathematics to extend Kalman filter theory to situations
where the system model and/or measurement model are non-linear functions
of state. The resultant Kalman filter is referred to as the extended Kalman
filter. As we will see, the extended Kalman filter uses the non-linear system
model for computing the predicted state estimate,
( )
1 k k + x , and the non-linear
measurement model to form the predicted measurement,
( )
1 k k + y . The
smoothed state then takes on the standard Kalman filter form similar to
Equation 5-78.

6.2 Extended Kalman Filter Development
6.2.1 Problem Definition
We are interested in designing a Kalman filter for a system defined by
the state equation
( ) ( ) ( ) ( ) ( ) 1 k k k k + = + x x w f G (6-1)
and the measurement equation
( ) ( ) ( ) ( ) 1 1 1 k k k + = + + + y x v h . (6-2)
In Equations 6-1 and 6-2 ( ) k w and ( ) 1 k + v are uncorrelated, zero-mean, white
random processes. They are also uncorrelated with the initial state, ( ) 0 x .
( ) k G is a known matrix and ( ) ( )
k x f and ( ) ( )
1 k + x h are known, vector, non-
linear functions of the state. They are of the forms
( ) ( )
( ) ( )
( ) ( )
( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
1 1 1 2
2 2 1 2
1 2
x , x , x
x , x , x
x , x , x
n
n
n n n
f k f k k k
f k f k k k
k
f k f k k k
( (
( (
( (
= =
( (
( (
( (

x
x
x
x
f (6-3)
and

90
( ) ( )
( ) ( )
( ) ( )
( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
1 1 1 2
2 2 1 2
1 2
1 x 1 , x 1 , x 1
1 x 1 , x 1 , x 1
1
1 x 1 , x 1 , x 1
n
n
m m n
h k h k k k
h k h k k k
k
h k h k k k
( ( + + + +
( (
+ + + +
( (
+ = =
( (
( (
( (
+ + + +

x
x
x
x
h . (6-4)

6.2.2 Filter Development
We begin by separating ( ) k x and ( ) 1 k + y into two parts as
( ) ( ) ( )
0
k k k o = + x x x (6-5)
and
( ) ( ) ( )
0
1 1 1 k k k o + = + + + y y y . (6-6)
( )
0
k x and ( )
0
1 k + y are termed the nominal values of ( ) k x and ( ) 1 k + y ,
respectively, while ( ) k o x and ( ) 1 k o + y are termed perturbations.
Next we expand ( ) ( )
k x f and ( ) ( )
k x h into a Taylor series expansion
about ( )
0
k x and ( )
0
1 k + x as
( ) ( ) ( ) ( )
( )
( ) ( )
( )
( )
( ) ( )
0
0
0
H.O.T.
k
k
k
k k k k
k
c
= + + (

c
x
x x x
x
x
x
f
f f x (6-7)
and
( ) ( ) ( ) ( )
( )
( ) ( )
( )
( )
( ) ( )
0
0
0
H.O.T.
k
k
k
k k k k
k
c
= + + (

c
x
x x x
x
x
x
h
h h x . (6-8)
We next drop the high order terms (H.O.T.), recognize that ( ) ( ) ( )
0
k k k o = x x x
and write
( ) ( ) ( ) ( ) ( ) ( )
0
k k k k o = + x x f f x F (6-9)
and
( ) ( ) ( ) ( ) ( ) ( )
0
k k k k o = + x x h h x H . (6-10)
In Equations 6-9 and 6-10, ( ) k F and ( ) k H are defined by
( )
( ) ( )
( )
( )
0
1 1 1 2 1
2 1 2 2 2
1 2
n
n
k
n n n n
f x f x f x
k f x f x f x
k
k
f x f x f x
c c c c c c (
(
c c c c c c c
(
= =
( c
(
c c c c c c

x
f
F
x
x
(6-11)

91
and
( )
( ) ( )
( )
( )
0
1 1 1 2 1
2 1 2 2 2
1 2
n
n
k
m m m n
h x h x h x
k h x h x h x
k
k
h x h x h x
c c c c c c (
(
c c c c c c c
(
= =
( c
(
c c c c c c

x
h
H
x
x
(6-12)
where the notation
i j
g x c c is interpreted as
( ) ( ) ( ) ( )
( )
( ) ( )
( ) ( )
1 10
0
1 2
x
x
x , x , , x
x
n n
i n
i j
k x k
j
k x k
g k k k
g x
k
=
=
c
c c =
c
. (6-13)
We want to digress to discuss the notation we have used. One of the
misconceptions associated with Equations 6-5 and 6-6 is that we have some
knowledge of the nominal state and measurement, ( )
0
k x and ( )
0
1 k + y , and
can thus insure that the perturbations, ( ) k o x and ( ) 1 k o + y , are small. This,
in turn, allows us to claim that the H.O.T. of the Taylor series expansions of
( ) ( )
k x f and ( ) ( )
1 k + x h are small and can thus be ignored. In fact, we have
no knowledge of the nominal states or measurements (if we did, we would not
need to build a Kalman filter!). We introduce the notation as a mathematical
tool that facilitates our derivation. We temporarily assume we know the
nominal state and measurement and that they are close to the actual state and
measurement. This allows us to claim that the perturbations are small and
thus that the first order approximations to the Taylor series expansions are
valid. We cannot determine the validity of our assumption and its
ramifications beforehand. Because of this we cannot always be sure that an
extended Kalman filter that we design will work well. While this may be
discouraging we should take solace from the fact that designers have been
designing extended Kalman filters for the past forty or so years and that most
of these filters work very well.

Returning to our development, we use Equations 6-5, 6-6, 6-9 and 6-10
to rewrite the system and measurement equations as
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
0 0
1 1 k k k k k k k o o + + + = + + x x w x f x F G (6-14)
and
( ) ( ) ( ) ( ) ( ) ( ) ( )
0 0
1 1 1 1 1 1 k k k k k k o o + + + = + + + + + + y x v y h x H . (6-15)
We next separate Equations 6-14 and 6-15 into two sets of equations. One of
these we term the nominal equations and the other we call the perturbation
equations. The nominal equations are
( ) ( ) ( )
0 0
1 k k + = x f x (6-16)

92
and
( ) ( ) ( )
0 0
1 1 k k + = + y h x , (6-17)
and the perturbation equations are
( ) ( ) ( ) ( ) ( ) 1 k k k k k o o + = + x x w F G (6-18)
and
( ) ( ) ( ) ( ) 1 1 1 1 k k k k o o + = + + + + y x v H . (6-19)
For now we set the nominal equations aside and build a Kalman filter for the
perturbation model. This results in the equations
( ) ( ) ( )
1 k k k k o o + = x x F , (6-20)
( ) ( ) ( )
1 1 1 k k k k k o o + = + + y x H , (6-21)
( ) ( ) ( ) ( ) ( )
1 1 1 1 1 k k k k k k k o o o o ( + = + + + + +

x x y y K , (6-22)
( ) ( ) ( ) ( ) ( ) ( ) ( )
1
1 1 1 1 1 1 1
T T
k k k k k k k k k

( + = + + + + + + +

K P H H P H R , (6-23)
( ) ( ) ( ) ( ) ( ) ( ) ( ) 1
T T
k k k k k k k k + = + P F P F G Q G and (6-24)
( ) ( ) ( ) ( )
1 1 1 1 k k k k k + = + + + (

P I K H P . (6-25)
With Equations 6-20 through 6-25 we have a means of estimating the
perturbation part of the state. While this is an interesting result, it is not what
we want. We want ( )
k x , the estimate of ( ) k x ; not ( )
k o x , the estimate of
( ) k o x .

Let us consider ( )
k x . Based on our results from earlier, we can write
( )
k x as
( ) ( ) ( )
0
k k k o = + x x x . (6-26)
In this equation ( )
0
k x is the nominal state discussed earlier and ( )
k o x is the
estimate of ( ) k o x that we just derived. Equation 6-26 tells us that to form an
estimate of the state we add an estimate of the perturbed state to the nominal
state (we conveniently ignore the fact that we dont really know the nominal
state).
If we consider the mean-squared error between ( ) k x and ( )
k x we get

93
( ) ( ) ( ) ( )
{ }
( ) ( ) ( ) ( )
{ }
2
2


T
x
T
x
e E k k k k
E k k k k e
o
o o o o
= ( (

= = ( (

x x x x
x x x x
. (6-27)
Recall that the Kalman filter results in a ( )
k o x that minimizes
2
x
e
o
. Since
2 2
x x
e e
o
= we tend to jump to the conclusion that we have also minimized
2
x
e .
This is not a totally correct conclusion. We can say that we have minimized
2
x
e
under the constraint that ( )
k x is given by Equation 6-26 and ( )
k o x is defined
by Equation 6-20 through 6-25. If we were to remove these constraints we
could conceivably find an ( )
k x such that
2 2
min min x x
e e
o
< . However, we dont have
a general means of finding ( )
k x that truly minimizes
2
x
e . Therefore, for want of
anything better, we will adopt Equation 6-26. As indicated earlier, the quality of
this decision can only be tested by constructing the extended Kalman filter and
testing it.

If we combine Equation 6-26 with our previous results we get
( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( )
0
0
1 1 1
1 1 1
k k k
k k k k k k k
o
o o o
+ = + + +
( = + + + + +

x x
x y y
x
f x F K
(6-28)
and if we expand ( ) ( )
k x f into a Taylor series about ( )
0
k x we get
( ) ( ) ( ) ( ) ( ) ( ) ( )
0 0
H.O.T. k k k k k = + + (

x x f f x F x . (6-29)
Dropping the higher order terms (H.O.T.) and recognizing ( ) ( )
0
k k x x as
( )
k o x we get
( ) ( ) ( ) ( ) ( ) ( )
0
k k k k o = + x x f f x F . (6-30)
Using this we can replace the first two terms in the last part of Equation 6-28 to
yield
( ) ( ) ( ) ( ) ( ) ( )
1 1 1 1 k k k k k k o o ( + = + + + +

x x y y f K . (6-31)

We next turn our attention to the last term in Equation 6-31. If we use
Equation 6-6 and write the predicted measurement as
( ) ( ) ( )
0
1 1 1 k k k k k o + = + + + y y y , (6-32)
we can rewrite the last term as
( ) ( ) ( ) ( )
1 1 1 1 k k k k k k o o + + = + + y y y y (6-33)

94
and the state estimation equation as
( ) ( ) ( ) ( ) ( ) ( )
1 1 1 1 k k k k k k ( + = + + + +

x x y y f K . (6-34)

We note that we have manipulated the state estimation equation into the
form that we want. That is, we have an equation for the state estimate at stage
1 k + in terms of the state estimate at stage k , the measurement at stage 1 k +
and the predicted measurement at stage 1 k + . We now need to perform some
more mathematical manipulations to complete our formulation of the extended
Kalman filter.
We start by examining the function ( ) ( )
k x f . If we wanted to predict the
value of ( ) 1 k + x given only ( )
k x we would use the original state equation of
Equation 6-1. However, this equation contains the system disturbance ( ) k w ,
which we dont know. Using the logic from Chapter 5 we can say that since we
dont know ( ) k w , our best choice for its replacement would be ( ) { }
E k w , which
is zero. Thus we are left with the equation
( ) ( ) ( )
1 k k k + = x x f (6-35)
for the predicted state at stage 1 k + given the state estimate at stage k .
As we did in Chapter 5, we can characterize the error between the
predicted and actual state as
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
1 1 1 k k k k k k k k k k o o A + = + + = + (

x x x x x w F G . (6-36)
With this we can use the results of Chapter 4 to write the covariance of
( )
1 k k A + x as
( ) ( ) ( ) ( ) ( ) ( ) ( ) 1
T T
k k k k k k k k + = + P F P F G Q G . (6-37)

We next consider the predicted measurement,
( )
1 k k + y . Following the
logic from Chapter 5, we can relate the predicted measurement to the predicted
state through the measurement model of Equation 6-2. However, we dont
know ( ) 1 k + v . As in Chapter 5, for want of anything better we will use
( ) { }
1 0 E k + = v instead. Thus we get
( ) ( ) ( )
1 1 k k k k + = + y x h . (6-38)

The final topic we need to consider concerns the matrices ( ) k F and
( ) 1 k + H . Recall that these matrices are given by

95
( )
( ) ( )
( )
( )
0
k
k
k
k
c
=
c
x
x
x
f
F (6-39)
and
( )
( ) ( )
( )
( )
0
1
1
1
1
k
k
k
k
+
c +
+ =
c +
x
x
x
h
H . (6-40)
To compute ( ) k F and ( ) 1 k + H we need ( )
0
k x and ( )
0
1 k + x , which we dont
know. In lieu of these, we use ( )
k x in place of ( )
0
k x and
( )
1 k k + x in place of
( )
0
1 k + x . We can offer no rationale for these substitutions except to state that
they are the only ones we have.

6.2.3 Summary
We now summarize the results of this section into the following
presentation of the extended Kalman filter. Given the state and measurement
models of Equations 6-1 and 6-2, along with the previously indicated properties
of ( ) k w , ( ) 1 k + v and ( ) 0 x an extended Kalman filter that can be used to
estimate the state of the system is given by the equations
( ) ( ) ( ) ( ) ( )
1 1 1 1 1 k k k k k k k ( + = + + + + +

x x y y K , (6-41)
( ) ( ) ( )
1 k k k + = x x f , (6-42)
( ) ( ) ( )
1 1 k k k k + = + y x h , (6-43)
( ) ( ) ( ) ( ) ( ) ( ) ( )
1
1 1 1 1 1 1 1
T T
k k k k k k k k k

( + = + + + + + + +

K P H H P H R , (6-44)
( ) ( ) ( ) ( ) ( ) ( ) ( ) 1
T T
k k k k k k k k + = + P F P F G Q G , (6-45)
( ) ( ) ( ) ( )
1 1 1 1 k k k k k + = + + + (

P I K H P , (6-46)
( )
( ) ( )
( )
( ) k
k
k
k
c
=
c
x
x
x
f
F and (6-47)
( )
( ) ( )
( )
( )
1
1
1
1
k k
k
k
k
+
c +
+ =
c +
x
x
x
h
H . (6-49)
Equations 6-41 through 6-49 constitute the tracking form of the extended
Kalman filter. The control theoretic form of the extended Kalman filter is defined
by the equations

96
( ) ( ) ( ) ( ) ( ) ( ) ( )
1 k k k k k ( + = +

x x y x f K h , (6-50)
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
1
T T
k k k k k k k k

( = +

K F P H H P H R , (6-51)
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1
T T
k k k k k k k k k + = + (

P F K H P F G Q G , (6-52)
( )
( ) ( )
( )
( ) k
k
k
k
c
=
c
x
x
x
f
F and (6-53)
( )
( ) ( )
( )
( ) k
k
k
k
c
=
c
x
x
x
h
H . (6-54)
The derivation of the control theoretic form of the extended Kalman filter is left
to the reader. Block diagrams of the tracking and control theoretic forms of the
extended Kalman filter are contained in Figures 6-1 and 6-2. It will be noted
that their structure is very similar to the normal Kalman filter block diagrams
of Figures 5-7 and 5-8. The difference is that the matrices ( ) k F and ( ) k H are
replaced by their non-linear equivalents, ( ) ( )
k x f and ( ) ( )
k x h .


6-1. Tracking Form of the Extended Kalman Filter

97



6.2.4 Properties of the Extended Kalman Filter
We now take some time to discuss the properties of the extended
Kalman filter. We start by noting that the extended Kalman filter is intuitively
appealing. It has the basic structure of the normal Kalman filter but also
incorporates the non-linear functions of the system and measurement models.
Since it incorporates these non-linear functions, the extended Kalman filter is a
non-linear filter. That is, the state prediction equation and the predicted
measurement equations are non-linear. Because of this the state estimate
equation is also a non-linear function of the previous state estimate.
Although not as obvious, the elements of ( ) k F and ( ) k H are also non-
linear functions of the state estimates. This means that the Kalman gain and
covariance matrices are also non-linear functions of the state estimate.
It should be obvious to the reader that the development of the extended
Kalman filter is very heuristic. That is, while the various steps are intuitively
appealing, as are the resultant equations, they are not always mathematically
rigorous. For example, we have no rigorous mathematical (or physical)
justification for ignoring the higher order terms in the various Taylor series
expansions. Nor can we mathematically or physically justify replacing ( )
0
k x
Figure 6-2. Control Theoretic Form of the Extended Kalman Filter

98
by ( )
k x and ( )
0
1 k + x by
( )
1 k k + x when evaluating ( ) k F and ( ) 1 k + H .
Because of this we must take care in implementing extended Kalman filters. It
is quite possible that we could design an unstable extended Kalman filter. In
other instances, the extended Kalman filter may be stable but do a poor job of
estimating the system states. Having made these statements, we also note that
designers have been very successful at constructing extended Kalman filters
that work very well. They are stable, they produce accurate state estimates,
their covariance matrices are representative of the actual errors between the
estimate and actual states, and they have good transient response. However,
this is not accomplished by blind application of the extended Kalman filter
equations. It results from very careful selection of the system and
measurement models and tuning of the filter via the ( ) k Q matrix.

6.3 Applications
As the reader has probably deduced by now, the main effort in designing
Kalman filters is devoted to deriving system and measurement models, and in
determining ( ) 1 k + R , ( ) k Q , ( )
0 x and ( ) 0 P . Once these are found the Kalman
filter will take on the established forms of Equations 6-41 through 6-49 or
Equations 6-50 through 6-54 depending upon whether we are building a
tracking or control theoretic type of Kalman filter. Because of this, we will
devote the rest of this chapter to developing system and measurement models
for some examples. As mentioned before, the system model is usually derived
from an understanding of the physics of the process or system, and the
measurement model is based on our understanding of how the measurements
take place.

6.3.1 Example 1: General Target Tracker in Cartesian Coordinates
For our first example we consider the system and measurement models
that might be used to design a Kalman filter for a broad range of target tracking
problems. We cast the problem in a Cartesian coordinate system (xyz). We will
limit ourselves to two dimensions (x and z) for ease of notation. The reader can
extend the results to three dimensions.
Since we are considering a general target tracker we have no detailed
knowledge of the system since we dont know the target type or its motion
characteristics. The target could be an aircraft, a ship, a ballistic missile, an
artillery round or a bicycle. In this case, our best choice for a system model
would be to use the Taylor series expansion discussed in Chapter 2.
Specifically, if we use position and velocity as our states we would have
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
2
2
2
2
1 2
1 2
1 2
1 2
x k x k x k T x k T
x k x k x k T x k T
z k z k z k T z k T
z k z k z k T z k T
+ = + + +
+ = + + +
+ = + + +
+ = + + +
. (6-55)

99
Recall that since we dont know the second and higher order derivatives we
lump them into the model disturbance terms, ( ) k w . With this we have
( ) ( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( )
1
1
1
1
x
x
z
z
x k x k x k T w k
x k x k w k
z k z k z k T w k
z k z k w k
+ = + +
+ = +
+ = + +
+ = +
. (6-56)
If we write this in matrix form we get
( ) ( ) ( ) ( ) ( ) x 1 k k k k k + = + x w F G (6-57)
where
( )
1 0 0
0 1 0 0
0 0 1
0 0 0 1
T
k
T
(
(
(
=
(
(

F , ( ) k = G I , ( )
( )
( )
( )
( )
( )
( )
( )
( )
1
2
3
4
x
x
x
x
k x k
k x k
k
k z k
k z k
( (
( (
( (
= =
( (
( (
( (

x and
( )
( )
( )
( )
( )
( )
( )
( )
( )
1
2
3
4
w
w
w
w
x
x
z
z
k w k
k w k
k
k w k
k w k
( (
( (
( (
= =
( (
( (
( (

w .
We have transitioned to representing the states and disturbances as random
processes as acknowledgment of the fact that we are now treating the higher
order terms of the Taylor series expansions as random processes to be
consistent with the requirements of the Kalman filter formulation.
The model described by the above equations is know as a constant
velocity model because of the fact that ( ) ( ) 1 x k x k + = and ( ) ( ) 1 z k z k + = . In
some instances it may be more appropriate to use a constant acceleration
model. A constant acceleration model would be described by the Equation 6-57
with the various matrices replaced by the following.

100
( )
2
2
1 2 0 0 0
0 1 0 0 0
0 0 1 0 0 0
0 0 0 1 2
0 0 0 0 1
0 0 0 0 0 1
T T
T
k
T T
T
(
(
(
(
=
(
(
(
(
(

F , ( ) k = G I , ( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
1
2
3
4
5
6
x
x
x
x
x
x
k x k
k x k
k x k
k
k z k
k z k
k z k
( (
( (
( (
( (
= =
( (
( (
( (
( (
( (

x , and
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
1
2
3
4
5
6
w
w
w
w
w
w
x
x
x
z
z
z
k w k
k w k
k w k
k
k w k
k w k
k w k
( (
( (
( (
( (
= =
( (
( (
( (
( (
( (

w .
The derivation of the above matrices is a simple exercise that is left to the
reader.

If we consider a sensor that is remote from the target, the measurements
made by this sensor will not be in the Cartesian coordinate system. Instead,
they are most often in a polar coordinate system. These types of sensors would
include radars, lasers, sonar and the like. The measurements and their
relation to the Cartesian coordinate system are illustrated in Figure 6-3. In this
figure, we assume that the Cartesian coordinate system is centered on the
sensor. This is a standard convention. As illustrated, the standard
measurements that a sensor makes are range and angle. In some cases the
sensor can also measure range-rate via Doppler frequency.



Figure 6-3. Coordinate System for Target Tracking Model

101
Since our system model is in the Cartesian coordinate system we need to
cast our measurement model in this coordinate system. From the geometry of
Figure 6-3 the appropriate equations are
( ) ( ) ( ) ( )
2 2
r
r v k x k z k k = + + (6-58)
and
( ) ( ) ( ) ( ) ( )
1
tan v k z k x k k

u
u = + . (6-59)
If we include range rate as a measurement, the appropriate equation would be
( )
( ) ( ) ( ) ( )
( ) ( )
( )
r
2 2
r v
x k x k z k z k
k k
x k z k
+
= +
+
(6-60)
The reader can substitute the elements of the appropriate x k ( ) vector (for the
constant velocity or constant accretion system model) into the above equations
to express them in terms of their elements. As an example of one of the vector
forms, the measurement vector for the case of range and angle measurement
with the constant velocity system model is
( ) ( ) ( ) ( )
( ) ( )
( ) ( ) ( )
( )
( )
2 2
1 1 3
1
2
3 1
v x x
v
tan x x
k k k
k k k
k
k k

(
+ (
( = + = +
(
(


y x v h (6-61)
where ( ) ( )
1 r
v v k k = and ( ) ( )
2
v v k k
u
= .
Recall that, for purposes of building the Kalman filter we need to
generate the ( ) 1 k + H matrix. For the tracking filter formulation this equation
is as defined in Equation 6-49. For the specific case associated with Equation
6-61 the ( ) 1 k + H becomes
( )
1 1 1 2 1 3 1 4
2 1 2 2 2 3 2 4
1
h x h x h x h x
k
h x h x h x h x
c c c c c c c c (
+ =
(
c c c c c c c c

H (6-62)
where the elements of the matrix are defined by
( ) ( )
( )
( )
( )
( ) ( )
2 2
1 3 1
1 1
2 2
1
1 3
1
x 1 x 1 x 1
x 1
x 1 x 1
k k
k k k k
h x
k
k k k k
+
c + + + +
c c = =
c +
+ + +
x
, (6-63)
( ) ( )
( )
( )
( )
( ) ( )
2 2
1 3 3
1 3
2 2
3
1 3
1
x 1 x 1 x 1
x 1
x 1 x 1
k k
k k k k
h x
k
k k k k
+
c + + + +
c c = =
c +
+ + +
x
, (6-64)
( ) ( ) ( )
( )
( )
( )
( ) ( )
1
3 3 1
2 1
2 2
1 1 3
1
-x 1 tan x 1 x 1
x 1 x 1 x 1
k k
k k k k
h x
k k k k k

+
+ c + +
c c = =
c + + + +
x
, (6-65)

102
( ) ( ) ( )
( )
( )
( )
( ) ( )
1
1 3 1
2 3
2 2
3 1 3
1
x 1 tan x 1 x 1
x 1 x 1 x 1
k k
k k k k
h x
k k k k k

+
+ c + +
c c = =
c + + + +
x
, (6-66)
and the rest of the terms are zero. With the above, we can rewrite ( ) 1 k + H as
( )
( )
( ) ( )
( )
( ) ( )
( )
( ) ( )
( )
( ) ( )
1 3
2 2 2 2
1 3 1 3
3 1
2 2 2 2
1 3 1 3
x 1 x 1
0 0
x 1 x 1 x 1 x 1
1
-x 1 x 1
0 0
x 1 x 1 x 1 x 1
k k k k
k k k k k k k k
k
k k k k
k k k k k k k k
( + +
(
+ + + + + + (
+ =
(
+ + (
(
+ + + + + +
(

H . (6-67)
The derivation of the other forms of ( ) ( )
k x h and ( ) 1 k + H are left as exercises
for the reader.
With the above we are in a position to proceed to the next phase of
designing the Kalman filter. We will consider this through a specific example.

6.3.2 Example 2: Application to a Ballistic Missile Tracking Problem Purely
Ballistic Trajectory
We now want to apply some of the developments of the previous
subsection to the development of a Kalman filter that can be used to estimate
the states of a ballistic missile based on measurements of range and angle. We
will start with the unrealistic, but instructive, case where the missile is flying a
purely ballistic trajectory across a non-rotating, flat earth. Consistent with the
developments in subsection 6.3.1, we will assume that the trajectory lies in the
x-z plane to simplify the system model and measurement equations. In this
coordinate system, the origin is located at the radar, which generates the range
and angle measurements, and includes the Kalman filter.
The continuous-time equations of motion for this ideal trajectory are

( ) ( ) ( ) ( )
( ) ( ) ( )
0
0
0 0 , 0 0
0 0, 0
x t x x x x
z t g z z z
= = =
= = =
(6-68)
where g is the acceleration of gravity (9.8 m/s
2
), ( )
0
, 0 x is the launch position
of the missile and ( )
0 0
, x z is the launch velocity.
Figure 6-4 contains a plot of the missile trajectory for the case where
0
100 Km x = and ( ) ( )
0 0
, 775, 632 m/s x z = . In the figure, the solid red curve
denotes the portion of the trajectory over which the radar (and Kalman filter)
tracks the target. Track is initiated when the missile is at a range of about 43
Km and an angle of about 27.
We will start with the system model of Equation 6-57. We deduce that
this model is representative in the x coordinate because we know that our
real system has a constant velocity in this coordinate (see Equation 6-68).

103
However, the model is not good in the z coordinate because we know that the
real system has a constant acceleration, and not a constant velocity, in this
coordinate. We will try to account for the inaccuracy of the model through the
( ) k Q matrix.


Since we are measuring range and angle the appropriate measurement
model is specified in Equations 6-58 and 6-59; and Equation 6-61 if we use the
state assignment of Equation 6-57. The appropriate ( ) 1 k + H matrix is given by
Equation 6-67.
In general, the range and angle measurement errors, ( )
r
v 1 k + and
( ) v 1 k
u
+ , will depend on signal-to-noise ratio (SNR), the radar compressed
pulse width,
p
t , and the radar antenna beam width,
B
u . There will also be a
lower limit on these errors that depends upon various radar design parameters.
The measurement error model we will assume is

2
2 2
r
2
2 2
1 1
20
1 1
20
p
B
SNR
SNR
o t
o u
u
(
| |
= +
(
|
\ .
(

(
| |
= +
(
|
\ .
(

. (6-69)
The terms
2
p
SNR t and
2
B
SNR u were obtained from Skolniks radar text (with
r
1 k k
u
= = see Skolnik). The ( )
2
1 20 factor limits the minimum errors to
1/20
th
of the pulse width and beam width. We will assume a pulse width of
1 s
p
t = (150 m) and a beam width of 2
B
u = (0.0349 rad).
The SNR would be obtained from the radar range equation (see Skolnik
reference) and is inversely proportional to
4
R , where R is the range from the

Figure 6-4 Ballistic Target Trajectory

104
radar to the target. For this example we will assume that the radar range
equation results in a SNR given by

20
4
10
SNR
R
= . (6-70)
This provides an SNR of about 15 dB at the start of tracking.
To implement the Kalman filter we need to specify ( )
0 x , ( ) 0 P , ( ) k Q
and ( ) 1 k + R . The system model matrices, ( ) k F and ( ) k G have been
specified. We will also need to discuss the computation of ( ) 1 k + H .
We know that tracking starts at a missile range of approximately 43 Km
and an angle of about 27. From this we can compute the initial position as

( ) ( ) ( )
( ) ( ) ( )
1
3
0 0 43000cos 27
0 0 43000sin 27
x x
y x
= =
= =
. (6-71)
We assume that we dont know the initial velocity but we assume it is in the
order of 1000 m/s. With this we let

( ) ( )
( ) ( )
2
4

0 0 1000

0 0 1000
x x
y x
= =
= =
. (6-72)
We reflect our uncertainty in these initial estimates by using
( )
2
2
2
2
200 0 0 0
0 200 0 0
0
0 0 500 0
0 0 0 500
(
(
(
=
(
(

P . (6-73)
This selection of the ( ) 0 P matrix says that we know the position to within
about 200 m and the velocity to within about 500 m/s. These are guesses.
Although we know we shouldnt, we will start with a ( ) k Q of all zeros.
This says that we believe that our system model is perfect, which we know it is
not.
We will assume that the range and angle measurements are
independent, which is a reasonable assumption. With this we get
( )
2
r
2
0
1
0
k
o
o
u
(
+ =
(

R (6-74)
where the range and angle variances are as defined in Equations 6-69. We note
that the measurement covariance matrix is not constant since the variances
depend upon target range, which varies with time. This means that ( ) 1 k + R
must be computed inside of the Kalman filter loop. Furthermore, to compute

105
( ) 1 k + R we will need to use a prediction of target range based on the predicted
state estimate.
Since ( ) 1 k + H depends upon the predicted states, it must also be
computed inside of the Kalman loop. It should be initialized to
( )
1 0 1 0
0
1 0 1 0
(
=
(

H (6-75)
or a 2X4 matrix of zeros outside of the Kalman filter loop.
The MATLAB script entitled MissileKfilt1.m contained in the Program
Files folder contains a script that implements the Kalman filter. The script is
well commented and should be easy to follow. The script reads a file entitled
Missile1.txt
1
that contains the true state. The data in the file is stored at
intervals of 0.1 seconds. We must down sample the data because we want the
update period of the Kalman filter to be 0.2 s T = .
The results of a simulation run are shown in Figures 6-5 through 6-9.
Figure 6-5 contains a plot of actual and estimated y position vs. x position.
This plot seems to indicate that the filter is working well. However, the scale
makes the errors difficult to see. More specific information on the errors is
provided in Figures 6-6 and 6-7. Figure 6-6 contains plots of x and y position
errors between the estimates and actual values (top plot) and the measured
and actual values (bottom plot). The measured x and y positions were obtained
from

( )
( )
cos
sin
meas meas meas
meas meas meas
x r
y r
u
u
=
=
. (6-76)
It will be noted that the errors between the estimated and actual positions is
large, and much larger than the measured values. This tells us that the filter is
not working very well. In fact, direct use of the measurements provides better
position estimates!
Figure 6-7 shows that the errors between the estimated and actual
velocities are also large. There is no curve of errors between measured and
estimated velocities because the measured velocities were not computed.
Figure 6-8 contains plots of what the Kalman filter thinks the rms
position and velocity errors are. These curves were obtained by plotting the
square root of the diagonal terms of the covariance matrix ( ( ) k P ). As we have
previously discussed, the rms errors indicated in these curves should be in
agreement with the actual errors of Figures 6-6 and 6-7. As can be seen, they
are not. The Kalman filter thinks it is doing a good job of estimating the
positions and velocities and is thus assigning small rms errors to them. This is
a clear indication of the fact that we have not provided the Kalman filter with

1
The file Missile.txt is a MATLAB .mat file. The .txt extension is used because many e-mail
programs will not allow down load of files with a .mat extension because they often contain
harmful viruses.

106
an adequate system model; and we have lied to the filter by telling it that the
model is good my setting the ( ) k Q matrix to zero.



Figure 6-5 Plot of Estimated and Actual Positions

Figure 6-6 Plot of Position Errors

107





Figure 6-7 Plot of Velocity Errors

Figure 6-8 Plot of RMS Position and Velocity Error from Covariance

108
The curves of Figure 6-9 contain plots of the eight Kalman gains. The
top two curves are the gains that are applied to the range measurement error
(difference between the actual range measurement and the estimated range
measurement) to update the position estimates. The second two curves are the
gains that are applied to the angle measurement error to update the position
estimates. The third two curves are the gains that are applied to the range
measurement error to update the position estimates and the fourth two curves
are the gains that are applied to the angle measurement error to update the
velocity estimates. It will be noted that all of the gains tend toward zero. This
is expected and means that the Kalman filter is de-emphasizing the
measurements and emphasizing the previous state estimates. This is the
reason the position and velocity errors increase with time: the estimates are
poor but the filter is not making use of the measurements to try and improve
the estimates. The reason we expect the gains to go to zero is that we have told
the Kalman filter that the system model is good and that it can rely on the
system model more than on the data, once the filter has been initialized by the
first few measurements.




Figure 6-9 Plot of Kalman Gains

109
As we learned with the Humvee tracking problem, we can probably
improve the performance of the missile tracking Kalman filter by choosing a
non-zero ( ) k Q matrix. This is left as a homework exercise.
Another alternative is to include acceleration as a state and attempt to
estimate it. This will also be considered as a homework problem. We will note
that this approach will be of considerable help when the trajectory is not purely
ballistic but includes atmospheric drag.
Still another way of improving the performance of the Kalman filter for
the ballistic missile case is to recognize that the trajectory is influenced by
gravity and include gravity in the system model by considering it as a known
input. This is discussed in Chapter 8.
It should be noted that there are many other ways to improve the
performance of the Kalman filter. As examples:
- For the trajectory with atmospheric drag, we could include the drag term
in the system model.
- For both trajectories we may be able to improve performance by
including the measurement of range-rate.
- We could use the combined continuous-time, discrete-time Kalman filter
discussed in Chapter 8.
In all cases, a critical factor affecting the performance of the filter is a proper
selection of the ( ) k Q matrix.


110
PROBLEMS
1. Derive the control theoretic form of the extended Kalman filter presented in
Equations 6-50 through 6-54.
2. Derive the Cartesian coordinate, constant acceleration system model
defined by the matricies and vectors above Figure 6-3.
3. Show that ( ) 0 o x is uncorrelated with ( ) k w and ( ) k v .
4. Derive Equation 6-60.
5. Derive the ( ) 1 k + H matrix given by Equations 6-62 through 6-67.
6. Derive the ( ) 1 k + H matrix for the six-state, Cartesian-coordinate system
model.
7. Extend the measurement model and ( ) 1 k + H matrix of Section 6.2.1 to
include Doppler.