8
Kalman Filter Engineering
We now consider the following, practical aspects of Kalman ®ltering applications:
1. how performance of the Kalman ®lter can degrade due to computer roundoff
errors and alternative implementation methods with better robustness against
roundoff;
2. how to determine computer memory, word length, and throughput require-
ments for implementing Kalman ®lters in computers;
3. ways to implement real-time monitoring and analysis of ®lter performance;
4. the Schmidt±Kalman suboptimal ®lter, designed for reducing computer
requirements;
5. covariance analysis, which uses the Riccati equations for performance-based
predictive design of sensor systems; and
6. Kalman ®lter architectures for GPS/INS integration.
8.1 MORE STABLE IMPLEMENTATION METHODS
8.1.1 Effects of Computer Roundoff
Computer roundoff limits the precision of numerical representation in the imple-
mentation of Kalman ®lters. It has been shown to cause severe degradation of ®lter
performance in many applications, and alternative implementations of the Kalman
®lter equations (the Riccati equations, in particular) have been shown to improve
robustness against roundoff errors.
229
Global Positioning Systems, Inertial Navigation, and Integration,
Mohinder S. Grewal, Lawrence R. Weill, Angus P. Andrews
Copyright # 2001 John Wiley & Sons, Inc.
Print ISBN 0-471-35032-X Electronic ISBN 0-471-20071-9
Computer roundoff for ¯oating-point arithmetic is often characterized by a single
parameter e
roundoff
, which is the largest number such that
1  e

machine precision, the product HP
0
H
T
with roundoff will equal
33 d
3  d 3 2d
!
;
which is singular. The result is unchanged when R is added to HP
0
H
T
. In this case,
then, the ®lter observational update fails because the matrix HP
0
H
T
 R is not
invertible.
8.1.2 Alternative Implementations
The covariance correction process (observational update) in the solution of the
Riccati equation was found to be the dominant source of numerical instability in the
Kalman ®lter implementation, with the more common symptoms of failure being
asymmetry of the covariance matrix (easily ®xed) or, worse by far, negative terms on
its diagonal. These implementation problems could be avoided for some problems
by using more precision, but they were eventually solved for most applications by
using alternatives to the covariance matrix P as the dependent variable in the
covariance correction equation. However, each of these methods required a com-
patible method for covariance prediction. Table 8.1 lists several of these compatible

Triangular
Cholesky
factor C
Carlson [20] Kailath±Schmidt
a
Triangular
Cholesky
factor C
Morf±Kailath combined [93]
Modi®ed
Cholesky
factors U; D
Bierman [10] Thornton [116]
a
From unpublished sources.
Fig. 8.1 Degradation of numerical solutions with problem conditioning.
8.1 MORE STABLE IMPLEMENTATION METHODS
231
[20] and Bierman [10] implementations degrade more gracefully than the others as
d  e, the machine precision limit. The Carlson and Bierman solutions still
maintain about nine digits (30 bits) of accuracy at d 

e

, when the other
methods have essentially no bits of accuracy in the computed solution.
These results, by themselves, do not prove the general superiority of the Carlson
and Bierman solutions for the Riccati equation. Relative performance of alternative
implementation methods may depend upon details of the speci®c application, and
for many applications, the standard Kalman ®lter implementation will suf®ce. For

(a diagonal matrix); 8:3
z
decorr

def
U
R
z
corr
; 8:4
H
decorr

def
U
R
H
corr
; 8:5
where R
corr
is the nondiagonal (i.e., correlated component to component) measure-
ment noise covariance matrix, and the new decorrelated measurement vector z
decorr
has a diagonal measurement noise covariance matrix R
decorr
and measurement
sensitivity matrix H
decorr
.


H
T
 1
1
P

H
T
; 8:8
P I 
K

HPI  K

H
T
 K K
T
: 8:9
These equations would replace those for
K and P within the loop in Table 8.2.
The Joseph stabilized implementation and re®nements (mostly taking advantage
of partial results and the redundancy due to symmetry) in [10], [46] and are
implemented in the MATLAB ®les Joseph.m, Josephb.m,andJosephdv.m,
respectively, on the accompanying diskette.
8.1.5 Factorization Methods
8.1.5.1 Historical Background Robust implementation methods were intro-
duced ®rst for the covariance correction (measurement updates), observed to be the
principal source of numerical instability. In [100, 8], the idea of using a Cholesky

