A Comparison of Unscented and Extended Kalman Filtering for Estimating Quaternion Motion

Transcription

1 A Comparson of Unscented and Extended Kalman Flterng for Estmatng Quaternon Moton Joseph J. LaVola Jr. Brown Unversty Technology Center for Advanced Scentfc Computng and Vsualzaton PO Box 9, Provdence, RI, 292, USA Abstract The unscented Kalman flter s a superor alternatve to the extended Kalman flter for a varety of estmaton and control problems. However, ts effectveness for mprovng human moton trackng for vrtual realty applcatons n the presence of nosy data has been unexplored. In ths paper, we present an emprcal study comparng the performance of unscented and extended Kalman flterng for mprovng human head and hand trackng. Specfcally, we examne human head and hand orentaton moton sgnals, represented wth quaternons, whch are crtcal for correct vewng perspectves n vrtual realty. Our expermental results and analyss ndcate that unscented Kalman flterng performs equvalently wth extended Kalman flterng. However, the addtonal computatonal overhead of the unscented Kalman flter and quas-lnear nature of the quaternon dynamcs lead to the concluson that the extended Kalman flter s a better choce for estmatng quaternon moton n vrtual realty applcatons. Keywords: extended Kalman flterng, unscented Kalman flterng, human moton trackng, quaternons, vrtual realty I. INTRODUCTION Accurate human moton trackng s a crtcal component n any vrtual realty (VR) applcaton []. Havng real tme head and hand moton nformaton enables the computer to draw mages n the correct perspectve. Unfortunately, trackng systems suffer from nose and small dstortons causng ncorrect vewng perspectves. To handle these mperfectons, flterng s often appled to the tracked data so the VR applcaton can obtan more accurate estmates of the user s moton. The Kalman flter (KF) s a popular choce for estmatng user moton n VR applcatons[2][3][4]. Snce poston nformaton s lnear, standard Kalman flterng can be easly appled to the trackng problem wthout much dffculty. However, human pose nformaton also contans nonlnear orentaton data, requrng a modfcaton to the KF. The extended Kalman flter (EKF) provdes ths modfcaton by lnearzng all nonlnear models (.e., process and measurement models) so the tradtonal KF can be appled[5]. Unfortunately, the EKF has two mportant potental drawbacks. Frst, the dervaton of the Jacoban matrces, the lnear approxmators to the nonlnear functons, can be complex causng mplementaton dffcultes. Second, these lnearzatons can lead to flter nstablty f the tmestep ntervals are not suffcently small[6]. To address these lmtatons, Juler and Uhlmann developed the unscented Kalman flter (UKF)[7]. The UKF operates on the premse that t s easer to approxmate a Gaussan dstrbuton than t s to approxmate an arbtrary nonlnear functon. Instead of lnearzng usng Jacoban matrces, the UKF usng a determnstc samplng approach to capture the mean and covarance estmates wth a mnmal set of sample ponts. The UKF s a powerful nonlnear estmaton technque and has been shown to be a superor alternatve to the EKF n a varety of applcatons ncludng state estmaton for road vehcle navgaton[8], parameter estmaton for tme seres modelng[9], and neural network tranng[]. The UKF s also effectve n certan types of vsual contour hand trackng[][2]. However, these systems dealt mostly wth trackng poston and dd not take orentaton nto account. Although the UKF has been appled to a wde range of estmaton problems, to the best of our knowledge there has been no attempt to use t to mprove human head or hand orentaton trackng. Therefore, n ths paper, we explore the potental benefts of the UKF over the more tradtonal EKF n human orentaton estmaton. We descrbe the results of an expermental study whch examnes the estmaton accuracy of the EKF and UKF on both head and hand orentaton represented wth quaternons. Quaternons are a common way to represent rotatons n trackng, robotcs, and mechancal engneerng because they are compact and avod gmbal lock[3]. The results of our study ndcate that, although the EKF and UKF have equvalent performance, the addtonal computatonal overhead of the UKF and the quas-lnear nature of the quaternon dynamcs makes the EKF a more approprate choce for orentaton estmaton n VR applcatons. The remander of ths paper s organzed as follows. In the next two sectons, we descrbe the algorthmc detals of the EKF and UKF formulatons used n our study. Secton IV descrbes our expermental methodology and setup. Secton V presents the expermental results and dscusses ther sgnfcance. Secton VI concludes the paper. II. EXTENDED KALMAN FILTERING The extended Kalman flter s a set of mathematcal equatons whch uses an underlyng process model to make an estmate of the current state of a system and then corrects the

