7/29/2019 KFILT_CH6

1/22

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 linearsystem based on measurements that

are a linearfunction of the states. Unfortunately, in many of the situations

where we would like to use a Kalman filter, we have a non-linear system modeland/or a non-linear measurement equation. Specifically, the system model is anon-linear function of the states and/or the measurements are non-linearfunctions of the states. Usually, the non-linearities dont extend to the systemdisturbances and measurement noise.

Because of the attractiveness of the Kalman filter, designers havedeveloped a set of mathematics to extend Kalman filter theory to situationswhere the system model and/or measurement model are non-linear functions

of state. The resultant Kalman filter is referred to as the extendedKalmanfilter. As we will see, the extended Kalman filter uses the non-linear system

model for computing the predicted state estimate, 1k kx , and the non-linear

measurement model to form the predicted measurement, 1k ky . Thesmoothed 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 bythe state equation

1k k k k x x wf G (6-1)and the measurement equation

1 1 1k k k y x vh . (6-2)

In Equations 6-1 and 6-2 kw and 1kv are uncorrelated, zero-mean, white

random processes. They are also uncorrelated with the initial state, 0x .

kG is a known matrix and kxf and 1kxh 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 kk

f k f k k k

x

xx

x

f (6-3)

and

7/29/2019 KFILT_CH6

2/22

90

1 1 1 2

2 2 1 2

1 2

1 x 1 , x 1 , x 1

1 x 1 , x 1 , x 11

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

xx

x

h . (6-4)

6.2.2 Filter Development

We begin by separating kx and 1ky into two parts as

0k k k x xx (6-5)

and

01 1 1k k k y yy . (6-6)

0 kx and 0 1ky are termed the nominalvalues of kx and 1ky ,respectively, while kx and 1k y are termedperturbations.

Next we expand kxf and kxh into a Taylor series expansionabout 0 kx and 0 1kx as

0

0

0H.O.T.

k

k

kk k k k

k

xx x x

xxx

ff f x (6-7)

and

0

0

0H.O.T.

k

k

kk k k k

k

xx x x

xxx

hh h x . (6-8)

We next drop the high order terms (H.O.T.), recognize that 0k k k x xx and write

0k k k k x xf f x F (6-9)

and

0k k k k x xh h x H . (6-10)

In Equations 6-9and 6-10, kF and kH 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 xk

k

f x f x f x

x

fF

x

x(6-11)

7/29/2019 KFILT_CH6

3/22

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 xk

k

h x h x h x

x

hH

x

x(6-12)

where the notation i jg x is interpreted as

1 10

0

1 2

x

x

x , x , , x

x

n n

i n

i j k x kj

k x k

g k k kg x

k

. (6-13)

We want to digress to discuss the notation we have used. One of the

misconceptions associated with Equations 6-5and 6-6is that we have some

knowledge of the nominal state and measurement, 0 kx and 0 1ky , and

can thus insure that the perturbations, kx and 1k y , are small. This,in turn, allows us to claim that the H.O.T. of the Taylor series expansions of

kxf and 1kxh are small and can thus be ignored. In fact, we haveno knowledge of the nominal states or measurements (if we did, we would notneed to build a Kalman filter!). We introduce the notation as a mathematical

tool that facilitates our derivation. We temporarilyassumewe know thenominal state and measurement and that they are close to the actual state andmeasurement. This allows us to claim that the perturbations are small andthus that the first order approximations to the Taylor series expansions arevalid. We cannot determine the validity of our assumption and its

ramifications beforehand. Because of this we cannot always be sure that anextended Kalman filter that we design will work well. While this may bediscouraging we should take solace from the fact that designers have beendesigning extended Kalman filters for the past forty or so years and that mostof these filters work very well.

Returning to our development, we use Equations 6-5, 6-6, 6-9and 6-10to rewrite the system and measurement equations as

0 01 1k k k k k k k x x wx f x F G (6-14)

and

0 01 1 1 1 1 1k k k k k k y x vy h x H . (6-15)

We next separate Equations 6-14and 6-15into two sets of equations. One of

