Advances in Derivative-Free State. Niels K. Poulsen? 2800 Lyngby, Denmark. Abstract

Transcription

1 Advances in Derivative-Free State Estimation or Nonlinear Systems Magnus N rgaard?} mn@iau.dtu.dk Niels K. Poulsen? nk@imm.dtu.dk Ole Ravn } or@iau.dtu.dk? Deartment o Mathematical Modelling } Deartment o Automation echnical University o Denmark 800 Lyngby, Denmark echnical reort: IMM-REP Aril 7, 000 REVISED EDIION Abstract In this aer we show that it has considerable advantages to use olynomial aroximations obtained with an interolation ormula or derivation o state estimators or nonlinear systems. he estimators become more accurate than estimators based on aylor aroximations yet the imlementation is signicantly simler as no derivatives are required. hus, it is believed that estimators derived in this way can relace well-known lters, such as the extended Kalman lter (EKF) and its higher order relatives, in most ractical alications. In addition to roosing a new set o state estimators, the aer also unies recent develoments in derivative-ree state estimation. Introduction When it comes to state estimation or nonlinear systems there is not a single solution available that clearly outerorms all other strategies. A series o estimators have been roosed over time, which or the most art are nonlinear extensions o the celebrated Kalman lter. For each alication one thereore has to ick the estimator

2 Introduction which is ound to best trade o various roerties such as estimation accuracy, ease o imlementation, numerical robustness, and comutational burden. U to now the extended Kalman lter (EKF) [GKN 74], [May8], [Lew86] has unquestionably been the dominating state estimation technique. he EKF is based on rst-order aylor aroximations o state transition and observation equations about the estimated state trajectory. Alication o the lter is thereore contingent uon the assumtion that the required derivatives exist and can be obtained with a reasonable eort. he aylor linearization rovides an insuciently accurate reresentation in many cases, and signicant bias, or even convergence roblems, are commonly encountered due to the overly crude aroximation. Several estimation techniques are available that are more sohisticated than the EKF, e.g., re-iteration, higher order lters, and statistical linearization [GKN 74], [May8]. he more advanced techniques generally imrove estimation accuracy, but it haens at the exense o a urther comlication in imlementation and an increased comutational burden. In this aer we roose a new set o estimators, which are based on olynomial aroximations o the nonlinear transormations obtained with articular multidimensional extension o Stirling's interolation ormula [Ste7], [Fr 70]. Concetually, the rincile underlying the new lters resembles that o the EKF and its higher order relatives. he imlementation is, however, quite dierent. In contrast to the aylor aroximation no derivatives are needed in the interolation ormula only unction evaluations. his accommodates easy imlementation o the lters, and it enables state estimation even when there are singular oints in which the derivatives are undened. Although the imlementation is less comlicated than or lters based on aylor aroximations, the comutational burden will oten be comarable in size or only slightly bigger. Additionally, under certain assumtions on the distribution o the estimation errors, the new lters rovide a similar or even suerior erormance. Recently there has been interesting develoments in derivative-ree state estimation techniques [JU94], [JUDW95], [JU97], [Sch97]. It is shown in the aer that these lters occur as secial cases o lters based on the interolation ormula. he lter described in [Sch97] corresonds to a subotimal imlementation o the lter derived using rst-order aroximations while the lter roosed in [JU94], [JUDW95] has the same a riori state estimate and a related (but less accurate) covariance estimate as the lter derived using second-order aroximations. Due to these relationshis we have ound it natural to adot some o the ideas on ractical imlementation suggested in [Sch97] and to analyze the erormance o the lters by using the same aroach asin[ju94]. he aer is organized as ollows. First weintroduce Stirling's interolation ormula and discuss under which circumstances it can rovide more accurate aroximations than aylor's ormula. A multidimensional extension o the interolation ormula is made, and it is discussed how it can be used or aroximation o mean and covariance o stochastic variables generated by nonlinear transormation o stochastic variables with known mean and covariance. Based on the obtained results, two new

3 Power Series Revisited 3 lters are roosed. he DD lter is based on rst-order aroximations and the DD lter is based on second-order aroximations. he erormance o the new lters are demonstrated on a benchmark examle. Readers only interested in the actual lter imlementation may choose to ski Section and Section 3. Power Series Revisited his section deals with olynomial aroximations o arbitrary unctions. In articular we will comare aroximations obtained with aylor's ormula, which commonly underlies lters or nonlinear systems, with aroximations obtained with an interolation ormula. Initially, unctions o only one variable will be considered. Later the treatment is extended to multile dimensions. I the unction is analytic we can reresent it by its aylor series exanded about some oint, x =x (x) =(x) 0 (x)(x ; x) 00 (x)! (x ; x) (3) (x) (x ; x) 3 ::: () 3! A commonly used aroximation is obtained by truncating the series ater a nite number o terms. As more terms are included, a locally better aroximation is achieved since the remainder (the sum o high-order terms) converges as O(jx;xj n ) (this holds even when is not analytic). he rincile o the aylor series is that the aroximation inherits still more characteristics o the true unction in one articular oint as the number o terms increases. Although the assumtion that is analytic imlies that any desired accuracy can be achieved rovided that a sucient number o terms are retained, it is in general adviced to use a truncated series only in the roximity o the exansion oint unless the remainder term has been roerly analyzed. Several interolation ormulas are available or deriving olynomial aroximations that are to be used over an interval. Most o these do not require derivatives but are instead based on a nite number o evaluations o the unction. Usually it is thereore much simler to derive aroximations with these ormulas. Several textbooks are available that deal with interolation, e.g., [DB74], [Ste7], [Fr 70]. In the ollowing we will consider one articular ormula, namely Stirling's interolation ormula. Let the oerators and erorm the ollowing oerations (h denotes a selected interval length) (x) = (x h ) ; (x ; h ) () (x) = (x h )(x ; h ) : (3) With these oerators Stirling's interolation ormula used around the oint x =x