2 estmate usng any avalable sensor measurements. Usng ths predctor-corrector mechansm, t approxmates an optmal estmate due to the lnearzaton of the process and measurement models[4]. To descrbe all the detals of the EKF s beyond the scope of ths paper. Therefore, we present a more algorthmc descrpton omttng some theoretcal consderatons. More detals on the EKF can be found n [5][6]. The process model we use s an orentaton/angular velocty (OV) model defned by f = dq dt = qω, () 2 where q s the current quaternon and ω s a pure vector quaternon representng angular velocty. We use a sngle EKF, where the state vector at tme k s defned by ˆx k = [q x, q y, q z, q w, ω, ω, ω 2 ] T. (2) Gven the state vector at step k, we frst perform the predcton step by fndng the a pror state estmate ˆx k by ntegratng equaton through tme by t (.e.,. dvded by the current samplng rate) usng a 4th Order Runge-Kutta scheme. Then, we fnd the a pror estmate of the error covarance matrx P k = Φ kp k Φ T k + Q k, (3) where Q k s the process nose covarance, P k s the a posteror estmate of the error covarance, and Φ k s an approxmaton to the fundamental matrx calculated by takng the Taylor expanson of Φ(t) around the system dynamcs matrx F k,[,j] = f () (ˆx k ), (4) x (j) a Jacoban matrx whch lnearzes the process functon f, and then substtutng t for t. After the predcton step, the correcton step calculates the a posteror state estmate usng ˆx k = ˆx k + K k(z k H kˆx k ), (5) where K k s the Kalman gan or blendng factor and H k s the measurement matrx used to combne the measurement vector z k, obtaned from the trackng devce, wth ˆx k. The Kalman gan s computed usng K k = P k HT k (H k P k HT k + R), (6) where R s the measurement nose covarance, and the measurement matrx s calculated usng Note that from a theoretcal perspectve, the EKF calculates F k each tme f s evaluated. In the 4th order Runge-Kutta routne, f s evaluated 8 tmes[7], meanng that F k should be a product of 8 ntermedate Jacoban evaluatons. In our formulaton, we only evaluate F k once from the output of the Runge-Kutta routne. Although ths approach devates slghtly from the defnton of the EKF, we fnd t faster, less complex, and works just as well for our applcatons. H k,[,j] = h () (x k ), (7) x (j) a Jacoban matrx that lnearzes around the nonlnear measurement functon h. In our case, h s quaternon normalzaton defned by q h = qx 2 + qy 2 + qz 2 + qw 2 for the quaternon n ˆx k. Fnally, we compute the a posteror estmate of the error covarance usng (8) P k = (I K k H k )P k. (9) Note that after we calculate the a posteror state estmate, the quaternon s renormalzed ensurng t s on the unt sphere, makng t a vald rotaton. A. EKF Parameters and Intalzaton The EKF has two parameters, Q k and R, whch represent the process nose covarance and the measurement nose covarance. R s determned emprcally and accounts for the uncertanty n the trackng data. Settng these matrces properly goes a long way toward makng the flters robust. We determne Q k usng the contnuous process nose matrx Q whch assumes that the process nose always enters the process model on the hghest dervatve[6]. Therefore, t Q k = Φ s Φ(τ) QΦ(τ) T dt, () where Φ s s a scalng parameter whch acts as a confdence value for how sure we are that the process model s an accurate descrpton of the the true moton dynamcs. The EKF also needs to be ntalzed on startup. The quaternon n the state vector at tme s smply set to the frst observaton n the moton sequence and the angular velocty components are set to. The a pror estmate of the error covarance and the elements n the these matrces are set to for the off-dagonal entres and to relatvely large numbers n the dagonal entres. For our mplementaton, the quaternon varance dagonals are set to and the angular velocty varances are set to. III. UNSCENTED KALMAN FILTERING The basc premse behnd the unscented Kalman flter s t s easer to approxmate a Gaussan dstrbuton than t s to approxmate an arbtrary nonlnear functon. Instead of lnearzng usng Jacoban matrces, the UKF uses a determnstc samplng approach to capture the mean and covarance estmates wth a mnmal set of sample ponts[9]. As wth the EKF, we present an algorthmc descrpton of the UKF omttng some theoretcal consderatons. More detals can be found n [7][6][8]. Gven the state vector at step k (we use the same state vector as n equaton 2, we compute a collecton of sgma

3 ponts, stored n the columns of the L (2L + ) sgma pont matrx X k where L s the dmenson of the state vector. In our case, L = 7 so X k s a 7 5 matrx. The columns of X k are computed by (X k ) = ˆx k () ( ) (X k ) = ˆx k + (L + λ)pk, =... L ( ) (X k ) = ˆx k (L + λ)pk, = L L, L ( (L ) where + λ)pk s the th column of the matrx square root and λ s defned by λ = α 2 (L + κ) L, (2) where α s a scalng parameter whch determnes the spread of the sgma ponts and ( κ s a secondary scalng parameter. (L ) Note that we assume + λ)pk s symmetrc and postve defnte whch allows us to fnd the square root usng a Cholesky decomposton. Once X k computed, we perform the predcton step by frst propagatng each column of X k through tme by t usng (X k ) = f((x k ) ), =... 2L, (3) where f s dfferental equaton defned n equaton. In our formulaton, snce L = 7, we perform 5 4th order Runge- Kutta ntegratons. Wth (X k ) calculated, the a pror state estmate s where ˆx k = 2L = are weghts defned by = = (X k ), (4) λ (L + λ) 2(L + λ), =... 2L. (5) As the last part of the predcton step, we calculate the a pror error covarance wth P k = 2L = W (c) [ (Xk ) ˆx ] [ k (Xk ) ˆx ] T k + Qk, (6) where Q k s once agan the process error covarance matrx, and the weghts are defned by W (c) = W (c) = λ (L + λ) + ( α2 + β) (7) 2(L + λ), =... 2L. Note that β s a parameter used to ncorporate any pror knowledge about the dstrbuton of x. To compute the correcton step, we frst must transform the columns of X k through the measurement functon. Therefore, let (Z k ) = h((x k ) ), =... 2L (8) ẑ k = 2L = (Y k ). (9) h s the same quaternon normalzaton functon found n equaton 8. Wth the transformed state vector ẑ k, we compute the a posteror state estmate usng ˆx k = ˆx k + K k(z k ẑ k ), (2) where K k s once agan Kalman gan. In the UKF formulaton, K k s defned by where Pẑk ẑ k = Pˆxk ẑ k = 2L = 2L = K k = Pˆxk ẑ k P ẑ k ẑ k, (2) W (c) [ (Zk ) ẑ ] [ k (Zk ) ẑ ] T k +R (22) W (c) [ (Xk ) ˆx ] [ k (Zk ) ẑ ] T k. (23) Note that as wth the EKF, R s the measurement nose covarance matrx. Fnally, the last calculaton n the correcton step s to compute the a posteror estmate of the error covarance gven by P k = P k K kpẑk ẑ k K T k. (24) As wth the EKF, we renormalze the state vector s quaternon to make sure t s on the unt sphere, makng t a vald rotaton. A. UKF Parameters and Intalzaton Q k, R, α, β, and κ are the fve parameters used n the UKF. We determne, R, α, β, and κ emprcally and use the formulaton descrbed n Secton II.A to fnd Q k. More detals on our choce for determnng Q k can be found n Secton V. The UKF s ntalzed n the same way as the EKF, usng the same values for the state vector and error covarance matrx upon startup. IV. EXPERIMENTAL STUDY To compare the performance of the EKF and UKF algorthms descrbed n sectons II and III, we conducted an experment to determne whch flterng algorthm s preferable for mprovng human orentaton trackng n vrtual realty systems.

4 A. Expermental Setup Two datasets (one head and one hand) were used n our study to represent common orentaton dynamcs found n our vrtual realty applcatons. Each dataset conssts of unt length quaternons runnng about 2 seconds n length. The orentaton sequences were captured usng an Intersense IS9 trackng system, a hybrd nertal/ultrasonc trackng devce. The head orentaton dataset, denoted HEAD and shown n Fgure, s an example of a user rotatng her head to vew mages on three orthogonal dsplay screens. The hand orentaton dataset, denoted by HAND and shown n Fgure 2, s an example of a user rotatng hs hand to navgate through the vrtual world qw The HAND Dataset qy qx The HEAD Dataset.2 qz.8 qw qz Fg. 2. The four sgnals that make up the quaternon sequence for the HAND dataset. The values for each quaternon component are untless qx Fg.. The four sgnals that make up the quaternon sequence for the HEAD dataset. The values for each quaternon component are untless. In the experment, the datasets were tested wth samplng rates of 25, 8, and 25Hz gvng us three dfferent test scenaros for each dataset. These samplng rates were chosen because VR trackng systems are commonly run at these rates. We use a small Monte Carlo smulaton on each test scenaro snce we have random Gaussan nose added to the moton sgnals, whch s used to smulate jttery trackng data. A constant value of 5e-6 was set for the random nose varance provdng nose added to the moton sgnals wth a Gaussan dstrbuted range of ±.9 degrees. All tests were run on a AMD Athelon XP 8+ wth 52Mb of man memory. B. Evaluaton Method To determne how well the EKF and UKF algorthms are performng, we need comparson data. Comparng estmated output wth reported user orentatons s problematc snce these records have nose and small dstortons assocated wth them. Thus, any comparson wth the recorded data would count trackng error wth the estmaton error. We obtan the ground truth datasets by passng them through a zero phase shft flter to remove hgh frequency nose. We determne the qy lowpass and hghpass flter parameters by examnng each sgnal s power spectrum. Dependng on the partcular dataset, the lowpass/hghpass pars were anywhere between /3 and 2/4 Hz. Ths cleanng step gves us the truth datasets we need to test aganst and makes t easy to add nose of known characterstcs for smulatng jttery trackng data. Wth the truth datasets, we can calculate the root mean square error (RMS) for each test and take the average over the Monte Carlo smulaton runs. For truth and estmated quaternons, q t and q e, RMS s defned by RMS q = n e 2 n, (25) where RMS q s n degrees and e = 2(8) π C. EKF and UKF Parameters = arccos ((q t (q e ) ) w ). (26) For the EKF and UKF algorthms, we needed to determne the R and Q k covarance matrces. Snce we know the varance of the Gaussan whte nose we are njectng nto the moton sgnals, we set the off-dagonal entres of R to and set the dagonal entres to be the value of the nose varance value (5e-6 n ths case). Thus we are makng the assumpton that our measurement nose s based on the varablty of a statonary tracker. As shown n Secton II.A, we calculate the Q k matrx usng equaton leavng Φ s as our free parameter. The search routnes ran over dfferent nteger values for Φ s and we found to be a good choce for the HEAD dataset and 2 for the HAND dataset. For the UKF, we also needed to set the α, β, and κ parameters. After runnng a number of tests, we found that,, and were approprate for these

5 parameters. See Secton V for a dscusson on our parameter choces..2. Qx State (EKF). Qy State (EKF) V. RESULTS AND DISCUSSION Tables I and II show the RMS errors for the HEAD and HAND datasets across the dfferent samplng rates. These results show that the EKF and UKF have roughly the same error n all cases. Note that we also nclude the RMS error for dong no flterng at all to show that both the EKF and UKF mprove trackng accuracy at samplng rates of 8 and 25Hz Qz State (EKF) Qw State (EKF) RMS Results for the HEAD Dataset EKF UKF NONE 25Hz: Hz: Hz: TABLE I THE RMS ERROR RESULTS (IN DEGREES) FOR THREE DIFFERENT SAMPLING RATES ON THE HEAD DATASET. THE DATA SHOWS THE EKF AND UKF HAVE ROUGHLY THE SAME ERROR WHEN ESTIMATING QUATERNIONS AND IMPROVE ACCURACY OVER NO FILTERING AT ALL Fg. 3. State errors from the EKF for the four quaternon components n the HEAD dataset sampled at 8Hz. The sold lnes represent the errors whle the dashed lnes show the 3 standard devaton bounds. The component estmates are untless..2 Qx State (UKF) Qy State (UKF) RMS Results for the HAND Dataset EKF UKF NONE 25Hz: Hz: Hz: TABLE II THE RMS ERROR RESULTS (IN DEGREES) FOR THREE DIFFERENT SAMPLING RATES ON THE HAND DATASET. THE DATA SHOWS THE EKF AND UKF HAVE ROUGHLY THE SAME ERROR WHEN ESTIMATING QUATERNIONS AND IMPROVE ACCURACY OVER NO FILTERING AT ALL. The tests that were run at 25Hz show there s only a slght mprovement n the EKF and UKF s estmaton performance for both the HEAD and HAND datasets. These numbers ndcate that samplng rates of 25Hz are probably not hgh enough for applyng flterng algorthms to quaternon moton data. However, more work s needed to verfy ths clam. Fgures 3 and 4 show the state errors from the EKF and UKF flters for the quaternon components n the HEAD dataset sampled at 8Hz. These graphs are representatve of the component wse error n our test scenaros and show that, on a component level, the accuracy of the EKF and UKF are roughly the same. From ths data and the data n Tables I and II, t s dffcult to make a decson about whch estmaton algorthm s the better choce. Therefore, we need to examne the algorthms n greater detal. Usng the test scenaros, we recorded the runnng tmes for each algorthm. On average, the EKF algorthm took mcroseconds per estmate whle the UKF algorthm took 3,294.2 mcroseconds per estmate. The reason the UKF algorthm takes sgnfcantly longer to make an estmate s because t has to handle all the sgma ponts. In our mplementaton, the Qz State (UKF) Qw State (UKF) Fg. 4. State errors from the UKF for the four quaternon components n the HEAD dataset sampled at 8Hz. The sold lnes represent the errors whle the dashed lnes show the 3 standard devaton bounds. The component estmates are untless. UKF has to perform 5 Runge-Kutta ntegratons to propagate the sgma ponts through tme whle the EKF only has to perform one ntegraton. Even f we used Juler and Uhlmann s method for reducng the number of sgma ponts[9], we would stll need to do 8 Runge-Kutta ntegratons for the UKF to only one for the EKF. If the estmaton accuracy of the UKF was better than the EKF, ths addtonal computatonal overhead would be warranted. However, snce the UKF does not gve us any addtonal accuracy, from a runnng tme standpont, the EKF seems the more approprate estmator n ths case. In addton to the ssue of tme complexty between the

6 EKF and UKF, we also need to examne ther theoretcal performance. From [6], we know that UKF can predct the state estmate and error covarance to 4th order accuracy whle the EKF only predcts up to 2nd order for the state estmate and 4th order for the error covarance. However, the UKF wll make more accurate estmates only f the kurtoss and hgher order moments n the state error dstrbutons are sgnfcant. In our applcaton, the magntudes of the quaternon component covarances are sgnfcantly less than unty (on the order of 4 to 6 n most cases) whch means the kurtoss and hgher order moments are very small. Ths fact s one ndcaton of why the UKF does not perform better than the EKF. Addtonally, ths ndcates why there s lttle, f any, effect n UKF performance wth dfferent values for the UKF parameters α, β, and κ. Samplng rate s another ndcaton why the UKF does not provde better performance when estmatng quaternon moton. In general, f the samplng rate s suffcently hgh, the quaternon dynamcs behave n a quaslnear fashon snce, wth small tmesteps, the ntegraton steps propagate the quaternons only small devatons away from the unt sphere, makng the error n lnearzaton mnmal. Fnally, one of the man advantages of the UKF s that t does not requre the calculaton of Jacoban matrces. In many applcatons, Jacoban matrx evaluaton can be nontrval and lead to mplementaton dffcultes[6]. However, n our applcaton, the calculaton of the Jacoban matrces s qute smple based on the structure of the process and measurement functons (see equatons and 8) and quaternon mathematcs[2]. Therefore, the UKF does not provde us wth any addtonal beneft n ths case. In fact, the smplcty of the Jacoban calculatons for the process model allowed us to use the same method for calculatng Q k n both the EKF and UKF formulatons. Although our work has focused on head and hand orentaton trackng n VR applcatons, we hypothesze that these results may extend to other domans, such as robotcs and underwater vehcle navgaton, requrng quaternon moton estmaton wth moton dynamcs that behave n a quaslnear fashon. Such moton dynamcs would have to have the mportant characterstc of small angle devatons and sampled at relatvely hgh rates. Future work can valdate ths hypothess. VI. CONCLUSION In ths paper, we have presented an experment whch compares extended and unscented Kalman flterng of head and hand orentaton data represented wth quaternons. Our results ndcate that, although the EKF and UKF have roughly the same accuracy, the computatonal overhead of the UKF, the smplcty of the Jacoban matrx calculatons, and the quas-lnear nature of the quaternon dynamcs makes the EKF a better choce for the task of mprovng trackng of nosy quaternon sgnals n vrtual realty applcatons. ACKNOWLEDGMENTS Specal thanks to Smon Juler, Gary Bshop, Greg Welch, John Hughes, and Andy van Dam for valuable gudance and dscusson. Ths work s supported n part by the NSF Graphcs and Vsualzaton Center, IBM, the Department of Energy, Alas/Wavefront, Mcrosoft, Sun Mcrosystems, and TACO. REFERENCES [] Stanney, Kay M. Handbook of Vrtual Envronments: Desgn, Implementaton, Applcatons, Lawrence Erlbaum Assocates, 22. [2] Azuma, Ronald and Gary Bshop. Improvng Statc and Dynamc Regstraton n a See-Through HMD. In Proceedngs of SIGGRAPH 94, 97-24, 994. [3] Foxln, Erc. Inertal Head-Tracker Sensor Fuson by a Complementary Separate-Bas Kalman Flter. In Proceedngs of the Vrtual Realty Annual Internatonal Symposum 96, 85-94, 996. [4] Welch, Greg, and Gary Bshop. SCAAT: Incremental Trackng wth Incomplete Informaton, In Proceedngs of SIGGRAPH 97, ACM Press, , 997. [5] Sorenson, H. W. Kalman Flterng: Theory and Applcaton, IEEE Press, 985. [6] Juler, Smon J., Jeffery K. Uhlmann, and Hugh F. Durrant-Whyte. A New Approach for Flterng Nonlnear Systems.In Proceedngs of the 995 Amercan Control Conference, , 995. [7] Juler, Smon J. and Jeffery K. Uhlmann. A New Extenson of the Kalman Flter to Nonlnear Systems. In The Proceedngs of AeroSense: The th Internatonal Symposum on Aerospace/Defense Sensng,Smulaton and Controls, Mult Sensor Fuson, Trackng and Resource Management II, SPIE, 997. [8] Juler, Smon J. and H. F. Durrant-Whyte. Navgaton and Parameter Estmaton of Hgh Speed Road Vehcles. In Robotcs and Automaton Conference, -5, 995. [9] Wan, E. A., and R. van der Merwe. The Unscented Kalman Flter for Nonlnear Estmaton. In Proceedngs of Symposum 2 on Adaptve Systems for Sgnal Processng, Communcaton and Control(AS-SPCC), IEEE Press, 2. [] van der Merwe, R. and E. A. Wan, Effcent Dervatve-Free Kalman Flters for Onlne Learnng, In European Symposum on Artfcal Neural Networks (ESANN), Bruges, Belgum, 2. [] Pehua, L and Tanwen Zhang. Unscented Kalman Flter for Vsual Curve Trackng. In Proceedngs of Statstcal Methods n Vdeo Processng, June, 22. [2] Stenger, B., P. R. S. Mendonça, and R. Cpolla. Model-Based Hand Trackng Usng an Unscented Kalman Flter. In Proceedngs of the Brtsh Machne Vson Conference, 63-72, September 2. [3] Grassa, F. Sebastan. Practcal Parameterzaton of Rotatons Usng the Exponental Map. In Journal of Graphcs Tools, 3(3):29-48, 998. [4] Welch, Greg and Gary Bshop. An Introducton to the Kalman Flter. Techncal Report TR 95-4, Department of Computer Scence, Unversty of North Carolna at Chapel Hll, 995. [5] Maybeck, Peter S. Stochastc models, estmaton, and control. Volume, Academc Press, 979. [6] Zarachan, Paul and Howard Musoff. Fundamentals of Kalman Flterng: A Practcal Approach. Progress n Astronautcs and Aeronautcs, Volume 9, Amercan Insttute of Aeronautcs and Astronautcs, Inc., 2. [7] Press, Wllam H., Bran P. Flannery, Saul A. Teukolsky, and Wllam T. Vetterlng. Numercal Recpes n C: The Art of Scentfc Computng, 2nd Edton, Cambrdge Unversty Press, 993. [8] Wan, E. A., and R. van der Merwe. The Unscented Kalman Flter, In Kalman Flterng and Neural Networks, S. Haykn (ed.), Wley Publshng, 2. [9] Juler, Smon J., and Jeffrey K. Uhlmann. Reduced Sgma Pont Flters for the Propagaton of Means and Covarances Through Nonlnear Transformatons. In Proceedngs of the 22 Amercan Control Conference, , 22. [2] Shoemake, Ken. Anmatng Rotatons wth Quaternon Curves. In Proceedngs of SIGGRAPH 85, ACM Press, , 985.