Invariant EKF Design for Scan Matching-aided Localization

Transcription

1 Invarant EKF Desgn for Scan Matchng-aded Localzaton Martn Barczyk, Slvère Bonnabel, Jean-Emmanuel Deschaud, Franços Goulette To cte ths verson: Martn Barczyk, Slvère Bonnabel, Jean-Emmanuel Deschaud, Franços Goulette. Invarant EKF Desgn for Scan Matchng-aded Localzaton. IEEE Transactons on Control Systems Technology, Insttute of Electrcal and Electroncs Engneers, 215, 23 (6), pp <1.119/TCST >. <hal > HAL Id: hal Submtted on 15 Jan 216 HAL s a mult-dscplnary open access archve for the depost and dssemnaton of scentfc research documents, whether they are publshed or not. The documents may come from teachng and research nsttutons n France or abroad, or from publc or prvate research centers. L archve ouverte plurdscplnare HAL, est destnée au dépôt et à la dffuson de documents scentfques de nveau recherche, publés ou non, émanant des établssements d ensegnement et de recherche franças ou étrangers, des laboratores publcs ou prvés.

2 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 1 Invarant EKF Desgn for Scan Matchng-aded Localzaton Martn Barczyk, Member, IEEE, Slvère Bonnabel, Jean-Emmanuel Deschaud, and Franços Goulette, Member, IEEE Abstract Localzaton n ndoor envronments s a technque whch estmates the robot s pose by fusng data from onboard moton sensors wth readngs of the envronment, n our case obtaned by scan matchng pont clouds captured by a low-cost Knect depth camera. We develop both an Invarant Extended Kalman Flter (IEKF)-based and a Multplcatve Extended Kalman Flter (MEKF)-based soluton to ths problem. The two desgns are successfully valdated n experments and demonstrate the advantage of the IEKF desgn. Index Terms Moble robots, State estmaton, Kalman flters, Iteratve closest pont algorthm, Covarance matrces, Least squares methods, Addtve nose I. INTRODUCTION AND LITERATURE REVIEW Localzaton s a fundamental buldng block for moble robotcs [1]. For operatons n envronments where GPS s not avalable e.g. ndoor navgaton, or for stuatons where GPS s avalable but degraded, the uncertanty n the vehcle pose can be reduced by usng nformaton from a 3D map [2]. Fundamentally ths represents a sensor fuson problem, and as such t s typcally handled va an Extended Kalman Flter (EKF) c.f. the textbooks [3], [4], although a number of drect nonlnear observer desgns have been proposed e.g. [5], [6], [7]. The EKF approach offers two practcal advantages over these: t s a fully systematc desgn procedure, and t admts a probablstc nterpretaton. Indeed t can be vewed as a maxmum lkelhood estmator that combnes sensor nformaton n an optmal way (n the lnear case), wth the confdence n each measurement beng descrbed by a covarance matrx. In ths paper, we address a localzaton problem, where pose estmates from proproceptve sensors are obtaned by matchng mages obtaned from a depth scanner (scan matchng or LIDAR-based odometry [8]) wth a 3D map. We use the Iteratve Closest-Pont (ICP) algorthm [9], [1] on 3D pont clouds obtaned from a low-cost Knect M. Barczyk s wth the Department of Mechancal Engneerng, Unversty of Alberta, Edmonton, AB, T6G 2G8 Canada martn.barczyk@ualberta.ca. S. Bonnabel, J-E. Deschaud and F. Goulette are wth MINES ParsTech, PSL - Research Unversty, Centre de robotque, 6 Bd St Mchel 756 Pars, France {frstname.lastname}@mnes-parstech.fr March 2, 215

3 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 2 camera mounted on our expermental wheeled robot. Durng the moton we assume an already bult 3D gross map made of clouds of ponts s avalable. When a scan contans substantally more nformaton than the map, t can be aggregated to the exstng map. Further detals wll be provded n Secton III. The sensor fuson EKF works by lnearzng a system about ts estmated trajectory, then usng estmates obtaned from an observer for ths lnearzed model to correct the state of the orgnal system. In ths way the EKF reles on a closed loop whch can be destablzed by suffcently poor estmates of the trajectory, known as dvergence [3]. Clearly, reducng or elmnatng the dependence of the EKF on the system s trajectory would ncrease the robustness of the overall system. An emergng methodology to accomplsh ths goal s the Invarant EKF [11], [12], bult on the theoretcal foundatons of nvarant (symmetry-preservng) observers [13], [14]. The IEKF technque has already demonstrated expermental performance mprovements over a typcal [4] Multplcatve EKF (MEKF) n aded nertal navgaton desgns [15], [16]. From a practcal vewpont, the advantage of EKF-lke technques s ther automatc tunng of gans based on the estmated covarance of the measurements. Applyng the IEKF to an aded scan matchng problem was frst demonstrated n [17] for 3D mappng. A prelmnary verson of ths work appears n conference proceedngs [18]. The contrbutons of the present paper are as follows: 1) Desgnng a scan matchng-aded localzaton system for a wheeled ndoor robot whch fuses robot odometry wth Knect-based scan matchng, 2) A method to compute a realstc covarance matrx assocated to the scan matchng step and mplctly detect underconstraned envronments, 3) Provdng both an IEKF (whose non-lnear structure takes advantage of the the geometry of the problem) and a MEKF verson of the state estmator for ths problem, 4) Expermentally valdatng both desgns, and comparng ther accuracy w.r.t. ground truth data provded by an optcal moton capture system. Ths paper s structured as follows. A bref lterature revew and problem motvaton have been provded above. Secton II provdes the model of the system dynamcs and derves lnearzng approxmatons used n the sequel. Secton III covers the process of obtanng adng measurements from scan matchng of pont clouds provded by a Knect depth camera mounted on the robot, and provdes a meanngful and smple method to compute a covarance matrx assocated to the measurements. Secton IV provdes the equatons of the nvarant observer, IEKF and MEKF state estmators, whch are then expermentally valdated n Secton V. Conclusons and future work drectons are gven n Secton VI. A. Noseless System II. SYSTEM MODELS In the absence of sensor nose, the dynamcs of a rgd-body vehcle are governed by Ṙ = RS(ω) ṗ = Rµ (1) March 2, 215

