2
KALMAN FILTER
2.1 TWO-STATE KALMAN FILTER
Up to now we have used a deterministic description for the target motion.
Specifically, we have assumed a target having a constant-velocity motion as
given by
x
nþ1
¼ x
n
þ T
_
x
n
ð1:1-1aÞ
_
x
nþ1
¼
_
x
n
ð1:1-1bÞ
In the real world the target will not have a constant velocity for all time. There
is actually uncertainty in the target trajectory, the target accelerating or turning
at any given time. Kalman allowed for this uncertainty in the target motion by
adding a random component to the target dynamics [19, 20]. For example, a
random component u
n
could be added to the target velocity as indicated by the
following equations for the target dynamics:

represents a random-velocity jump occurring just prior to the
n þ 1 observation.
We now have a system dynamics model with some randomness. This model
is called the constant-velocity trajectory model with a random-walk velocity.
64
Tracking and Kalman Filtering Made Easy. Eli Brookner
Copyright # 1998 John Wiley & Sons, Inc.
ISBNs: 0-471-18407-1 (Hardback); 0-471-22419-7 (Electronic)
The random-velocity component u
n
is sized to account for a possible target
acceleration or unexpected target turn. The random dynamics model component
u
n
in the literature goes by the names process noise [6, 30], plant noise [8, 29,
30], driving noise [5, 8], dynamics noise [119], model noise, and system noise
(see Appendix).
Let x
nþ1
represent the true location of the target at time n þ 1. Let x
Ã
nþ1;n
represent an estimated predicted position of the target at time n þ 1 based on
the measurements made up to and including time n. Kalman addressed the
question of finding the optimum estimate among the class of all linear and
nonlinear estimates that minimizes the mean square error
x
Ã
nþ1;n
À x

n; nÀ1
þ T
_
x
Ã
nþ1; n
þ g
n
ðy
n
À x
Ã
n; nÀ1
Þð2:1-3bÞ
But these are identical to the g–h filter given previously, specifically (1.2-11).
For the Kalman filter the weights g
n
and h
n
depend on n. Furthermore, as shall
be seen later, g
n
and h
n
are functions of the variance of the radar position
measurement, that is, the variance of 
n
; see (1.2-17). These filter constants are
also a function of the accuracy to which we know the position and velocity
before any measurements are made, that is, of our prior knowledge of the target


2
u

2
x
¼
g
4
ð2 À gÞ
2
ð1 À gÞ
¼
h
2
1 À g
ð2:1-5Þ
which relates the target update period to the variance of the target dynamics 
2
u
as well as to the noise measurement error and the filter parameter g. [See
problem 2.4-1 for derivation of (2.1-4) and (2.1-5).] Figure 1.2-7 to 1.2-9 were
actually developed for optimum g–h filter in Reference 11. However, in
developing these figures, (2.1-5) is not used, only (2.1-4).
It remains to determine how to specify the variance of the target dynamics

2
u
. Let the maximum expected target acceleration be


Ã
is obtained for the g–h Kalman filter in steady state
[12]:
b
Ã

x
¼
B
ffiffiffiffiffiffiffiffiffiffiffi
1 À g
p
ð2:1-7Þ
2.2 REASONS FOR USING THE KALMAN FILTER
Since the steady-state Kalman filter is identical to the Benedict–Bordner filter,
the question arises as to why we should use the Kalman filter. The benefits
accrued by using the Kalman filter are summarized in Table 2.2-1. First, while
in the process of computing the filter weights g
n
and h
n
for the Kalman filter,
calculations of the accuracy of the Kalman filter predictions are made. This
prediction information is needed for a weapon delivery system to determine if
the predicted position of the target is known accurately enough for a target kill.
It is also needed to accurately predict where a detected SCUD, intermediate-
66
KALMAN FILTER
range ballistic missile (IRBM), or intercontinental ballistic missile (ICBM)
would land. It makes a difference whether the SCUD, IRBM, or ICBM is