these we term the nominal equationsand the other we call theperturbation

equations. The nominal equations are

0 01k k x f x (6-16)

7/29/2019 KFILT_CH6

4/22

92

and

0 01 1k k y h x , (6-17)

and the perturbation equations are

1k k k k k x x wF G (6-18)

and

1 1 1 1k k k k y x vH . (6-19)

For now we set the nominal equations aside and build a Kalman filter for theperturbation model. This results in the equations

1k k k k x xF , (6-20)

1 1 1k k k k k y xH , (6-21)

1 1 1 1 1k k k k k k k x x y yK , (6-22)

1

1 1 1 1 1 1 1T Tk k k k k k k k k

K P H H P H R , (6-23)

1 T Tk k k k k k k k P F P F G Q G and (6-24)

1 1 1 1k k k k k P I K H P . (6-25)

With Equations 6-20through 6-25we have a means of estimating theperturbation part of the state. While this is an interesting result, it is not what

we want. We want kx , the estimate of kx ; not kx , the estimate of

kx .

Let us consider kx . Based on our results from earlier, we can write

kx as

0 k k k x xx . (6-26)

In this equation 0 kx is the nominal state discussed earlier and kx is the

estimate of kx that we just derived. Equation 6-26tells us that to form anestimate of the state we add an estimate of the perturbed state to the nominalstate (we conveniently ignore the fact that we dont really know the nominalstate).

If we consider the mean-squared error between kx and kx we get

7/29/2019 KFILT_CH6

5/22

93

2

2

T

x

T

x

e E k k k k

E k k k k e

x x x x

x x x x

. (6-27)

Recall that the Kalman filter results in a kx that minimizes 2xe . Since2 2

x xe e we tend to jump to the conclusion that we have also minimized2

xe .

This is not a totally correct conclusion. We can say that we have minimized2

xe

under the constraint that kx is given byEquation 6-26and kx is definedbyEquation 6-20through 6-25. If we were to remove these constraints we

could conceivably find an kx such that 2 2min minx xe e . However, we dont have

a general means of finding kx that truly minimizes 2xe . Therefore, for want ofanything better, we will adopt Equation 6-26. As indicated earlier, the quality ofthis decision can only be tested by constructing the extended Kalman filter andtesting it.

If we combine Equation 6-26with our previous results we get

0

0

1 1 1

1 1 1

k k k

k k k k k k k

x x

x y y

x

f x F K (6-28)

and if we expand kxf into a Taylor series about 0 kx we get

0 0 H.O.T.k k k k k x xf f x F x . (6-29)

Dropping the higher order terms (H.O.T.) and recognizing 0 k kx x as

kx we get

0 k k k k x xf f x F . (6-30)

Using this we can replace the first two terms in the last part ofEquation 6-28toyield

1 1 1 1k k k k k k x x y yf K . (6-31)

We next turn our attention to the last term in Equation 6-31. If we useEquation 6-6and write the predicted measurement as

0 1 1 1k k k k k y yy , (6-32)

we can rewrite the last term as

1 1 1 1k k k k k k y y yy (6-33)

7/29/2019 KFILT_CH6

6/22

94

and the state estimation equation as

1 1 1 1k k k k k k x x y yf K . (6-34)

We note that we have manipulated the state estimation equation into theform that we want. That is, we have an equation for the state estimate at stage

1k in terms of the state estimate at stage k , the measurement at stage 1k and the predicted measurement at stage 1k . We now need to perform somemore mathematical manipulations to complete our formulation of the extendedKalman filter.

We start by examining the function kxf . If we wanted to predict thevalue of 1kx given only kx we would use the original state equation of

Equation 6-1. However, this equation contains the system disturbance kw ,which we dont know. Using the logic from Chapter 5 we can say that since we

dont know kw , our best choice for its replacement would be E kw , whichis zero. Thus we are left with the equation

1k k k x xf (6-35)

for the predicted state at stage 1k given the state estimate at stage k .

As we did in Chapter 5, we can characterize the error between thepredicted and actual state as

1 1 1k k k k k k k k k k x x x x x wF G . (6-36)

