A new smallest sigma set for the Unscented Transform and its applications on SLAM

Transcription

1 5th IEEE Conference on Decson and Control and European Control Conference (CDC-ECC) Orlando, FL, USA, December -5, A new smallest sgma set for the Unscented ransform and ts applcatons on SLAM Henrque M Menegaz, João Y Ishhara, member, IEEE, Geovany A Borges, member, IEEE Abstract In ths work we propose a new set of sgma ponts for the Unscented ransform that uses the mnmum number of ponts We than compare ths new set wth the symmetrc set, the reduced set, and the sphercal set Smulatons comparng ths sets are done to verfy the propertes of ths set and to verfy ther transforms Lastly, we smulate each of these sets n a recursve flter for SLAM he results show that our set s a better choce for a non symmetrc pror dstrbuton and stll a good alternatve for symmetrc pror dstrbutons I INRODUCION Based on the ntuton that t s t s easer to appromate a probablty dstrbuton than t s to appromate an arbtrary nonlnear functon or transformaton, Juler et all 4 proposed a non-lnear estmaton technque whch was lately called the Unscented ransform (U) he Unscented ransform has been etensvely used on the Smultaneous Localzaton and Map Buldng (SLAM) o cte some, one can check 5 5 However, the Unscented s computatonal load s proportonal to the number of sgma ponts whch s, n ts turn, ncreasngly related to the state s dmenson 6 SLAM s problems usually have a huge state s dmenson, for t s the sum of the dmenson of the robot s pose and the dmensons of all the coordnates of the map s landmarks Moreover, the computatonal cost can be a tough requrement for a robot, because of t s processng capactes or t s power consumpton, snce t uses an embedded hardware n a great number of cases herefore, reducng the computatonal cost n a SLAM problem s especally mportant and, n consequence, reducng the number of sgma ponts s also manly mportant As the symmetrc sgma sets of Juler use n + sgma ponts, t s etremely desrable to fnd a set of sgma set that uses less ponts In partcular, n +, for t s the mnmum number of ponts that can be used to estmate both mean and covarance matr 6 In ths drecton, 6 proposed a n + scheme and 7 offered a n + set whch n + of them le on a hypersphere 7 6 has a specfc problem of stablty for hgh values of n 7 Addtonally, both algorthm s have the drawback that ther mean and covarance matr are not equal to the mean and covarance matr of the pror dstrbuton, whch s a very mportant property for the unscented framework, for t he authors are wth the Laboratóro de Robótca e Automação (LARA) at the Department of Electrcal Engneerng, Unversty of Brasíla, Brasíla, DF, Brasl n s the state s dmenson mples, wth the condton of the dfferentablty of the nonlnear functon, that the mean and the covarance matr of the posteror random varable are estmated up to the second order of ther aylor Seres Our efforts go precsely on the drecton of fndng a mnmum set of sgma ponts whch captures both the mean and covarance matr of the pror dstrbuton and to apply t n a SLAM s framework Our set accomplshes these propertes and does not present the nstablty ssues of 6 hese propertes are verfed on smulatons Fnally, a comparson s made by smulatng the recursve flter usng our new set, the symmetrc set, the set of 6 and the set of 7 he results show that our set of n + sgma ponts s a good alternatve for the problem of SLAM hs work s organzed as follows: secton II provdes a background on the Unscented ransform, on the Unscented Kalman Flter and ntroduces the new sgma set; secton III shows the smulatons eamples and secton IV provdes the conclusons A he prevous sgma sets II HE SIGMA SES Consder that X R n s a vector of random varables and consder that f : R n R m s a transformaton that defnes Y as follows: Y = f (X) he Unscented ransform appromates the probablty densty functon (pdf) of the pror random varable (RV) - n ths case X - by a group of ponts - called sgma ponts - whch are obtaned n a determnstc fashon From another pont of vew, one can consder ths transformaton as an appromaton of the random varable s pdf by a probablty mass functon Hence, the U can be seen as a dscrete appromaton of a random varable One mportant advantage of ths non-lnear estmaton technque s that t does not requre the calculatons of the Jacobans or Hessans of the nonlnear functon as the technques based on the lnearzaton do Another mportant property of the Unscented ransform s that f ts sample central moments are equal to the central moments of the pror RV up to the k-th order nclusvely, the aylor Seres of the sample mean and of the sample covarance matr of the transformed ponts wll be equal to the aylor Seres of the mean and of the covarance matr, respectvely, of the posteror RV up to the k-th order nclusvely //$6 IEEE 37