and h
n
are also adjusted to allow for nonequal times between measurements.
The Kalman filter optimally makes use of a priori information. Such a priori
information could come from another radar that had been previously tracking
the target and from which the target is being handed over—such as handover
from a search to tracking radar or from one air route surveillance radar (ARSR)
to another. The data from the other radar can be used to optimally set up the
Kalman g–h filter for the new radar. The Kalman filter automatically chooses
weights that start with large g and h values as needed for optimum track
initiation. The weights slowly transition to a set of small constant g’s and h’s
after track initiation. The target dynamics model incorporated by the Kalman
filter allows direct determination of the filter update rate by the use of (2.1-5).
Finally the addition of the random-velocity variable u
n
forces the Kalman filter
to always be stable.
TABLE 2.2-1. Benefits of Kalman Filter
Provides running measure of accuracy of predicted position needed for weapon
kill probability calculations; impact point prediction calculation
Permits optimum handling of measurements of accuracy that varies with n;
missed measurements; nonequal times between measurements
Allows optimum use of a priori information if available
Permits target dynamics to be used directly to optimize filter parameters
Addition of random-velocity variable, which forces Kalman filter to be always stable
REASONS FOR USING THE KALMAN FILTER
67
2.3 PROPERTIES OF KALMAN FILTER
We will now give some physical feel for why the Kalman filter is optimum. Let
us go back to our discussion in Section 1.2. Recall that for our two-state g–h

y
n
VARðY
n
Þ
"#
1
1=VARðx
Ã
n; nÀ1
Þþ1=VARðy
n
Þ
ð2:3-1Þ
That (2.3-1) provides a good combined estimate can be seen by examining
some special cases. First consider the case where y
n
and x
Ã
n;nÀ1
have equal
accuracy. To make this example closer to what we are familiar with, we use the
example we used before; that is, we assume that y
n
and x
Ã
n;nÀ1
represent two
independent estimates of your weight obtained from two scales having equal
accuracy (the example of Section 1.2.1). If one scale gives a weight estimate of

þ y
n
2
ð2:3-2Þ
Thus in Figure 1.2-3 the combined estimate x
Ã
n; n
is placed exactly in the middle
between the two estimates y
n
and x
Ã
n; nÀ1
.
Now consider the case where x
Ã
n; nÀ1
is much more accurate than the estimate
y
n
. For this case VARðx
Ã
n; nÀ1
Þ(VARðy
n
Þ or equivalently 1=VARðx
Ã
n; nÀ1
Þ)
1=VARðy

, as it should be because
the accuracy of x
Ã
n; nÀ1
is much better than that of y
n
. For this case, in Figure 1.2-3
the combined estimate x
Ã
n
is placed very close to the estimate x
Ã
n; nÀ1
(equal
to it).
68
KALMAN FILTER
Equation (2.3-1) can be put in the form of one of the Kalman g–h tracking
filters. Specifically, (2.3-1) can be rewritten as
x
Ã
n; n
¼ x
Ã
n; nÀ1
þ
VARðx
Ã
n; n
Þ

n
¼
VARðx
Ã
n; n
Þ
VARðy
n
Þ
ð2:3-6Þ
Thus we have derived one of the Kalman tracking equations, the one for
updating the target position. The equation for the tracking-filter parameter h
n
is
given by
h
n
¼
COVðx
Ã
n;n
_
x
Ã
n;n
Þ
VARðy
n
Þ
ð2:3-7Þ

¼ state vector ð2:4-1aÞ
and
È ¼
1 T
01

¼ state transition matrix for constant-velocity trajectory [5, 43] ð2:4-1bÞ
To show that (2.4-1) is identical to (1.1-1), we just substitute (2.4-1a) and
(2.4-1b) into (2.4-1) to obtain
x
nþ1
_
x
nþ1

¼
1 T
01

x
n
_
x
n

ð2:4-1cÞ
which on carrying out the matrix multiplication yields
x
nþ1
_

It is now a simple matter to give the random system dynamics model
represented by (2.1-1) in matrix form. Specifically, it becomes
X
nþ1
¼ ÈX
n
þ U
n
ð2:4-2Þ
where
U
n
¼
0
u
n

¼ dynamic model driving noise vector ð2:4-2aÞ
To show that (2.4-2) is identical to (2.1-1), we now substitute (2.4-1a), (2.4-1b),
70
KALMAN FILTER
and (2.4-2a) into (2.4-2) to obtain directly from (2.4-1d)
x
nþ1
_
x
nþ1

¼
x

_
x
n
þ u
n