With this we can use the results of Chapter 4 to write the covariance of

1k k x as

1 T Tk k k k k k k k P F P F G Q G . (6-37)

We next consider the predicted measurement, 1k ky . Following thelogic from Chapter 5, we can relate the predicted measurement to the predicted

state through the measurement model ofEquation 6-2. However, we dont

know 1kv . As in Chapter 5, for want of anything better we will use

1 0E k v instead. Thus we get

1 1k k k k y xh . (6-38)

The final topic we need to consider concerns the matrices kF and

1kH . Recall that these matrices are given by

7/29/2019 KFILT_CH6

7/22

95

0 k

kk

k

x

xx

fF (6-39)

and

0 1

111

k

kkk

x

xx

hH . (6-40)

To compute kF and 1kH we need 0 kx and 0 1kx , which we dont

know. In lieu of these, we use kx in place of 0 kx and 1k kx in place of

0 1kx . We can offer no rationale for these substitutions except to state thatthey are the only ones we have.

6.2.3 Summary

We now summarize the results of this section into the followingpresentation of the extended Kalman filter. Given the state and measurement

models ofEquations 6-1 and 6-2, along with the previously indicated properties

of kw , 1kv and 0x an extended Kalman filter that can be used toestimate the state of the system is given by the equations

1 1 1 1 1k k k k k k k x x y yK , (6-41)

1k k k x xf , (6-42)

1 1k k k k y xh , (6-43)

1

1 1 1 1 1 1 1T Tk k k k k k k k k

K P H H P H R , (6-44)

1 T Tk k k k k k k k P F P F G Q G , (6-45)

1 1 1 1k k k k k P I K H P , (6-46)

k

kk

k

x

x

x

fF and (6-47)

1

11

1k k

kk

k

x

x

x

hH . (6-49)

Equations 6-41 through 6-49constitute the trackingform of the extended

Kalman filter. The control theoreticform of the extended Kalman filter is definedby the equations

7/29/2019 KFILT_CH6

8/22

96

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

1

T Tk k k k k k k k

K F P H H P H R , (6-51)

1 T Tk k k k k k k k k P F K H P F G Q G , (6-52)

k

kk

k

x

x

x

fF and (6-53)

k

kk

k

x

x

x

hH . (6-54)

The derivation of the control theoretic form of the extended Kalman filter is leftto 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 diagramsofFigures 5-7and 5-8. The difference is that the matrices kF and kH are

replaced by their non-linear equivalents, kxf and kxh .

6-1. Tracking Form of the Extended Kalman Filter

7/29/2019 KFILT_CH6

9/22

97

6.2.4 Properties of the Extended Kalman FilterWe now take some time to discuss the properties of the extended

Kalman filter. We start by noting that the extended Kalman filter is intuitivelyappealing. It has the basic structure of the normal Kalman filter but alsoincorporates the non-linear functions of the system and measurement models.Since it incorporates these non-linear functions, the extended Kalman filter is anon-linear filter. That is, the state prediction equation and the predictedmeasurement equations are non-linear. Because of this the state estimateequation is also a non-linear function of the previous state estimate.

Although not as obvious, the elements of kF and kH 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 extendedKalman filter is very heuristic. That is, while the various steps are intuitivelyappealing, as are the resultant equations, they are not always mathematicallyrigorous. 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 kx

Figure 6-2. Control Theoretic Form of the Extended Kalman Filter

7/29/2019 KFILT_CH6

10/22

98

by kx and 0 1kx by 1k kx when evaluating kF and 1kH .Because of this we must take care in implementing extended Kalman filters. Itis quite possible that we could design an unstable extended Kalman filter. Inother instances, the extended Kalman filter may be stable but do a poor job ofestimating the system states. Having made these statements, we also note that

designers have been very successful at constructing extended Kalman filtersthat work very well. They are stable, they produce accurate state estimates,their covariance matrices are representative of the actual errors between theestimate and actual states, and they have good transient response. However,this is not accomplished by blind application of the extended Kalman filterequations. It results from very careful selection of the system and