2 Now, we present three of the sgma ponts proposed n the lterature ) he symmetrc set: he symmetrc set has more than one format, whch are the ones of 4 Here, we choose the one of For =,,n, ths set can be wrtten as follows: χ = X, χ = X + ( n w P XX ) w = w n (, ) () χ +n = X n P XX, w w +n = w n, n whch w and (A) represents the -th column or the lne of the matr A ) he reduced set: he reduced set of Juler presented n 6 can be descrbed by the followng algorthm: ) Choose w, regardng w ) Calculate the weghts: w = w n,for = ; w,for = ; w,for = 3,,n + 3) Intate the vector sequence χ j : χ = ; χ = ; w χ = w 4) Epand the vector sequence for j =,,n accordng to χ j,for = ; χ j χ j+ =,for =,,j; wj j wj,for = j + 3) he sphercal set: he n+ sphercal set of Juler presented n 7 can be descrbed by the followng algorthm: ) Choose w, regardng w ) Calculate the weghts: w = w n 3) Intate the vector sequence χ j : χ = ; If the matr square root A of P s of the form P = A A, then the sgma ponts are formed from the rows of A However, f the matr square root s of the form P = AA, the columns of A are used, χ = ; w χ = w 4) Epand the vector sequence for j =,,n accordng to χ j,for = ; χ j χ j =,for =,,j; j(j+)w j,for = j + j(j+)w B he new mnmum sgma set he new transform can be resumed n the followng theorem heorem Let X R n be a random varable wth mean X and covarance matr P XX > wth ts matr square root, and let be the set of ponts and weghts, {χ,w }, =,,n + wth the followng form 3 : A χ = χ χ n () := α P n ( XX w C W ) + X:n+, n whch w (3) w W :=, (4) w n w wn w wn wn := w α C n n (C ), (5) C := I n α n n, (6) w α :=, (7) n < w < (8) Let, stll, be the non-lnear mappng f : R n R m dfferentable up, at least, to the second order Stll, let f defne the random varable Y accordng to Y f(x), and let be the set of ponts and weghts {γ,w γ = f (χ )}, the followng statements are true: 3 he notaton a p q, wth a R, represents a matr of dmenson p q whch all of ts terms are equal to a In ts turn, v :q, wth v R n, represents a matr of dmenson n q whch all the columns are equal to v 373

3 ) he sample mean of {χ,w }, µ χ = n = w χ, s equal to the mean of X ) he sample covarance matr of {χ,w }, Σ XX = n = w (χ µ χ ) (χ µ χ ), s equal to the covarance matr of X 3) he aylor Seres of the sample mean of {γ,w } s equal to the aylor Seres of the mean of Y up to the second order nclusve 4) he aylor Seres of the sample covarance matr of {γ,w } s equal to the aylor Seres of the covarance matr of Y up to the second order nclusve Proof: Accordng to the aylor Seres epansons of the mean and the covarance of {γ,w } and of Y, one can see that f the frst two tems of ths theorem are proved, the remanng tems wll be also proven Let us than frstly prove tem and n sequence tem he sample mean of {χ,w }, µ χ, can be wrtten as: µ χ = Aχ + X :n+ w w n w w n = α P n XX w + P ( XXC W ) w + X = P XX w α n + C w wn w w n + X (9) Substtutng (7) n (9) and usng (6) the mean s proof wll be accomplshed Now t remans to prove that the sample covarance matr of {χ,w }, Σ XX, s equal to the covarance matr of X For ths, defne W a = w W () han, from the defnton of sample covarance matr: Σ XX = A χ µ χ :n W a A χ µ χ :n = A χ Xχ W a A χ X () :n Usng (3) and () on () 4 : ( Σ XX = :n P XX α n w C ( W ) 4 he epresson (QP)( ) s equal to (QP)(QP) w W ) ( ) = P XXC ( W ) W ( (W ) ) C P XX + P XX α n w α n w = P XXCC P XX + α P XX n n = P XX In α n n + α P XX n n = P XX In the heorem, the condton (7) assures that the weghts sum equals the unt he weght w, whch s restrcted to (8), gves a degree of freedom on the parameters choce A study of a partcular choce of ths parameter has not been yet studed One could try to, for eample, choose a value that mnmzes the dfference on the thrd moments Furthermore, smulatons results made for dverse pror dstrbutons and varous functons ndcate good robustness of ths sgma set (see 8) In comparson to the symmetrc set, our new sgma set has the advantage of usng only n+ sgma ponts, whch s the mnmum possble amount Both these sgma sets share the propertes of havng ther sample mean and covarance matr equal to the mean and covarance matr of the pror dstrbuton whch result, wth the condton of the dfferentablty of the non-lnear functon, n the ablty to estmate the mean and the covarance matr of the posteror random varable (RV) up to the second and frst order, respectvely, of ther aylor Seres When the pror dstrbuton s not symmetrc, there s no reason for usng the symmetrc set nstead of the new mnmum set, because they offer, for ths case, eact estmatves up to the second order but the frst requres more computatonal effort For a symmetrc pror dstrbuton, the two sets wll offer a trade-off choce he symmetrc set wll probably offer a better estmatve, but wth a cost to the computatonal effort Meanwhle, the new mnmum set wll probably offer a poorly estmatve, but wth a lghter computatonal effort However, the new sgma set stll gves better estmatves for certan functons even for a Gaussan assumpton We shall see one eample on secton III-A Let us now turn our attenton to the other two sets of sgma sets, the ones of 6 and 7 he set of 6 also uses the mnmum amount of sgma ponts However, we can see two drawbacks on ths set s propertes One s that 6 may be unstable for hgh values of n ( see 7) he other s that nether the sample mean nor the covarance matr of the set of 6 equals the mean and covarance matr respectvely of the pror RV when n s greater than one here s no dffculty on checkng t If one takes the algorthm of secton II-A and calculates any set {χ j,w } for a j greater than - try - ths affrmaton wll be confrmed 374