4 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 3 where R SO(3) s a rotaton matrx measurng the 3D atttude of the vehcle, ω R 3 s the body-frame angular velocty vector, S( ) s the 3 3 skew-symmetrc matrx such that S(x)y = x y where denotes the R 3 crossproduct,p R 3 s the poston vector of the vehcle expressed n coordnates of the ground-fxed frame, and µ R 3 s the velocty vector of the vehcle expressed n body-frame coordnates. The body-frame angular velocty ω and lnear velocty µ vectors can be measured drectly usng on-board moton sensors, e.g. a traxal rate gyro for ω and a Doppler radar for µ. In the case of a wheeled vehcle travelng over flat terran, odometry nformaton from the left and rght wheels can be used to compute the forward component of vector µ and the vertcal component of vector ω, takng the remanng components as zero. As wll be dscussed n Secton III the vehcle s atttude R and poston p are computed by scan matchng mages from the on-board Knect depth camera wth a map of 3D ponts va the Iteratve Closest Pont (ICP) algorthm. Ths gves output equatons y R = R (2) p y p B. Sensor nose models In realty the nput sgnals ω and µ n (1) and the output readngs y R, y p n (2) are corrupted by sensng nose. For the former we employ the sensor model typcally used n aded navgaton desgn [4] ω = ω +ν ω (3) µ = µ+ν µ where ν N(,σ 2 ) terms represent addtve Gaussan whte nose vectors whose covarance can be drectly dentfed from logged sensor data. Remark the nose vectors ν ω and ν µ are expressed n coordnates of the bodyfxed frame. The nose models for scan matchng outputs (2) are not standard and wll be derved n Secton III-D. C. Geometry of SO(3) The specal orthogonal group SO(3) s a Le group. For any Le group G, there exsts an exponental map exp : g G whch maps elements of the Le algebra g to the Le group G. It s known (e.g. [19, p. 519]) that for any X g, (exp X) 1 = exp( X), that exp s a smooth map from g to G, and that exp restrcts to a dffeomorphsm from some neghborhood of n g to a neghborhood of the dentty element e n G. The last fact means there exsts n a neghborhood U of e an nverse smooth map log : G g such that exp log(g) = g, g U. For the partcular case of SO(3), the assocated Le algebra so(3) s the set of 3 3 skew-symmetrc matrces ξ := S(x),x R 3 and exp : so(3) SO(3) s the matrx exponental whle log : SO(3) so(3) s gven by expξ = I +ξ + 1 2! ξ2 + logr = αs(β) where α : 2cosα+1 = trace(r) and S(β) = 1 2snα (R RT ) March 2, 215

5 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 4 Now consder the specal case of R SO(3) close to I. Snce the exponental map restrcts to a dffeomorphsm from a neghborhood of zero n so(3) to a neghborhood of dentty n SO(3), ξ so(3) s close to the matrx 3 3 such that ξ 2 and hgher-order terms n R expξ are neglgble and thus Snce (expξ) 1 = exp( ξ) we also have R I +ξ, R SO(3) close to I (4) R 1 I ξ, R SO(3) close to I (5) Stll for R close to I, trace(r) 3 such that α and so logr (R R T )/2. We defne the projecton map π : SO(3) so(3), π(r) = R RT 2 whch s defned everywhere but s the nverse of exp only for R expξ close to I. In ths case π(r) ξ and (4) becomes Approxmatons (4), (5) and (6) wll be employed n the sequel. R I π(r), R SO(3) close to I (6) III. DEPTH CAMERA AIDING A. Camera Hardware Our test robot s equpped wth a Knect depth camera, a low-cost (currently around 1 e) gamng perpheral sold by Mcrosoft for the XBox 36 console. The Knect s sensng technology s descrbed n [2]. The unt employs an nfrared laser to project a speckle pattern ahead of tself, whose mage s read back usng an nfrared camera offset from the laser projector. By correlatng the acqured mage wth a stored reference mage correspondng to a known dstance, the Knect computes a dsparty map (standard termnology n stereo camera vson) of the scene whch s employed to construct a 3D pont cloud of the envronment n the robot-fxed frame. The dsparty maps computed by the Knect, correspondng to ndvdual pxels of the IR camera mage, are avalable as mages at a rate of 3 Hz. Physcally the IR camera has a total angular feld of vew of 57 horzontally and 43 vertcally, such that the constructed pont cloud s farly narrow as compared to tme-of-flght scannng laser unts such as the Hokuyo UTM-3LX (27 ) or the Velodyne HDL-32E (36 ). The maxmum depth rangng lmt of the Knect s 5 m, n contrast to the UTM-3LX (3 m) and HDL-32E (7 m). The Knect can only be utlzed under ndoor lghtng condtons. But conversely, the Knect s much less expensve than the Hokuyo (about 45 e) and Velodyne (about 22e) unts and unlke the scannng unts s not prone to pont cloud deformaton at hgh vehcle speeds. The scan matchng algorthms developed for Knect pont clouds are adaptable to these unts. A comprehensve analyss of the Knect s accuracy and precson s carred out n [21], showng that pont cloud measurements are affected by both nose and resoluton errors whch grow sgnfcantly wth dstance from the camera. Consequently the recommended usable range nterval s 1 z 3 m. March 2, 215