P =
P-KHP;
end;
x
Ã
k
[+]
= x
P
k
[+]
=(P+
P')/2;
8.1 MORE STABLE IMPLEMENTATION METHODS
233
this to modi®ed Cholesky factors (de®ned in Section B.1.8.1), which are diagonal
and unit triangular matrices D and U, respectively, such that
UDU
T
 P 8:10
and U is triangular with 1's along its main diagonal.
Compatible covariance prediction methods were discovered by Thomas Kailath
and Stanley F. Schmidt (for Carlson's method) and Catherine Thornton [116] (for
Bierman's method).
8.1.6 Square-Root Filtering Methods
8.1.6.1 Problems with the Riccati Equation Many early applications of
Kalman ®ltering ran into serious numerical instability problems in solving the
ancillary Riccati equation for the Kalman gain. The problem was eventually solved
(over the next decade or so) by reformulating the Riccati equation so that its solution
was more robust against computer roundoff errors. Some of the more successful of

1  e
machine

machine
1 8:12
1
See Section B.1.8.1 for the de®nition and properties of Cholesky factors.
2
e
machine
has the reserved name eps in MATLAB. Its value is returned when you type ``eps''.
234
KALMAN FILTER ENGINEERING
in machine precision. That is, the result of adding e
machine
to 1 has no effect in
machine precision.
8.1.6.3 Triangularization Methods The so-called ``QR'' theorem of linear
algebra states that every real m  n matrix S can be factored in the form S  QR,
where Q is an m  m orthogonal matrix and R is an m  n upper triangular matrix.
Depending on the relative magnitudes of m and n, the resulting triangular matrix R
may have any of the forms
m < nm nm< n
R 
with the nonzero part of the upper triangular submatrix in the upper right corner.
There are several algorithms for computing the triangular and orthogonal factors,
including some with the order of the factors reversed (effectively ``RQ'' algorithms).
These are also called triangularization methods. They are key to square-root
®ltering, because they can transform a nontriangular Cholesky factor M into a
triangular one

3
are orthogonal matrices of the form
vI 
2vv
T
v
T
v
; 8:15
where v is a column vector and I is the compatibly dimensioned identity matrix.
The condition number of an othogonal matrix is perfect (i.e., 1), making it well
suited for robust operations in numerical linear algebra. The QR decomposition of a
matrix M is effectively accomplished by a series of products by Householder
transformation matrices, in the partitioned form
v 0
0 I
!

3
Named for Alston S. Householder (1904±1993), who developed many of the more robust methods used
in numerical linear algebra.
8.1 MORE STABLE IMPLEMENTATION METHODS
235
with the vector v chosen to annihilate all but the end element of the remaining
subrow x of M until only the upper triangular part remains. It suf®ces to let
v  x
T
x
0
.

 CMM
T
C
T
 CC
T
8:17
and C is triangular. This is the basis for the following two types of square-root
®ltering.
8.1.6.6 Morf±Kailath Square-Root Filter In Morf±Kailath square-root
®ltering, the entire Riccati equation, including prediction and correction steps, is
implemented in a single triangularization procedure. It effectively computes the
Cholesky factors of successive covariance matrices of prediction error (required for
computing Kalman gain) without ever explicitly computing the intermediate values
for corrected estimation errors. Assume the following:
G
k
is the dynamic disturbance distribution matrix of the system model,
C
Q
k
is a Cholesky factor of Q
k
;
F
k
is the state transition matrix from the previous epoch;
C
P
k

C
R
k
!

0 C
P
k1
C
k1
00C
E
k1
!
; 8:18
a partitioned upper triangular matrix.
Then C
P
k1
is the square triangular Cholesky factor of P
k1
, the covariance matrix of
prediction error, and the Kalman gain
K
k1
 C
k1
=C
E
k1


3

n
; 8:20
where C
P;k
 sought-for triangular Cholesky factor of P
k

C
Q;k
 a Cholesky factor of Q
k
 F
k
C
P;k1
 G
k
C
Q;k
is a Cholesky factor of P
k
, and the sequence of
Householder transformation matrices 
1

2


k
C
Q;k

T
 F
k
C
P;k1
F
k
C
P;k1

T
 G
k
C
Q;k
G
k
C
Q;k

T
 F
k
C
P;k1
C

k
:
The triangularization in Eq. 8.20 is implemented in the Matlab m-®le schmidt.m
on the accompanying diskette.
8.1 MORE STABLE IMPLEMENTATION METHODS
237
8.1.7 Bierman±Thornton UD Filter
The Bierman±Thornton square-root ®lter is analogous to the Carlson±Schmidt
square root ®lter, but with modi®ed Cholesky factors of P in place of ordinary
Cholesky factors. It is also called ``UD ®ltering,'' in reference to the modi®ed
Cholesky factors U and D.
The principal differences between Bierman±Thornton UD ®ltering and Carlson±
Schmidt square-root ®ltering are as follows:
1. The Bierman±Thornton square-root ®lter uses U and D in place of C.
2. The observational update (due to Bierman [10] ) requires no square roots.
3. The temporal update (due to Thornton [116]) uses modi®ed weighted Gram±
Schmidt orthogonalization in place of Householder triangularization.
The methods of Carlson and Bierman are ``rank 1 modi®cation'' algorithms for
Cholesky factors and modi®ed Cholesky factors, respectively. A rank 1 modi®cation
algorithm for a triangular Cholesky factor, for example, calculates the triangular
Cholesky factor C such that
CC
T
 CC
T
 vv
T
;
given the prior Cholesky factor C and the vector v. Rank 1 modi®cation in this
case refers to the matrix rank of the modi®cation vv

bierman.m on the accompanying diskette) is given in Table 8.4.
238
KALMAN FILTER ENGINEERING
8.1.7.1 Potter Implementation The original square-root ®lter is due to James
H. Potter, who ®rst introduced the idea of recasting the Riccati equation in terms of
Cholesky factors of the covariance matrix P. The Matlab m-®le potter.m on the
accompanying diskette is a Matlab implementation of the Potter square-root ®lter.
The Potter approach handles only the observational update. It has been generalized
in [5] for vector-valued observations, with a corresponding differential equation for
the temporal propagation of the covariance equation.
8.2 IMPLEMENTATION REQUIREMENTS
Computer requirements for implementing Kalman ®lters tend to be dominated by the
need to solve the matrix Riccati equation, and many of these requirements can be
expressed as functions of the dimensions of the matrices in the Riccati equation.
TABLE 8.3 UD Filter Part 1: Thornton Predictor
function [x,U,D] = thornton(xin,Phi,Uin,Din,Gin,Q)
x = Phi*xin; % state prediction
[n,r]= size(Gin); % get model dimensions
G = Gin; % move to internal array
U = eye(n); % initialize U
PhiU = Phi*Uin;
for i=n:-1:1,
sigma = 0;
for j=1:n,
sigma = sigma + PhiU(i,j)^2 *Din(j,j);
if (j <= r)
sigma = sigma + G(i,j)^2 *Q(j,j);
end;
end;
D(i,i) = sigma;

natural resonant frequencies. Sampling rates much faster than the largest eigenvalue
of F are likely to be suf®cient for Kalman ®lter implementation, but they may not be
necessary. This sort of analysis is used for calculating the size of the time steps
required for reliably integrating the differential equations for the system state
estimate
^
x and its associated covariance matrix P, but determining workable
update rates for a particular application usually relies on simulation studies. Only
in simulation can we calculate differences beween the true solution and an
approximated solution.
TABLE 8.4 UD Filter Part 2: Bierman Corrector
a = U'*H';% a is not modi®ed, but
b = D*a; % b is modi®ed to become unscaled Kalman gain.
dz = z - H*xin;
alpha = R;
gamma = 1/alpha;
for j=1:length(xin),
beta = alpha;
alpha = alpha + a(j)*b(j);
lambda = -a(j)*gamma;
gamma = 1/alpha;
D(j,j) = beta*gamma*D(j,j);
for i=1:j-1,
beta = U(i,j);
U(i,j) = beta + b(i)*lambda;
b(i) = b(i) + b(j)*beta;
end;
end;
dzs = gamma*dz; % apply scaling to innovations
x = x + dzs*b; % multiply by unscaled Kalman gain

(iii) DSP processors designed for pipelined dot products and analog
interfaces.
(b) The types of arithmetic operations available as machine instructions versus
those that must be implemented in software, such as square roots.
(c) Hardware interrupt structure, which may determine how well the processor
supports real-time programming constraints.
(d) Availability of real-time debugging hardware and software (compilers,
editors, source-on-line code debuggers).
(e) Data wordlength options.
(f) Arithmetic processing speed with representative instruction mix for
Kalman ®ltering.
8.2 IMPLEMENTATION REQUIREMENTS
241
4. Whether the implementation includes a dynamic disturbance noise distribution
matrix G.
5. Whether any or all of the following matrices must be computed on each
interation:
(a) F (n  n state transition matrix),
(b) H (`  n measurement sensitivity matrix),
(c) R (`  ` measurement noise covariance matrix),
(d) Q ( p  p dynamic disturbance noise covariance matrix), and
(e) G (n  p dynamic disturbance noise distribution matrix).
6. Whether the estimate and covariance propagation is done using a state
transition matrix or (for nonlinear ®ltering) by numerical integration.
7. Whether the predicted measurement is computed using a measurement
sensitivity matrix or a nonlinear measurement function.
8. The sparse structure (if any) of the matrix parameters in the Kalman ®lter
equations. For example:
(a) For Kalman ®lters integrating independent systems with no dynamic
interactions (e.g., GPS and INS), dynamic coef®cient matrices and state

indexing) without physically relocating the blocks.
242
KALMAN FILTER ENGINEERING