4 As for the set of 7 we can see that t uses one etra pont, whch s not a compromsng drawback However, ths set of sgma ponts also carres the second mentoned drawback for the set of 6 and t can also be verfed as easy as for ths other set C he Unscented Kalman Flter he sets of sgma ponts can be used n a Kalman Flter frame as a recursve flter for non-lnear systems resultng n the Unscented Kalman Flter (UKF) he UKF has been presented manly n two approaches accordng to the way the algorthm treats the nose terms 9 he frst one treats both the process nose and the measurement nose as addtve On the other hand, the second Unscented Kalman Flter algorthm ncorporates the nose terms nto the state vector, creatng an augmented state vector for the generaton of the sgma ponts Here we epose only the augmented UKF (aukf), for t s the most used and s the one that we are gong to use Consder the followng stochastc non-lnear dscrete-tme dynamc system X k = fx k,u k,w k, Y k = hx k,u k,v k, n whch X k s the state s vector at tme k, Y k s the vector of the measurements at tme k, u k s the control nput at tme k and w k N(,Q) and v k N(,R) are the process nose and the observaton s nose respectvely Frst, we must restructure the vectors, covarance matrces and functons as above : he augmented pror state vector s meda wll be: X k X k a = nw nw, n whch nw and nw are vectors of n w (process nose s dmenson) and n v (measurement nose s dmenson) zeros respectvely he augmented covarance matr wll be 5 : P XX,k,k a = Q R he process and measurements functon must be rewrtten as functons of X a k : X a k = f a Xa k,u k,w k ; Y k = h a Xa k,u k,v k he aukf s algorthm s : ) Generates the augmented sgma ponts and ts weghts from the pror dstrbuton: χ,a k,w X a k 5 We consder that the state and the noses are mutually uncorrelated ) Apply the augmented sgma ponts nto the process functon to obtan the predcted sgma ponts: χ,a k k fa χ,a k,u k,w k 3) Calculate the predcted augmented meda and covarance matr: Σ a χχ,k k = = µ a χ,k k = = w χ,a k k ; w µ a χ,k k χ k k µ a χ,k k χk k,a 4) Apply the predcton s augmented sgma ponts nto the observaton functon; γk k = ha χ,a k k,u k,v k 5) Calculate the observaton s predcted meda and covarance matr: Σ γγ,k k = = µ γ,k k = = w γ k k ; w µγ,k k γ k k µγ,k k γ k k 6) Calculate the cross covarance error: Σ χγ,k k = = w µχ,k k χ k k µγ,k k γ k k 7) Calculate the update usng Kalman Flter s equatons: G k = Σ χγ,k k Σ γγ,k k ; ν k = y k µ γ,k k ; µ γ,k = mu γ,k + G k ν k ; P XX,k = Σ χχ,k k G k Σ γγ,k k G k For more nformaton, see he only dfference between the unscented flters usng dfferent sgma sets wll be n the frst stage of the aukf algorthm In the smulatons, wll shall use ths flter n a SLAM frame for each of the sgma sets descrbed n sectons II-A and II-B III EXAMPLES A Smulatons of the transforms In ths secton we perform a smulaton comparng the estmatves of our mnmum set of sgma ponts (MU) wth the estmatves of the symmetrc set (SyU), of the reduced set of 6 (RU) and of the sphercal set of 7 (SpU) In all smulatons, the pror RV s dstrbutons 375