6 f(x) = IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 5 B. ICP Algorthm The ICP algorthm [9], [1] s an teratve procedure for fndng the optmum rgd-body transformaton(δr, δt) SO(3) R 3 between two sets of ponts n R 3 (clouds) {a } and {b }, whch do not necessarly have equal number of entres. Followng the taxonomy n [22] an ICP algorthm conssts of the followng (terated) sequence of steps: 1) Select source ponts from one or both clouds. We select a total of 3 ponts (about 1% of the avalable) from both clouds and located away from edges, compute ther correspondng unt normals {n } by fttng a plane through neghborng ponts [23], and employ the strategy of normal-space samplng [22]. 2) Match the source ponts wth those n the other cloud(s) and reject poor matches. We employ a nearestneghbor search accelerated by a k d tree [24], then reject pars whose pont-to-pont dstance exceeds the threshold value of.25 m or whose assocated normals form an angle larger than 45. 3) Mnmze the error cost functon. We choose the pont-to-plane ICP varant [1] f(δr,δt) = N [ ] 2 (δra +δt b ) n (7) =1 and apply lnearzaton to solve (7) as explaned below. 4) Termnate f convergence crtera met, else goto 1). For smplcty we do not employ an early stop condton and always run 25 teratons of the ICP algorthm. The heart of the ICP algorthm les n the way the matchng step s done, as the ponts of the two clouds are arbtrarly labeled and do not ntally match correctly. Lnearzaton s justfed provded δr s close to I, equvalent to clouds {a } and {b } startng off close to each other. As explaned n Secton III-D the closeness assumpton s vald due to pre-algnng of the two clouds usng an estmate of pose obtaned at the predcton step of the overall flter. We thus employ lnearzaton (4) n (7) to obtan f(x) = [ ] 2 [ (xr a +x T +a b ) n = (a n ) x R +n x T +n (a b ) ] 2 where we have used the scalar trple product crcular property (a b) c = (b c) a. In the lnearzed context, mnmzng the ICP cost functon (7) s equvalent to mnmzng (8) f : R 6 R by choce of x = [x R x T ] T. We defne the terms H := [ ] (a n ) T n T y := n T (a b ) (8) to rewrte (8) as [ H x+y ] 2 (9) The mnmum of (9) f(x) s acheved at x for whch f/ x = snce 2 f/ x 2 = 2A everywhere wth A := (H ) T H = (a n )(a n ) T (a n )n T n (a n ) T n n T b := (H ) T y = (a n )n T (a b ) n n T (a b ) March 2, 215

