In this case, although H clearly has rank 2 inmachine precision, the product HP0HT with roundoff will equal 8.1.2 Alternative Implementations The covariance correction process observatio
Trang 1Kalman 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 roundofferrors and alternative implementation methods with better robustness againstroundoff;
2 how to determine computer memory, word length, and throughput ments for implementing Kalman ®lters in computers;
require-3 ways to implement real-time monitoring and analysis of ®lter performance;
4 the Schmidt±Kalman suboptimal ®lter, designed for reducing computerrequirements;
5 covariance analysis, which uses the Riccati equations for performance-basedpredictive 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 mentation of Kalman ®lters It has been shown to cause severe degradation of ®lterperformance in many applications, and alternative implementations of the Kalman
imple-®lter equations (the Riccati equations, in particular) have been shown to improverobustness against roundoff errors
Trang 2Computer roundoff for ¯oating-point arithmetic is often characterized by a singleparameter eroundoff, which is the largest number such that
1 eroundoff 1 in machine precision 8:1The following example, due to Dyer and McReynolds [32], shows how a problemthat is well conditioned, as posed, can be made ill-conditioned by the ®lterimplementation
Example 8.1 Let Indenote the n n identity matrix Consider the ®ltering problemwith measurement sensitivity matrix
H 1 11 1 1 d1
and covariance matrices
P0 I3, and R d2I2;where d2< eroundoff but d > eroundoff In this case, although H clearly has rank 2 inmachine precision, the product HP0HT with roundoff will equal
8.1.2 Alternative Implementations
The covariance correction process (observational update) in the solution of theRiccati equation was found to be the dominant source of numerical instability in theKalman ®lter implementation, with the more common symptoms of failure beingasymmetry of the covariance matrix (easily ®xed) or, worse by far, negative terms onits diagonal These implementation problems could be avoided for some problems
by using more precision, but they were eventually solved for most applications byusing alternatives to the covariance matrix P as the dependent variable in thecovariance correction equation However, each of these methods required a com-patible method for covariance prediction Table 8.1 lists several of these compatibleimplementation methods for improving the numerical stability of Kalman ®lters.Figure 8.1 illustrates how these methods perform on the ill-conditioned problem
of Example 8.1 as the conditioning parameter d ! 0 For this particular test case,using 64-bit ¯oating-point precision (52-bit mantissa), the accuracy of the Carlson
Trang 3TABLE 8.1 Compatible Methods for Implementing the
Riccati Equation
Covariance Implementation Methods
Matrix Corrector Predictor
Symmetric
nonnegative
de®nite
Kalman [71], Joseph [19] Kalman [71]Kalman [71]
Square
Cholesky
factor C
Potter [100, 8] Ck1 FkCk
a From unpublished sources.
Fig 8.1 Degradation of numerical solutions with problem conditioning.
Trang 4[20] and Bierman [10] implementations degrade more gracefully than the others as
d ! e, the machine precision limit The Carlson and Bierman solutions stillmaintain about nine digits (30 bits) of accuracy at d pe, when the othermethods have essentially no bits of accuracy in the computed solution
These results, by themselves, do not prove the general superiority of the Carlsonand Bierman solutions for the Riccati equation Relative performance of alternativeimplementation methods may depend upon details of the speci®c application, andfor many applications, the standard Kalman ®lter implementation will suf®ce Formany other applications, it has been found suf®cient to constrain the covariancematrix to remain symmetric
8.1.3 SerialMeasurement Processing
It is shown in [73] that it is more ef®cient to process the components of ameasurement vector serially, one component at a time, than to process them as avector This may seem counterintuitive, but it is true even if its implementationrequires a transformation of measurement variables to make the associated measure-ment noise covariance R a diagonal matrix (i.e., with noise uncorrelated from onecomponent to another)
8.1.3.1 Measurement Decorrelation If the covariance matrix R of ment noise is not a diagonal matrix, then it can be made so by UDUTdecomposition(Eq B.22) and changing the measurement variables,
where Rcorr is the nondiagonal (i.e., correlated component to component) ment noise covariance matrix, and the new decorrelated measurement vector zdecorrhas a diagonal measurement noise covariance matrix Rdecorr and measurementsensitivity matrix Hdecorr
measure-8.1.3.2 Serial Processing of Decorrelated Measurements The nents of zdecorr can now be processed one component at a time using thecorresponding row of Hdecorr as its measurement sensitivity matrix and thecorresponding diagonal element of Rdecorr as its measurement noise variance
compo-A Mcompo-ATLcompo-AB implementation for this procedure is listed in Table 8.2, where the
®nal line is a ``symmetrizing'' procedure designed to improve robustness
Trang 58.1.4 Joseph Stabilized Implementation
This implementation of the Kalman ®lter is in [19], where it is demonstrated thatnumerical stability of the solution to the Riccati equation can be improved byrearranging the standard formulas for the measurement update into the followingformats (given here for scalar measurements):
8.1.5 Factorization Methods
8.1.5.1 Historical Background Robust implementation methods were duced ®rst for the covariance correction (measurement updates), observed to be theprincipal source of numerical instability In [100, 8], the idea of using a Choleskyfactor (de®ned in Section B.8.1) of the covariance matrix P, as the dependentvariable in the Riccati equation is introduced
intro-Carlson [20] discovered a more robust method using triangular Cholesky factors,which have zeros either above or below their main diagonals Bierman [10] extended
TABLE 8.2 Matlab Implementation of SerialMeasurement Update
Trang 6this to modi®ed Cholesky factors (de®ned in Section B.1.8.1), which are diagonaland unit triangular matrices D and U, respectively, such that
and U is triangular with 1's along its main diagonal
Compatible covariance prediction methods were discovered by Thomas Kailathand Stanley F Schmidt (for Carlson's method) and Catherine Thornton [116] (forBierman's method)
8.1.6 Square-Root Filtering Methods
8.1.6.1 Problems with the Riccati Equation Many early applications ofKalman ®ltering ran into serious numerical instability problems in solving theancillary 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 solutionwas more robust against computer roundoff errors Some of the more successful ofthese approaches are collectively called ``square-root ®ltering.''
8.1.6.2 Square-Root Filtering The concept for square-root ®ltering camefrom James H Potter when he was at the MIT Instrumentation Laboratory (later theCharles Stark Draper Laboratory) in the early 1960s, and his concept wasimplemented successfully in the Kalman ®lter used for onboard navigation in allthe Apollo moon missions Potter's algorithm is implemented on the Matlab m-®lepotter.m on the accompanying diskette It was originally called square-root
®ltering because it is based, in part, on an algorithm for taking a symmmetric squareroot of a special form of a symmetric matrix
The improved robustness of Potter's approach comes from replacing the iance matrix P with its Cholesky factor1as the dependent parameter of the Riccatiequation Some of the observed improvement in numerical stability is attributed toimprovement in the condition number cond C (ratio of largest to smallest char-acteristic value) over cond P, because
covar-cond C pcond P: 8:11
A matrix is considered ill-conditioned for inversion in a particular computer(``machine'' ) if its condition number is close to 1=emachine, where emachine is thelargest positive number2for which
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''.
Trang 7in machine precision That is, the result of adding emachine to 1 has no effect inmachine precision.
8.1.6.3 Triangularization Methods The so-called ``QR'' theorem of linearalgebra 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 Rmay have any of the forms
m < n m n m < 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 atriangular one T, because
®lter, we will use alternative symbols here
8.1.6.4 QR Decomposition by Householder Transformations holder transformation matrices3are orthogonal matrices of the form
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 wellsuited for robust operations in numerical linear algebra The QR decomposition of amatrix M is effectively accomplished by a series of products by Householdertransformation matrices, in the partitioned form
H v 0
3 Named for Alston S Householder (1904±1993), who developed many of the more robust methods used
in numerical linear algebra.
Trang 8with the vector v chosen to annihilate all but the end element of the remainingsubrow x of M until only the upper triangular part remains It suf®ces to let
v xT jxj
0
001
2664
377
However, operations with Householder matrices are typically not implemented bycalculating the appropriate Householder matrix and taking a matrix product Theycan be implemented quite ef®ciently as an algorithm operating ``in place'' on thematrix M, destroying M and leaving only the matrix T in its place when completed.The MATLAB function housetri.m on the accompanying diskette does just this
8.1.6.5 Triangularization of Cholesky Factors If A is any Cholesky factor
of P and A CM is a QR decomposition of A such that M is the orthogonal factorand C is the triangular factor, then C is a triangular Cholesky factor of P That is,
8.1.6.6 Morf±Kailath Square-Root Filter In Morf±Kailath square-root
®ltering, the entire Riccati equation, including prediction and correction steps, isimplemented in a single triangularization procedure It effectively computes theCholesky factors of successive covariance matrices of prediction error (required forcomputing Kalman gain) without ever explicitly computing the intermediate valuesfor corrected estimation errors Assume the following:
Gk is the dynamic disturbance distribution matrix of the system model,
CQk is a Cholesky factor of Qk;
Fk is the state transition matrix from the previous epoch;
CPk is a Cholesky factor of Pk ; the covariance matrix of prediction error fromthe previous epoch;
Hk is the measurement sensitivity matrix of the previous epoch;
CR is the measurement noise covariance matrix of the previous epoch; and
Trang 9H is a triangularizing orthogonal matrix for the partitioned matrix such that
a partitioned upper triangular matrix
Then CPk1 is the square triangular Cholesky factor of Pk1, the covariance matrix ofprediction error, and the Kalman gain
8.1.6.7 Carlson±Schmidt Square±Root Filtering In Carlson±Schmidtsquare-root ®ltering, only the temporal update (predictor) of the Riccati equation
is implemented using triangularization The observational update is implemented by
an algorithm due to Carlson [20] The Carlson algorithm is implemented in theMatlab m-®le carlson.m on the accompanying diskette It calculates theCholesky factor CP;k 1 of the covariance matrix Pk 1 corrected for theeffect of taking the measurement
The temporal update is implemented as
0 CP;k FkCP;k 1 GkCQ;kH1H2H3 Hn; 8:20where CP;k sought-for triangular Cholesky factor of Pk
CQ;k a Cholesky factor of Qk
FkCP;k 1 GkCQ;k is a Cholesky factor of Pk , and the sequence of
Householder transformation matrices H1H2H3 Hntransforms it into the appropriate triangular form
It can be shown that the matrix FkCP;k 1 GkCQ;k is, indeed, a Choleskyfactor of Pk by multiplying it out:
FkPk 1 FT
k GkQkGT
k
Pk :
The triangularization in Eq 8.20 is implemented in the Matlab m-®le schmidt.m
on the accompanying diskette
Trang 108.1.7 Bierman±Thornton UD Filter
The Bierman±Thornton square-root ®lter is analogous to the Carlson±Schmidtsquare root ®lter, but with modi®ed Cholesky factors of P in place of ordinaryCholesky factors It is also called ``UD ®ltering,'' in reference to the modi®edCholesky 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 forCholesky factors and modi®ed Cholesky factors, respectively A rank 1 modi®cationalgorithm for a triangular Cholesky factor, for example, calculates the triangularCholesky factor C such that
C C T C C T vvT;
given the prior Cholesky factor C and the vector v Rank 1 modi®cation in thiscase refers to the matrix rank of the modi®cation vvT of C C T In thisparticular application of rank 1 modi®cation, the matrix and vector are
The corresponding UD predictor algorithm was discovered by Catherine ton, and was the subject of her Ph.D dissertation [116] It is based on a relativelyrobust orthogonalization method developed by AÊke BjoÈrck [11] and called ``modi-
Thorn-®ed Gram±Schmidt.'' Bierman [10] refers to it as ``modiThorn-®ed weighted Gram±Schmidt'' (MWGS), which is much longer than its appropriate name, ``Thornton'smethod.'' A listing of its implementation in Matlab (from thornton.m on theaccompanying diskette) is presented in Table 8.3
The corresponding Matlab listing of the Bierman corrector algorithm (frombierman.m on the accompanying diskette) is given in Table 8.4
Trang 118.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 ofCholesky factors of the covariance matrix P The Matlab m-®le potter.m on theaccompanying 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 forthe temporal propagation of the covariance equation
8.2 IMPLEMENTATION REQUIREMENTS
Computer requirements for implementing Kalman ®lters tend to be dominated by theneed to solve the matrix Riccati equation, and many of these requirements can beexpressed 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
end;
for k=1:r, sigma = sigma + G(i,k)*Q(k,k)*G(j,k);
end;
U(j,i) = sigma/D(i,i);
for k=1:n, PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k);
end;
for k=1:r, G(j,k) = G(j,k) - U(j,i)*G(i,k);
end;
end;
end;
Trang 128.2.1 Throughput
Computer throughput is measured in arithmetic operations per second (ops).Minimum throughput required for implementing the Kalman ®lter will be theproduct
Throughput (ops) operations per cycle cycles per second;
where the operations per cycle depends on the number of state variables (n) andmeasurement variables (`) and cycles per second depends on attributes of the sensorsand the dynamic system model
8.2.1.1 Cycles per Second The eigenvalues of the dynamic coef®cientmatrix F determine the natural frequencies of the dynamic system model, with thereal parts representing inverse decay times and the imaginary parts representingnatural 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 benecessary This sort of analysis is used for calculating the size of the time stepsrequired for reliably integrating the differential equations for the system stateestimate ^x and its associated covariance matrix P, but determining workableupdate rates for a particular application usually relies on simulation studies Only
in simulation can we calculate differences beween the true solution and anapproximated 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.
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
Trang 138.2.1.2 Operations per Cycle Factors that in¯uence the numbers of metic operations per cycle of a Kalman ®lter implementation on a speci®capplication include the following:
arith-1 The dimensions of the state vector x (n), measurement vector z (`), andprocess noise vector w (p) These and the implementation methods (next item)are the only factors considered in Fig 8.2, so the results should be consideredupper bounds on just the estimation loop and Riccati equation Computationsrequired to compute any of the parameter matrices are not included in thecalculations
2 The implementation methods used, such as
(a) the original Kalman implementation,
(b) the Carlson±Schmidt square-root implementation,
(c) the Bierman±Thornton UD implementation, and
(d) the Morf±Kailath combined square-root implementation
However, choices among these methods are more likely to be driven by costand numerical stability issues than by computational requirements The morestable implementation methods may perform as well with shorter wordlengths
as the less stable methods with longer wordlengths, which could make a bigdifference in processor speed and cost
3 Processor hardware architecture issues, including the following:
(a) Processor type, including
(i) reduced instruction set computers (RISCs), in which all arithmeticoperations take place between registers and can be completed in onemachine cycle and all data transfers between registers and memoryalso require one machine cycle;
(ii) complex instruction set computers (CISCs), which can perform manyRISC-type operations per instruction but with each instructionexecuting in several machine cycles, and
(iii) DSP processors designed for pipelined dot products and analoginterfaces
(b) The types of arithmetic operations available as machine instructions versusthose that must be implemented in software, such as square roots.(c) Hardware interrupt structure, which may determine how well the processorsupports 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 forKalman ®ltering
Trang 144 Whether the implementation includes a dynamic disturbance noise distributionmatrix G.
5 Whether any or all of the following matrices must be computed on eachinteration:
(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 statetransition matrix or (for nonlinear ®ltering) by numerical integration
7 Whether the predicted measurement is computed using a measurementsensitivity matrix or a nonlinear measurement function
8 The sparse structure (if any) of the matrix parameters in the Kalman ®lterequations For example:
(a) For Kalman ®lters integrating independent systems with no dynamicinteractions (e.g., GPS and INS), dynamic coef®cient matrices and statetransition matrices will have blocks of zeros representing the dynamicuncoupling
(b) Because most sensors measure only a limited number of variables, thenumber of nonzero elements in the measurement senstivity matrix Husually tends to grow as the number of sensors (`) and not as the totalnumber of possible elements (n`)
(c) It is uncommon that the dynamic disturbance noise covariance matrix Qand=or sensor noise covariance matrix R are dense (i.e., no zeros), and it isnot uncommon that they are diagonal matrices
9 Details of the programming implementation, such as
(a) whether the programming takes advantage of any offered matrix ness by skipping multiplications by zero,
sparse-(b) whether the programming takes advantage of symmetry in the Riccatiequation solution, and
(c) multiplication order in evaluating matrix expressions, which can make asigni®cant difference in the computation required
In Carlson±Schmidt square-root ®ltering (Section 8.1.6.7), for example, it isnot necessary to place the blocks of the matrix
FkCP;k 1 jGkCQ;kinto a separate common array for triangularization The additional array spacecan be saved by doing an implicit triangularization of the array (by modi®edindexing) without physically relocating the blocks
Trang 15Not Quite Upper Bounds The contour plots in Fig 8.2 are ``not-quite-worst-casecomputational cost'' plotted in terms of equivalent multiply-and-accumulate opera-tions per update cycle as functions of the parameter matrix dimensions and thecompatible implementation methods used These are effectively ``not-quite upperbounds'' because they assume the worst-case parameter matrix conditions (i.e., allparameter matrices full and time varying) but do not include the added computa-tional cost of calculating the time-varying parameter matrices We cannot present acomparable upper bound (other than Eq 8.21) for the computational costs forcalculating the parameter matrices (F, H, G, Q and R), because these computationalcosts will be very much application-dependent.
The results also assume that advantage is taken of symmetry, where possible.Decorrelation Costs These computational costs for all implementations of thecorrector step (observational update) include those for diagonalizing R, which is onthe order of ` `2 1=6 multiply-and-accumulate operations per update cycle,where R is ` ` They also include the measurement error decorrelation operationsfor calculating the matrix ratios URnH and URnz; which is in the order of
Kalman±Kalman The computational cost for the Kalman ®lter corrector step(observational update, including decorrelation) will be on the order of
1
6` 9n2 `2 3`n 18n 3` 2 multiply-and-accumulate operations per date cycle [10, p 104, plus decorrelation], and that of the predictor step (temporalupdate) will be on the order of n 2n2 p2 n multiply-and-accumulate operationsper update cycle, where p is the number of components in the dynamic disturbancenoise vector (i.e., Q is p p)
up-Carlson±Schmidt Computational complexity of the Carlson algorithm is on theorder of 1
6` 12n2 3`n `2 75n 3` 4 multiply-and-accumulate operationsper update cycle [10, p 108, plus decorrelation] (This algorithm requires takingsquare roots, the computational costs for which were approximated as beingequivalent to six multiply-and-accumulate operations.)
The corresponding Schmidt±Kailath temporal update (using Householder gularization) has computational cost on the order of 1
trian-3n 2n2 3np 6n 12p 25 multiply-and-accumulate operations per update cycle in the worst-casescenario (G and Q time varying and Q nondiagonal) This includes the cost offorming the products FCPand GCQ, where CPis a Cholesky factor of P and CQis aCholesky factor of Q
Bierman±Thornton The computational cost of the Bierman algorithm is on theorder of 1
6` 9n2 3`n `2 3` 12n 4 multiply-and-accumulate operations
Trang 16per update cycle, which is not signi®cantly different from that of the conventionalKalman ®lter [10, p 107, plus decorrelation].
The corresponding not-quite-worst-case computational cost for the Thorntonupdate algorithm is on the order of 1
6n 4n2 3np p2 n 1=2 p p2 1multiply-and-accumulate operations per update cycle These computational costsinclude those for diagonalizing Q (by UD factorization), which is on the order of
Trang 17implementa-operations per update cycle required for taking the product GUQ, where UQis the factor from UD factorization of Q, before beginning the Thornton algorithm listed in[10].
U-Not shown in the plots is the ``Joseph stabilized'' implementation, whichgenerally has better numerical stability than the conventional Kalman implementa-tion but requires approximately three times the number of arithmetic operations perupdate cycle [10, pp 104 105]
The additional computational cost required for computing the parameter matrices
F, H, G, Q, and R tends to grow as
1
multiply-and-accumulate operations per update cycle, where a is the average number
of multiply-and-accumulate operations per matrix element
8.2.2.1 Wordlength Bierman [10] has conjectured that square-root ®ltering(Section 8.1.6.2) gives comparable accuracy with half as many bits of precision asconventional Kalman ®ltering No proof of this conjecture was offered, but Example8.1 and Fig 8.1 would seem to support it
8.3 KALMAN FILTER MONITORING
8.3.1 Rejecting Anomalous Sensor Data
8.3.1.1 Effects of Anomalous Sensor Data Anomalous sensor data canresult from sensor failures or from corruption of the signals from sensors, and it isimportant to detect these events before the anomalous data corrupts the estimate The
®lter is not designed to accept errors due to sensor failures or signal corruption, andthey can seriously degrade the accuracy of estimates The Kalman ®lter has in®niteimpulse response, so errors of this sort can persist for some time
Trang 188.3.1.2 Detecting Anomalous Sensor Data Fortunately, the Kalman ®lterimplementation includes parameters that can be used to detect anomalous data TheKalman gain matrix
(i.e., without the sign change and division by 2, but divided by the dimension of nk)
is nonnegative with a minimum value of zero If the Kalman ®lter were perfectlymodeled and all white-noise sources were Gaussian, this would be a chi-squaredstatistic with distribution as plotted in Fig 8.3 An upper limit threshold value on w2
can be used to detect anomalous sensor data, but a practical value of that thresholdshould be determined by the operational values of w2, not the theoretical values That
is, ®rst its range of values should be determined by monitoring the system inoperation, then a threshold value w2
max chosen such that the fraction of good datarejected when w2> w2
max will be acceptable