A simplified algorithm for suboptimal non-linear state estimation

Automatica, Vol. 6, pp. 477--480. Pergamon Press, 1970. Printed in Great Britain.

A Simplified Algorithm for Suboptimal Non-linear State Estimation* Un algorithme simplifi6 pour l'estimation suboptimale non-lin6aire d'6tat Ein vereinfachter Algorithmus ft~r suboptimale nichtlineare Zustandssch~itzung YnpomeHnsI~ a . r w O p H T M ]I.rLq c y 6 O H T H M a : I B H O f i H e . l l H H e f t r l O ~ OReHKH COCTORHHff

R. M. D R E S S L E R a n d D. W. ROSS'I"

numerical results are presented in section 3, and conclusions are presented in the last section.

Snmmary--This paper describes a technique that may be used to modify and approximate the covariance and weighting matrix computations of the extended Kalman filter. The resulting simplified algorithm, referred to as the piecewise-recursive, extended, Kalman filter, yields a significant improvement in computational speed without appreciable degradation in performance relative to the fully implemented, extended, Kalman filter. The performance of this filter is illustrated by application of the results to the problem of ballistic-trajectory estimation.

2. Theory Several techniques that may be used to modify and approximate the Kalman filtering algorithm in order to reduce the computation time without significantly degrading filter performance have been studied in Ref. [4]. The most promising of these simplified filter algorithms is the piecewise-recursive Kalman filter, which will be described below. Consider the discrete-time, non-linear system with state equation

1. Introduction SEVERALapproaches to the problem of estimating the state of a non-linear system from noise-corrupted measurements have been described in the literature. One of these, the extended Kalman filter, is an application to non-linear systems of results obtained by Kalman in optimal linear estimation theory [1], in which the estimate generated at each time is the maximum likelihood estimate, in the Bayesian sense, conditioned on all measurements up to that time. The extended Kalman filter has been shown to be an extremely accurate algorithm for non-linear state estimation as indicated, for example, in Refs. [2 and 3]. However, the computational requirements of this filter are quite severe. The extended Kalman filter is particularly suited to realtime digital computation with a set of recursive equations for the state estimate, the covariance matrix of the error in this estimate, and the filter's weighting matrix. However, the recursive calculation of the covadance matrix and the weighting matrix, which comprises the major portion of the computation time per filter iteration, is very time-consuming, since it involves several matrix multiplications and a matrix inversion. This places a severe limitation on the use of the extended Kalman filter for real-time state estimation. This paper describes a technique that may be used to modify and approximate the covadance and weighting matrix computations of the extended Kalman filter. The basic concept of this technique is that the covariance loop can be cycled less often than the state estimate loop in the Kalman filter. The resulting simplified algorithm, which will be referred to as the piecewise-recursive Kalman filter, yields a significant improvement in computational speed without appreciable degradation in filter performance. The basic theory of the technique is discussed in section 2, some

x(k)=f[x(k-1), k-1]+w(k-1)

(1)

and measurement equation