7 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 6 Ths mnmum s found by solvng the lnear system of equatons Ax = b. Then up to second order terms, (R,p) = (I +S(x R ),x p ) s the rgd-body transformaton whch mnmzes the pont-to-plane error (7). C. Covarance of ICP output As stated n Secton III-A the measured pont clouds {a } and {b } are affected by sensor nose and resoluton errors. We requre a method to estmate the covarance (uncertanty) of the rgd-body transformaton obtaned by runnng the ICP algorthm between measured pont clouds. We contnue to assume {a } and {b } start close to each other (the open-loop ntegraton of the moton sensors allows to pre-algn the clouds). We can thus model the (pont-to-plane) ICP as a lnear least-squares estmator mnmzng (9) resp. (8). Let x ICP denote the estmate computed by the ICP from nosy pont cloud data {a } and {b } and x the true transformaton. The assocated covarance s cov(x ICP ) = E (x ICP x )(x ICP x ) T (1) Based on the lnear least-squares cost functon (9) we defne the resduals as the error purely due to sensor nose, that s, the error between each measured pont and the orgnal pont transformed through the true transformaton: r := H x +y (11) The estmate x ICP mnmzng (9) was already shown to be x ICP = A 1 b = [ (H ) T H ] 1 (H ) T y. Rewrte ths usng (11) as x ICP = [ (H ) T H ] 1 (H ) T (H x r ) = x A 1 (H ) T r and (1) becomes ( cov(x ICP ) = E A 1 )( (H ) T r A 1 ) T (H ) T r [ ] 1 ) = (H ) T H ((H [ ] 1 (12) ) T E r r j H j (H ) T H j In order to evaluate (12) we need to assume a nose model on the resduals r n (11). Expandng the rght-hand sde usng the defntons of H and y we have r = (a n ) T x R +nt x T +nt (a b ) = [(x R a )+x T +(a b )] n := w n where w R 3 represents the post-algnment error of the th pont par due (only) to the presence of sensor nose n pont clouds {a } and {b }. The resdual r = w n = n T w now represents the projecton of w and (12) becomes [ ] 1 ) cov(x ICP ) = (H ) T H ((H [ ) T n T E w wj T n ] 1 jh j (H ) T H (13) j The classcal Hessan method [25], [17], [18] conssts of assumng the post-algnment errors w are ndependent and dentcally normally sotropcally dstrbuted wth standard devaton σ. Under these assumptons E w wj T = E w E wj T =, j and the double sum n (13) reduces to a sngle sum (nt n = 1 for unt normals): [ ] 1 cov(x ICP ) = (H ) T H σ 2 [ ] 1 [ ] 1 (H ) T H (H ) T H = σ 2 (H ) T H March 2, 215

8 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 7 Ths s precsely the covarance of a lnear unbased estmator usng observatons wth addtve whte Gaussan nose c.f. [26, p. 85]. However, there s a catch: for a Knect sensor wth N 3 ponts per cloud and σ 1 cm for depth range 1 z 3 m [21, Fg. 1], the above expresson yelds a covarance matrx wth entres on the order of nanometers, a completely overoptmstc result for the low-cost Knect sensor. Ths result stems from the ndependence assumpton whch makes the estmator converge as 1/ N where N s the (hgh) number of ponts. We thus need to consder a more complete error model for the resduals. In addton to random nose on the resduals whose contrbuton to cov(x ICP ) rapdly becomes neglgble as the number of pont pars N ncreases, the Knect exhbts resoluton errors on the order of δ 1 cm for 1 z 3 m [21, Fg. 1] due to quantzaton errors ncurred durng IR mage correlaton to a reference mage as dscussed n Secton III-A. These resoluton errors are non-zero mean and are not ndependent of each other and actually consttute the domnant lmtaton n accuracy. As shown n our prelmnary work [27], ths leads to the followng approxmaton of the covarance matrx (13): cov(x ICP ) δ 2 N [ ] 1 (H ) T H = δ 2 N N (a n )(a n ) T N p N p n (a n ) T =1 1 (a n )n T (14) n n T where N p = 3 s the number of plane buckets used for normal-space samplng [22] n Step 1 of the ICP algorthm, c.f. Secton III-B. Note that the covarance matrx correctly reflects the observablty of the envronment: for nstance f the envronment conssts of a sngle plane, all n s are dentcal leadng to rank defcency of the matrx to be nverted, thus the covarance s nfnte along drectons parallel to the plane. Also note that the overall ICP varance s on the order of the Knect s precson δ 2 n the case of full observablty, as expected. D. Scan Matchng as Measured Output Suppose a 3D map of the envronment made up of pont clouds s already avalable, bult ether by the robot or from lasergrammetry. Assumng the scan from the on-board depth camera and the map contan a set of dentcal features, the robot s pose wth respect to the map can be dentfed through an ICP algorthm. However, due to sensor nose and quantzaton effects, the computed pose s nosy. Denote the ntal estmate of the robot s pose (obtaned from numercal ntegraton of the vehcle dynamcs (1)) as (R,p ). Ths estmate s used to pre-algn the two scans (the current scan and the map) n the body-fxed frame; ths s requred snce the ICP does not guarantee global convergence, and n fact can easly get stuck at a local mnmum. The robot s true pose can then be expressed up to second order terms as (R (I+S(x R )),R x p +p ), that s the vector (x R,x p) R 6 s defned as the dscrepancy of the ntal estmaton (R,p ) and the true pose (R,p ) projected n the Le algebra R 6 and s the vector to be estmated by the ICP algorthm. Based on Secton III-C, we have seen that the ICP output wrtes x ICP = (x R,x p)+(ν R,ν p ) where the covarance of the vector (ν R,ν p ) R 6 s gven by (13) and can be approxmated by the smple to compute expresson (14). Usng the lnearty of the map S, we see that I +S((x ICP ) R +ν R ) = I +S(x R )+S(ν R) and so the pose output March 2, 215

9 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 8 from scan matchng s (up to second-order terms) ỹr = R + R S(ν R ) (15) ỹ p p R ν p The above s a nosy verson of (2), where the nose covarance cov(υ), υ := [ν R ν p ] T s (approxmately but effcently) computed by (14). A. Invarant Observer IV. STATE ESTIMATOR DESIGN We frst desgn an nvarant observer for the nomnal (nose-free) system (1), (2) by followng the constructve method n [13], [14]; a tutoral presentaton s avalable n [16] and so the calculatons wll be omtted. We consder the case where the Specal Eucldean Le group SE(3) = SO(3) R 3 acts on the SE(3) state of (1) by left translaton, whch physcally represents applyng a constant rgd-body transformaton (R,p ) to ground-fxed frame vector coordnates. Ths leads to the nonlnear nvarant observer ˆR = ˆRS(ω) + ˆp ˆR S( L R R E R +L R p E ) p (16) ˆRµ L p R E R +L p pe p where L R R, LR p, Lp R, Lp p are R3 3 gan matrces and E R := S 1 (π(ˆr T y R )), E p = ˆR T (ˆp y p ) are the nvarant output error column vectors. Standard computatons n the framework of symmetry-preservng observers show the assocated nvarant estmaton errors η R = R T ˆR, ηp = R T (ˆp p) have dynamcs η R = S(ω)η R +η R S(ω)+η R S ( L R R E R +L R p E ) p ( η p = S(ω)η p +η R µ µ+η R L p R E R +L p ) pe p and stablzng the (nonlnear) dynamcs (17) to η = I by choce of gans L leads to an asymptotcally stable nonlnear observer (16). The stablzaton process s smplfed by (17) not beng dependent on the estmated system state ˆx; ndeed the fundamental feature of the nvarant observer s that t guarantees η = Υ(η,I(ˆx,u)) [13, Thm. 2] where I(ˆx,u) = [ω µ] s the set of nvarants n the present example. We wll compute the stablzng gans usng the Invarant EKF method n order to handle varable scan matchng observablty of the envronment. (17) B. Invarant EKF The Invarant EKF [11] s a systematc approach to computng the gans K of an nvarant observer by lnearzng ts nvarant estmaton error dynamcs. A nosy verson of (1) s readly obtaned by accountng for the sensors nose by expressng the measured angular and lnear veloctes as the nosy vectors ω = ω+ν ω and µ = µ+ν µ and lettng Q ν := cov[ν ω ν µ ] T denote the process whte nose covarance matrx. The entres of Q ν can be dentfed drectly from logged sensor data, whle R ν := cov[ν R ν p ] T s computed by (13) (or more smply (14)). March 2, 215

10 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 9 Followng the IEKF method, we frst wrte a nosy verson of the error dynamcs (17) where we let ω = ω ν ω and µ = µ ν µ, where ω, µ denote the nosy nputs read from onboard sensors, and the output s gven by (15). Ths equaton s lnearzed usng the standard methodology of symmetry-preservng observers,.e. usng vectors of R 3 to denote the orentaton error by lettng η R := I +S(ζ R ), ζ R R 3 by (4) and lettng η p := ζ p R 3. Up to second order terms n ζ, ν, the nosy verson of (17) s approxmated by the followng lnear equaton: d ζ R = S( ω) ζ R + ν ω + LR R LR p ζ R LR R LR p ν R dt ζ p S( µ) S( ω) ζ p ν µ L p R Lp p ζ p L p R Lp p ν p The ratonale of the IEKF s to tune the gans through Kalman theory n order to mnmze at each step the ncrease n the covarance of the lnearzed error ζ. Ths s done through the standard Kalman flter equatons, lettng A = S( ω), B = I, C = I, D = I, K = LR R LR p S( µ) S( ω) I I I L p R Lp p (18) and tunng the gans through the standard Rccat equatons n contnuous tme of the Kalman-Bucy flter P = AP +PA T PR 1 ν P +Q ν K = PR 1 ν where the gans L makng up K are employed n the nvarant observer (16). Remark the IEKF flter matrces (A,B,C,D) are dependent on the system trajectory only through the ω m and µ m terms n A, and do not depend on the estmates (ˆR, ˆp) as n the usual Extended Kalman Flter. The nterest of the Invarant EKF s ndeed the reduced dependence of the lnearzed system on the estmated trajectory of the target system. In our present example the (A,B,C,D) matrces are guaranteed not to depend on the estmated state, whch ncreases the flter s robustness to poor state estmates and precludes dvergence (c.f. Secton I). (19) C. Multplcatve EKF desgn For comparson purposes consder a typcal [4] Multplcatve Extended Kalman Flter (MEKF) desgn for our system. The governng system equatons are gven by (1) wth nose models (3) and output model (15): Ṙ = RS( ω ν ω ) ṗ = R( µ ν µ ) y R = R+RS(ν R) y p p+rν p (2) We lnearze (2) about a nomnal system trajectory (ˆR, ˆp). Remark (R ˆR) / SO(3) s not a vald lnearzed system state. Instead defne the multplcatve atttude error Γ := ˆR T R SO(3) such that for R close to ˆR, Γ s close to I. By (4) Γ := I + S(δγ), δγ R 3 and so ˆR T R = I + S(δγ) = R ˆR = ˆRS(δγ). We defne δp = p ˆp, the output errors δy R := S 1 [π(ˆr T y R )] wth π : SO(3) so(3) from Secton II-C and δy p := y p ˆp March 2, 215

11 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 1 and obtan the lnearzed system δ γ = S( ω) δγ + I ν ω δṗ ˆRS( µ) δp ˆR ν µ δy R = I δγ + I ν R δy p I δp ˆR ν p (21) an LTV system tractable usng the classcal Kalman Flter. The resultng [δp δγ] estmate s used to update the estmated state of the nonlnear system (2) as follows. Note S(δγ) so(3) corresponds to Γ = ˆR T R SO(3) and by Secton II-C exp : so(3) SO(3) s the matrx exponental. Thus the estmated states are updated as ˆp + = ˆp+δp and ˆR+ = ˆRexpS(δγ) The man dfference between the MEKF and the IEKF s that the former lnearzes the system dynamcs (2) about a nomnal trajectory, whle the latter lnearzes the nvarant estmaton error dynamcs (17) about dentty. As dscussed n Secton IV-A and IV-B, the latter dynamcs do not depend on the estmated state ˆx such that the lnearzed IEKF system s guaranteed to be robust to poor estmates of state. Meanwhle the MEKF cannot make ths guarantee and ndeed the lnearzed system matrces (21) depend on the estmated state va ˆR. The dfference n expermental estmaton performance of the two desgns wll be demonstrated n Secton V. A. Hardware platform V. EXPERIMENTAL VALIDATION Fg. 1. The Wfbot Lab v4 Robot (left); Experment area wth moton-capture system (rght) The wheeled robot used for our experments s shown on the left of Fgure 1. The robot s equpped wth an Intel Core 5-based sngle-board computer runnng Ubuntu Lnux, WLAN 82.11g wreless networkng, all-wheel drve va 12 V brushless DC motors, and a Knect camera provdng 3-D pont cloud scans of the envronment. The March 2, 215

12 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 11 expermental testng area s shown on the rght sde of Fgure 1, and conssts of an open area surrounded by a set of seven S25e cameras employed by an OptTrack moton capture system to provde a set of ground truth (reference trajectory) data for the experments wth sub-mllmeter precson at a rate of 12 Hz. The wooden parquet floor seen n Fgure 1 s not perfectly flat, causng small oscllatons n the vehcle s atttude and heght whch wll be vsble n the expermental plots. A number of vsual landmarks (rectangular boxes) were placed randomly around the expermental area n order to provde good scan-matchng condtons. The robot s equpped wth ndependent odometers on the left and rght wheels. The two odometer counts are averaged and converted to forward velocty µ x by dvdng by an expermentally-dentfed constant κ 1 = 788 m 1 representng the number of encoder counts per meter of travel. Assumng zero sde slp as well as zero out-of-plane velocty, we obtan the body-fxed velocty vector µ = [µ x ] T m/s used n noseless vehcle dynamcs (1). Snce the present vehcle s not equpped wth a rate gyro, we employ the dfference n odometer counts, dentfed constant κ 2 =.44 m representng the lateral dstance between wheel-ground contact ponts and κ 1 to compute n-plane angular velocty ω z then employ the angular velocty vector ω = [ ω z ] T rad/s n (1),.e. assume zero roll and ptch angular velocty due to the vehcle beng level, wth the non-flatness of the floor reflected by addtve sensor nose vectors ν µ and ν ω. The covarances of ν µ and ν ω were assgned by frst dentfyng the on-axs varances σµ 2 x =.1 2 m 2 /s and σω 2 z =.2 2 rad 2 /s of data logged durng respectvely constantvelocty advance and constant-velocty crcle trajectores, then takng dag(cov(ν µ )) = [σµ 2 x.1σµ 2 x.1σµ 2 x ] and dag(cov(ν ω )) = [.1σω 2 x.1σω 2 x σω 2 z ] on account of the uneven terran. Clearly the assumptons n the prevous paragraph are both optmstc and ad-hoc, for nstance the zero-slp assumpton does not fully hold, the nose covarances are assgned heurstcally, and the encoder-derved data s subject to dentfcaton errors of κ and quantzaton effects. Ths s acceptable for our purposes, however, snce we are nterested n comparng the expermental performance of two competng flter desgns more than the absolute accuracy of the estmates. Snce we wll employ dentcal sensor data n both desgns, we wll be able to make a far comparson between the two. For each experment, the samplng rate of the wheel odometers was set to 5 Hz and the Knect depth mages to 1 Hz. The latter s a farly slow rate for scan matchng and was chosen specfcally to test the robustness of each of the two flters. The trajectores were steered n open-loop mode by settng left and rght wheel veloctes. The map was bult from the ntal robot s pose. The resultng sensor data was logged to the on-board memory and then run through both the Invarant EKF and the Multplcatve EKF whose desgns were covered throughout Secton IV. When an mage was found to possess a substantal amount of novel nformaton compared to the exstng map t was aggregated to the exstng map. For farness of comparson the ICP algorthm and assocated parameters e.g. number of pont pars and rejecton crtera (c.f. Secton III-B) were dentcal among the flters. B. Frst Experment In the frst experment, the Wfbot starts statonary near an edge of the boundng area wall, then advances n a straght lne towards the opposte wall, where t stops. The poston and atttude (converted to Euler angles) estmates March 2, 215

13 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 12 from the IEKF and MEKF desgns are plotted aganst the OptTrack ground truth n Fgure 2. For vsualzaton purposes Fgure 2 also provdes an overhead vew of the postons, plus the values of the 3 3 subset of the Kalman gan matrx K actng on planar poston and headng angle error states va ther correspondng errors. 5 x [m] y [m].1 x [m] 1 z [m] y [m] φ [deg] θ [deg] ψ [deg] K 3,3 [rad/rad] K 4,3 [m/rad] K 5,3 [m/rad] K 3,4 [rad/m] K 4,4 [m/m] K 5,4 [m/m] K 3,5 [rad/m] K 4,5 [m/m] K 5,5 [m/m] Fg. 2. Lnear trajectory experment: IEKF ( ), MEKF ( ), Ground truth ( ) From Fgure 2 we see that the IEKF and MEKF desgns perform nearly the same. In the fnal statonary confguraton, the IEKF exhbts an n-plane error of ( x, y, ψ) = (6.2 cm,8.5 cm,1.6 ) from the ground truth, whle for the MEKF ths error s (6. cm,7.8 cm,1.9 ) the dfference beng wthn the uncertanty of the system. The RMS of the vector of errors between estmated trajectores and ground truth throughout the full experment are RMS( x, y, ψ) = (3.8 cm,4.9 cm,1.1 ) for the IEKF and (3.6 cm,4.4 cm,1. ) for the MEKF, agan wthn uncertanty. In addton the Kalman gan matrx entres are seen to behave nearly the same. Ths almost-dentcal performance s as expected because for a lnear trajectory both flters reduce to a lnear Kalman flter. We wll be consderng a non-lnear robot trajectory n the next experment. Note that the OptTrack data reflects the unevenness of the wooden floor mentoned n Secton V-A, whch causes the robot to sway wth an order-of-magntude ampltude of 1 n the roll and ptch axes and heave wth ampltude 1 cm n the vertcal axs. Ths acts as an exogenous dsturbance to the system, and as prevously dscussed s accounted for by the addtve process nose vectors ν µ and ν ω. March 2, 215

14 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 13 C. Second Experment In the second experment the robot begns statonary, then executes a crcular (thus non-lnear) trajectory consstng of two dentcal crcles traveled n the counter-clockwse drecton, obtaned by settng dfferng constant speeds on the left and rght wheels. As n the prevous experment, for each of the two flters we plot the estmated system states versus the ground truth reported by the OptTrack system, as well as an overhead vew of the postons for vsualzaton purposes and a 3 3 subset of the Kalman gan matrx K entres. The results are shown n Fgure x [m] y [m] x [m] z [m] y [m].5 ψ [deg] φ [deg] θ [deg] K 3,3 [rad/rad] K 4,3 [m/rad] K 5,3 [m/rad] K 3,4 [rad/m] K 4,4 [m/m] K 5,4 [m/m] K 3,5 [rad/m] K 4,5 [m/m] K 5,5 [m/m] Fg. 3. Crcular trajectory experment: IEKF ( ), MEKF ( ), Ground truth ( ) Fgure 3 clearly shows that the performance of the two estmators s no longer dentcal, and that the IEKFcomputed estmates are closer to the ground truth than the MEKF ones. In the fnal confguraton, the IEKF has an error of ( x, y, ψ) = (14.3 cm,1.2 cm,9.7 ) whle the MEKF has (29.4 cm,5.7 cm,21.3 ). For RMS values of the estmaton dscrepancy vectors, the IEKF shows RMS( x, y, ψ) = (1.6 cm,14.2 cm,5.7 ) and the MEKF (18.5 cm,21.3 cm,12.4 ). Thus n ths non-lnear trajectory case, the IEKF exhbts an estmaton performance advantage over the MEKF. The Kalman gan matrx entres plotted n Fgure 3 demonstrate a feature of the IEKF already dscussed n Secton IV-B: the ndependence of the lnearzed Kalman Flter dynamcs (as the matrces (A, B, C, D) used to compute P hence K) on the estmated system state ˆR, c.f. (18) for IEKF versus (21) for the MEKF. Indeed March 2, 215

15 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 14 throughout the crcular trajectory where ˆR vares wth tme, the IEKF gans reman approxmately level whle the MEKF gans oscllate note that they can not reman absolutely level, as the IEKF gans depend on the scan matchng covarance R ν whch vares throughout the trajectory due to nhomogenetes n the envronment. Ths s logcal as the gans should defntely adapt to the envronment s observablty, such that the flter does not trust the scan matchng output when the envronment s underconstraned and contans no nformaton along some specfc drecton(s). The IEKF desgn s greater ndependence from estmated states s expected to provde better estmaton accuracy than n the MEKF desgn, and ths s ndeed what we see here. VI. CONCLUSIONS We have successfully desgned and expermentally valdated a Knect depth camera scan matchng-aded Invarant EKF-based localzaton system and compared t aganst a Multplcatve EKF-based desgn n terms of theoretcal features and estmaton performance relatve to a ground truth. The fundamental advantage of the IEKF s ts guaranteed ncrease n robustness to poor estmates of state, a fundamental weakness of the MEKF desgn. We descrbed both flter desgns and tested them expermentally, demonstratng that the state estmates follow the ground truth and confrmng the consstency of the novel ICP covarance estmaton (14) derved n Secton III-C. Expermental testng llustrated the theoretcal features and advantages of the IEKF and demonstrated ts mproved estmaton accuracy over the MEKF desgn for a crcular (non-lnear) robot trajectory. Future work nvolves mxng the ntroduced technques wth pose-slam and smoothng n order to take advantage of the loop closures whch may occur durng the moton, tacklng the case where a 3D map s not avalable, and applyng the IEKF to more complcated problems such as outdoor moble cartography. ACKNOWLEDGMENTS We thank Tony Noël for hs extensve help wth settng up the Wfbot platform. The work reported n ths paper was partly supported by the Cap Dgtal Busness Cluster TerraMoblta Project. REFERENCES [1] S. Thrun, W. Burgard, and D. Fox, Probablstc Robotcs. MIT Press, 25. [2] S. Zhao and J. A. Farrell, 2D LIDAR aded INS for vehcle postonng n urban envronments, n Proceedngs of the 213 IEEE Mult-Conference on Systems and Control, Hyderabad, Inda, August 213, pp [3] R. G. Brown and P. Y. Hwang, Introducton to Random Sgnals and Appled Kalman Flterng, 3rd ed. John Wley & Sons, [4] J. A. Farrell, Aded Navgaton: GPS wth Hgh Rate Sensors. McGraw Hll, 28. [5] T. Chevron, T. Hamel, R. Mahony, and G. Baldwn, Robust nonlnear fuson of nertal and vsual data for poston, velocty and atttude estmaton of UAV, n Proceedngs of the 27 IEEE Internatonal Conference on Robotcs and Automaton, Roma, Italy, Aprl 27, pp [6] J. F. Vasconcelos, R. Cunha, C. Slvestre, and P. Olvera, A nonlnear poston and atttude observer on SE(3) usng landmark measurements, Systems & Control Letters, vol. 59, no. 3, pp , March 21. [7] G. G. Scandarol, P. Morn, and G. Slvera, A nonlnear observer approach for concurrent estmaton of pose, IMU bas and camerato-imu rotaton, n Proceedngs of the 211 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, San Francsco, CA, September 211, pp March 2, 215

16 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY 15 [8] F. Lu and E. E. Mlos, Robot pose estmaton n unknown envronments by matchng 2D range scans, n Proceedngs of the 1994 IEEE Computer Socety Conference on Computer Vson and Pattern Recognton, Seattle, WA, June 1994, pp [9] P. J. Besl and N. D. McKay, A method for regstraton of 3-D shapes, IEEE Transactons on Pattern Analyss and Machne Intellgence, vol. 14, no. 2, pp , February [1] Y. Chen and G. Medon, Object modellng by regstraton of multple range mages, Image and Vson Computng, vol. 1, no. 3, pp , Aprl [11] S. Bonnabel, Left-nvarant extended Kalman flter and atttude estmaton, n Proceedngs of the 46th IEEE Conference on Decson and Control, New Orleans, LA, December 27, pp [12] S. Bonnabel, P. Martn, and E. Salaün, Invarant Extended Kalman Flter: theory and applcaton to a velocty-aded atttude estmaton problem, n Proceedngs of the Jont 48th IEEE Conference on Decson and Control and 28th Chnese Control Conference, Shangha, P.R. Chna, December 29, pp [13] S. Bonnabel, P. Martn, and P. Rouchon, Symmetry-preservng observers, IEEE Transactons On Automatc Control, vol. 53, no. 11, pp , December 28. [14], Non-lnear symmetry-preservng observers on Le groups, IEEE Transactons On Automatc Control, vol. 54, no. 7, pp , July 29. [15] P. Martn and E. Salaün, Generalzed Multplcatve Extended Kalman Flter for aded atttude and headng reference system, n AIAA Gudance, Navgaton, and Control Conference, Toronto, Canada, August 21, AIAA [16] M. Barczyk and A. F. Lynch, Invarant observer desgn for a helcopter UAV aded nertal navgaton system, IEEE Transactons on Control Systems Technology, vol. 21, no. 3, pp , May 213. [17] T. Herver, S. Bonnabel, and F. Goulette, Accurate 3D maps from depth mages and moton sensors va nonlnear kalman flterng, n Proceedngs of the 212 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, Vlamoura, Algarve, Portugal, October 212, pp [18] M. Barczyk, S. Bonnabel, J.-E. Deschaud, and F. Goulette, Expermental mplementaton of an Invarant Extended Kalman Flter-based scan matchng SLAM, n Proceedngs of the 214 Amercan Control Conference, Portland, OR, June 214, pp [19] J. M. Lee, Introducton to Smooth Manfolds, 2nd ed., ser. Graduate Texts n Mathematcs. Sprnger, 213, vol [2] K. Konolge and P. Mhelch, Techncal descrpton of Knect calbraton, onlne. [21] K. Khoshelham and S. Oude Elbernk, Accuracy and resoluton of Knect depth data for ndoor mappng applcatons, Sensors, vol. 12, no. 2, pp , February 212. [22] S. Rusnkewcz and M. Levoy, Effcent varants of the ICP algorthm, n Proceedngs of the Thrd Internatonal Conference on 3-D Dgtal Imagng and Modelng, Quebec Cty, Canada, May 21, pp [23] K. Klasng, D. Althoff, D. Wollherr, and M. Buss, Comparson of surface normal estmaton methods for range sensng applcatons, n Proceedngs of the 29 IEEE Internatonal Conference on Robotcs and Automaton, Kobe, Japan, May 29, pp [24] J. H. Fredman, J. L. Bentley, and R. A. Fnkel, An algorthm for fndng best matches n logarthmc expected tme, ACM Transactons on Mathematcal Software, vol. 3, no. 3, pp , September [25] O. Bengtsson and A.-J. Baerveldt, Robot localzaton based on scan-matchng estmatng the covarance matrx for the IDC algorthm, Robotcs and Autonomous Systems, vol. 44, no. 1, pp. 29 4, July 23. [26] S. M. Kay, Fundamentals of Statstcal Sgnal Processng: Estmaton Theory. Prentce Hall, [27] M. Barczyk, S. Bonnabel, and F. Goulette, Observablty, covarance and uncertanty of ICP scan matchng, October 214, arxv: [cs.cv]. March 2, 215