measurement models and tuning of the filter via the kQ 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 indetermining 1kR , kQ , 0x and 0P . Once these are found the Kalmanfilter will take on the established forms ofEquations 6-41 through 6-49or

Equations 6-50through 6-54depending upon whether we are building atracking or control theoretic type of Kalman filter. Because of this, we willdevote the rest of this chapter to developing system and measurement modelsfor some examples. As mentioned before, the system model is usually derivedfrom an understanding of the physics of the process or system, and themeasurement model is based on our understanding of how the measurementstake place.

6.3.1 Example 1: General Target Tracker in Cartesian Coordinates

For our first example we consider the system and measurement modelsthat 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 (xand z) for ease of notation. The reader canextend the results to three dimensions.

Since we are considering a general target tracker we have no detailedknowledge of the system since we dont know the target type or its motioncharacteristics. The target could be an aircraft, a ship, a ballistic missile, anartillery 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)

7/29/2019 KFILT_CH6

11/22

99

Recall that since we dont know the second and higher order derivatives we

lump them into the model disturbance terms, kw . 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 1k k k k k x wF G (6-57)

where

1 0 0

0 1 0 0

0 0 1

0 0 0 1

T

kT

F ,

k G I ,

1

2

3

4

x

x

x

x

k x k

k x kk

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 kk

k w k

k w k

w .

We have transitioned to representing the states and disturbances as randomprocesses as acknowledgment of the fact that we are now treating the higherorder terms of the Taylor series expansions as random processes to beconsistent 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 1x k x k and 1z k z k . Insome instances it may be more appropriate to use a constant acceleration

model. A constant acceleration model would be described by the Equation 6-57with the various matrices replaced by the following.

7/29/2019 KFILT_CH6

12/22

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

kT 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 kk

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 kk

k w k

k w k

k w k

w .

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

If we consider a sensor that is remote from the target, the measurementsmade 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 wouldinclude radars, lasers, sonar and the like. The measurements and their

relation to the Cartesian coordinate system are illustrated in Figure 6-3. In thisfigure, we assume that the Cartesian coordinate system is centered on thesensor. This is a standard convention. As illustrated, the standardmeasurements that a sensor makes are range and angle. In some cases thesensor can also measure range-rate via Doppler frequency.

Figure 6-3. Coordinate System for Target Tracking Model

7/29/2019 KFILT_CH6

13/22

101

Since our system model is in the Cartesian coordinate system we need tocast our measurement model in this coordinate system. From the geometry of

Figure 6-3the appropriate equations are

2 2 rr vk x k z k k (6-58)

and

1tan vk z k x k k . (6-59)

If we include range rate as a measurement, the appropriate equation would be

r

2 2r v

x k x k z k z kk k

x k z k

(6-60)

The reader can substitute the elements of the appropriate x k vector (for theconstant velocity or constant accretion system model) into the above equationsto 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 measurementwith the constant velocity system model is

2 2

11 3

123 1

vx x

vtan x x

kk kk k k

kk k

y x vh (6-61)

where 1 rv vk k and 2v vk k .

Recall that, for purposes of building the Kalman filter we need to

generate the 1kH matrix. For the tracking filter formulation this equationis as defined in Equation 6-49. For the specific case associated with Equation

6-61 the 1kH becomes

1 1 1 2 1 3 1 42 1 2 2 2 3 2 4

1h x h x h x h x

kh x h x h x h x

H (6-62)

where the elements of the matrix are defined by

2 2

1 3 1

1 12 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

x

, (6-63)

2 21 3 3

1 32 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

x

, (6-64)

1

33 1

2 1 2 2

1 1 3 1

-x 1tan x 1 x 1

x 1 x 1 x 1k k

k kk kh x

k k k k k

x

, (6-65)

7/29/2019 KFILT_CH6

14/22

102

1

13 1

2 3 2 2

3 1 3 1

x 1tan x 1 x 1

x 1 x 1 x 1k k

k kk kh x

k k k k k

x

, (6-66)

and the rest of the terms are zero. With the above, we can rewrite 1kH as

1 3

2 2 2 2

1 3 1 3

3 1

2 2 2 2

1 3 1 3

