- Email: [email protected]
Givens transformation techniques for Kalman filtering
Acta Astronautica. Vol. 4, pp. 847463. Pergamon Press 1977. Printed in Great Britain
Givens transformation techniques for Kalman filtering? C. L. T H O R N T O N ~
AND G. J. B I E R M A N §
Jet Propulsion Laboratory P a s a d e n a , CA 91103, U.S.A. (Received 21 January 1977)
Abstract----This paper e x a m i n e s the numerical stability and accuracy of a new K a l m a n filtering technique. T h e filter algorithm is based upon square-root-free Givens transformation m e t h o d s and involves an upper triangular covariance factorization P = U D U T. Stability of the U - D algorithm is studied by applying this method and several other K a l m a n filter algorithms to a realistic orbit determination problem. This s t u d y d e m o n s t r a t e s how the U - D filter can produce results which are orders of magnitude more accurate than those obtained with the conventional and stabilized K a l m a n algorithms. Computational efficiency of our algorithms is d e m o n s t r a t e d by showing that C P U timing requirements for our U - D formulation differ negligibly from the conventional K a l m a n requirements.
Introduction IN TInS paper we present a new method for solving the following nonstationary discrete-time linear filtering problem, X,+, = ~ , X , + B,to, z, = A , X , + v,
(Process)
(1)
(Scalar Measurement)
(2)
where X~ = X(t,) E R., to, = to(t,) E Rk, • , = ~(t,+,, t,),
z, = z(t,) E R,
A, = A(t,)
and Xo is a zero mean Gaussian random variable with covariance Po. The stochastic variables to, and v, are independent, zero mean purely Gaussian sequences with variances Q, and r, respectively. Properly defining B, allows one to assume that Q, is diagonal. Kalman's recursive solution to this linear filtering problem, introduced in Kalman (1960), involves computing minimum variance state estimates as fol-
? P a p e r presented at the X X V I I t h International Astronautical C o n g r e s s of the International Astronautical Federation, A n a h e i m , California, 10-16 October 1976. This paper p r e s e n t s the results of one p h a s e of r e s e a r c h carried out at the Jet Propulsion Laboratory, California Institute of Technology, under Contract No. N A S 7-100, s p o n s o r e d by N A S A . ~tSenior R e s e a r c h Engineer. § M e m b e r of Technical Staff. 847
848
c . L . Thornton and G. J. Bierman
lOWS" ~'~+, = q)3~, (Estimate time update) f ~ = X , + K , ( z i - A,~',)
(Estimate m e a s u r e m e n t update)
(3) (4)
where X~ = X ( t , lt,_,) is the one stage predicted estimate of Xi and X, = X ( t , lt~) is the filter estimate. K a l m a n ' s formula for computing the o p t i m u m gain, K,, involves the estimate error covariance recursions J~i+l = &;Pi&/ + BiQ~B~ T p~ = 6 _ K ~ A , t 5,
(Covariance time update)
(Covariance m e a s u r e m e n t update)
(5) (6)
where K, = #,A,~[A,,f,A,'
+ r~] '
t5o = Po.
(7) (8)
Although the K a l m a n algorithm has been successfully e m p l o y e d in a variety of filtering problems, practical applications of this method have often been plagued with numerical difficulties. Instances of serious a c c u r a c y loss have been r e p o r t e d . t Computational problems with K a l m a n ' s method are related to the error covariance recursions, eqns (5) and (6). These recursions are susceptible to numerical errors which can lead to indefinite c o m p u t e d covariance matrices and subsequent filter divergence. The numerical instability of K a l m a n ' s formula has motivated researchers to derive alternative formulations of his solution. While these methods are algebraically equivalent to the K a l m a n algorithm, they represent different computational techniques designed for improved numerical accuracy. One such method, suggested by Joseph and referred to as the s t a b i l i z e d K a l m a n a l g o r i t h m (see Bierman (1973), involves the following covariance m e a s u r e m e n t update formula. P = (I - KAr)lfi(I
- KA'r) r + KrK ~
(9)~:
The stabilized K a l m a n algorithm is considered reliable by m a n y estimation practioners because this formulation does not depend on whether or not K is given by eqn (7). Although this method is widely used it is subject to numerical deterioration and it can, in instances, be even less accurate than the conventional K a l m a n algorithm. This little publicized fact is d e m o n s t r a t e d in Thornton (1976) and Bierman and Thornton (1977). +See. for example. Bellantoni and Dodge (1%7), Schmidt et al. (1%8) and Dyer and McReynolds (1%9), :~AIIof the quantities in eqn (9) are time dependent, but this dependence is suppressed to simplify the notation.
Givens trans[ormation techniques f o r Kalman liltering
849
More successful methods for improving filter accuracies have involved covariance matrix factorizations. The square-root factorization P = S S r has yielded several reliable filter algorithms such as the Potter-Schmidt filter (see Kaminski et al. (1971) and Bierman (1977). These square-root algorithms replace the matrix computations in eqns (5) and (6) by equivalent recursions for S, the matrix square root. In the Potter-Schmidt filter, measurement updating of S is accomplished using Potter's formula (Bierman, 1973, and Kaminski et al., 1971), while time updating is accomplished by triangularizing the array [d~S BQ '~2] using an orthogonal transformation applied from the right.t The Potter-Schmidt method is based upon sound computational techniques and has produced reliable results. However, this method lacks the computational efficiency of the conventional Kalman formulation, and in many real-time applications it is unacceptably expensive (Thornton, 1976). The new filter algorithm presented in this paper combines the superior numerical precision of square root filtering techniques with an efficiency comparable to that of Kalman's original formulae. Our U-D covariance factorization filter relies upon numerically stable orthogonal transformation techniques. Measurement and time updating of the U-D factors involve separate applications of Gentleman's, fast square-root-free Givens rotations (Gentleman, 1973). The computational advantages of our U-D filter algorithm are demonstrated by a filter numerical comparison study which involves the conventional Kalman filter, the stabilized Kalman filter, the Potter-Schmidt square root filter, and our U-D formulation. Our analysis of a realistic planetary navigation problem provides a dramatic illustration of the improved numerical precision of the U-D and Potter-Schmidt covariance factorization methods. The study also demonstrates that the U-D algorithm, using modified Givens rotations, is numerically stable and efficient. Comparison of CPU times for this study shows that the U-D and conventional Kalman filters are equally efficient and that they are considerably less expensive than the stabilized Kalman and Potter-Schmidt methods. We note in passing that the use of matrix factorization techniques for enhancing numerical accuracy, simplifying mathematical analysis and generating new insights is symptomatic of a trend in numerical mathematics. Prominent applications, discussed in many publications~t and the references therein, include the least-squares applications that we relate to Kalman filtering and regression analysis, as well as applications to linear and quadratic programming, and unconstrained and constrained function optimization. The flow of the paper is as follows. In the following section the U-D measurement and time update algorithms are described, and their numerical characteristics are discussed. The next section contains a description of the filter comparison study, and in the final section our conclusions are stated. tSee Bierman (1973), Kaminski et al. (1971), Thornton (1976) and Bierman (1977). ~See: Dyer and McReynolds (1969), Bierman (1973), Kaminski et al. (1971), Thornton (1976), Bierman and Thornton (1977), Bierman (1977), Gentleman (1973), Gentleman (1977), Gill et al. (1974) Fletcher and Powell (1974), Gill and Murray (1973 and 1974), Stewart (1973), Bartels et al. (1970) and Gill (1973).
850
C.L. Thornton and G. J. Bierman
U-D
covarianee
factorization
filter
Suppose the n-dimensional error covariance matrix, P, is factored such that (10)
P = UDU'
where U is unit upper triangular and D = diag (d, . . . . . d,). The matrices U and D are referred to as the U - D factors of P. T h e y are unique, provided that P is positive, definite, and can be constructed using a Cholesky factorization (see Thornton (1976) and Bierman (1977)). Algorithms are presented here for performing m e a s u r e m e n t and time updating of the U - D factors. These algorithms c o r r e s p o n d to the conventional K a l m a n formulas, eqns (5) and (6). U-D
measurement
update
algorithm
Given a priori covariance factors /] and /) and scalar m e a s u r e m e n t z = Ax+u, where E(v2) = r, Bierman (1975) has shown that the updated U - D covariance factors and the Kalman gain (U, D and K respectively) can be obtained as follows:
f " = ( f . . . . . . f.)
f' =AO; v = lSf',
ill) (12)
vj = d, fj n -
1
/~1T = ( V l , 0 . . . . .
(13)
0)
~, = r + v,f,
(14)
d, = (d~,)d,.
(15)
For j = 2 . . . . . n cycle through eqns (16)-(20). Oti = Oli
di =
I +
(aj
(16)
t~ifi
,la~)d~
& = - fj/a,
(17) (18)
,
(19)
Ui = Oj + aiR~ ,
(20) where U = [ U , , U~ . . . . .
U,,]. The c o m p o n e n t U vectors have the form
u/=(u,(1)
and D = diag (d . . . . . .
d,).
.....
u~(j-
1), 1 , 0 . . . . .
0)
The Kalman gain is given by K = /~o/a,,.
(21)
Givens transformation techniques for Kalman filtering
851
T h e salient f e a t u r e of this algorithm is the w a y in which the u p d a t e d diagonal D e l e m e n t s are c o m p u t e d . Since the quantities a; are calculated as positive s u m s (see eqn (16)), it follows that the u p d a t e d d ' s are fractions of their a priori values (see eqn (17)) and t h e r e f o r e cancellation errors p r e s e n t in the c o n v e n t i o n a l m e a s u r e m e n t u p d a t i n g e q u a t i o n are avoided. T h e positivity of D, and h e n c e o f P, is t h e r e f o r e assured. F u r t h e r m o r e , the e l e m e n t s of D can diminish to near-zero w i t h o u t affecting the stability of the algorithm. T h e numerical integrity of this m e t h o d is f u r t h e r established by the a n a l y s e s of G e n t l e m a n (1973 and 1977), w h i c h relates the U - D m e a s u r e m e n t u p d a t e to numerically stable, squareroot-free Givens transformations. Modified G i v e n s techniques can be e m p l o y e d to a c c o m p l i s h time updating o f the U - D f a c t o r s (cf T h o r n t o n (1976)), and the resulting algorithm is the following: Modified Givens time update algorithm Let
W = [ B ] O U ] = [w,. w2. . . . .
w.+k]
13 = Diag (D, Q) = Diag (d, . . . . .
d.+D.
T h e U - D f a c t o r s o f /5 -- W / 3 W r (cf eqn (5)) can be c o m p u t e d as follows: F o r j = n . . . . . l cycle t h r o u g h eqns (22)-(36) as indicated. m: = j + k F o r i = m - 1. . . . .
(22)t
1 evaluate eqns (23)-(36) as indicated. (23):~
oe: = d . . w . . ( j )
(24)
~ : = d,w,(j) d " : = cew.,(j) + f l w , ( j )
(25)t
g: = f l / d "
(26)
~: = ~d-,.Id"
(27)
v: = w,
(28)
If i = m - 1 evaluate eqns (29)-(32) g: = a i d "
(29)
w,(l): = w , , ( j ) v ( l ) - v ( j ) w m ( l ) lI" w,.(l): = e w . , ( l ) + gv(l)
]
=l ..... j--1
fThe symbol ": = " denotes replacement (i.e., replace m by j + k). ~When / < m - l , w~(j)~ 1.
(30) (31)
852
C . L . Thornton and G. J. Bierman
w,(j): = 0,
win(j): = !
(32)
If i < m - 1 evaluate eqns (33)-(35) w,(/): = v ( l ) - v ( j ) w , , ( I ) ]
(33) l =1, .... j-1
win(l): = w,.(l) + gw;(l) J
(34)
w;(j): = 0
(35)
d,,: = d ' .
(36)
Upon completion of this recursion the W and D arrays contain U and D, stored as follows k
n
W = ~iU-]}n d, -- d;+k
j = 1.....
(37) n.
(38)
The work of Gentleman (1973) and Fletcher and PoweU (1974) demonstrates the numerical stability of this propagation algorithm. Their analysis indicates that this method rarely requires additional procedures, such as column pivoting or ratio testing, to assure stability. Numerical safeguards may be incorporated, however, when unusually illconditioned problems warrant such measures (Gentleman (1973) and Fletcher and Powell (1974)). Propagation of the U - D covariance factors may also be accomplished by applying the Modified Weighted G r a m - S c h m i d t (MWGS) algorithm derived in Thornton (1976) and Thornton and Bierman (1975). Numerical results reported in Thornton (1976) and Bierman and Thornton (1977) demonstrate that the MWGS algorithm is computationally reliable and is efficient. The modified Givens algorithm given here involves more equations to describe it than does the MWGS algorithm. It turns out, however, that the modified Givens method usually requires less computation (see Thornton (1976)) and this makes the method attractive for problems where computational e c o n o m y is of paramount importance. Time propagation represents a major filtering expense, particularly in applications where measurements are sparse (see Thornton (1976)). Filtering costs may often be significantly reduced by designing propagation algorithms to take advantage of special system structure. For systems composed of bias parameters and colored process noise the U - D covariance factorization lends itself to an especially efficient time update scheme. Space limitations prevent us from including that algorithm in this paper, but details are available in the literature.§ The reader will note that formulas for U - D updating are not as compactly represented as are the corresponding covariance eqns (5) and (6). This does not, §See Thornton (1976), Bierman (1977) and Thornton and Bierman (1975).
Givens trans[ormation techniques [or Kalman filtering
853
however, detract f r o m their utility. Detailed comparisons, in T h o r n t o n (1976) and Bierman (1975), have shown that our factorization algorithms require no more c o m p u t e r storage, are no harder to mechanize and involve negligibly more c o m p u t e r time than does the conventional K a l m a n filter.
Numerical comparison study P r o b l e m description The numerical stability and efficiency of the previously described U - D filter are d e m o n s t r a t e d by applying this algorithm and several other covariance-type filters to a realistic planetary navigation problem. The problem chosen for this study is a portion of the forthcoming MJS (1977) deep space mission which involves the a p p r o a c h to Saturn. The period of our interest extends from 30 days before Saturn encounter (point of closest approach) up to the encounter. For the initial 20 days the s p a c e c r a f t trajectory is very nearly rectilinear, a situation that is characteristic of the major portion of most deep space missions. The last portion of the trajectory has a hyperbolic bend due to the effect of Saturn's gravity. H e n c e , the portion of the trajectory up to encounter is especially useful for an accurate determination of planetary mass and spacecraft position and velocity. This trajectory is thus characteristic of a large n u m b e r of orbit determination situations. A nominal s p a c e c r a f t trajectory and transition matrices were obtained by integrating the equations of motion and variational equations (see M o y e r (1971)).t Calculations were p e r f o r m e d in an earth-centered, cartesian coordinate system. Because this study is intended to assess only the effects of filter numerical errors, the simulation was constructed f r o m a linear model. The actual trajectory, x(t), is defined by x ( t ) = X,om(t) + Ax(t)
(39)
where x is represented by a linear dynamic stochastic model of the f o r m (1). The c o m p o n e n t s of X°om(t) are the earth-centered spacecraft cartesian coordinates of position, velocity and acceleration. The acceleration c o m p o n e n t s of the perturbation Ax(t) are modeled as colored noise with time constants of 12hr and standard deviations of 10 " km/sec2; and these are used to define variances of the white noise, to(-), appearing in (1). The s p a c e c r a f t model used for the orbit determination problem has a piecewise constant.acceleration model with b+16 = At, taken as 2 hr. The effect of Saturn's mass on the spacecraft trajectory is very significant near encounter, and because of this our model includes the G M of Saturn with a 0.1% uncertainty ltr. O b s e r v a t i o n s of the s p a c e c r a f t are assumed to be made from three earthbased tracking stations. Their locations are such that there is c o v e r a g e at all #A detailed description of the trajectory and variational differential equations which generate the transition matrices are omitted because their inclusion would take us too far astray from the goals of this paper.
854
C . L . T h o r n t o n and G. J. Bierman
times. The simulation includes 2-3 doppler points e v e r y two hours with 1 m m / s e c a c c u r a c y (for 1 minute averaging time) and occasional range points with an a c c u r a c y of 3 meters. There are a total of 535 doppler and 72 range m e a s u r e m e n t s in the 30 day tracking period preceding the Saturn encounter. Since our test problem is intended to include the significant error sources, the station location position uncertainties were also included in the model (see Table 1). Table 1. S u m m a r y of a priori statistics used to generate sample data Variable
ltr Standard deviation
Position Velocity Acceleration
1000 k m 100 m/s 10 " km/s 2 (~" = 12 hr) spin a x i s - I m Longitude--2 m Latitude--5 m 0.1%
Stn loc. error G M (Saturn) Range Doppler
3 m
1 m m / s (for 1 min count time)
Data for our linear simulation analysis were generated as follows. Doppler and range partial derivatives were evaluated analytically a b o u t the nominal trajectory, using J P L ' s orbit determination software (Moyer, 1971). P s e u d o observables, z, were c o m p u t e d f r o m the differential correction m e a s u r e m e n t model (2), where the elements of A are the partial derivative coefficients; X is the state perturbation, augmented with the gravitational constant of Saturn error (GM), and the station location errors. Thus, X has a total of 19 c o m p o n e n t s ; 9 dynamic and 10 bias parameters. T h e y are position, velocity and acceleration (3 c o m p o n e n t s each), G M and the tracking station location errors (3 cartesian c o m p o n e n t s for each of the three stations); and v in eqn (2) is white data noise obtained f r o m a Gaussian r a n d o m number generator. The statistics used to generate our sample trajectory and data sequence are collected in Table 1. This simulation is representative of a large n u m b e r of interplanetary navigation problems (see Christiansen (1976) and O'Neill (1973)). Our model is in no way contrived to produce poor results in the K a l m a n algorithms. On the contrary, a priori state covariances were chosen slightly smaller than usual for this type of problem in order to avoid obvious initialization errors in the K a l m a n filters. Similarly, process noise uncertainties were assumed to be an order of magnitude higher than usual for this kind of mission because the K a l m a n algorithms are known to experience less numerical difficulties in high process noise environments. This estimation problem is well posed in an engineering sense. The problem is observable; the transition matrices are not ill-conditioned; and the measurement coefficients and a priori error variances are not unusually large.
Givens transformation techniques for Kalman filtering
855
The filter algorithms
The four covariance-type filter algorithms included in this study were the conventional Kalman filter, Joseph's stabilized Kalman filter, the P o t t e r - S c h m i d t square root filter, and the B i e r m a n - T h o r n t o n U - D filter described in the second section. All of these algorithms propagate estimates according to eqns (3) and (4). The algorithms differ principally in the way in which they compute the Kalman gains {K~} via covariance matrix or covariance factor recursions (see first two sections). Special attention was given to efficient algorithm implementation. Thus, vector outer products are used where the algorithm can be so arranged, and symmetry of the covariance matrix is exploited by computing only the upper triangular non-redundant entries. An exception to this principle of non-redundant computation is our mechanization of Joseph's stabilized algorithm ff = P - K ( f i A T) r
(40)
P = ( P - ( P A r ) K T) + ( K r ) K T.
(41)
S y m m e t r y is exploited in (40) because K is computed using (7). The matrix P can be obtained from (41) by arranging the computations as indicated by the parentheses, using vector outer products and computing only the upper triangular elements. Significantly more accurate results were obtained, however, when all n 2 elements of the first term of eqn (41) were computed and the off-diagonal elements were averaged. (Incidentally, we note that factoring K r in (41) tends to create numeric instability.) The fact that numerical results are sensitive to such mechanization minutiae is indicative of the algorithm's numerical instability. Filter c o m p a r i s o n rationale
The complexity of the orbit determination study problem prohibits a closed form solution, and consequently, the numerical solution computed using double precisiont arithmetic is used as a reference. Estimates and sigmas, computed using the B i e r m a n - T h o r n t o n and P o t t e r - S c h m i d t factorization algorithms agreed to 10 or more digits when computed using double precision arithmetic. The conventional and stabilized Kalman filter algorithms, computed using double precision, agreed to 8 or more digits with the other results. These comparisons established: (i) Confidence that our computer implementation of the various filter algorithms was correct. (ii) Assurance that when double precision arithmetic is used numerical errors due to round-off and cancellation are of no major consequence (to the orbit determination filtering problem); all of the algorithms were sufficiently accurate for this problem. (iii) Limitations on computable accuracy. Even when all filter computations were in (18 digit) double precision the results could not be trusted to more than 10 digits. tOur simulations were carried out on a Univac 1108 having a 27 bit characteristic (8-9 decimal digits) in single precision and a 60 bit characteristic (18 decimal digits) in double precision.
856
C . L . Thornton and G. J. Bierman
The carefully checked double precision programs were converted to single precision¢ by removing the FORTRAN IV "implicit double precision" statement. Estimates are retained in double precision because significant accuracy deterioration was noted in all the filter implementation results when single precision arithmetic was used in the estimate recursions, (3) and (4). To assure consistency of the inputs, the filter programs were arranged so that both single and double precision versions used the very same (single precision) inputs. These precautions guaranteed that the sometimes marked differences in the single and double precision estimation results were due solely to the numerics of the filtering algorithms. When the various single precision algorithms were applied to our sample problem, widely varying results were obtained. Numerical effects were evidenced by the differences in computed variances and gain profiles of the various algorithms. Especially prominent was the frequent appearance of negative variances arising from both the conventional and stabilized covariance filter algorithms. One might surmise from these results that since the gains and sigmas computed using the factorization algorithms stayed close to the double precision values, the estimates based on these gains could be trusted. On the other hand, the covariance filters produced negative variances and markedly different gain profiles. Thus one might expect that estimates based on these algorithms would be inaccurate. These speculations were easily corroborated using a double precision covariance error analysis program which evaluates the effects of nonoptimal gains by computing actual error variances (see Thornton (1976) and Bierman (1977)). Since each filter operates on the same data and state transition matrices and computes estimates in double precision, only the gain calculations differ. Hence, it is the gain algorithms that are being compared. Two principal results of the gain evaluations were: (i) The U-D and squareroot covariance algorithms performed as anticipated; i.e. the gain profiles were nearly optimal in that the actual and the (single precision) computed covariances were close; and these were close to the optimal double precision results. (ii) Actual covariances corresponding to the conventional and stabilized filters were considerably larger than the optimal covariances. The magnitudes of the actual variances, however, indicated that the filter estimates would at least track the actual trajectory. To illustrate the results predicted by the evaluation program, an actual trajectory (a perturbation to the nominal that was consistent with the assumed filter statistics) and a data noise sequence (consistent with the assumed measurement accuracies) were included into the study problem. The gain profiles were applied to this simulated problem, and estimate errors consistent with those predicted by the actual variances resulted. Variations were introduced into the simulation model to assure that the results were not coincidental. The consistency of the results convinced us that the sample estimate results to be described are not happenstance, but can truly be regarded as typical. ~When dealing with single precision computations, it is good numerical practice to accumulate inner products in double precision and then to round the resul! to single precision.
Givens trans[ormation techniques [or Kalman filtering
857
Simulation results Of the four filter algorithms mechanized for this illuminating study problem we had the most difficulty with the stabilized Kalman formulation. This is somewhat surprising because the equations appear so simple (and we have previous experience coding Kalman filters). Our difficulty can be traced to numerical inconsistencies between the single and double precision mechanization.t It turned out that there were no programming errors, only that the single precision results are sensitive to the a priori statistics and to the grouping of terms in the computer code. By contrast the single precision factorization results were always consistent with the double precision reference. Actual filter performance for this study is illustrated in Figs. 1 and 2. These figures display actual position and velocity uncertainties~ associated with each filter algorithm. Remark: The root-sum-square of position and velocity are chosen as a measure of estimation accuracy because these parameters are of primary interest in navigation and are representative of the general filtering results that were recorded.
10 6
! 10 5
RSS ACTUAL POSITION VARIANCES, z04 km
CONVENTIONAL KALMAN (SP~
%..
10 3
ALL DP FILTERS, U-D AND POIT"ER(SP)
io2L 0
5
10
15
20
DAYS PAST EPOCH
25
3O A
ENCOUNTER
Fig. i. Comparison of actual position uncertainties.
tWampler (1970) points out that these are sufficient reasons to declare an algorithm numerically unstable and to abandon it. Our findings are consistent with his conclusion. *Actual accuracies were obtained from the error analysis program which evaluates computed gain profiles from the various filter algorithms.
858
C.L. Thornton and G. J. Bierman
103 I t
I
I ' I ! 1 I I
102 RSS ACTUAL VELOCITY VARIANCES m/sec
kol
loOf
O
~
U-DAN i0-I
I
~
0
5
I
I
I0
I
I~5
L
20
DAYS PAST EPOCH
I
I
25
I
30
~.COUNTE.
Fig. 2. Comparison of actual velocity uncertainties.
In Figs. 1 and 2 the position and velocity uncertainties of the factored single precision algorithms are shown to agree with the double precision references. It is important to note that this consistency was observed in every case studied; i.e. the single precision factorization results always agreed well with the double precision reference cases. The single precision Kalman algorithms, on the other hand, exhibit no such numerical stability. Obvious numeric deterioration, in the form of negative computed variances, appear at inexplicable times. Negative variances first appear in the conventional Kalman mechanization after four days of filtering and after ten days when the stabilized mechanization is used. Several other surprising phenomena warrant mention. (1) Both the conventional and stabilized algorithms compute intermittent negative variances. From a total of 607 measurement updates the conventional algorithm computes negative variances 177 times and the stabilized algorithm produces negative variances 69 times. (2) Bias parameter variances are also intermittently negative. This violates the theoretic monotonicity of constant parameter variances (i.e. bias parameter variances should always decrease as more data is processed). To illustrate the erratic behavior exhibited we note that at 9.75 days the stabilized algorithm computes
2 O'~M
-1.8x
109
Givens transformation techniques for Kalman filteringi
859
and at 10 days adjusts this to 1.7 x 10". The correct (double precision) value is 2 O'GM~
5 X 1 0 3.
(3) The numerical instability discussed here is related to the choice of a priori statistics (see Bierman and Thornton (1977)). H o w e v e r , even in the case
of the conventional algorithm (which exhibits numerical failure earlier) it takes more than 48 time and 80 measurement updates before negative computed variances appear. (4) The appearance of negative diagonal elements in the computed covariance is not necessarily related to filter variances which are tending toward zero. Their appearance in this case acts instead as an indicator of algorithm numeric deterioration. Perhaps the most surprising result of this example is the fact that the Kalman algorithms, despite their unsatisfactory computed covariances, are able to generate meaningful (but not accurate) state estimates. According to the error analysis results, the gain profiles generated by the Kalman algorithms do lead to estimates which track the actual trajectory (see Figs. 1 and 2). The results, while not acceptable, are better than we anticipated they would be considering the intermittant appearance of negative diagonals. A simulation was performed to demonstrate the accuracies predicted by the error analysis. A data noise sequence and a trajectory were generated using the same model assumed in the error analysis. These in turn were used to construct a sequence of observations {z~}, upon which the filter algorithms would operate. This simulated data was filtered by each of the algorithms of our study and estimate errors were then compared. The results are illustrated in Figs. 3 and 4. Notice how closely the gain evaluation results of Figs. I and 2 predict the error curves of the sample path. As anticipated, the conventional algorithm in single precision produces enormous errors in position and velocity at 4 days. We note with interest that the estimates and computed sigmas obtained from the stabilized Kalman filter, when monitored at one day intervals, show few telltale signs of numeric deterioration. Except for the times when negative variances are printed (only 3) these estimates and sigmas appear reasonable and consistent. Only when the results are compared with the double precision reference does it become apparent that the computed Kalman estimates are far from optimum. By comparison, estimates computed using the factorization algorithms agree to about 4 or 5 digits with the double precision values. This agreement corresponds to better than 1 km in pc-sition and 50 mm/s in velocity. These single precision accuracies are particularly impressive when it is noted that estimation uncertainties are two orders of magnitude greater than these differences; i.e. the differences in the single and double precision results are in the noise level. Recall that the stabilized update formula was introduced as a computational improvement to the conventional formula and was supposed to assure nonnegativity of the computed covariance matrix. The results of our study show that the stabilized algorithm does not guarantee nonnegativity of the computed
860
C . L . Thornton and G. J. Bierman
i0 6
105
RSS POSITION ERRORS, l°4 km
10 3
102 0
5
i0
15
20
DAYS PAST EPOCH
25
30 A
FNCOU~ ' TFR
Fig. 3. Comparison of actual position errors, single simulation results.
10 3
102
RSS VELOCITY ERRORS, l ° l m/sec
i0-I 0
5
10
15
20
D A Y S PAST EPOCH
25
30
LN~.N~E"
Fig. 4. Comparison of actual velocity errors, single simulation results.
Givens transformation techniques for Kalman filtering
861
covariance (or even nonnegativity of the diagonal elements). Indeed our study shows, contrary to popular belief, that one can actually obtain worse results using the stabilized formula in place of the conventional one (worse in the sense that negative diagonal elements appear more often, and position errors are at times larger in the case of the stabilized algorithm). Because it does give improved performance in various other applications we do not suggest that one abandon the stabilized algorithm and return to the simpler conventional formula. Actually we think that both formulae are bad and should not be used as computational algorithms. Since good things are seldom free, one might surmise that the accuracy and stability associated with the factorization methods must be balanced with additional, and perhaps prohibitive, amounts of computation. Thornton (1976) and Bierman (1975) contain detailed arithmetic operation counts which show that the B i e r m a n - T h o r n t o n U - D algorithms are competitive with the computationally efficient conventional Kalman mechanization. For the problem at hand we have more complete information about computer costs, viz., computer overhead costs associated with indexing, logic, etc., are included in our C P U timing records. Table 2 gives the C P U times for the 19-state model used in this study problem. The P o t t e r - S c h m i d t algorithm is the most expensive of the algorithms, and this is our primary objection to it. Indeed, it was this cost problem that triggered our quest for a more efficient factorization algorithm. Our success is evidenced in Table 2, viz. the U - D filter was even faster than the conventional Kalman algorithm. Note that time propagation costs are influenced by the numbers of bias and colored noise variables (see Thornton (1976) and Thornton and Bierman (1975)), and therefore relative C P U times can vary somewhat. Thus, while the U - D method discussed in this paper is not always cheaper than the conventional Kalman algorithm, it is generally competitive with it. Table 2. Comparison of filter execution timest for 19 state orbit determination filter
Conventional Kalman Stabilized Kalman U-D Potter-Schmidt
Single precision
Double precision
39 45 36 63
49 59 45 80
t c P u time in seconds Conclusions This study of a meaningful engineering problem has demonstrated the numerical stability and accuracy of a new U - D covariance factorization filter. The filter algorithm, based upon Gentleman's efficient modification of the Givens matrix triangularization method, produced reliable results throughout the study. In fact, both the single precision U - D and P o t t e r - S c h m i d t algorithms produced results which remained close to the double precision reference results. These methods
862
C.L. THORNTONand G. J. BIERMAN
c o n s i s t e n t l y o u t p e r f o r m e d the c o n v e n t i o n a l a n d s t a b i l i z e d K a l m a n algorithms. A c c u r a c y i m p r o v e m e n t s were s u b s t a n t i a l , at t i m e s r e a c h i n g o r d e r s of m a g n i t u d e . A c o m p a r i s o n of C P U t i m e s for this a n a l y s i s s h o w s that the U - D a n d c o n v e n t i o n a l K a l m a n filters r e q u i r e e q u i v a l e n t a m o u n t s of c o m p u t a t i o n while the P o t t e r - S c h m i d t a n d s t a b i l i z e d K a l m a n m e t h o d s are c o n s i d e r a b l y m o r e time c o n s u m i n g . T h u s , this s t u d y illustrates h o w the U - D filter c o m b i n e s s u p e r i o r n u m e r i c a l p r e c i s i o n with e x c e p t i o n a l c o m p u t a t i o n a l efficiency. B a s e d o n o u r e x p e r i e n c e , a n d that r e p o r t e d b y n u m e r i c a l a n a l y s t s w o r k i n g in related areas, we state w i t h o u t r e s e r v a t i o n that U - D a n d r e l a t e d f a c t o r i z a t i o n a l g o r i t h m s are c o m p u t a t i o n a l l y s u p e r i o r to s e q u e n t i a l K a l m a n - t y p e c o v a r i a n c e algorithms. R e s e a r c h o n U - D f a c t o r time u p d a t i n g c o n t i n u e s , h o w e v e r , e v e n t h o u g h the r e s u l t s r e p o r t e d here are quite positive. A d d i t i o n a l r e s e a r c h m a y yield f u r t h e r i m p r o v e m e n t s in c o m p u t a t i o n a l efficiency a n d a c c u r a c y . References
Bartels, R. H., Golub, G. H. and Saunders, M. A. (1970) Numerical techniques in mathematical programming. Nonlinear Programming, pp. 123-176. Academic Press, New York. Bellantoni, J. F. and Dodge, K. W. (1967) Square root formulation of the Kalman-Schmidt filter. A I A A J., 5, 1309-1314. Bierman, G. J. (1973) A comparison of discrete linear filtering algorithms. IEEE Trans. Aero. Elect. Systems, AES-9, 28-37. Bierman, G. J. (1975) Measurement updating using the U-D factorization. Proc. 1975 Con[. on Decision and Control, pp. 337-346. Bierman, G. J. (1977) Factorization Methods [or Discrete Sequential Estimation. Academic Press, New York. Bierman, G. J. and Thornton, C. L. (1977) Numerical comparison of Kalman filter algorithms: Orbit determination case study. Automatica 13, 23-35. Christensen, C. S. (1976) Performance of the square root information filter for navigation of the mariner 10 spacecraft. Jet Propulsion Laboratory Technical Memorandum 33-757. Dyer, P. and McReynolds, S. R. (1969) Extension of Square-Root filtering to include process noise. J. Opt. Theory and Appl. 3, a.A,4 458.
Fletcher, R. and Powell, M. (1974) On the modification of LDL T factorizations. Math. of Comp., 28(128), 1067-1087. Gentleman, W. M. (1973) Least squares computations by Givens transformations without square roots. J. Inst. Maths. Appl., 12, 329-336. Gentleman, W. M. (1977) Error analysis of QR decompositions by Givens transformations submitted for publication. Gill, P. E. and Murray, W. (1973) A numerically stable form of the simplex algorithm. Linear Algebra and Its Applcs. 7, 99-138. Gill, P. E., Golub, G. H., Murray, W. and Saunders, M. A. (1974) Methods for modifying matrix factorizations. Math. of Comp., Vo[. 28(216), 505-535. Gill, P. E. and Murray, W. (1973) A numerically stable form of the simplex algorithm. Linear Algebra and Its Applcs. 7, 99-138. Gill, P. E. and Murray, M. (1974) Numerical Methods for Constrained Optimization. Academic Press, England. Kalman, R. E. (1960) A new approach to linear filtering and prediction problems. Trans. ASME, J. Basic Eng. Ser. D, 83, 34-45. Kaminski, P. G., Bryson, A. E. and Schmidt, S. F. (1971) Discrete square-root filtering: A survey of current techniques. IEEE Trans. Automatic Control AC-16, 727-735. Moyer, T. D. (1971) Mathematical formulation of the double precision orbit determination program (DPODP). Jet Propulsion Laboratory Technical Report, TR 32-1527. O'Neill, W. J. et al. (1973) Mariner 9 navigation. Jet Propulsion Laboratory Technical Report 32-1586.
Givens transformation techniques for Kalman filtering
863
Schmidt, S. F., Weinberg, J. D. and Lukesh, J. S. (1968) Case study of Kalman filtering in the C-5 aircraft navigation system. Joint Automatic Control Conf. Case Studies in System Control. Stewart, G. W. (1973) Conjugate direction methods for solving systems of linear equations. Numer. Math. 21,285-297. Thornton, C. L. (1976) Triangular covariance factorizations for Kalman filtering. Ph.D in Engineering, System Science Department, University of California, Los Angeles. Thornton, C. L. and Bierman, G. J. (1975) Gram-Schmidt algorithms for covariance propagation. Proc. 1975 Conf. of Decision and Control, pp. 489-498. Wampler, R. H. (1970) A report on the accuracy of some widely used least squares computer programs. J. Amer. Stat. Assoc. 65(330), 549-565.
Givens transformation techniques for Kalman filtering? C. L. T H O R N T O N ~
AND G. J. B I E R M A N §
Jet Propulsion Laboratory P a s a d e n a , CA 91103, U.S.A. (Received 21 January 1977)
Abstract----This paper e x a m i n e s the numerical stability and accuracy of a new K a l m a n filtering technique. T h e filter algorithm is based upon square-root-free Givens transformation m e t h o d s and involves an upper triangular covariance factorization P = U D U T. Stability of the U - D algorithm is studied by applying this method and several other K a l m a n filter algorithms to a realistic orbit determination problem. This s t u d y d e m o n s t r a t e s how the U - D filter can produce results which are orders of magnitude more accurate than those obtained with the conventional and stabilized K a l m a n algorithms. Computational efficiency of our algorithms is d e m o n s t r a t e d by showing that C P U timing requirements for our U - D formulation differ negligibly from the conventional K a l m a n requirements.
Introduction IN TInS paper we present a new method for solving the following nonstationary discrete-time linear filtering problem, X,+, = ~ , X , + B,to, z, = A , X , + v,
(Process)
(1)
(Scalar Measurement)
(2)
where X~ = X(t,) E R., to, = to(t,) E Rk, • , = ~(t,+,, t,),
z, = z(t,) E R,
A, = A(t,)
and Xo is a zero mean Gaussian random variable with covariance Po. The stochastic variables to, and v, are independent, zero mean purely Gaussian sequences with variances Q, and r, respectively. Properly defining B, allows one to assume that Q, is diagonal. Kalman's recursive solution to this linear filtering problem, introduced in Kalman (1960), involves computing minimum variance state estimates as fol-
? P a p e r presented at the X X V I I t h International Astronautical C o n g r e s s of the International Astronautical Federation, A n a h e i m , California, 10-16 October 1976. This paper p r e s e n t s the results of one p h a s e of r e s e a r c h carried out at the Jet Propulsion Laboratory, California Institute of Technology, under Contract No. N A S 7-100, s p o n s o r e d by N A S A . ~tSenior R e s e a r c h Engineer. § M e m b e r of Technical Staff. 847
848
c . L . Thornton and G. J. Bierman
lOWS" ~'~+, = q)3~, (Estimate time update) f ~ = X , + K , ( z i - A,~',)
(Estimate m e a s u r e m e n t update)
(3) (4)
where X~ = X ( t , lt,_,) is the one stage predicted estimate of Xi and X, = X ( t , lt~) is the filter estimate. K a l m a n ' s formula for computing the o p t i m u m gain, K,, involves the estimate error covariance recursions J~i+l = &;Pi&/ + BiQ~B~ T p~ = 6 _ K ~ A , t 5,
(Covariance time update)
(Covariance m e a s u r e m e n t update)
(5) (6)
where K, = #,A,~[A,,f,A,'
+ r~] '
t5o = Po.
(7) (8)
Although the K a l m a n algorithm has been successfully e m p l o y e d in a variety of filtering problems, practical applications of this method have often been plagued with numerical difficulties. Instances of serious a c c u r a c y loss have been r e p o r t e d . t Computational problems with K a l m a n ' s method are related to the error covariance recursions, eqns (5) and (6). These recursions are susceptible to numerical errors which can lead to indefinite c o m p u t e d covariance matrices and subsequent filter divergence. The numerical instability of K a l m a n ' s formula has motivated researchers to derive alternative formulations of his solution. While these methods are algebraically equivalent to the K a l m a n algorithm, they represent different computational techniques designed for improved numerical accuracy. One such method, suggested by Joseph and referred to as the s t a b i l i z e d K a l m a n a l g o r i t h m (see Bierman (1973), involves the following covariance m e a s u r e m e n t update formula. P = (I - KAr)lfi(I
- KA'r) r + KrK ~
(9)~:
The stabilized K a l m a n algorithm is considered reliable by m a n y estimation practioners because this formulation does not depend on whether or not K is given by eqn (7). Although this method is widely used it is subject to numerical deterioration and it can, in instances, be even less accurate than the conventional K a l m a n algorithm. This little publicized fact is d e m o n s t r a t e d in Thornton (1976) and Bierman and Thornton (1977). +See. for example. Bellantoni and Dodge (1%7), Schmidt et al. (1%8) and Dyer and McReynolds (1%9), :~AIIof the quantities in eqn (9) are time dependent, but this dependence is suppressed to simplify the notation.
Givens trans[ormation techniques f o r Kalman liltering
849
More successful methods for improving filter accuracies have involved covariance matrix factorizations. The square-root factorization P = S S r has yielded several reliable filter algorithms such as the Potter-Schmidt filter (see Kaminski et al. (1971) and Bierman (1977). These square-root algorithms replace the matrix computations in eqns (5) and (6) by equivalent recursions for S, the matrix square root. In the Potter-Schmidt filter, measurement updating of S is accomplished using Potter's formula (Bierman, 1973, and Kaminski et al., 1971), while time updating is accomplished by triangularizing the array [d~S BQ '~2] using an orthogonal transformation applied from the right.t The Potter-Schmidt method is based upon sound computational techniques and has produced reliable results. However, this method lacks the computational efficiency of the conventional Kalman formulation, and in many real-time applications it is unacceptably expensive (Thornton, 1976). The new filter algorithm presented in this paper combines the superior numerical precision of square root filtering techniques with an efficiency comparable to that of Kalman's original formulae. Our U-D covariance factorization filter relies upon numerically stable orthogonal transformation techniques. Measurement and time updating of the U-D factors involve separate applications of Gentleman's, fast square-root-free Givens rotations (Gentleman, 1973). The computational advantages of our U-D filter algorithm are demonstrated by a filter numerical comparison study which involves the conventional Kalman filter, the stabilized Kalman filter, the Potter-Schmidt square root filter, and our U-D formulation. Our analysis of a realistic planetary navigation problem provides a dramatic illustration of the improved numerical precision of the U-D and Potter-Schmidt covariance factorization methods. The study also demonstrates that the U-D algorithm, using modified Givens rotations, is numerically stable and efficient. Comparison of CPU times for this study shows that the U-D and conventional Kalman filters are equally efficient and that they are considerably less expensive than the stabilized Kalman and Potter-Schmidt methods. We note in passing that the use of matrix factorization techniques for enhancing numerical accuracy, simplifying mathematical analysis and generating new insights is symptomatic of a trend in numerical mathematics. Prominent applications, discussed in many publications~t and the references therein, include the least-squares applications that we relate to Kalman filtering and regression analysis, as well as applications to linear and quadratic programming, and unconstrained and constrained function optimization. The flow of the paper is as follows. In the following section the U-D measurement and time update algorithms are described, and their numerical characteristics are discussed. The next section contains a description of the filter comparison study, and in the final section our conclusions are stated. tSee Bierman (1973), Kaminski et al. (1971), Thornton (1976) and Bierman (1977). ~See: Dyer and McReynolds (1969), Bierman (1973), Kaminski et al. (1971), Thornton (1976), Bierman and Thornton (1977), Bierman (1977), Gentleman (1973), Gentleman (1977), Gill et al. (1974) Fletcher and Powell (1974), Gill and Murray (1973 and 1974), Stewart (1973), Bartels et al. (1970) and Gill (1973).
850
C.L. Thornton and G. J. Bierman
U-D
covarianee
factorization
filter
Suppose the n-dimensional error covariance matrix, P, is factored such that (10)
P = UDU'
where U is unit upper triangular and D = diag (d, . . . . . d,). The matrices U and D are referred to as the U - D factors of P. T h e y are unique, provided that P is positive, definite, and can be constructed using a Cholesky factorization (see Thornton (1976) and Bierman (1977)). Algorithms are presented here for performing m e a s u r e m e n t and time updating of the U - D factors. These algorithms c o r r e s p o n d to the conventional K a l m a n formulas, eqns (5) and (6). U-D
measurement
update
algorithm
Given a priori covariance factors /] and /) and scalar m e a s u r e m e n t z = Ax+u, where E(v2) = r, Bierman (1975) has shown that the updated U - D covariance factors and the Kalman gain (U, D and K respectively) can be obtained as follows:
f " = ( f . . . . . . f.)
f' =AO; v = lSf',
ill) (12)
vj = d, fj n -
1
/~1T = ( V l , 0 . . . . .
(13)
0)
~, = r + v,f,
(14)
d, = (d~,)d,.
(15)
For j = 2 . . . . . n cycle through eqns (16)-(20). Oti = Oli
di =
I +
(aj
(16)
t~ifi
,la~)d~
& = - fj/a,
(17) (18)
,
(19)
Ui = Oj + aiR~ ,
(20) where U = [ U , , U~ . . . . .
U,,]. The c o m p o n e n t U vectors have the form
u/=(u,(1)
and D = diag (d . . . . . .
d,).
.....
u~(j-
1), 1 , 0 . . . . .
0)
The Kalman gain is given by K = /~o/a,,.
(21)
Givens transformation techniques for Kalman filtering
851
T h e salient f e a t u r e of this algorithm is the w a y in which the u p d a t e d diagonal D e l e m e n t s are c o m p u t e d . Since the quantities a; are calculated as positive s u m s (see eqn (16)), it follows that the u p d a t e d d ' s are fractions of their a priori values (see eqn (17)) and t h e r e f o r e cancellation errors p r e s e n t in the c o n v e n t i o n a l m e a s u r e m e n t u p d a t i n g e q u a t i o n are avoided. T h e positivity of D, and h e n c e o f P, is t h e r e f o r e assured. F u r t h e r m o r e , the e l e m e n t s of D can diminish to near-zero w i t h o u t affecting the stability of the algorithm. T h e numerical integrity of this m e t h o d is f u r t h e r established by the a n a l y s e s of G e n t l e m a n (1973 and 1977), w h i c h relates the U - D m e a s u r e m e n t u p d a t e to numerically stable, squareroot-free Givens transformations. Modified G i v e n s techniques can be e m p l o y e d to a c c o m p l i s h time updating o f the U - D f a c t o r s (cf T h o r n t o n (1976)), and the resulting algorithm is the following: Modified Givens time update algorithm Let
W = [ B ] O U ] = [w,. w2. . . . .
w.+k]
13 = Diag (D, Q) = Diag (d, . . . . .
d.+D.
T h e U - D f a c t o r s o f /5 -- W / 3 W r (cf eqn (5)) can be c o m p u t e d as follows: F o r j = n . . . . . l cycle t h r o u g h eqns (22)-(36) as indicated. m: = j + k F o r i = m - 1. . . . .
(22)t
1 evaluate eqns (23)-(36) as indicated. (23):~
oe: = d . . w . . ( j )
(24)
~ : = d,w,(j) d " : = cew.,(j) + f l w , ( j )
(25)t
g: = f l / d "
(26)
~: = ~d-,.Id"
(27)
v: = w,
(28)
If i = m - 1 evaluate eqns (29)-(32) g: = a i d "
(29)
w,(l): = w , , ( j ) v ( l ) - v ( j ) w m ( l ) lI" w,.(l): = e w . , ( l ) + gv(l)
]
=l ..... j--1
fThe symbol ": = " denotes replacement (i.e., replace m by j + k). ~When / < m - l , w~(j)~ 1.
(30) (31)
852
C . L . Thornton and G. J. Bierman
w,(j): = 0,
win(j): = !
(32)
If i < m - 1 evaluate eqns (33)-(35) w,(/): = v ( l ) - v ( j ) w , , ( I ) ]
(33) l =1, .... j-1
win(l): = w,.(l) + gw;(l) J
(34)
w;(j): = 0
(35)
d,,: = d ' .
(36)
Upon completion of this recursion the W and D arrays contain U and D, stored as follows k
n
W = ~iU-]}n d, -- d;+k
j = 1.....
(37) n.
(38)
The work of Gentleman (1973) and Fletcher and PoweU (1974) demonstrates the numerical stability of this propagation algorithm. Their analysis indicates that this method rarely requires additional procedures, such as column pivoting or ratio testing, to assure stability. Numerical safeguards may be incorporated, however, when unusually illconditioned problems warrant such measures (Gentleman (1973) and Fletcher and Powell (1974)). Propagation of the U - D covariance factors may also be accomplished by applying the Modified Weighted G r a m - S c h m i d t (MWGS) algorithm derived in Thornton (1976) and Thornton and Bierman (1975). Numerical results reported in Thornton (1976) and Bierman and Thornton (1977) demonstrate that the MWGS algorithm is computationally reliable and is efficient. The modified Givens algorithm given here involves more equations to describe it than does the MWGS algorithm. It turns out, however, that the modified Givens method usually requires less computation (see Thornton (1976)) and this makes the method attractive for problems where computational e c o n o m y is of paramount importance. Time propagation represents a major filtering expense, particularly in applications where measurements are sparse (see Thornton (1976)). Filtering costs may often be significantly reduced by designing propagation algorithms to take advantage of special system structure. For systems composed of bias parameters and colored process noise the U - D covariance factorization lends itself to an especially efficient time update scheme. Space limitations prevent us from including that algorithm in this paper, but details are available in the literature.§ The reader will note that formulas for U - D updating are not as compactly represented as are the corresponding covariance eqns (5) and (6). This does not, §See Thornton (1976), Bierman (1977) and Thornton and Bierman (1975).
Givens trans[ormation techniques [or Kalman filtering
853
however, detract f r o m their utility. Detailed comparisons, in T h o r n t o n (1976) and Bierman (1975), have shown that our factorization algorithms require no more c o m p u t e r storage, are no harder to mechanize and involve negligibly more c o m p u t e r time than does the conventional K a l m a n filter.
Numerical comparison study P r o b l e m description The numerical stability and efficiency of the previously described U - D filter are d e m o n s t r a t e d by applying this algorithm and several other covariance-type filters to a realistic planetary navigation problem. The problem chosen for this study is a portion of the forthcoming MJS (1977) deep space mission which involves the a p p r o a c h to Saturn. The period of our interest extends from 30 days before Saturn encounter (point of closest approach) up to the encounter. For the initial 20 days the s p a c e c r a f t trajectory is very nearly rectilinear, a situation that is characteristic of the major portion of most deep space missions. The last portion of the trajectory has a hyperbolic bend due to the effect of Saturn's gravity. H e n c e , the portion of the trajectory up to encounter is especially useful for an accurate determination of planetary mass and spacecraft position and velocity. This trajectory is thus characteristic of a large n u m b e r of orbit determination situations. A nominal s p a c e c r a f t trajectory and transition matrices were obtained by integrating the equations of motion and variational equations (see M o y e r (1971)).t Calculations were p e r f o r m e d in an earth-centered, cartesian coordinate system. Because this study is intended to assess only the effects of filter numerical errors, the simulation was constructed f r o m a linear model. The actual trajectory, x(t), is defined by x ( t ) = X,om(t) + Ax(t)
(39)
where x is represented by a linear dynamic stochastic model of the f o r m (1). The c o m p o n e n t s of X°om(t) are the earth-centered spacecraft cartesian coordinates of position, velocity and acceleration. The acceleration c o m p o n e n t s of the perturbation Ax(t) are modeled as colored noise with time constants of 12hr and standard deviations of 10 " km/sec2; and these are used to define variances of the white noise, to(-), appearing in (1). The s p a c e c r a f t model used for the orbit determination problem has a piecewise constant.acceleration model with b+16 = At, taken as 2 hr. The effect of Saturn's mass on the spacecraft trajectory is very significant near encounter, and because of this our model includes the G M of Saturn with a 0.1% uncertainty ltr. O b s e r v a t i o n s of the s p a c e c r a f t are assumed to be made from three earthbased tracking stations. Their locations are such that there is c o v e r a g e at all #A detailed description of the trajectory and variational differential equations which generate the transition matrices are omitted because their inclusion would take us too far astray from the goals of this paper.
854
C . L . T h o r n t o n and G. J. Bierman
times. The simulation includes 2-3 doppler points e v e r y two hours with 1 m m / s e c a c c u r a c y (for 1 minute averaging time) and occasional range points with an a c c u r a c y of 3 meters. There are a total of 535 doppler and 72 range m e a s u r e m e n t s in the 30 day tracking period preceding the Saturn encounter. Since our test problem is intended to include the significant error sources, the station location position uncertainties were also included in the model (see Table 1). Table 1. S u m m a r y of a priori statistics used to generate sample data Variable
ltr Standard deviation
Position Velocity Acceleration
1000 k m 100 m/s 10 " km/s 2 (~" = 12 hr) spin a x i s - I m Longitude--2 m Latitude--5 m 0.1%
Stn loc. error G M (Saturn) Range Doppler
3 m
1 m m / s (for 1 min count time)
Data for our linear simulation analysis were generated as follows. Doppler and range partial derivatives were evaluated analytically a b o u t the nominal trajectory, using J P L ' s orbit determination software (Moyer, 1971). P s e u d o observables, z, were c o m p u t e d f r o m the differential correction m e a s u r e m e n t model (2), where the elements of A are the partial derivative coefficients; X is the state perturbation, augmented with the gravitational constant of Saturn error (GM), and the station location errors. Thus, X has a total of 19 c o m p o n e n t s ; 9 dynamic and 10 bias parameters. T h e y are position, velocity and acceleration (3 c o m p o n e n t s each), G M and the tracking station location errors (3 cartesian c o m p o n e n t s for each of the three stations); and v in eqn (2) is white data noise obtained f r o m a Gaussian r a n d o m number generator. The statistics used to generate our sample trajectory and data sequence are collected in Table 1. This simulation is representative of a large n u m b e r of interplanetary navigation problems (see Christiansen (1976) and O'Neill (1973)). Our model is in no way contrived to produce poor results in the K a l m a n algorithms. On the contrary, a priori state covariances were chosen slightly smaller than usual for this type of problem in order to avoid obvious initialization errors in the K a l m a n filters. Similarly, process noise uncertainties were assumed to be an order of magnitude higher than usual for this kind of mission because the K a l m a n algorithms are known to experience less numerical difficulties in high process noise environments. This estimation problem is well posed in an engineering sense. The problem is observable; the transition matrices are not ill-conditioned; and the measurement coefficients and a priori error variances are not unusually large.
Givens transformation techniques for Kalman filtering
855
The filter algorithms
The four covariance-type filter algorithms included in this study were the conventional Kalman filter, Joseph's stabilized Kalman filter, the P o t t e r - S c h m i d t square root filter, and the B i e r m a n - T h o r n t o n U - D filter described in the second section. All of these algorithms propagate estimates according to eqns (3) and (4). The algorithms differ principally in the way in which they compute the Kalman gains {K~} via covariance matrix or covariance factor recursions (see first two sections). Special attention was given to efficient algorithm implementation. Thus, vector outer products are used where the algorithm can be so arranged, and symmetry of the covariance matrix is exploited by computing only the upper triangular non-redundant entries. An exception to this principle of non-redundant computation is our mechanization of Joseph's stabilized algorithm ff = P - K ( f i A T) r
(40)
P = ( P - ( P A r ) K T) + ( K r ) K T.
(41)
S y m m e t r y is exploited in (40) because K is computed using (7). The matrix P can be obtained from (41) by arranging the computations as indicated by the parentheses, using vector outer products and computing only the upper triangular elements. Significantly more accurate results were obtained, however, when all n 2 elements of the first term of eqn (41) were computed and the off-diagonal elements were averaged. (Incidentally, we note that factoring K r in (41) tends to create numeric instability.) The fact that numerical results are sensitive to such mechanization minutiae is indicative of the algorithm's numerical instability. Filter c o m p a r i s o n rationale
The complexity of the orbit determination study problem prohibits a closed form solution, and consequently, the numerical solution computed using double precisiont arithmetic is used as a reference. Estimates and sigmas, computed using the B i e r m a n - T h o r n t o n and P o t t e r - S c h m i d t factorization algorithms agreed to 10 or more digits when computed using double precision arithmetic. The conventional and stabilized Kalman filter algorithms, computed using double precision, agreed to 8 or more digits with the other results. These comparisons established: (i) Confidence that our computer implementation of the various filter algorithms was correct. (ii) Assurance that when double precision arithmetic is used numerical errors due to round-off and cancellation are of no major consequence (to the orbit determination filtering problem); all of the algorithms were sufficiently accurate for this problem. (iii) Limitations on computable accuracy. Even when all filter computations were in (18 digit) double precision the results could not be trusted to more than 10 digits. tOur simulations were carried out on a Univac 1108 having a 27 bit characteristic (8-9 decimal digits) in single precision and a 60 bit characteristic (18 decimal digits) in double precision.
856
C . L . Thornton and G. J. Bierman
The carefully checked double precision programs were converted to single precision¢ by removing the FORTRAN IV "implicit double precision" statement. Estimates are retained in double precision because significant accuracy deterioration was noted in all the filter implementation results when single precision arithmetic was used in the estimate recursions, (3) and (4). To assure consistency of the inputs, the filter programs were arranged so that both single and double precision versions used the very same (single precision) inputs. These precautions guaranteed that the sometimes marked differences in the single and double precision estimation results were due solely to the numerics of the filtering algorithms. When the various single precision algorithms were applied to our sample problem, widely varying results were obtained. Numerical effects were evidenced by the differences in computed variances and gain profiles of the various algorithms. Especially prominent was the frequent appearance of negative variances arising from both the conventional and stabilized covariance filter algorithms. One might surmise from these results that since the gains and sigmas computed using the factorization algorithms stayed close to the double precision values, the estimates based on these gains could be trusted. On the other hand, the covariance filters produced negative variances and markedly different gain profiles. Thus one might expect that estimates based on these algorithms would be inaccurate. These speculations were easily corroborated using a double precision covariance error analysis program which evaluates the effects of nonoptimal gains by computing actual error variances (see Thornton (1976) and Bierman (1977)). Since each filter operates on the same data and state transition matrices and computes estimates in double precision, only the gain calculations differ. Hence, it is the gain algorithms that are being compared. Two principal results of the gain evaluations were: (i) The U-D and squareroot covariance algorithms performed as anticipated; i.e. the gain profiles were nearly optimal in that the actual and the (single precision) computed covariances were close; and these were close to the optimal double precision results. (ii) Actual covariances corresponding to the conventional and stabilized filters were considerably larger than the optimal covariances. The magnitudes of the actual variances, however, indicated that the filter estimates would at least track the actual trajectory. To illustrate the results predicted by the evaluation program, an actual trajectory (a perturbation to the nominal that was consistent with the assumed filter statistics) and a data noise sequence (consistent with the assumed measurement accuracies) were included into the study problem. The gain profiles were applied to this simulated problem, and estimate errors consistent with those predicted by the actual variances resulted. Variations were introduced into the simulation model to assure that the results were not coincidental. The consistency of the results convinced us that the sample estimate results to be described are not happenstance, but can truly be regarded as typical. ~When dealing with single precision computations, it is good numerical practice to accumulate inner products in double precision and then to round the resul! to single precision.
Givens trans[ormation techniques [or Kalman filtering
857
Simulation results Of the four filter algorithms mechanized for this illuminating study problem we had the most difficulty with the stabilized Kalman formulation. This is somewhat surprising because the equations appear so simple (and we have previous experience coding Kalman filters). Our difficulty can be traced to numerical inconsistencies between the single and double precision mechanization.t It turned out that there were no programming errors, only that the single precision results are sensitive to the a priori statistics and to the grouping of terms in the computer code. By contrast the single precision factorization results were always consistent with the double precision reference. Actual filter performance for this study is illustrated in Figs. 1 and 2. These figures display actual position and velocity uncertainties~ associated with each filter algorithm. Remark: The root-sum-square of position and velocity are chosen as a measure of estimation accuracy because these parameters are of primary interest in navigation and are representative of the general filtering results that were recorded.
10 6
! 10 5
RSS ACTUAL POSITION VARIANCES, z04 km
CONVENTIONAL KALMAN (SP~
%..
10 3
ALL DP FILTERS, U-D AND POIT"ER(SP)
io2L 0
5
10
15
20
DAYS PAST EPOCH
25
3O A
ENCOUNTER
Fig. i. Comparison of actual position uncertainties.
tWampler (1970) points out that these are sufficient reasons to declare an algorithm numerically unstable and to abandon it. Our findings are consistent with his conclusion. *Actual accuracies were obtained from the error analysis program which evaluates computed gain profiles from the various filter algorithms.
858
C.L. Thornton and G. J. Bierman
103 I t
I
I ' I ! 1 I I
102 RSS ACTUAL VELOCITY VARIANCES m/sec
kol
loOf
O
~
U-DAN i0-I
I
~
0
5
I
I
I0
I
I~5
L
20
DAYS PAST EPOCH
I
I
25
I
30
~.COUNTE.
Fig. 2. Comparison of actual velocity uncertainties.
In Figs. 1 and 2 the position and velocity uncertainties of the factored single precision algorithms are shown to agree with the double precision references. It is important to note that this consistency was observed in every case studied; i.e. the single precision factorization results always agreed well with the double precision reference cases. The single precision Kalman algorithms, on the other hand, exhibit no such numerical stability. Obvious numeric deterioration, in the form of negative computed variances, appear at inexplicable times. Negative variances first appear in the conventional Kalman mechanization after four days of filtering and after ten days when the stabilized mechanization is used. Several other surprising phenomena warrant mention. (1) Both the conventional and stabilized algorithms compute intermittent negative variances. From a total of 607 measurement updates the conventional algorithm computes negative variances 177 times and the stabilized algorithm produces negative variances 69 times. (2) Bias parameter variances are also intermittently negative. This violates the theoretic monotonicity of constant parameter variances (i.e. bias parameter variances should always decrease as more data is processed). To illustrate the erratic behavior exhibited we note that at 9.75 days the stabilized algorithm computes
2 O'~M
-1.8x
109
Givens transformation techniques for Kalman filteringi
859
and at 10 days adjusts this to 1.7 x 10". The correct (double precision) value is 2 O'GM~
5 X 1 0 3.
(3) The numerical instability discussed here is related to the choice of a priori statistics (see Bierman and Thornton (1977)). H o w e v e r , even in the case
of the conventional algorithm (which exhibits numerical failure earlier) it takes more than 48 time and 80 measurement updates before negative computed variances appear. (4) The appearance of negative diagonal elements in the computed covariance is not necessarily related to filter variances which are tending toward zero. Their appearance in this case acts instead as an indicator of algorithm numeric deterioration. Perhaps the most surprising result of this example is the fact that the Kalman algorithms, despite their unsatisfactory computed covariances, are able to generate meaningful (but not accurate) state estimates. According to the error analysis results, the gain profiles generated by the Kalman algorithms do lead to estimates which track the actual trajectory (see Figs. 1 and 2). The results, while not acceptable, are better than we anticipated they would be considering the intermittant appearance of negative diagonals. A simulation was performed to demonstrate the accuracies predicted by the error analysis. A data noise sequence and a trajectory were generated using the same model assumed in the error analysis. These in turn were used to construct a sequence of observations {z~}, upon which the filter algorithms would operate. This simulated data was filtered by each of the algorithms of our study and estimate errors were then compared. The results are illustrated in Figs. 3 and 4. Notice how closely the gain evaluation results of Figs. I and 2 predict the error curves of the sample path. As anticipated, the conventional algorithm in single precision produces enormous errors in position and velocity at 4 days. We note with interest that the estimates and computed sigmas obtained from the stabilized Kalman filter, when monitored at one day intervals, show few telltale signs of numeric deterioration. Except for the times when negative variances are printed (only 3) these estimates and sigmas appear reasonable and consistent. Only when the results are compared with the double precision reference does it become apparent that the computed Kalman estimates are far from optimum. By comparison, estimates computed using the factorization algorithms agree to about 4 or 5 digits with the double precision values. This agreement corresponds to better than 1 km in pc-sition and 50 mm/s in velocity. These single precision accuracies are particularly impressive when it is noted that estimation uncertainties are two orders of magnitude greater than these differences; i.e. the differences in the single and double precision results are in the noise level. Recall that the stabilized update formula was introduced as a computational improvement to the conventional formula and was supposed to assure nonnegativity of the computed covariance matrix. The results of our study show that the stabilized algorithm does not guarantee nonnegativity of the computed
860
C . L . Thornton and G. J. Bierman
i0 6
105
RSS POSITION ERRORS, l°4 km
10 3
102 0
5
i0
15
20
DAYS PAST EPOCH
25
30 A
FNCOU~ ' TFR
Fig. 3. Comparison of actual position errors, single simulation results.
10 3
102
RSS VELOCITY ERRORS, l ° l m/sec
i0-I 0
5
10
15
20
D A Y S PAST EPOCH
25
30
LN~.N~E"
Fig. 4. Comparison of actual velocity errors, single simulation results.
Givens transformation techniques for Kalman filtering
861
covariance (or even nonnegativity of the diagonal elements). Indeed our study shows, contrary to popular belief, that one can actually obtain worse results using the stabilized formula in place of the conventional one (worse in the sense that negative diagonal elements appear more often, and position errors are at times larger in the case of the stabilized algorithm). Because it does give improved performance in various other applications we do not suggest that one abandon the stabilized algorithm and return to the simpler conventional formula. Actually we think that both formulae are bad and should not be used as computational algorithms. Since good things are seldom free, one might surmise that the accuracy and stability associated with the factorization methods must be balanced with additional, and perhaps prohibitive, amounts of computation. Thornton (1976) and Bierman (1975) contain detailed arithmetic operation counts which show that the B i e r m a n - T h o r n t o n U - D algorithms are competitive with the computationally efficient conventional Kalman mechanization. For the problem at hand we have more complete information about computer costs, viz., computer overhead costs associated with indexing, logic, etc., are included in our C P U timing records. Table 2 gives the C P U times for the 19-state model used in this study problem. The P o t t e r - S c h m i d t algorithm is the most expensive of the algorithms, and this is our primary objection to it. Indeed, it was this cost problem that triggered our quest for a more efficient factorization algorithm. Our success is evidenced in Table 2, viz. the U - D filter was even faster than the conventional Kalman algorithm. Note that time propagation costs are influenced by the numbers of bias and colored noise variables (see Thornton (1976) and Thornton and Bierman (1975)), and therefore relative C P U times can vary somewhat. Thus, while the U - D method discussed in this paper is not always cheaper than the conventional Kalman algorithm, it is generally competitive with it. Table 2. Comparison of filter execution timest for 19 state orbit determination filter
Conventional Kalman Stabilized Kalman U-D Potter-Schmidt
Single precision
Double precision
39 45 36 63
49 59 45 80
t c P u time in seconds Conclusions This study of a meaningful engineering problem has demonstrated the numerical stability and accuracy of a new U - D covariance factorization filter. The filter algorithm, based upon Gentleman's efficient modification of the Givens matrix triangularization method, produced reliable results throughout the study. In fact, both the single precision U - D and P o t t e r - S c h m i d t algorithms produced results which remained close to the double precision reference results. These methods
862
C.L. THORNTONand G. J. BIERMAN
c o n s i s t e n t l y o u t p e r f o r m e d the c o n v e n t i o n a l a n d s t a b i l i z e d K a l m a n algorithms. A c c u r a c y i m p r o v e m e n t s were s u b s t a n t i a l , at t i m e s r e a c h i n g o r d e r s of m a g n i t u d e . A c o m p a r i s o n of C P U t i m e s for this a n a l y s i s s h o w s that the U - D a n d c o n v e n t i o n a l K a l m a n filters r e q u i r e e q u i v a l e n t a m o u n t s of c o m p u t a t i o n while the P o t t e r - S c h m i d t a n d s t a b i l i z e d K a l m a n m e t h o d s are c o n s i d e r a b l y m o r e time c o n s u m i n g . T h u s , this s t u d y illustrates h o w the U - D filter c o m b i n e s s u p e r i o r n u m e r i c a l p r e c i s i o n with e x c e p t i o n a l c o m p u t a t i o n a l efficiency. B a s e d o n o u r e x p e r i e n c e , a n d that r e p o r t e d b y n u m e r i c a l a n a l y s t s w o r k i n g in related areas, we state w i t h o u t r e s e r v a t i o n that U - D a n d r e l a t e d f a c t o r i z a t i o n a l g o r i t h m s are c o m p u t a t i o n a l l y s u p e r i o r to s e q u e n t i a l K a l m a n - t y p e c o v a r i a n c e algorithms. R e s e a r c h o n U - D f a c t o r time u p d a t i n g c o n t i n u e s , h o w e v e r , e v e n t h o u g h the r e s u l t s r e p o r t e d here are quite positive. A d d i t i o n a l r e s e a r c h m a y yield f u r t h e r i m p r o v e m e n t s in c o m p u t a t i o n a l efficiency a n d a c c u r a c y . References
Bartels, R. H., Golub, G. H. and Saunders, M. A. (1970) Numerical techniques in mathematical programming. Nonlinear Programming, pp. 123-176. Academic Press, New York. Bellantoni, J. F. and Dodge, K. W. (1967) Square root formulation of the Kalman-Schmidt filter. A I A A J., 5, 1309-1314. Bierman, G. J. (1973) A comparison of discrete linear filtering algorithms. IEEE Trans. Aero. Elect. Systems, AES-9, 28-37. Bierman, G. J. (1975) Measurement updating using the U-D factorization. Proc. 1975 Con[. on Decision and Control, pp. 337-346. Bierman, G. J. (1977) Factorization Methods [or Discrete Sequential Estimation. Academic Press, New York. Bierman, G. J. and Thornton, C. L. (1977) Numerical comparison of Kalman filter algorithms: Orbit determination case study. Automatica 13, 23-35. Christensen, C. S. (1976) Performance of the square root information filter for navigation of the mariner 10 spacecraft. Jet Propulsion Laboratory Technical Memorandum 33-757. Dyer, P. and McReynolds, S. R. (1969) Extension of Square-Root filtering to include process noise. J. Opt. Theory and Appl. 3, a.A,4 458.
Fletcher, R. and Powell, M. (1974) On the modification of LDL T factorizations. Math. of Comp., 28(128), 1067-1087. Gentleman, W. M. (1973) Least squares computations by Givens transformations without square roots. J. Inst. Maths. Appl., 12, 329-336. Gentleman, W. M. (1977) Error analysis of QR decompositions by Givens transformations submitted for publication. Gill, P. E. and Murray, W. (1973) A numerically stable form of the simplex algorithm. Linear Algebra and Its Applcs. 7, 99-138. Gill, P. E., Golub, G. H., Murray, W. and Saunders, M. A. (1974) Methods for modifying matrix factorizations. Math. of Comp., Vo[. 28(216), 505-535. Gill, P. E. and Murray, W. (1973) A numerically stable form of the simplex algorithm. Linear Algebra and Its Applcs. 7, 99-138. Gill, P. E. and Murray, M. (1974) Numerical Methods for Constrained Optimization. Academic Press, England. Kalman, R. E. (1960) A new approach to linear filtering and prediction problems. Trans. ASME, J. Basic Eng. Ser. D, 83, 34-45. Kaminski, P. G., Bryson, A. E. and Schmidt, S. F. (1971) Discrete square-root filtering: A survey of current techniques. IEEE Trans. Automatic Control AC-16, 727-735. Moyer, T. D. (1971) Mathematical formulation of the double precision orbit determination program (DPODP). Jet Propulsion Laboratory Technical Report, TR 32-1527. O'Neill, W. J. et al. (1973) Mariner 9 navigation. Jet Propulsion Laboratory Technical Report 32-1586.
Givens transformation techniques for Kalman filtering
863
Schmidt, S. F., Weinberg, J. D. and Lukesh, J. S. (1968) Case study of Kalman filtering in the C-5 aircraft navigation system. Joint Automatic Control Conf. Case Studies in System Control. Stewart, G. W. (1973) Conjugate direction methods for solving systems of linear equations. Numer. Math. 21,285-297. Thornton, C. L. (1976) Triangular covariance factorizations for Kalman filtering. Ph.D in Engineering, System Science Department, University of California, Los Angeles. Thornton, C. L. and Bierman, G. J. (1975) Gram-Schmidt algorithms for covariance propagation. Proc. 1975 Conf. of Decision and Control, pp. 489-498. Wampler, R. H. (1970) A report on the accuracy of some widely used least squares computer programs. J. Amer. Stat. Assoc. 65(330), 549-565.