5 X ABLE I SIMULAIONS RESULS Montecarlo SyU RU P XX Ȳ Ȳ P Y Y X SpU P XX MU Ȳ Ȳ P Y Y are a 3 dmenson multvarable Gaussan he errors of the posteror dstrbutons are n relaton to a Monte Carlo smulaton runnng 5 samples Fnally, all the errors are a mean of the errors of smulatons able I and able II contan the three smulatons results In able II, the frst two lnes show, respectvely, the prors mean and covarance matr errors, whch are represented as E X and E he thrd lne shows the errors on the mean (E Ȳ ) of the posteror RV transformed by a second order polynomal functon (f below) he fourth and ffth lnes present, respectvely, the errors on the mean (E Ȳ ) and on the covarance matr (E PY Y ) of the posteror RV transformed by a Cartesan to polar transformaton (f below) he two smulated functons, f and f, are the followng: f (X) = ( + ) 3 3 f (X) = arctan arctan ( 3 ) From the results of the frst two lnes, we can see that nether 6 nor 7 matched the pror s RV mean and covarance matr, whle our new set and the symmetrc set dd From the thrd lne, one can check that our set and the symmetrc set really estmated precsely the mean of the posteror RV for a second order polynomal functon Fnally, the last two lnes showed that our new set provded the best ABLE II PERCEN ERRORS IN RELAION O MONE CARLO SyU RU SpU MU E X E E Ȳ E Y E PY Y estmatve for the mean of the posteror RV and second best one for the covarance matr, even usng only n + sgma ponts B SLAM s smulatons In ths secton, we consder the same system and UKF- SLAM algorthm of Dr m Baley 6 he algorthm performed each of the four sgma sets (MU, SyU, SpU, RU) runnng n a Matlab R9a he moton functon f and the measurement functon h are the followng 7 : f ( R k,u k ) 6 Avalable at www personalacfrusydeduau/tbaley/softwares /slam smulatonshtm 7 For convenence, we wll use a new notaton: R k = k R 376