z(k) = h[x(k), k'] + v(k)

(2)

where x is the n-dimensional state vector, z is the m-dimensional measurement vector (m<~n), w is a zero-mean, white gaussian noise process that may represent either actual system disturbances or model inaccuracies, and v is a zeromean, white gaussian noise process that represents measurement noise; w and v are assumed to be independent, with covariances denoted by Q and R, respectively. The piecewise-recursive Kalman filter operates as follows: State estimates are generated by the filter at sample intervals of At sec, while the weighting matrix of the filter and the covariance matrix of the state estimation error are only calculated at intervals of Ar sec, where Ar/At=N>~ 1; i.e. the weighting matrix is only updated every N iterations, and is held constant in the filter equations for these iterations before being updated anew. The equations describing this filter are given below:* The state estimate is

Y~(klk)= £ ( k l k - 1) + W(i + 1 ) { z ( k ) - h[YC(klk- 1), k]},

(3)

where

2 ( k / k - 1) = f [ 2 ( k - l / k - 1), k - 1] k=i+l,

* Received 28 April 1969; revised 22 September 1969. The original version of this paper was not presented at any IFAC meeting. It was recommended for publication in revised form by associate editor M. Athans. t Information and Control Group, Stanford Research Institute, Menlo Park, California.

i+2 .....

(4)

i+N;

* ~(i/J9denotes the estimate of the state x at time i, conditioned on all measurements through time j; (Pi/j) denotes the covariance of the error in this estimate. 477

478

Brief P a p e r s

for this basic cycle (N filter iterations), the weighting matrix is

W(i +'1) = P(i + 1/i)HT( i + I ) [ R ( i + 1) + H ( i + l ) P ( i + l / i ) H T ( i + l ) ] -1 ,

(5)

to the effect of the system noise on the propagation of the state covariance matrix. The results presented in section 3 indicate that these assumptions are very reasonable. It can be shown (see Ref. [4]) that the computational speed of the piecewise-recursive Kalman filter, relative to the fully implemented Kalman filter, is approximately

where

SN--

NSo 2So+(N-

P(i+ 1/i)=@(i+ 1, i)P(i/i)dp'r(i+ 1, i)+Q(i)

(11) l)

(6)

(I)(i + 1, i) = d~.fI

(7)

OXl:~(i/i). i

where N=Ar/At and So is the ratio of the computational spiced of a Kalman filter using a precomputed weighting matrix to that of a fully implemented Kalman filter.

3. Numerical results

(8)

H ( i + 1)=~_-h

t3X ~(i+1/i), i+ 1

before proceeding to the next basic cycle, P(iq-N/iq-N), the covarianee of the error in the state estimate fl(i+N/i+N) at the end of the previous cycle, is computed as

P(i + N/i + N) = P(i + Nil) - P(i + N/i)Hr(i + N ) ~ ~ + H(i + N)P(i + N / i ) n r ( i + N ) ] - 1

(9)

•H(i + N ) e ( i + N/t), where

P(i + N/i) = ~ ( i + N, i)P(i/i)t~r(i + N, i) + Q(i)N (10)

*<,+N, 0=NOX[:~(i/i), ] i The updating of the state covariance matrix in equations (9) and (10) corresponds to the standard Kalman filter covariance equations for a sample period of NAt, where use is made of the following two simplifying assumptions for approximating the effective measurement noise and random disturbance during a basic cycle (N iterations): (i) The filtering done at the times k = i + 1, i + 2 . . . . . i + N constitutes a presmoothing of measured data z(k). In effect, it can be assumed that at the end of this time interval, one measurement, with an appropriately reduced noise covarianee, is processed. With theassumption that the presmoothing yields an effective measureent that is equal to the sample mean of the N previous measurements, this reduced measurement-noise covariance is given by k/N, where the N measurements are statistically independent and ~ is the average value of the measurement-noise covariances for the times k=i+l, i+2 . . . . . i+N; for slowly varying noise statistics, /~ can be taken equal to R(i+N)in equation (9). (ii) The random disturbance w affects the covariance matrix propagated by equation (10) as though no measurements are filtered at the times k=-i-]-l, i - F 2 , . . . , i+N. For this assumption the effective random-disturbance covarianee over the time interval is Q(i)+Q(i+l)+ . . . +Q(i+N-1); for slowly varying noise statistics, this quantity is taken to be Q(i)N in equation (10). The above simplifying assumptions yield an approximation

Figures 1 and 2 illustrate the application of the piecewiserecursive Kalman filter to the problem of ballistic-trajectory estimation. A more extensive set of examples is contained in Ref. [4]. Figures 1 and 2 pertain to the trajectory estimation of a ballistic vehicle, having a large ballistic coefficient, that is tracked from an altitude of about 205,000 ft and impacts approximately at the radar site. The figures compare the perfon'nanee of the piecewise-recursive Kalman filter (curve ii) with the fully implemented Kalman filter (curve i). Position and velocity components in cartesian coordinates and the ballistic coefficient are estimated by the filters. These estimates are based upon noise-corrupted radar measurements of position. For the example of Figs. 1 and 2, the rms value of the measurement noise is initially about 2000 ft; after 10 sec, it is about 400 ft. Plotted in Fig. 1 are the magnitudes of the estimation errors in position and velocity and the actual and estimated ballistic coefficient; plotted in Fig. 2 are the standard deviations of these errors, which are obtained from the covarianee matrix computed by the filter equations, From Fig. 1 it can be seen that there is little degradation in the performance of the piecewise-recursive Kalman filter and, as shown by computer simulation, this filter has a computational speed that is about four times faster than the fully implemented Kalman filter--for this case, where $0~15 and N=10, equation (11) yields S~v~3.8, which is in close agreement with the above result. Hence, as indicated by this example, it is possible to obtain large improvements in computational speed with little sacrifice in estimation accuracy by using this filtering method. The results in Fig. 2 show that even when the covarianee matrix for the pieeewiserecursive Kalman filter is updated 10 times less frequently than that of the fully implemented Kalman filter) the results are very comparable. This demonstrates that the simplifying assumptions (i) and (ii), which am used in equations (9) and (10) for propagating the state covariance matrix, are reasonable approximations.

4. Conclusions A simplified filtering algorithm, the piecewise-recursive Kalman filter, has been described that yields a substantial improvement in computational speed with negligible degradation in performance relative to the fully implemented Kalman filter. The performance of this filter has been demonstrated on the problem of ballistic-trajectory estimation. The use of a piecewise-linear weighting matrix in the filter, rather than a piecewise-constant weighting matrix as described above, should yield further improvement in the filter's computational speed for comparable performance by allowing larger values of N=A~:/At.

References [1] R. E. KALMAN: A new approach to linear filtering and prediction problems. Trans. ASME, J. Bas Engng 35-45 (March 1960). [2] G. L. SMITH: Multivariable filter theory applied to space vehicle guidance. SIAMJ. Control2, 19-32 (1964). [3] R. E. LARSON,R. M. DR~SLF~ and R. S. RATNER: Precomputation of the weighting matrix in an extended

Brief Papers

479 2000

2OOO

I

t

I

i,'~

n,o

n-

IOOO

,,, lO00 Z 0

z o

O3

II

2 0

I

[

]

}