x 1 x 10 0

x 1 x 1 x 1 x 11

-x 1 x 10 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 kxh and 1kH are left as exercisesfor the reader.

With the above we are in a position to proceed to the next phase ofdesigning the Kalman filter. We will consider this through a specific example.

6.3.2 Example 2: Application to a Ballistic Missile Tracking ProblemPurelyBallistic Trajectory

We now want to apply some of the developments of the previoussubsection to the development of a Kalman filter that can be used to estimatethe states of a ballistic missile based on measurements of range and angle. Wewill start with the unrealistic, but instructive, case where the missile is flying apurely ballistic trajectory across a non-rotating, flat earth. Consistent with thedevelopments in subsection 6.3.1, we will assume that the trajectory lies in thex-z plane to simplify the system model and measurement equations. In this

coordinate system, the origin is located at the radar, which generates the rangeand 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/s2), 0 ,0x is the launch position

of the missile and 0 0,x z is the launch velocity.

Figure 6-4contains a plot of the missile trajectory for the case where

0100 Kmx and 0 0, 775,632 m/sx 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 43Km and an angle of about 27.

We will start with the system model ofEquation 6-57. We deduce thatthis 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).

7/29/2019 KFILT_CH6

15/22

103

However, the model is not good in the z coordinate because we know that thereal system has a constant acceleration, and not a constant velocity, in thiscoordinate. We will try to account for the inaccuracy of the model through the

kQ matrix.

Since we are measuring range and angle the appropriate measurement

model is specified in Equations 6-58and 6-59; and Equation 6-61 if we use the

state assignment ofEquation 6-57. The appropriate 1kH matrix is given byEquation 6-67.

In general, the range and angle measurement errors, rv 1k and

v 1k , will depend on signal-to-noise ratio (SNR), the radar compressed

pulse width, p , and the radar antenna beam width, B . 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

. (6-69)

The terms2

p

SNR and 2

B

SNR were obtained from Skolniks radar text (with

r1k k see Skolnik). The

21 20 factor limits the minimum errors to

1/20th of the pulse width and beam width. We will assume a pulse width of

1 sp (150 m) and a beam width of 2B (0.0349 rad).

The SNR would be obtained from the radar range equation (see Skolnik

reference) and is inversely proportional to4

R , where R is the range from the

Figure 6-4Ballistic Target Trajectory

7/29/2019 KFILT_CH6

16/22

104

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

20

4

10SNR

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 0x , 0P , kQ

and 1kR . The system model matrices, kF and kG have been

specified. We will also need to discuss the computation of 1kH .

We know that tracking starts at a missile range of approximately 43 Kmand 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 theorder 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 00

0 0 500 00 0 0 500

P . (6-73)

This selection of the 0P matrix says that we know the position to withinabout 200 m and the velocity to within about 500 m/s. These are guesses.

Although we know we shouldnt, we will start with a kQ of all zeros.This says that we believe that our system model is perfect, which we know it isnot.

We will assume that the range and angle measurements areindependent, which is a reasonable assumption. With this we get

2

r

2

01

0k

R (6-74)

where the range and angle variances are as defined in Equations 6-69. We notethat the measurement covariance matrix is not constant since the variances

depend upon target range, which varies with time. This means that 1kR must be computed inside of the Kalman filter loop. Furthermore, to compute

7/29/2019 KFILT_CH6

17/22

105

1kR we will need to use a prediction of target range based on the predictedstate estimate.

Since 1kH depends upon the predicted states, it must also becomputed inside of the Kalman loop. It should be initialized to

1 0 1 0

01 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 ProgramFiles folder contains a script that implements the Kalman filter. The script iswell commented and should be easy to follow. The script reads a file entitledMissile1.txt1 that contains the true state. The data in the file is stored atintervals of 0.1 seconds. We must down sample the data because we want the

update period of the Kalman filter to be 0.2 sT .

The results of a simulation run are shown in Figures 6-5through 6-9.Figure 6-5contains a plot of actual and estimated y position vs. x position.This plot seems to indicate that the filter is working well. However, the scalemakes the errors difficult to see. More specific information on the errors is