6 RMSE (m) ABLE III PAH ERRORS FOR HE SLAM S SIMULAIONS SyU MU RU SpU RMSE (m) unstable unstable SyU MU = Fg Iteraton Path errors n each teraton R k + u k cos ( u k + 3 R k) t R k + u k sn( u k + 3 R k) t 3 R k + u k sn( u k) t d h (k) ( ) ( ) f = k R k + f k R ( k ) f arctan k R k f k R k 3 R k where R k k s the predcted state at tme k, u k = u k u k s the control nput, t s the tme step, d s the dstance between the wheel of the robot, R k -th scalar component of R k, and f s the coordnates of the -th feature; able III contans the root mean square error (RMSE) of the robot s poses n meters usng each sgma set It can be seen that the flters that used the reduced set of 6 and the sphercal set of 7 present numercal nstablty It happened because state s covarance matr tended to loss ts postveness Fg shows the errors of the robot s poses n each teraton for the symmetrc set and for the new mnmum set of heorem Analysng both able III and Fg, we can see the SLAM algorthm embedded wth the new mnmum sgma set, even usng less ponts, gave a better estmatve n comparson to the symmetrc set IV CONCLUSION We develop a new set of sgma ponts for non-lnear estmaton whch uses the mnmum amount of ponts hs set s sample mean and sample covarance matr are equal to the pror s RV mean and covarance matr respectvely Numercal eamples showed that the estmatves based on the proposed sgma set outperform the estmatves based on the most commonly used sgma sets Fnally, we test the same sgma sets for a recursve flter applcaton n a SLAM algorthm, V ACKNOWLEDGMENS he authors are supported by research grants 3348 / 9 7, 358 / - and / 8 3 from the Natonal Councl for Scentfc and echnologcal Development - CNPQ, Brazl he authors would also lke to thank the support of the Federal Dstrct s Foundaton for Assstance to Research - FAPDF REFERENCES S Juler, J Uhlmann, I Ind, and M Jefferson Cty, Unscented flterng and nonlnear estmaton, Proceedngs of the IEEE, vol 9, no 3, pp 4 4, 4 S Juler and H Durramt-whyte, Process Modes For he Hgh-speed Navgaton of Road Vehcles, In Practce, pp 5, S Juler and J Uhlmann, A new etenson of the Kalman flter to nonlnear systems, n Int Symp Aerospace/Defense Sensng, Smul and Controls, vol 3 Cteseer, 997, p 6 4 S Juler, J Uhlmann, and H F Durrant-whyte, A New Method for the Nonlnear ransformaton of Means and Covarances n Flters and Estmators, IEEE ransactons on Automatc Control, vol 45, no 3, pp , 5 S L and P N, Square-root unscented Kalman flter based smultaneous localzaton and mappng, n Informaton and Automaton (ICIA), IEEE Internatonal Conference on IEEE,, pp H Wang, S We, and Y Chen, An mproved rao-blackwellzed partcle flter for slam, Intellgent Informaton echnology Applcaton Workshops, 8 IIAW 8 Internatonal Symposum on, pp 55 58, Dec 8 7 N Sunderhauf, S Lange, and P Protzel, Usng the unscented kalman flter n mono-slam wth nverse depth parametrzaton for autonomous arshp control, Safety, Securty and Rescue Robotcs, 7 SSRR 7 IEEE Internatonal Workshop on, pp 6, Sept 7 8 S Holmes, G Klen, and D Murray, A square root unscented kalman flter for vsual monoslam, Robotcs and Automaton, 8 ICRA 8 IEEE Internatonal Conference on, pp , May 8 9 L Zhang, X Meng, and Y Chen, Unscented transform for slam usng gaussan mture model wth partcle flter, Electronc Computer echnology, 9 Internatonal Conference on, pp 7, Feb 9 B He, L Yang, K Yang, Y Wang, N Yu, and C Lu, Localzaton and map buldng based on partcle flter and unscented Kalman flter for an auv, n Industral Electroncs and Applcatons, 9 ICIEA 9 4th IEEE Conference on, May 9, pp J Zhu, N Zheng, Z Yuan, Q Zhang, and X Zhang, Unscented slam wth condtonal teratons, n Intellgent Vehcles Symposum, 9 IEEE, June 9, pp W Yu-we and Z Zong-y, Improvement of the smultaneous localzaton and map buldng algorthm applyng scaled unscented transformaton, n Industral Mechatroncs and Automaton, 9 ICIMA 9 Internatonal Conference on, May 9, pp C Km, H Km, and W K Chung, Eactly rao-blackwellzed unscented partcle flters for slam, n Robotcs and Automaton (ICRA), IEEE Internatonal Conference on, may, pp V Char and C Jawahar, Multple plane trackng usng unscented kalman flter, n Intellgent Robots and Systems (IROS), IEEE/RSJ Internatonal Conference on, oct, pp M Anjum, J Park, W Hwang, H l Kwon, J hyeon Km, C Lee, K soo Km, and D l Danr Cho, Sensor data fuson usng unscented kalman flter for accurate localzaton of moble robots, n Control Automaton and Systems (ICCAS), Internatonal Conference on, oct, pp S Juler and J Uhlmann, Reduced sgma pont flters for the propagaton of means and covarances through nonlnear transformatons, n Amercan Control Conference, Proceedngs of the, vol IEEE,, pp S Juler, I Ind, and M Jefferson Cty, he sphercal smple unscented transformaton, n Amercan Control Conference, 3 Proceedngs of the 3, vol 3, 3, pp H Menegaz, A Sstematzaton of the Sgma Pont Estmaton, Master s thess, Unversty of Brasíla, 9 R van der Menve, Sgma-Pont Kalman Flters for Probablstc Inference n Dynamc State-Space Models, PhD dssertaton, Unversty of Stellenbosch, Abrl 4 377