0

I

5

]

1

I

I

I

I

I0 TIME-

I

I

I

I

I

I

]

15

i

I

,

0

20

,

i

5

i

,

,

,

i

IO TIME ~

sec

I

i

i

[

]

15

20

sec

(a)

(a)

5000

5000 'i k

~ 4000 -

4000

I

3000

n-" uJ >- 2 0 0 0 t-

>_ 2000 -

I000

~ I000= J uJ >

...I U.I

>

0

I I

I I I

I I I I II

5

I0

0

TIME

I[11 --

I-k//~

15

20

~ ~ ii

i~'k , 0

;

I

~

O

I

I

I

,

I

5

sec

[

I

I

I0 TI ME

- -

I

I

15

I

'1~1

i

20

sec

(b)

(b)

3ooo~ N

. ~

/

0

~ - A C T U A L

5

I0 TIME--sec

15

20

(c) (i)

A t = 0.1 sec, and A T = 0.1 sec

(ii)

A t = 0,1 sec, and A ' r = 1.0 sec

Fie. I. Estimation errors.

FIG. 2. Standard deviations.

Kalman filter. Preprints 1967 JACC, 634-645 (June 1967). [4] R. M. DgF.ssI~R and D. W. Ross: Real-time implementation of the Kalman filter for trajectory estimation. Memorandum 36, Contract DA-01-021-AMC-90006(Y), SRI Project 5188-305, Stanford Research Institute, Menlo Park, California (June 1968).

filtre 6tendu, recurrent par segments de Kalman, assure une am61ioration sensible ¢n ce qui concerne la rapidit6 du calcul sans une diminution appr6ciable des performances en comparison avec le filtre 6tendu, pleinement pourvu, de Kalman. Les performances de ce filtrc sent iUustr6es par l'application des r6sultats au probl6me d'estimation d'un¢ trajEctoir¢ ballistique.

R ~ C e t article d6crit uno technique qui peut etre ufdis6e pour modifier et approximer les calculs de covariance et de la matrice de pond6rations du filtre 6tendu de Kaiman. L'algorithme simplifi6 qui en resulte, portant le n o m de

Z n m m m m f a . ~ m g ~ B e s c h r i c b c n wird eine Technlk, die bcmtlht werden kann, u m die Berechnung der Covarianzund Gcwichtsmatrizcn des erweiterten KALMAN-Filtcrs zu modifiziercn und zu approximiercn. Der resultiercnde

480

Brief Papers

vereinfachte Algorithmus, bezogen auf das sttickweise rekursive erweiterte KALMAN-Filter ergibt eine bedeutsame Verbessertmg der Rechengeschwindigkeit ohne merkliche Verminderung der Leistungsftihigkeit relativ zu dem voll eingesetzten erweiterten KALMAN-Filter. Die Wirkungsweise dieses Filters wird durch die Anwendung der Ergebnisse auf das Problem der Schtitzung ballistischer Trajektorien veranschaulicht.

Pe3mMe---HacTosttlaa CTaTbR OHI'ICblBaeT TeXHHKy KOTOpaa Mo~KeT 6r~ITb ltcrlosIh3OBaHa }IJIg H3MeHeHHg H //Jig np146nH~KeHHg paccqeTOB KoBapHaHTHOCTH 14 BeCOBO~ MaTpHtl~ pacmHpeHHOrO ~HnbTpa KaYlbMaHa. Bl,I T e K a i O ~ a3 3TOFO yIIpOII~eHHbIl~ aYlrOpHTM, Ha3BaHI~Ifi paccIIIHpeHHbIM KyCOtlHO-peKyppeHTHbIM d~llII/,TpOM Ka~bMaHa, I/pHHOCMT 3Ha~IRTeJIbHOe yny~merme B OTSOtUeHrmCKOpOCTHpaccqeTa ~e3 qyBCTBI4TeYIBHOFO yMeHblIIeHH~t KaqeCTBa pa6oTbl rIO cpaBHeHHIO C pacmFlpeHHbIM, IIO.rIHblM, qbHJIbTpOM Ka.llbMaria. Pa6oTa 3TOFO qbH.FIbTpa H.rI.JIIOCTpHpyeTCfl laprlMeHeHHeM pe3yJlbTaTOB K rlpo{5.rieMe oIIettKH 6anl,ICTHqeCKOi:i TpaeKTopHH.