4 Power Series Revisited 4 can be exressed as [Fr 70] (x) =(x h) = (x) (x)! (x) 3 (x) 3 ( ;) 4 (x) 5 (x) ::: 4! 5 (4) Commonly, ; <<, but in our alication we will allow occasional use outside this interval as we shall see in later sections. In this aer the attention is restricted to rst and second-order olynomial aroximations. he ormula (4) is in this case articularly simle where (x) (x) 0 DD DD (x)(x ; x) (x) 00 (x ; x) (5)! 0 h) ; (x ; h) DD (x) =(x h 00 h) (x ; h) ; (x) DD (x) =(x : (6) h One can be interret (5) as a aylor aroximation with the derivatives relaced by central divided dierences. o assess the accuracy o the aroximation it is useul to insert the ull aylor series () in lace o (x h) and (x;h). Wemust assume that is analytic to carry out this analysis (x) 0 00 DD DD (x)(x ; x) (x) (x ; x) =! (x) 0 (x)(x ; x) 00 (x) (x ; x)! (3) (x) 3! h (5) (x) 5! h 4 ::: (x ; x) (4) (x) h (6) (x) h 4 ::: (x ; x) : 4! 6! (7) he rst three terms on the right hand side o (7) are indeendent otheinterval length, h, and are recognized as the rst three terms o the aylor series exansion o. he remainder term given by the dierence between (7) and the second-order aylor aroximation is controlled by h and will in general deviate rom the higher order terms o the aylor series exansion o. As we shall see in the ollowing section, the ossibility o controlling the remainder term is what makes the interolation ormula more attractive thanaylor aroximation in some alications. Certain interval lengths can ensure that the remainder term in some sense will be close to the higher order terms o the ull aylor series. Fig. shows a tyical examle on the dierence between a aylor aroximation and an aroximation obtained with the interolation ormula. We will now roceed with the multidimensional case. Let x be a vector, x R n,and let y = (x) be a vector unction. here are dierent ways in which the interolation

5 Power Series Revisited 5 wo olynomial aroximations o the same unction (x) x Figure. Comarison o a second-order olynomial aroximation obtained with aylor's ormula and one obtained with the interolation ormula. he exansion oint is x = :5 and or the interolation ormula the interval length was selected to h =3:5. he solid line shows the true unction, the dot-dashed line is the secondorder aylor aroximation while the dashed line is the aroximation obtained with the interolation ormula. Obviously, the aylor olynomial is a better aroximation near the exansion oint while urther away the error is much higher than or the aroximation obtained with the interolation ormula. ormula can be extended to multile dimensions but beore addressing this recall rst that the multidimensional aylor series exansion o about x =x is given by y = (x x) = i=0 i! Di x = (x) D x! D x 3! D3 x ::: where the oerator descrition emloyed by [JU94] has been adoted: i D i @ x x x n (x) @x n he oerators can also be written: D x = D x x q= x x q x=x x=x (8) (0)

6 3 Aroximation o Mean and Covariance 6 By again restricting our attention to second-order olynomials we will write the multidimensional interolation ormula as y (x) ~ D x! As the divided dierence oerators, ~ D x ~ D x,wewilluse ~D x = h ~D x = h ~D x: () x! (x) () (x ) q= q6= x x q ( )( q q ) where has been introduced as the artial dierence oerator! (x) (3) (x) =(x h e ) ; (x ; h e ) (4) and e is the th unit vector. A similar extension was made o the average oerator. he ormula () is just one examle o a multidimensional extension o the interolation ormula. o illustrate how others can be derived, the ollowing linear transormation o x is introduced: and the unction ~ is dened by z = S ; x (5) ~ (z) (Sz)=(x) : (6) While the aylor aroximation o ~ is identical to that o, itisobviously not the case that the multidimensional interolation ormula () yields the same results or and ~. Since ~ (z) = ~ (z he ) ; ~ (z ; he )=(x hs ) ; (x ; hs ) (7) where s denotes the th column o S, D ~ x and D ~ x will clearly deviate rom ~D ~ z and D ~ ~ z. In the ollowing section we are going to use the interolation ormula in a stochastic ramework. In this case a articularly useul choice o transormation matrix (S) and interval length (h) exists. 3 Aroximation o Mean and Covariance Let x be a vector o stochastic variables or which the exectation and covariance are available x = E[x] P x = E (x ; x)(x ; x) : (8)

7 3 Aroximation o Mean and Covariance 7 We would now like to determine y = E[(x)] (9) (P y ) = E ((x) ; y )((x) ; y ) (0) (P xy ) = E (x ; x)((x) ; y ) : () As is nonlinear we cannot rely on being able to calculate the exact exectations. Instead it is customary to insert a rst or second-order olynomial aroximation in lace o beore taking the exectations. In this section we will ocus on estimates o the exectations obtained using the interolation ormula in () or aroximation o. Additionally, we shall nd it articularly useul to work with a linear transormation o x as described above. he transormation matrix is selected as a square Cholesky actor o the covariance matrix [Sch97]: z = S ; x x P x = S x S x : () his transormation is sometimes said to erorm a stochastic decouling o the variables in x as the elements o z become mutually uncorrelated (and each with unity variance): E (z ; E[z])(z ; E[z]) = I: (3) We shall in the ollowing use a rather wide interretation o the so-called Cholesky actorization. For any symmetric matrix roduct M = SS we will reer to S as a Cholesky actor. hus, the Cholesky actor need not be square and triangular. However, most oten a triangular Cholesky actor is considered as comutationally ecient methods are available or erorming such actorizations. In the ollowing subsections we shall work with (z) ~ directly as this is most convenient. A ew assumtions on ~ () andz will be invoked. ~ must in rincile be dened or all z R n and the elements o z = z ; E[z] are assumed to belong to the same (zero mean) distribution. In Section 3. it is additionally assumed that z is Gaussian. For analysis uroses it is in Section 3.3 assumed that ~ is analytic and that z is Gaussian. It should be stressed, however, that it is not necessary or ~ to be analytic to aly the estimators. 3. A First-order Aroximation First estimates o mean and covariance will be derived by relacing the unction ~ by a rst-order aroximation y = ~ (z z) ~ (z) ~ D z ~ : (4) As the exectation E[z] =0by denition, the exectation o (4) is y = E[ ~ (z) ~ D z ~ ]= ~ (z) =(x) (5)

8 3 Aroximation o Mean and Covariance 8 An estimate o the covariance (0) is derived along the same lines. As beore, the rst-order moments can be neglected since z is zero mean. Moreover, the cross-terms evaluate to zero as z has been generated so that the cross-correlations E[z i z j ]=0 i6= j. P y = E = E = E = = 4h ~(z) ~ D z ~ ; ~ (z) ~D z ~ (z) X 4 n ~D z ~ (z) z ~ (z) ~ (z)! n X ~ (z) ~ (z) D ~ ~ z ; (z) ~ z ~ (z)! 3 5 ~ (z he ) ; (z ~ ; he ) ~ (z he ) ; (z ~ ; he ) : (6) We shall denote the ith moment o an arbitrary elementinz by i. As all elements are assumed to be equally distributed their moments are obviously identical. As discussed above, =. Higher moments deend on the distribution o z. Recalling that ~ (z he )=(x hs x ),wheres x is the th column o the square Cholesky actor o the covariance matrix S x, (6) can also be written P y = 4h ((x hs x ) ; (x ; hs x )) ((x hs x ) ; (x ; hs x )) (7) he estimate o the cross-covariance matrix can be derived a long the same lines P xy = E (x ; x) ~(z) D ~ ~ z ; (z) ~ = E (S x z) ~D ~ z = E X 4 n = " n X = h s x z s x ~ (z) z ~ (z) #! 3 5 s ~ x (z he ) ; (z ~ ; he ) (8)

9 3 Aroximation o Mean and Covariance 9 which we can also write P xy = h P n s x ((x hs x ) ; (x ; hs x )) (9) It is not clear rom the derivations how theinterval length, h, should be selected. he mean estimate is indeendent o the arameter while it has an obvious imact on the estimate o the covariance matrices. In Section 3.3 covering the analysis o the estimates it is shown that the otimal setting o h is dictated by the distribution o z. It turns out that h should equal the kurtosis o the distribution, h = A Second-order Aroximation More accurate estimates o mean and covariance o ~ can be obtained with a limited extra eort by aroximating the unction with a second-order olynomial derived with the interolation ormula: y (z) ~ D ~ ~ z ~D z ~! = (z) ~ z ~(z) h h (z ) q= q6= z z q ( )( q q )! ~(z) : (30) o obtain useul results the assumtions on z will now beslightly more restrictive as we demand that it is Gaussian. Since z is zero mean and the elements are uncorrelated, this new assumtion imlies that the elements are indeendent and the distribution is symmetric. he assumtion is not needed or derivation o the mean estimate, but it is imortant when deriving the imroved covariance estimate. Utilizing that z is zero mean and its elements are uncorrelated, the exectation o ~ can be estimated by y = E " ~(z) = ~ (z) = ~ (z) h ~ (z) = h ; n h ~ (z) h (z )! ~(z) # ~ (z he ) (z ~ ; he ) ; n (z) ~ h ~(z he ) (z ~ ; he ) (3)

10 3 Aroximation o Mean and Covariance 0 m y = h ; n h (x) h (x hs x )(x ; hs x ) (3) We will now roceed with a derivation o a covariance estimate. First we observe that (P y ) = E[(y ; y)(y ; y) ] = E[(y ; ~ (z))(y ; ~ (z)) ] ; E[y ; ~ (z)]e[y ; ~ (z)] : (33) he estimate can thereore be written P y = E = E " ; E ~D ~ z ~D z ~ ~D z ~ ~D z ~ ~D z ~ ; 4 E h ~ D z ~ ~D z ~ # ~D ~ z ~D ~ z E ~D ~ z ~D ~ z 4 E i E h ~D z ~ ~D z ~ ~D z ~ i : (34) he second ste was taken by using the act that all odd order moments cancel as the elements o z are indeendent and the distribution symmetric. he rst term in (34) is recognized as the covariance based on a rst-order aroximation o ~ and has already been dealt with. Let us instead take a closer look at the two remaining terms: E E E h ~D z ~ ~D ~ z is comosed o 3 kinds o terms E E z i z j i i j j ~ (z i ) ~ i (z i ) ~ i (z i ) i ~ (z j ) j ~ z i z j i i j j ~ i h ~D ~ z E ~D ~ z i is comosed o kinds o terms E E h i h (z i ) ~ i E (z i ) ~ i i = h (z i ) i ~ i E h (z j ) j ~ i = = = = ~ i ~ i 4 (35) i ~ j ~ (36) i i j j ~ ~ i ~ i i ~ i i j j ~ : (37) j ~ : (39) (38)

11 3 Aroximation o Mean and Covariance All o the above terms aear or 8i 8j i 6= j. he terms in (36) and (39) are identical and cancel when subtracted. Additionally, we will discard the terms containing cross-dierences (37). his is done because their inclusion would lead to an excessive increase in the amount o comutations as the number o such terms grows raidly with the dimension o z. Moreover, the terms each require our additional evaluations o or each dimension. he reason or not considering the extra eort worthwhile is that we are unable to cature all ourth moments anyway. his would require that was aroximated by a third-order olynomial (more details on this are given in Section 3.3). hus, we arrive at the ollowing covariance estimate ~ (z) P y = = 4h 4 ; 4h 4 ~ (z) 4 ; 4 ~ (z) ~ (z) ~(z he ) ; (z ~ ; he ) ~(z he ) ; (z ~ ; he ) ~ (z he ) (z ~ ; he ) ; (z) ~ ~(z he ) (z ~ ; he ) ; (z) ~ : (40) Inserting that =and setting h = 4 (=3or a Gaussian distribution) give P n [(x hs x ) ; (x ; hs x )] [(x hs x ) ; (x ; hs x )] P y = 4h h ; 4h 4 P n [(x hs x ) (x ; hs x ) ; (x)] [(x hs x )(x ; hs x ) ; (x)] (4) As 4 ; = E[(z)4 ] ; E[(z) ] = Var[(z) ] > 0 (4) 4 or all robability distributions. hereore, we should always select h. Obviously, this imlies that the covariance estimate will always be ositive semideinite. he cross-covariance estimate, P xy, turns out to be the same as when the rst-order aroximation is emloyed (9): P xy = E = E " = h (S x z) ~D z ~ (S x z) ~D ~ z # ~D ~ z s x ((x hs x ) ; (x ; hs x )) : (43)

12 3 Aroximation o Mean and Covariance 3.3 Analysis o the Aroximations In this section the erormance o the roosed mean and covariance estimators will be evaluated. he analysis roceeds according to the aroach emloyed in [JU94]. hat is, under the assumtion that z is Gaussian and the unction is analytic, the aylor series o the true mean and covariance are comared on a term-by-term basis with the aylor series exansion o the estimators. he derivative oerator, D i z, has already been introduced in (9): D i z ~ =! z (z) z=z : (44) Additionally, the ollowing artial derivative oerator will be useul during the analysis: It is not dicult to see that h E D i he ~ = h i r i ~ = h ~ i D i he ~ = h i; h D i z ~ i = i z=z : (45) r i ~ (46) r i ~ [cross-terms i i 4] : (47) It was mentioned reviously that the Gaussian assumtion imlies that the elements o z are mutually indeendent and that the distribution o z is symmetric. hus, all odd moments evaluate to zero in (47). he cross-terms are terms containing roducts o derivatives w.r.t. dierent variables and terms containing cross-derivatives. In a similar ashion we can evaluate the roducts: h E D i he ~ D i z ~ D j he ~ D j ~ z = h ij; = ij r i ~ r j ~ (48) r i ~ r j ~ [cross-terms i i j 4] : (49) For the reasons called attention to above, (49) evaluate to zero or i j odd. I, or a moment, we neglect the cross-terms in (47) and (49), the dierence between the air (46), (48) and the air (47), (49) is or the even terms alone given by the discreancy between h ij and ij.as z is Gaussian we have that[pa84] i =3(i ; ).hus, i the momentgrows actorially with i. As =

13 3 Aroximation o Mean and Covariance 3 we have i = :::g. In the second-order case (i.e., i =or i = j =, resectively) the terms will agree regardless o the choice o h. I we select h as the kurtosis, h = 4 =3, the terms will also agree in the ourth-order case (excet or the cross-terms, which remain unmatched). In the higher order cases, (46) and (48) will underestimate (47) and (49), resectively, ash (ij) grows geometrically and thereore will be exceeded by ij rom the sixth order. Series exansion o the true quantities First the aylor series exansion o the true exressions or mean and covariances (9), (0), () are determined. As the aylor series o ~ exanded around z =z is given by y = (z) ~ D i z ~ (50) (i)! we have or the true mean y = E[y] = ~ (z) E = ~ (z) For the true covariance we get " i (i)! # D i ~ z (i)! (P y ) = E[(y ; ~ (z))(y ; ~ (z)) ] ; E[y ; ~ (z)]e[y ; ~ (z)] " # r i ~ [cross-terms i i 4]: (5) " D i z = E ~ (D j z ) ~ ; E i! j! j= " = E D ~ z D ~ z D ~ z (D 3 ~ z ) 3! " # " # D z ; E ~ D z E ~ = i=0 j=0! (ij)! (i )!(j )! j= (ij) ; i j (i)!(j)! D i ~ z (i)! # E " D z ~ (D z ~ )! r i ~ r j ~ r i ~ r j ~ D i ~ z (i)! # D3 z ~ (D z ~ ) [cross-terms i i j 4] (5) while or the cross-covariance we have i (P xy ) = E h(x ; x)( (z) ~ ; y ) 3! #

14 3 Aroximation o Mean and Covariance 4 i = E h(x ; x)( (z) ~ ; ~ (z)) = E = E = " S x z S x z s x X i=0 i=0 D z ~ (D i z ~) (i )! E!# i (i )! ri ~ " S x z(d 3 z ~ )! (3)! # [cross-terms i i 3] : (53) Series exansion o the mean estimates he mean estimate based on the rst-order aroximation o ~ (5) is simly the rst term o the aylor series: y = ~ (z) (54) while the aylor series exansion o the mean estimate based on the second-order aroximation in (3) is y = h ; n h ~ (z) h = h ; n h ~ (z) h = ~ (z) h X = ~ (z) h i; (i)! ~(z he ) ~ (z ; hz ) D i he ~ (z) (i)! ~ (z) i=0 D i he ~ (z) i!! (;D he ) i (z) ~ i! r i ~ : (55) he estimate based on a rst-order aroximation (54) is the same as i we had used an ordinary aylor linearization o. hat is, the aroximation error equals the second and higher order terms in the series exansion o the true mean (5). For the estimate based on the second-order aroximation we have the ollowing aroximation error or element k (obtained by subtracting (55) rom (5)): R (k) = i=3 i ; h i; (i)! r i ~ k cross-terms: (56) Notice that the outer sum starts in i =3as h = 4.Fourth-order derivatives are still resent in the cross-terms, however. It is interesting to comare this aroximation

15 3 Aroximation o Mean and Covariance 5 error to the error o a mean estimate obtained by emloying a second-order aylor aroximation o ~ as this is the traditional aroach: R (k) = i= i (i)! r i ~ k cross-terms: (57) In the general case it is not ossible to conclude that j R (k)j always will be smaller than jr (k)j as the various derivatives can take any sign. However, one thing that can be said is that the magnitude o R (k) will be bounded rom above by jr (k)j M = while j R (k)j will be bounded by j R (k)j M = i=3 i= i (i)! i ; h i; (i)! r i ~ k jcross-termsj (58) r i ~ k jcross-termsj : (59) As h i; < i 8i 3 we have that M M. he equality sign holds only when all the sums o derivatives in (58), (59) are 0. hus, in general j R (k)j has a lower uer bound than jr (k)j. o get an imression o the magnitude o the uer bound we observe that (recall that =, h = 4 =3, i =3(i ; ) i ): = (i)! h i; = (i)! i 4 6 i = 0:5 0:5 0:008 0:006 :::g (60) ::: = 0:5 0:5 0:05 0:00067 :::g : (6) Both ractions decay raidly with i. Esecially the ractions in (6) as the numerator in this case does not grow actorially. It is thereore reasonable to assume that also i (i)! r i ~ k (6) tyically will decay raidly with i and that the rst ew terms o the sum in (58) will dominate. I the uer bounds, M M, are not dominated by the cross-terms, M M as i;h i; P n (i)! ri ~ k is 0 or i =and less than hal the size o (6) or i =3. Recall that in the one-dimensional case there are no cross-terms. In this case errors are not introduced until the terms o order 6 i.e., a sixth-order aylor aroximation o ~ would be necessary to achieve a better accuracy than what is oered by (55).

16 3 Aroximation o Mean and Covariance 6 Series exansion o the covariance estimates he same aroach asabove will now be used or assessing the accuracy o the covariance estimates. Note rst that ; ~(z he ) ; ~ (z ; he ) = D i ~ i he ;Dhe ; ~ h h i! i! ~(z he ) (z ~ ; he ) ; ~ (z) = h = h = i=0 i=0 i=0 D (i) he ~ (i )! D i he ~ (i)! D i he ~ i! ; ;Dhe i ~ i! (63) : (64) hus, when inserting the aylor series in the estimate based on the rst-order aroximation (6) the ollowing is obtained P y = h X i=0 = h = h i=0 j=0 j=0 D he ~ D (i) he ~ (i )! D ~ he D he ~ (D 3 he ~ ) h (ij) 3! (i )!(j )! D (j) ~ he (j )! D 3 he (D ~ he ~ ) 3!! r i ~ r ~ j : (65) Similarly, we get or the estimate based on the second-order aroximation (4): P y = h = h h ; h 4 i=0 D i he ~ (i )!! i=0 D i he ~ (i)! D i he ~ (i)! D ~ he D ~ he!! D i he ~ (i )! D i he ~ (i)! D i he ~ (i)!!!!

17 3 Aroximation o Mean and Covariance 7 = h ; h 4 i=0 j=0 D ~ he (D 3 ~ he ) D he (D ~ he ) ~ D 3 he (D ~ he ~ ) 3! (!)(!) 3! D he ~ (D he ~ ) h (ij) (!)(!) (i )!(j )! j=! h (ij); ; h i; h j; (i)!(j)! r i ~ r j ~ r i ~! r j ~ : (66) As beore we will comare the new estimates with estimates obtained using aylor aroximations in lace o. ~ For convenience we shall rst look at the secondorder aroximation. he aroximation error or element (k l) in the covariance estimate obtained by emloying a second-order aylor aroximation in lace o ~ is Q (k l) = i=0 j6=0 j=0 i6=0 j6= (ij) (i )!(j )! j= i6= (ij) ; i j (i)!(j)! r i ~ k r j ~ l r i ~ k r j ~ l [cross-terms] : (67) he subscrits on the rst double sum mean that the case i = j = 0 is not included. Likewise, or the second double sum the case i = j =is not included. o allow a comarison, the terms containing roducts o second-order cross-derivatives have been discarded as (37) was discarded or comutational convenience (i.e., the terms are included in the cross-terms). It should be noticed that in the covariance estimate emloyed by the conventional second-order Gaussian lter these terms are usually calculated. In a similar ashion as above, by subtracting (5) and (66), it is ossible to write u the aroximation error or the covariance estimate based on the new second-order aroximation o : ~ Q (k l) = As j= j6= (ij) ; h (ij) (i )!(j )! j= i6= r i ~ k r j ~ l (ij) ; i j ; h (ij);4 (h ; ) (i)!(j)! r i ~ k r j ~ l [cross-terms] : (68) (ij) > (ij) ; h (ij) > 0 (ij) ; i j > (ij) ; i j ; h (ij);4 (h ; ) > 0 (69)

18 4 State Estimation or Nonlinear Systems 8 we can use the same argumentation as was alied to evaluate the mean estimates and conclude that jq (k l)j has a lower uer bound than jq (k l)j. he new covariance estimate is thereore better than i we had inserted a second-order ayloraroximation (without the cross-derivatives) o. ~ he missing ourth-order in (66) are the terms taking the orm @ q and ~ q. he last mentioned terms could have been resent in the estimate had the crossdierences (37) not been discarded rom the aroximation @z q Notice that or the one-dimensional case there are no cross-terms and all the sums are made over ositivenumbers. hus, one can in this case ski the jj. Additionally, errors will obviously not aear until in the sixth-order terms or the estimate (66). he aroximation error or the covariance estimate based on the divided dierence linearization o ~ is Q (k l) = j= (ij) ; h (ij) (i )!(j )! j= (ij) ; i j (i)!(j)! r i ~ k r j ~ l r i ~ k r j ~ l [cross-terms] (70) he aroximation error or the covariance estimate based on a aylor linearization o, ~ Q (k l), isidentical excet that the quantity h (ij) is not subtracted. Obviously, jq (k l)j will thereore have alower uer bound than jq (k l)j. he estimate will also have alower uer bound than the estimate suggested in [Sch97] as in this aer h =. For the estimate o the cross-covariance matrix, P xy,given by (8)wehave P xy = h s x X i=0 D i he ~ (i )!! = h s x X i=0 h ~! i (i )! ri : (7) he conclusions above are valid or this estimate as well. he errors are again introduced on ourth-order terms in the series as the cross-derivative s 3 q 6= q, do not aear in the series exansion o the estimate. However, unlike or the estimate based on a aylor aroximation, some o the ourth-order terms are matched with the new estimate. 4 State Estimation or Nonlinear Systems We have now arrived at the central issue o this note, namely state estimation or nonlinear systems. wo new lters will be suggested that are based on the reviously derived olynomial aroximations. he lters are undamentally dierent

19 4 State Estimation or Nonlinear Systems 9 rom lters based on aylor aroximations in that the olynomial aroximations underlying the new lters take into account the uncertainty on the state estimate. he aylor aroximation underlying conventional lter designs or nonlinear systems, such as the EKF, deends only on the current state estimate and not on its variance. Nevertheless, the new lters can generally be imlemented more easily as no derivatives are required. he rst lter we shall derive is based on a rst-order olynomial aroximation. his estimator is a generalized version o the lter resented in [Sch97]. Subsequently, a more accurate lter will be derived that also includes second-order terms. It turns out that this lter has certain similarities with the unscented lter described in [JU94], [JUDW95]. 4. Review o State Estimation or Nonlinear Systems Consider the ollowing general nonlinear model o a dynamic system whose states are to be estimated x k = (x k u k v k ) (7) y k = g(x k w k ) : (73) v k and w k are assumed i.i.d. and indeendent o current and ast states, v k (v k Q(k)) w k (w k R(k)). he commonly used state estimation rincile or nonlinear systems is briey outlined in the ollowing. In-deth treatments o the toic can be ound in [Lew86], [GKN 74], [May8]. Ideally, wewould like to determine the a riori state and covariance estimates like in the Kalman lter. hat is, as the conditional exectations x k = E[x k jy k; ] (74) P (k) = E (x k ; x k )(x k ; x k ) jy k; (75) where Y k; is a matrix containing the ast measurements Y k; = y 0 y ::: y k; : (76) For convenience, the measurement (aosteriori) udate o the state estimate is usually restricted to be linear in the measurements. Selecting the udate so that the (conditional) covariance o the estimation error is minimized, we obtain the ollowing [Lew86]: where K k = P xy (k)p ; y (k) (77) ^x k = x k K k [y k ; y k ] (78) y k = E[y k jy k; ] (79) P xy (k) = E (x k ; x k )(y k ; y k ) jy k; (80) P y (k) = E (y k ; y k )(y k ; y k ) jy k; : (8)

20 4 State Estimation or Nonlinear Systems 0 he corresonding udate o the covariance matrix is ^P (k) =E (x k ; ^x k )(x k ; ^x k ) jy k = P (k) ; K k P yy (k)k k : (8) As the various exectations generally are intractable, some kind o aroximation is commonly used e.g., it is well-known that the extended Kalman lter is based on aylor linearization o state transition and outut equations (7), (73). he EKF equations are listed below to allow the reader to comare its comlexity with that o the lters derived in the ollowing. A treatment o the second-order lters may be ound in [May8]. he state transition and observation equations are aroximated by rst-order olynomials x k (^x k u k v k )F x (k)(x k ; ^x k )F v (k)(v k ; v k ) (83) y k g(x k w k )G x (k)(x k ; x k )G w (k)(w k ; w k ) (84) where F x (k) u k v k G x (k) w x=x k x=^x k F v (k) k u k G w (k) k v=v k w= w k : (85) When these aroximations are inserted we arrive at [Lew86]: A riori udate: x k = (^x k u k v k ) (86) y k = g(x k w k ) (87) P (k ) = F x (k) ^P (k)f x (k) F v (k)q(k)f v (k) (88) A osteriori udates: K k = P (k)g x (k) G x (k) P (k)g x (k) G w (k)r(k)g w (k) ; (89) ^x k = x k K k [y k ; y k ] (90) ^P (k) = [I ; K k G w (k)] P (k) (9) In the ollowing subsections we will ursue the use o aroximations obtained with the interolation ormula or derivation o state estimators or nonlinear systems. 4. he DD Filter In this section a generalized version o the nonlinear state estimation scheme suggested in [Sch97] will be described. he lter is derived by emloying the rst-order

21 4 State Estimation or Nonlinear Systems aroximation resented in Section 3.. In rincile this corresonds to the EKF excet that the Jacobians (85) are relaced by divided dierences. he state udate is thereore the same as in the extended Kalman lter. he dierence is alone ound in the udate o the various covariance matrices. Generally, they can be imlemented more easily. We will use an aroach much like the one suggested in [Sch97]. One o the articularly useul ideas rovided in this aer is to udate the Cholesky actors o the covariance matrices directly. First we will introduce the ollowing our square Cholesky actorizations Q = S v S v P = S x S x R = S w S w ^P = ^S ^S : (9) x x Let the jth column o S x be denoted s x j and vice versa or the other actors. Four matrices containing divided dierences are now dened by S () x^x (k) = n S () x^x (i j) o = ( i (^x k h^s x j u k v k ) ; i (^x k ; h^s x j u k v k ))=hg S () xv (k) = S () xv (i j) = S () yx (k) = n S () yx (i j) o ( i(^x k u k v k hs v j ) ; i (^x k u k v k ; hs v j ))=hg = (g i (x k hs x j w k ) ; g i (x k ; hs x j w k ))=hg S () yw (k) = S () yw (i j) = (g i(x k w k hs w j ) ; g i (x k w k ; hs w j ))=hg : (93) he a riori udate o understand how the results rom Section 3. can be alied in a state estimation context it is useul to think o an augmented state vector consisting o state vector and rocess (or measurement) noise: ^x x x = x x = : (94) v v As the rocess noise is assumed to be indeendent o the state, the (conditional) covariance o x is ^P 0 ^S ^P x 0 ^S x 0 x = = = 0 Q 0 S v 0 S ^S ^S : (95) x x v Introducing the vector z by stochastical decouling o x, x = S x z, it is not dicult to see how the state estimation roblem can be maed into the treatment othe general vector unction ~ (z), which was resented in Section 3.. For the a riori udate o the state estimate we will use (5): which is the same as or the EKF. x k ~ (z k )=(^x k u k v k ) (96)

22 4 State Estimation or Nonlinear Systems As the basis o the covariance udate we shall use (7). By alication o the matrices dened in (93) the udate can obviously be exressed in the ollowing matrix notation P (k ) = h S () x^x (k) S() xv (k) ih S () x^x (k) S() xv (k) = S () x^x (k) S () x^x (k) S () xv (k) ; S () xv (k) : (97) Due to the assumed indeendence between v k and x k, the udate can be written as a sum o two matrix roducts. It is well-known that a straightorward text-book imlementation o the (extended) Kalman lter results in numerical roblems ater a number o iterations as the eect o round-o errors accumulates, thus making the covariance matrix asymmetric and non-ositive denite. he usual remedy or this is to use a actored udate. As the covariance udate (97) is a sum o two quadratic terms, numerical roblems o this kind should not occur with this udate. Nevertheless, it is temting to use a actored udate anyway since the actor will be needed or the a osteriori udate. Moreover, the (rectangular, nontriangular) Cholesky actor is immediately available as the ollowing comound matrix: S x (k )= h S () x^x (k) S() xv (k) his is a rectangular matrix and or later use it must be transormed to a square Cholesky actor. his can be achieved through Householder triangularization [GA93], [GvL89]. i i (98) he a osteriori udate he a riori estimate o outut and covariance matrix or the outut estimation error is derived in a similar ashion. he outut estimate is given by y k = g(x k w k ) (99) and the comound matrix S y (k) = h S () yx (k) S () yw (k) i (00) is a Cholesky actor o the covariance o the outut estimation error, P y (k) =S y (k)s y (k) : (0) As or S x, S y (k) should be transormed to a quadratic matrix by Householder triangularization.

23 4 State Estimation or Nonlinear Systems 3 For aroximation o the cross-covariance between state and outut estimation error we will use the result in (9) P xy (k) = S x (k) he Kalman gain can now be calculated according to (77) S () yx (k) : (0) K k = P xy (k) S y (k)s y (k) ; (03) and the state vector is udated according to to (78) ^x k =x k K k (y k ; y k ) (04) he actorization o P y has deliberately been maintained in (03) because it is useul in the ractical comutation o the gain. Since S y is triangular the equation Sy (k)s y (k) K k = P xy (k) is easily solved using only orward and back substitutions. he aosteriori covariance can be udated according to (8). However, as suggested in [Sch97] one can also in this case udate its Cholesky actor directly. As the ollowing exressions are identical ; KP y K = S x S () yx K = KS () yx S x = KS () yx ; S () yx K KS () yw the a osteriori udate can clearly be rewritten as ; S () yw K ^P = P ; KP y K = P ; KP y K ; KP y K KP y K = S x S x ; S x S () yx K ; KS () = S x ; KS () yx S x ; KS () yx KS () yw yx S x KS() yx KS () yw S () yx K KS () yw S () yw K (05) imlying that a square Cholesky actor o the covariance matrix can be obtained by triangularization o the comound matrix ^S(k) = h S x (k) ; K k S () yx (k) K k S () yw (k) i (06) 4.3 he DD Filter he DD lter is obtained by using the estimates o mean and covariance derived in Section 3.. First we shall dene our additional matrices containing divided

24 4 State Estimation or Nonlinear Systems 4 dierences S () h ; x^x (k) = ( h i (^x k h^s x j u k v k ) i (^x k ; h^s x j u k v k ) ; i (^x k u k v k )) S () xv (k) = h ; S () yx (k) = ( h i (^x k u k v k hs v j ) i (^x k u k v k ; hs v j ) ; i (^x k u k v k )) h ; (g h i (x k hs x j w k )g i (x k ; hs x j w k ) ; g i (x k w k )) S () yw (k) = h ; h (g i (x k w k hs w j )g i (x k w k ; hs w j ) ; g i (x k w k )) : he a riori udate Proceeding as or the DD lter, we can obtain an imroved state estimate by using (3): x k = h ;n x;n v h (^x k u k v k ) h P nx (^x k h^s x u k v k )(^x k ; h^s x u k v k ) (07) h P nv (^x k u k v k hs v )(^x k u k v k ; hs v ) n x denotes the dimension o the state vector and n v denotes the dimension o rocess noise vector. It turns out that this estimate o the mean is identical to the one roosed in [JU94], [JUDW95]. his is interesting as the aroach used in these aers is quite dierent rom the one used here. In agreement with the covariance estimate in (7), a triangular Cholesky actor o the a riori covariance is obtained by Householder transormation o the ollowing comound matrix S x (k )= h S () x^x (k) S() xv (k) S () x^x (k) S() xv (k) i (08) he covariance estimate S x S x is not the same as the one derived in [JU94], [JUDW95], which was the case or the mean estimate. In Aendix A it is shown how the covariance estimate o [JU94] (which is less accurate than the one resented here) can be derived along the same lines as above.

25 4 State Estimation or Nonlinear Systems 5 he a osteriori udate he a riori estimate o the outut and its covariance is calculated in a similar ashion as or the states y k = h ;n x;n w h g(x k w k ) h P nx g(x k hs x w k )g(x k ; hs x w k ) (09) and h P nw g(x k w k hs w )g(x k w k ; hs w ) S y (k) = h S () yx (k) S () yw (k) S () yx (k) S () yw (k) n w denotes the dimension o the measurement noise vector. i : (0) It ollows rom the discussion in Section 3. and (43) that the a riori crosscovariance matrix is the same as or the DD lter (0): P xy (k) = S x (k)s yx (k) : () Kalman gain and aosteriori udate o the state is carried out exactly as or the DD lter: Kalman gain: K k = P xy (k) S y (k)s y (k) ; () A osteriori udate o state vector ^x k =x k K k (y k ; y k ) (3) he aosteriori udate o the estimation error covariance has a ew additional terms. Following the derivations in (05) we can write the covariance matrix ^P = S x ; KS () yx S x ; KS () yx KS () yw KS () yw (4) KS () yx KS () yx KS () yw KS () yw which obviously has the Cholesky actor ^S x (k) = h S x (k) ; K k S () yx (k) K k S () yw (k) K k S () yx (k) K k S () yw (k) i (5) 4.4 he Comlete Filter Algorithm he ollowing rocedure outlines the imlementation o the new lters. Recall that h =3since 4 =3 or a Gaussian distributed variable.. Initialize x 0, P (0), k =0. a osteriori udate

26 5 Examle 6. Comute y k, S () yx (k), S () yw (k), S () yx (k), S () yw (k) 3. Comute P xy according to (0) and determine S y (k) using Householder triangularization on (00) or (0). 4. Solve K k S y (k)s y (k) = P xy or the Kalman gain. Since S y is square and triangular only orward and back-substitutions are needed: First solve ork 0 : k 0 S y = P xy and then solve ork k : K k S y = k Aosteriori udate o the state estimate ^x k =x k K k (y k ; y k ) 6. Aosteriori udate o covariance matrix actor, ^S x (k), is erormed using Householder triangularization on (06) or (5). a riori udate 7. Determine x k, S () x^x (k ), S() xw(k ), S () x^x (k ), S() xw(k ). 8. Use Householder triangularization on (98) or (08) to comute S x (k) 9. k = k,gotoste Several textbooks rovide details on how to erorm the Householder triangularization, e.g., [PFV88], [GvL89], [GA93]. 5 Examle o demonstrate the erormance o the new lters they will in this section be evaluated on the oten used vertically alling body examle originating rom [AWB68]. Several lter designs have been evaluated on this examle [AWB68], [May8], [JU94]. he setu is briey outlined below. he reader is reerred to [AWB68] or a more detailed introduction to the roblem. We wish to estimate altitude (x ), downward velocity(x ), and a (constant) ballistic arameter (x 3 )oavertically alling body. he setu is deicted in Fig.. he radar measures the range (r). he measurements, which aear with intervals o second, are aected by additive, white Gaussian noise. he model has the ollowing orm: _x (t) = ;x (t) (6) _x (t) = ;e ;x(t) x (t) x 3 (t) (7) _x 3 (t) = 0 q (8) y k = r k w k = M (x k ; H) w k : (9)

27 5 Examle 7 RANGE, r x M ALIUDE, x H Figure. Geometry o the vertically alling body roblem. he model arameters are given by: and the initial state o the system is 8 < : M = t H = t = 5 0 ;5 E[w k ] = 04 t (0) x 0 = t x 0 = t/s () x 3 0 = 0 ;3 We will comare the erormances o the DD and DD lters with those o the EKF and the modied Gaussian second-order lter [AWB68]. he reader is reerred to [JU94] or an evaluation o the unscented lter. Due to the nature o the roblem it is common ractice to emloy a continuous-discrete lter imlementation. he state equations (6)-(8) are integrated using a ourth-order Runge-Kutta method with 64 stes taken between each observation. It is straightorward to imlement continuous-discrete versions o the DD and DD lters as there is no rocess noise. In [AWB68] it is described how to imlement the EKF and the modied Gaussian second-order lter or the considered alication. In accordance with [AWB68] and [JU94] the ollowing initialization o the state estimates is used 8 < : and the covariance matrix is initialized to ^P (0) = ^x 0 = t ^x 0 = t/s () ^x 3 0 = 3 0 ; ;4 3 5 : (3)

28 5 Examle 8 o enable a air comarison o the estimates roduced by each o the our lters, the estimates are averaged across a Monte Carlo simulation consisting o 50 runs. Each run is carried out with a dierent noise samle. he results o the Monte Carlo simulation are shown in Figure 3Figure 5. Absolute value o average altitude error (t) Comarison o DD ilter, DD ilter, EKF, and second order ilter DD EKF DD Sec ime (sec) Absolute value o average velocity error (t/sec) Comarison o DD ilter, DD ilter, EKF, and second order ilter Sec EKF DD DD ime (sec) Figure 3. Absolute error in osition estimate (50 run average). Figure 4. Absolute error in velocity estimate (50 run average). Absolute value o average error in ballistic coeicient Comarison o DD ilter, DD ilter, EKF, and second order ilter DD 0 5 EKF 0 6 DD Sec ime (sec) Figure 5. Absolute error in estimate o ballistic coecient (50 run average). RMS altitude error (t) Actual and estimated RMS errors DD (actual) EKF (actual) Sec (actual) DD (actual) DD (estimated) ime (sec) Figure 6. Actual (50 run average) RMS altitude errors q comared with the estimated RMS error, ^P(k) or the DD lter. Not surrisingly, Figure 3Figure 5 show that the DD lter exhibits a erormance which is comletely suerior to the EKF and the DD lter. It is even better than the erormance o the second-order lter. However, in contrast to what we would exect, the erormance o the DD lter is slightly worse than that o the EKF. he dierence is, however, marginal and must be contributed to the act that the assumtions on which the accuracy o the DD lter was analyzed are artly violated. In articular, the assumtion that the state estimate is unbiased is ar rom being satised here.

29 6 Conclusions 9 Comarison with the study o the unscented lter carried out in [JU94] shows that the erormances o the unscented lter and the DD lter are similar. his agrees well with our exectations as the a riori state estimate is the same and the dierence between the covariance udates are limited to ourth and higher order terms in their resective series exansions. he RMS value o the altitude error is shown in Figure 6 or each o the our lters. For comarison, the estimated values ^P have also been lotted or the DD lter. Note that the variations in the erormance o the DD lter are seemingly smaller than or the EKF and DD lters. For all our lters, the actual estimation error variances exceed the variance estimates roduced by the lters. However, the estimated variance is closer to the actual variance o the DD estimates than or the other three lters. It should be noted that the simulation study also showed that there is little dierence between the estimates o ^P roduced by the our lters. his is why only the estimates roduced by the DD lter have been lotted in Figure 6. he marginal dierence might lead to the suggestion that the (a riori) state estimate o the DD lter is used in conjunction with the covariance estimate o the DD lter in order to save comutations. 6 Conclusions In this aer we have roosed two new lters or nonlinear state estimation. Whereas lters or nonlinear systems commonly are based on olynomial aroximations obtained with aylor's ormula, the aroximations underlying the new lters are obtained with a multivariable extension o Stirling's interolation ormula. he lters are extremely simle to imlement as no derivatives are needed, yet they rovide an excellent accuracy. he DD lter is the simlest o the two lters. Essentially, it is similar to the lter roosed in [Sch97]. However, as it aears rom Section 3.3, the (a riori) estimate o the covariance reresents a more aithul aroximation o the true covariance. he most imortant contribution o this note is the suerior DD lter. his lter has the same a riori estimate as the unscented lter described in [JU94], [JUDW95], but a better covariance estimate. he characteristics o the lters are briey summarized below: Based on Gaussian assumtions, the accuracy o the DD lter will be comarable to the EKF in terms o exected error. he accuracy o the DD lter is comarable to the modied Gaussian second-order lter. As the emloyed olynomial aroximations utilize knowledge about the covariance o the state estimates, we exect that the new lters will be suerior to conventional (aylor aroximation based) lters or highly nonlinear systems, and systems with high noise levels.

30 7 Acknowledgements 30 For one-dimensional systems (reerring to the dimension o z) the accuracy o the DD lter is comarable to a ourth-order lter. he imlementation is very simle as the lters do not require derivative inormation. Yet, the comutational burden is relatively limited and will oten be comarable to that o the EKF. As the user needs only rovide models o dynamics and observation rocess, the lters are attractive or imlementation o generic comuter rograms or nonlinear ltering. he lters are very useul or model calibration. It is straightorward to include a varying number o arameters in the state vector or joint state and arameter estimation. he user needs only initialize the arameter estimates and their variances and then run the lter again. he lters were derived based on considerations on how to estimate mean and covariance o arbitrary nonlinear transormations o variables with known mean and covariance. hese results are not limited to state estimation the aroximations can easily be adoted by several other areas o statistics. Although the erormance o the new lters was demonstrated based on the assumtion that the nonlinear transormations are analytic, this is not a requirement or alication o the lters. In act, it is not even necessary to assume dierentiability. he range o alications is thereore wider than or the EKF, which requires that the Jacobians exist. 7 Acknowledgements his work was suorted by the Danish echnical Research Council under contract no Reerences [AWB68] [DB74] M. Athans, R. P. Wishner, and A. B. Bertolini. Subotimal state estimation or continuous-time nonlinear systems rom discrete noisy measurements. IEEE ransactions on Automatic Control, 3(5):50454, 968. G. Dahlquist and. Bj rck. Numerical Methods. Prentice-Hall, Englewood Clis, NJ, 974. [Fr 70] C.-E. Fr berg. Introduction to Numerical Analysis. Addison-Wesley, Reading, MA, 970.

31 REFERENCES 3 [GA93] M. S. Grewal and A. P. Andrews. Kalman Filtering: heory and Practice. Prentice Hall, Englewood Clis, NJ, 993. [GKN 74] A. Gelb, J. F. Kaser, R. A Nash, C. F. Price, and A. A. Sutherland. Alied Otimal Estimation. MI Press, Cambridge, MA, 974. [GvL89] G. H. Golub and C. F. van Loan. Matrix Comutations. he Johns Hokins University Press, Baltimore, MD, 989. [JU94] S. J. Julier and J. K. Uhlmann. A general method or aroximating nonlinear transormations o robability distributions. echnical reort, Robotics Research Grou, Deartment o Engineering Science, University o Oxord, 994. (Internet ublication: htt:// [JU97] S. J. Julier and J. K. Uhlmann. A new extension o the Kalman lter to nonlinear systems. In Proceedings o AeroSense: he th International Symosium on Aerosace/Deense Sensing, Simulation and Controls, Orlando, Florida, 997. [JUDW95] S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte. A new aroach or ltering nonlinear systems. In Proceedings o the 995 American Control Conerence, Seattle, Washington, ages 6863, 995. [Lew86] F. L. Lewis. Otimal Estimation. John Wiley & Sons, New York, NY, 986. [May8] P. S.Maybeck. Stochastic Models, Estimation, and Control, volume. Academic Press, London, 98. [Pa84] A. Paoulis. Probability, Random Variables, and Stochastic Processess. McGraw-Hill, Singaore, 984. [PFV88] W. H. Press, B. P. Flannery, S. A. evkolsky, and W.. Vetterling. Numerical Recies in C-he Art o Scientic Comuting. Cambridge University Press, Cambridge, UK, 988. [Sch97]. S. Schei. A nite-dierence method or linearization in nonlinear estimation algorithms. Automatica, 33():05058, 997. [Ste7] J. F. Steensen. Interolation. Williams & Wilkins, Baltimore, MD, 97.