ð2:4-2cÞ
which is identical to (2.1-1), as we desired to show.
We now put the trivial measurements equation given by (1.2-17) into matrix
form. It is given by
Y
n
¼ MX
n
þ N
n
ð2:4-3Þ
where
M ¼½10¼observation matrix ð2:4-3aÞ
N
n
¼½
n
¼observation error ð2:4-3bÞ
Y
n
¼½y
n
¼measurement matrix ð2:4-3cÞ
Equation (2.4-3) is called the observation system equation. This is because it

n
¼½x
n
þ½
n
ð2:4-3eÞ
Finally, carrying out the addition yields
½ y
n
¼½x
n
þ 
n
ð2:4-3fÞ
which is identical to (1.2-17).
KALMAN FILTER IN MATRIX NOTATION
71
Rather than put the g–h tracking equations as given by (1.2-11) in matrix
form, we will put (1.2-8) and (1.2-10) into matrix form. These were the
equations that were combined to obtain (1.2-11). Putting (1.2-10) into matrix
form yields
X
Ã
nþ1;n
¼ ÈX
Ã
n;n
ð2:4-4aÞ
where
X

Ã
n; n
¼ X
Ã
n; nÀ1
þ H
n
ðY
n
À MX
Ã
n; nÀ1
Þð2:4-4dÞ
Equation (2.4-4d) is called the Kalman filtering equation because it provides the
updated estimate of the present position and velocity of the target.
The matrix H
n
is a matrix giving the tracking-filter constants g
n
and h
n
.Itis
given by
H
n
¼
g
n
h
n

where
S
Ã
n;nÀ1
¼ ÈS
Ã
nÀ1;nÀ1
È
T
þ Q
n
(predictor equation) ð2:4-4fÞ
72
KALMAN FILTER
and
Q
n
¼ COV½U
n
¼E½U
n
U
T
n
 (dynamic model noise covariance)
ð2:4-4gÞ
S
Ã
n; nÀ1
¼ COVðX

nÀ1; nÀ2
(corrector equation) ð2:4-4jÞ
As was the case for (1.4-1), covariances in (2.4-4g) and (2.4-4i) apply as long as
the entries of the column matrices U
n
and N
n
have zero mean. Otherwise U
n
and N
n
have to be replaced by U
n
À E½ U
n
 and N
n
À E½ N
n
, respectively.
These equations at first look formidable, but as we shall see, they are not that
bad. We shall go through them step by step.
Physically, the matrix S
Ã
n;nÀ1
is an estimate of our accuracy in prediciting the
target position and velocity at time n based on the measurements made at time
n À 1 and before. Here, S
Ã
n;nÀ1

"#
½
x
Ã
n;nÀ1
_
x
Ã
n;nÀ1
¼
x
Ã
n;nÀ1
x
Ã
n;nÀ1
x
Ã
n;nÀ1
_
x
Ã
n;nÀ1
_
x
Ã
n;nÀ1
x
Ã
n;nÀ1

Ã
2
n;nÀ1
"#
¼
s
Ã
00
s
Ã
01
s
Ã
10
s
Ã
11
"#
¼ S
Ã
n;nÀ1
ð2:4-4kÞ
where for convenience E½Z has been replaced by
"
Z, that is, E½Á is replaced by
the overbar. Again, the assumption is made that mean of X
Ã
n;nÀ1
has been
substracted out in the above.

¼½
2

¼½
2
x
ð2:4-4lÞ
KALMAN FILTER IN MATRIX NOTATION
73
where it is assumed as in Section 1.2.4.4 that 

and 
x
are the rms of 
n
independent of n. Thus 
2

and 
2
x
are the variance of 
n
, the assumption being
that the mean of 
n
is zero; see (1.2-18).
The matrix Q
n
, which gives the magnitude of the target trajectory

n
Á 0 u
n
Á u
n

¼
00
0
u
2
n

ð2:4-4mÞ
Equation (2.4-4f ) allows us to obtain the prediction covariance matrix S
Ã
n;nÀ1
from the covariance matrix of the filtered estimate of the target state vector at
TABLE 2.4-1. Kalman Equation
Predictor equation:
X
Ã
nþ1; n
¼ ÈX
Ã
n; n
ð2:4-4aÞ
Filtering equation:
X
Ã

Predictor covariance matrix equation:
S
Ã
n; nÀ1
¼ COVðX
Ã
n; nÀ1
Þð2:4-4hÞ
S
Ã
n; nÀ1
¼ ÈS
Ã
nÀ1; nÀ1
È
T
þ Q
n
ð2:4-4fÞ
Covariance of random system dynamics model noise vector U
a
:
Q
n
¼ COVðU
n
Þ¼E½ U
n
U
T