provided in Figures 6-6and 6-7. Figure 6-6contains plots of x and y positionerrors between the estimates and actual values (top plot) and the measuredand actual values (bottom plot). The measured x and y positions were obtainedfrom

cos

sin

meas meas meas

meas meas meas

x r

y r

. (6-76)

It will be noted that the errors between the estimated and actual positions islarge, and much larger than the measured values. This tells us that the filter isnot working very well. In fact, direct use of the measurements provides betterposition estimates!

Figure 6-7shows that the errors between the estimated and actualvelocities are also large. There is no curve of errors between measured andestimated velocities because the measured velocities were not computed.

Figure 6-8contains plots of what the Kalman filter thinks the rmsposition and velocity errors are. These curves were obtained by plotting the

square root of the diagonal terms of the covariance matrix ( kP ). As we have

previously discussed, the rms errors indicated in these curves should be inagreement with the actual errors ofFigures 6-6and 6-7. As can be seen, theyare not. The Kalman filter thinks it is doing a good job of estimating thepositions and velocities and is thus assigning small rms errors to them. This isa 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.

7/29/2019 KFILT_CH6

18/22

106

an adequate system model; and we have lied to the filter by telling it that the

model is good my setting the kQ matrix to zero.

Figure 6-5Plot of Estimated and Actual Positions

Figure 6-6Plot of Position Errors

7/29/2019 KFILT_CH6

19/22

107

Figure 6-7Plot of Velocity Errors

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

7/29/2019 KFILT_CH6

20/22

108

The curves ofFigure 6-9contain plots of the eight Kalman gains. Thetop two curves are the gains that are applied to the range measurement error(difference between the actual range measurement and the estimated rangemeasurement) to update the position estimates. The second two curves are thegains that are applied to the angle measurement error to update the positionestimates. The third two curves are the gains that are applied to the range

measurement error to update the position estimates and the fourth two curvesare the gains that are applied to the angle measurement error to update thevelocity estimates. It will be noted that all of the gains tend toward zero. Thisis expected and means that the Kalman filter is de-emphasizing themeasurements and emphasizing the previous state estimates. This is thereason the position and velocity errors increase with time: the estimates arepoor but the filter is not making use of the measurements to try and improvethe estimates. The reason we expect the gains to go to zero is that we have toldthe Kalman filter that the system model is good and that it can rely on thesystem model more than on the data, once the filter has been initialized by thefirst few measurements.

Figure 6-9Plot of Kalman Gains

7/29/2019 KFILT_CH6

21/22

109

As we learned with the Humvee tracking problem, we can probablyimprove the performance of the missile tracking Kalman filter by choosing a

non-zero kQ matrix. This is left as a homework exercise.

Another alternative is to include acceleration as a state and attempt toestimate 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 purelyballistic but includes atmospheric drag.

Still another way of improving the performance of the Kalman filter forthe ballistic missile case is to recognize that the trajectory is influenced bygravity and include gravity in the system model by considering it as a knowninput. This is discussed in Chapter 8.

It should be noted that there are many other ways to improve theperformance of the Kalman filter. As examples:

For the trajectory with atmospheric drag, we could include the drag termin the system model.

For both trajectories we may be able to improve performance byincluding the measurement of range-rate.

We could use the combined continuous-time, discrete-time Kalman filterdiscussed in Chapter 8.

In all cases, a critical factor affecting the performance of the filter is a proper

selection of the kQ matrix.

7/29/2019 KFILT_CH6

22/22

PROBLEMS

1. Derive the control theoretic form of the extended Kalman filter presented inEquations 6-50through 6-54.

2. Derive the Cartesian coordinate, constant acceleration system modeldefined by the matricies and vectors above Figure 6-3.

3. Show that 0x is uncorrelated with kw and kv .4. Derive Equation 6-60.5. Derive the 1kH matrix given byEquations 6-62through 6-67.6. Derive the 1kH matrix for the six-state, Cartesian-coordinate system

model.

7. Extend the measurement model and 1kH matrix ofSection 6.2.1 toinclude Doppler.