where z

k

 is the measurement at time t

k

, H

k

 is the measurement matrix, (or

sometimes called the measurement observation, or measurement sensitivity matrix),

and {v

T

k

} is a white, zero mean Gaussian sequence with covariance matrix E[ v

k

 v

k

] =

R

k

.

9.2.2  Kalman Filter Algorithm

The Kalman filter algorithm recursively estimates the error state vector.  It also

calculates the uncertainty in its estimate as given by its covariance matrix.  Define

x

k

 to be the estimate of the error state vector at time t

k

.  The estimation error is the

error in this estimate, or  dx   x 

x

.  The covariance matrix of the estimation error at

time t

k

 gives a measure of the uncertainty in the estimated error state vector and is

defined as

T

T

P

k

   =   E[ x ( x ) ]   =   E[( x       x ) ( x       x ) ]

k

k

k

k

The system dynamics model defines the behavior of the error state vector as a

function of time.  The measurement model defines the correspondence between the

measurement and the error state.  The measurements are assumed to be available

at discrete times.  The Kalman filter uses the dynamics model to propagate its

estimated state vector between measurements.  It then incorporates the

measurement into the error state estimate.  A Kalman filter repetitively performs

propagations and updates of its estimated error state and its associated covariance

matrix.  Figure 9 1 is a simplified diagram of the Kalman filter as it processes new

measurements and propagates in time.

9.2.2.1  Propagation

In the following equations, the notation `( )' is appended to a variable to denote that

variable at a measurement time before the measurement is incorporated.  The

symbol `(+)' appended to a variable represents that parameter at a measurement

time immediately after the  measurement is incorporated.

COVARIANCE

COVARIANCE

UPDATE

UPDATE

OCCURS WHEN NEW

MEASUREMENT IS

KALMAN GAIN

INCORPORATED

CALCULATION

CURRENT BEST

NEW MEASUREMENT

ESTIMATE OF STATE

STATE

UPDATE

+

OCCURS WHEN NEW

STATE

MEASUREMENT IS

PROPAGATION

INCORPORATED

Figure 9 1.  Simplified Diagram of Kalman Filter

9 3