nÀ1
MÞS
Ã
nÀ1; nÀ2
ð2:4-4jÞ
a
If E½U¼E½N
n
¼0.
74
KALMAN FILTER
time nÀ 1 given by S
Ã
nÀ1; nÀ1
. The filtered estimate covariance matrix S
Ã
nÀ1; nÀ1
is in turn obtained from the previous prediction covariance matrix S
Ã
nÀ1;nÀ2
using (2.4-4j). Equations (2.4-4e), (2.4-4f ), and (2.4-4j) allow us to obtain the
filter weights H
n
at successive observation intervals. For the two-state g–h filter
discussed earlier, the observation matrix is given by (2.4-3a) and the filter
coefficient matrix H
n
is given by (2.4-5). The covariance matrix for the initial a
priori estimates of the target position and velocity given by S
Ã

x
Ã
n; nÀ1
y
Ã
n; nÀ1
_
y
Ã
n; nÀ1

y
Ã
n; nÀ1
z
Ã
n; nÀ1
_z
Ã
n; nÀ1
z
Ã
n; nÀ1

Ã
n; nÀ1
2
6
6
6

7
7
7
7
7
7
7
7
7
5
ð2:4-6Þ
where  is the atmospheric drag on the target. One can assume that the sensor
measures R;;, and the target Doppler
_
R so that Y
n
is given by
Y
n
¼
R
n
_
R
n

n

n
2

3
7
7
7
5
ð2:4-8Þ
where y
in
is the ith target parameter measured by the sensor at time n.
The atmosheric ballistic coefficient  is given by
 ¼
m
C
D
A
ð2:4-9Þ
where m is the target mass, C
D
is the atmospheric dimensionless drag
coefficient dependent on the body shape, and A is the cross-sectional area of the
target perpendicular to the direction of motion. [See (16.3-18), (16.3-19),
(16.3-27) and (16.3-28) of Section 16.3 for the relation between drag constant
and target atmospheric deceleration.]
For the g–h Kalman filter whose dynamics model is given by (2.1-1) or
(2.4-2), the matrix Q is given by (2.4-4m), which becomes
Q ¼
00
0 
2
u

w
.
Physically w
n
represents a random-acceleration jump occurring just prior to the
n þ 1 observation. For this case
Q ¼
00 0
00 0
00
2
w
2
4
3
5
ð2:4-12Þ
The variance of the target acceleration dynamics 
2
w
(also called 
2
a
) can be
specified using an equation similar to that used for specifying the target velocity
dynamics for the Kalman g–h filter. Specifically

w
¼
T _x

H
n
¼
g
n
h
n
T
2k
n
T
2
2
6
6
6
6
4
3
7
7
7
7
5
ð2:4-15Þ
This is a slightly underdamped filter, just as is the steady-state g–h Kalman filter
that is the Benedict–Bordner filter. Its total error E
TN
¼ 3
nþ1;n

Ã
1
and x
Ã
2
. Designate
x
Ã
n;n
, the optimum combined estimate, by x
Ã
c
. We desire to find an optimum
linear estimate for x
Ã
c
. We can designate this linear estimate as
x
Ã
c
¼ k
1
x
Ã
1
þ k
2
x
Ã
2

1
ð2:5-4Þ
Substituting (2.5-4) into (2.5-1) yields
x
Ã
c
¼ k
1
x
Ã
1
þð1 À k
1
Þx
Ã
2
ð2:5-5Þ
Let the variances of x
Ã
c
, x
Ã
1
, and x
Ã
2
be designated as respectively 
2
c
, 

to k
1
and set the result to zero, obtaining
2 k
1

2
1
À 2ð1 À k
1
Þ
2
2
¼ 0 ð2:5-7Þ
Hence
k
1
¼

2
2

2
1
þ 
2
2
ð2:5-8Þ
Substituting (2.5-8) into (2.5-5) yields
x

x
Ã
c
¼
x
Ã
1

2
1
þ
x
Ã
2

2
2

1
1=
2
1
þ 1=
2
2
ð2:5-10Þ
which is identical to Eq. (2.3-1), as we desired to show. Note that substituting
78
KALMAN FILTER
(2.5-8) into (2.5-6) yields

n;n
shown in Figure 1.2-
3 there are two errors. One is the distance of x
Ã
n;n
from y
n
; the other is its
distance of x
Ã
n;n
from x
Ã
n;nÀ1
. For the minimum least-squares estimate in Section
1.2.6 we minimized the sum of the squares of the distances (errors) between the
measurements and the best-fitting line (trajectory) to the measurements. We
would like to similarly minimize the sum of the two errors here between x
Ã
n;n
and y
n
and x
Ã
n;nÀ1
in some sense. One could minimize the sum of the squares of
the errors as done in Section 1.2.6, but this is not the best tactic because the two
errors are not always equally important. One of the estimates, either y
n
or

each term by 1 over the accuracy of their respective estimates as the following
equation does:
E ¼
ðy
n
À x
Ã
n;n
Þ
2
VARy
n
þ
ðx
Ã
n;nÀ1
À x
Ã
n;n
Þ
2
VARðx
Ã
n;nÀ1
Þ
ð2:5-12Þ
Here the error ðx
Ã
n;nÀ1
À x

n;nÀ1
À x
Ã
n;n
Þ
2
to be
much smaller than the error ðy
n
À x
Ã
n;n
Þ
2
when minimizing the weighted sum of
errors E of (2.5-12). This thus forces x
Ã
n;n
to be close to x
Ã
n;nÀ1
, as it should be.
The more accurate x
Ã
n;nÀ1
, the closer x
Ã
n;n
is to x
Ã

n;n
Þ
VARðy
n
Þ
þ
2ðx
Ã
nnÀ1
À x
Ã
n;n
Þ
VARðx
Ã
n;nÀ1
Þ
¼ 0 ð2:5-13Þ
Solving for x
Ã
n;n
yields (2.3-1), the desired result.
2.6 EXACT DERIVATION OF r-DIMENSIONAL
KALMAN FILTER
We will now extend the second derivation given in Section 2.5.2 to the case
where a target is tracked in r-dimensions. An example of an r-dimensional state
vector for which case r ¼ 10 is given by (2.4-6). For this case the target is
tracked in the three-dimensional rectangular (x, y, z) coordinates in position,
velocity, and acceleration. In addition, the atmospheric drag parameter  is also
kept track of to form the 10th parameter of the 10-dimensional state vector

dimensional state vector X
Ã
n;nÀ1
is the same as for Y
n
. Let X
Ã
n;n
be our desired
combined estimate of the state vector after the measurement Y
n
. The combined
estimate X
Ã
n;n
will lie somewhere in between the predicted state vector X
Ã
n;nÀ1
and the measurement vector Y
n
as was the case in the one-dimensional situation
depicted in Figure 1.2-3. Figure 2.6-1 shows the situation for our present
multidimensional case. As done in the second derivation for the one-
dimensional case discussed in Section 2.5.2, we will choose for our best com-
bined estimate the X
Ã
n;n
that minimizes the weighted sum of the error differences
between Y
n

Ã
n; nÀ1
À X
Ã
n; n
Þ
2
VARX
Ã
n; nÀ1
ð2:6-1Þ
This equation is only conceptually correct; the mathematically correct
equivalent will be given shortly.
It remains now to use (2.6-1) to solve for the new combined estimate X
Ã
n;n
that minimizes the weighted sum of the squares of the errors. Conceptually, this
is done just as it was done for the equivalent one-dimensional (2.5-12).
Specifically, (2.6-1) is differentiated with respect to the combined estimate X
Ã
n;n
with the resulting equation set equal to zero in order to solve for the combined
estimate X
Ã
n;n
that minimizes the weighted sum of the errors squared. There is
one problem though: (2.6-1) is not correct when one is dealing with matrices. It
is only conceptually correct as indicated.
When using matrix notation, the first term on the right of (2.6-1) must be
written as

R
n
¼ COVðY
n
Þð2:6-3Þ
which is the same as defined by (2.4-4i). The inverse of the covariance matrix
R
n
, which is designed as R
À1
n
, takes the place of dividing by the variance of Y
n
when dealing with matrices. Note that if R
n
is diagonal with all the diagonal
Figure 2.6-1 Filtering problem. Determination of X
Ã
n;n
based on measurement Y
n
and
prediction X
Ã
n;nÀ1
.
EXACT DERIVATION OF r-DIMENSIONAL KALMAN FILTER
81