Detecting and Dealing with Hovering Maneuvers in Vision-aided Inertial Navigation Systems

Transcription

1 Detectng and Dealng wth Hoverng Maneuvers n Vson-aded Inertal Navgaton Systems Dmtros Kottas, Kejan J Wu, and Stergos I Roumelots Abstract In ths paper, we study the problem of hoverng (e, absence of translatonal moton) detecton and compensaton n Vson-aded Inertal Navgaton Systems (VINS) We examne the system s unobservable drectons for two common hoverng condtons (wth and wthout rotatonal moton) and propose a robust moton-classfcaton algorthm, based on both vsual and nertal measurements By leveragng our observablty analyss and the proposed moton classfer, we modfy exstng state-of-the-art flterng algorthms, so as to ensure that the number of the system s unobservable drectons s mnmzed Fnally, we valdate expermentally the proposed modfed sldng wndow flter, by demonstratng ts robustness on a quadrotor wth rapd transtons between hoverng and forward motons, wthn an ndoor envronment I INRODUCION AND RELAED WORK Current approaches to 3D localzaton rely on nertal measurements unts (IMUs) that provde rotatonal velocty and lnear acceleraton measurements Low-cost, commercalgrade IMUs, however, suffer from the presence of nose and bas n the nertal measurements, whch when ntegrated even over a short perod of tme, can result n unrelable estmates When avalable, PS measurements can be employed for adng an nertal navgaton system (INS) Many robotc applcatons, however, requre operaton n PS-dened areas (eg, ndoors or wthn urban canyons) A Vson-aded INS (VINS) employs camera observatons of tracked features over multple tme steps for mposng geometrc constrants between the moton of the vehcle and the structure of the observed scene Such geometrc constrants, provde correctons to the pose (poston and orentaton) estmates of an INS, and can sgnfcantly mprove the localzaton accuracy wthn PS-dened areas As a result, recent advances n VINS have led to successful applcatons to ground [1, [2, aeral [3, [4, and space exploraton [5 vehcles Exstng approaches to VINS rely ether on flterng or bundle-adjustment (BA)-based optmzaton methods BA methods, orgnally developed for problems n photogrammetry [6 and computer vson [7, [8, perform batch optmzaton, wthout margnalzaton, over all the varables, ncludng the entre robot trajectory and every detected landmark usng all avalable measurements In order to reduce D Kottas, and S I Roumelots are wth the Department of Computer Scence and Engneerng, Unv of Mnnesota, Mnneapols, MN 55455, USA Emals: {dkottas,stergos}@csumnedu K J Wu s wth the Department of Electrcal and Computer Engneerng, Unv of Mnnesota, Mnneapols, MN 55455, USA Emal: kejan@csumnedu hs work was supported by the Unversty of Mnnesota (UMN) through the Dgtal echnology Center (DC) and AFOSR (FA ) the BA s computatonal complexty, dfferent approxmate methods have been developed that ether optmze over a subset of measurements and varables or solve the BA problem ntermttently An example of the frst category of relaxed solutons to the vson-only BA formulaton s the Parallel rackng and Mappng (PAM) algorthm, orgnally developed by Klen and Murray [9 for augmented realty applcatons wthn confned spaces PAM manages to bound the ncreasng complexty of BA-based methods as new poses and features are added to the state vector, by optmzng over a fxed number of camera poses (keyframes) and mapped features Such a framework s effcent and robust under the assumpton that the camera observes the same scene over long perods of tme [10 As the scene changes, new key-frames and mapped features may be added, whle past poses and features, as well as all ther assocated measurements are gnored Although observng the same scene s a common scenaro for augmented realty applcatons, t s rather restrctve for robotc vehcles, where exploraton of large areas s often requred As a result, PAM, when modfed to be fused wth an INS n a loosely coupled manner for the purpose of mcro aeral vehcle (MAV) localzaton, needs specal consderaton for falure detecton durng rapd changes of the observed scene [11 Among the methods that ncrementally solve the BA problem, SAM2 has been appled to a PS-aded INS, employng vsual measurements [12 In order to reduce the prohbtvely hgh computatonal complexty of solvng the full BA problem, the authors employ factorzaton-updatng methods whch allow reusng the nformaton matrx avalable from prevous steps Computatonally demandng procedures, however, such as relnearzaton followed by batch factorzaton, are only performed when a varable sgnfcantly devates from the current estmate Nevertheless, due to the accumulaton of fll-ns between perodc batch steps, the SAM2 s effcency degrades when many varables are affected at every relnearzaton step [13 Recursve flterng approaches to VINS can be classfed nto two man categores he frst one comprses nontrval extensons of EKF-based SLAM algorthms [14, approprately modfed for VINS [2, where the estmator s state vector ncludes both the pose of the vehcle and a map of the envronment EKF-SLAM can deal wth both cases of hoverng and exploraton Its hgh computatonal complexty (quadratc n the number of mapped features), however, lmts ts applcablty to small-sze areas In contrast, sldng wndow flterng approaches, avod the ncluson of a map of the envronment by mantanng a sldng wndow

2 of past camera poses Among these methods, the Mult- State Constraned Kalman Flter (MSC-KF) [1 explots all avalable geometrc nformaton provded by the camera measurements, whle keepng ts computatonal complexty lnear n the number of features observed over the flter s wndow Although the MSC-KF has been successfully appled to varous applcatons (eg, [1, [5), and has been demonstrated to operate n real tme [15, [16, t s not sutable for scenaros that nclude hoverng over the same scene, snce t requres suffcent baselne between the camera poses wthn the sldng wndow At ths pont, we defne two dstnct cases of moton By hoverng we descrbe the case of zero translaton, whle by generc moton we refer to moton profles that excte suffcent degrees of freedom, so that the number of unobservable drectons of the VINS reaches ts mnmum [17, [15 Recent work on VINS, addresses the case of hoverng by utlzng hybrd flter estmators that nclude both a sldng wndow of camera poses, as well as a fxed number of mapped landmarks [3, [18, or by separately buldng a map of the envronment [19 Although such methods bound the processng cost of SLAM (the number of mapped landmarks n the state vector s kept small), ther performance durng a hoverng scenaro hnges upon the crteron employed for selectng whch features to be ncluded n the state vector he present paper s contrbutons address the above lmtatons by approprately modfyng the sldng wndow over whch the MSC-KF operates, so as to perform robustly both under hoverng and generc moton condtons, wthout the need of buldng a map of the envronment Specfcally: We analyze the observablty propertes of a VINS when hoverng, wth and wthout rotatonal motons, and show that t has 5 and 7 unobservable degrees of freedom (dof), respectvely hs s n contrast to the case of a VINS under generc motons where the number of unobservable dof s 4 [15 We prove that for a sldng wndow-based estmator, such as the MSC-KF, whose state vector comprses camera poses correspondng to both moton profles (e, generc motons and hoverng), the number of unobservable dof remans 4 We propose a method for classfyng the vehcle s moton nto hoverng versus non-hoverng, by utlzng vsual nformaton from the feature tracks We leverage the results of our observablty analyss, as well as the proposed moton-classfcaton algorthm, for decdng whch frames to be added/dropped from the MSC-KF, whle keepng the flter s computatonal complexty lnear n the number of observed features Fnally, we demonstrate the robustness of the proposed approach by testng t on a MAV rapdly transtonng between hoverng and generc motons he rest of the paper s organzed as follows: In Sect II, we descrbe the system and measurement models used by the MSC-KF Subsequently, (Sect III) we present the observablty analyss of a VINS under hoverng, whch we leverage for approprately modfyng the MSC-KF he proposed method s valdated expermentally n Sect IV Fnally, we provde our concludng remarks and outlne our future research drectons n Sect V II BACKROUND In what follows, we frst present the system model used for state and covarance propagaton based on nertal measurements (Sect II-A), and then descrbe the measurement model for performng tghtly-coupled vsual-nertal odometry through the MSC-KF framework A IMU State Model he 16 1 IMU state vector s: x R = [I q b g v I b a p I (1) he frst component of the IMU state s I q (t) whch s the unt quaternon representng the orentaton of the global frame {} n the IMU frame, {I}, at tme t he frame {I} s attached to the IMU, whle {} s a local-vertcal reference frame whose orgn concdes wth the ntal IMU poston he IMU state also ncludes the poston, p I (t), and velocty, v I (t), of {I} n {}, whle b g (t) and b a (t) denote the gyroscope and accelerometer bases, respectvely he system model descrbng the tme evoluton of the state s (see [20): I q (t) = 1 2 Ω(I ω(t)) I q (t), ṗ I (t) = v I (t), v I (t) = a(t) ḃ g (t) = n wg (t), ḃ a (t) = n wa (t) (2) where I ω and a are the rotatonal velocty and lnear acceleraton, n wg and n wa (t) are the whte-nose processes drvng the IMU bases, and Ω(ω) [ ω ω ω 0, ω [ 0 ω3 ω 2 ω 3 0 ω 1 ω 2 ω 1 0 he gyroscope and accelerometer measurements are: ω m (t) = I ω(t) + b g (t) + n g (t) (3) a m (t) = C( I q (t))( a(t) g) + b a (t) + n a (t) (4) where C( q) s the rotaton matrx correspondng to the quaternon q, g s the gravtatonal acceleraton expressed n {}, and n g (t), n a (t) are whte-nose processes contamnatng the correspondng measurements Lnearzng at the current estmates and applyng the expectaton operator on both sdes of (2), we obtan the IMU state propagaton model: I ˆ q (t) = 1 2 Ω(I ˆω(t)) I ˆ q (t), ˆp I (t) = ˆv I (t) (5) ˆv I (t) = C ( I ˆ q (t))â(t) + g, ˆb g (t) = 0 3 1, ˆb a (t) = where â(t) a m (t) ˆb a (t), and I ˆω(t) ω m (t) ˆb g (t) By defnng the 15 1 error-state vector as: 1 x R = [ I δθ b ṽ g I b a p I, (6) 1 For the IMU poston, velocty, and bases, we use a standard addtve error model (e, x = x ˆx s the error n the estmate ˆx of a random varable x) o ensure mnmal representaton for the covarance, we employ a multplcatve atttude error model where the error between the quaternon q and ts estmate ˆ q s the 3 1 angle-error [ vector, δθ, mplctly defned by the error quaternon δ q = q ˆ q δθ 1, where δ q descrbes the small rotaton that causes the true and estmated atttude to concde

3 the contnuous-tme IMU error-state equaton becomes: x R (t) = F R (t) x R (t) + R (t)n(t) (7) where F R (t) s the error-state transton matrx and R (t) s the nose nput matrx, wth ˆω(t) I F R (t) = C ( I ˆ q (t)) â(t) C ( I ˆ q (t)) I I I R (t) = C ( I ˆ q (t)) I and n(t) [ n g n wg n a nwa s the system nose wth autocorrelaton E[n(t)n (τ) = Q R δ(t τ), where δ() s the Drac delta; Q R depends on the IMU nose characterstcs and s computed offlne he state transton matrx from tme t 1 to t k, Φ, s computed n analytcal form [21 as the soluton to the matrx dfferental equaton Φ = F R (t k )Φ, Φ 1,1 = I 15 : Φ (1,1) Φ (1,2) I Φ = Φ (3,1) Φ (3,2) I 3 Φ (3,4) 0 3 (8) I Φ (5,1) Φ (5,2) (t k t 1 )I 3 Φ (5,4) I 3 Fnally, the dscrete-tme system nose covarance matrx s computed as: Q k = t k+1 t Φ k k,τ R (τ)q R R (τ)φ k,τ dτ B MSC-KF Propagaton Model As the sensor platform moves n the envronment, the camera observes pont features, whch are tracked across mages enerally, n a VINS [22, these measurements are exploted to concurrently estmate the moton of the sensng platform and, optonally, the structure of the envronment he MSC-KF [1 s a VINS that performs tghtly-coupled vsual-nertal odometry over a sldng wndow of N poses, whle mantanng lnear complexty n the number of observed features he key advantage of the MSC-KF s that t utlzes all constrants for each feature observed by the camera over N poses, wthout requrng to buld a map or estmate the features as part of the state vector Each tme the camera records an mage, a stochastc clone [23, of the sensor pose s created hs enables the utlzaton of delayed mage measurements; n partcular, t allows all observatons of a gven feature f to be processed durng a sngle update step (when the frst pose that observed the feature s about to be margnalzed) Hence, at a gven tme-step k, the flter tracks the 16 1 evolvng state, x Rk [see (1), as well as the cloned sensor poses {x C = [ I k N+ q p I k N+ }, = 0,,N 1 correspondng to the last N mages hat s: x k = [ [ x R k xc = x R k xc k 1 xc k N (9) Correspondngly, the covarance conssts of the block of the evolvng state, P RR, the 6N 6N block correspondng to the cloned robot poses, P CC, and ther cross-correlatons, P RC Hence, the covarance of the augmented state vector has the followng structure: [ PRR P P = RC (10) P RC P CC Durng propagaton, the current state estmate evolves forward n tme by ntegratng (5), whle the cloned poses are statc he covarance propagaton of the entre state s gven by: P RR Φ k+1,k P RR Φ k+1,k + Q k (11) C MSC-KF Update Model P RC Φ k+1,k P RC (12) P CC P CC (13) In ths secton, we descrbe the processng of a sngle feature f, whch was frst observed by the oldest clone correspondng to tme-step k N, and then reobserved over the cloned camera poses correspondng to tme-steps k N,,k 1 2 We employ the pnhole camera model to descrbe the perspectve projecton of the 3D pont f on the mage plane and model the measurement z l at tme step l as: z l = 1 [ x x l z y + η l, l y l = I l f = C( I l q ) ( ) f p Il (14) l l z l where the nose η l follows a aussan dstrbuton wth zero mean and covarance E[η l η l = σηi 2 2 Note also that, wthout loss of generalty, we express the mage measurement n normalzed pxel coordnates, and consder the camera frame to be concdent wth the IMU frame 3 By dfferentatng the nonlnear measurement model (14) wth respect to the augmented state (9), we obtan the lnearzed measurement Jacoban: z l = [ H c,l H θ,l H pi,l xcl + H c,l H f,l f + η l (15) [ [ 15 = H H θ,l H pi,l c,l }{{} x k l-th clone poston + H c,l H f,l f + η l (16) = H x,l x k + H c,l H f,l f + η l (17) where H c,l = ẑ l 0 1 ˆx l ẑ l ŷ l ẑ l, H θ,l = lˆf I (18) H pi,l = C ( I l ˆ q ), H f,l = C ( I l ˆ q ) 2 he nterested reader s referred to [1 on how the same methodology can be appled effcently to multple features 3 We perform both ntrnsc camera and extrnsc IMU-camera calbraton off-lne [24, [25

4 After collectng all measurements of feature f across tmesteps k N,,k 1, we arrve at: z H k 1 x,k 1 H c,k 1 H f,k 1 = x k + f + η k (19) z k N }{{} z H x,k N }{{} H x H c,k N H f,k N }{{} H f So as to avod ncludng f nto the state vector, the feature s margnalzed by projectng (19) onto the left nullspace W of H f hs yelds W z = W H x x k + W η k (20) whch we employ to update the state and covarance estmates usng the standard EKF update equatons After the update, we margnalze out the oldest cloned pose, by removng x Ck N from x k, and droppng the correspondng rows and columns of P III PROBLEM DESCRIPION AND SOLUION As descrbed n the prevous secton, the MSC-KF algorthm processes vsual observatons over a sldng wndow of camera poses hus, at every tme-step, we need to decde whch camera pose to nclude/remove from the sldng wndow, so that ts sze remans constant A natural choce, especally durng exploraton tasks, would be the frst-n-frstout (FIFO) scheme, e, to remove the oldest camera pose and replace t wth the newest (current) one hs scheme performs robustly when the platform s undergong generc motons [5 In the case of hoverng, however, (e, when the platform stays at the same poston for a perod of tme) FIFO-based MSC-KF would fal o address ths ssue, we propose a last-n-frst-out (LIFO) mage management approach for the MSC-KF where we replace the mage last ncluded n the sldng wndow wth the one currently provded by the camera he motvaton for swtchng from a FIFO to a LIFO strategy s that we want to ensure that there s always suffcent baselne between camera poses ncluded n the sldng wndow he exact mpact of ths selecton (e, FIFO vs LIFO) on the observablty propertes of the system s dscussed n the followng sectons (Sect III-A and Sect III-B), whle the crteron for swtchng between FIFO and LIFO s presented n Sect III-C A FIFO: Exploraton Mode 1) FIFO Scheme: Assume that at tme-step k, the sldng wndow of camera poses corresponds to the states [see (9) x k = [ [ x R k xc = x R k xc k 1 xc k N (21) hen, the camera observatons from tme steps k N,k N + 1,, k 1, are processed for updatng the state and covarance usng the measurement model of (20) For the next tme-step, the FIFO scheme frst drops the state x Ck N whch corresponds to the oldest camera pose nsde ths wndow hen, the newest state x Rk s cloned nto x Ck, (a) Fg 1: FIFO vs LIFO: he evoluton of the sldng wndow of flter states (shaded area) durng hoverng (poses n red crcles) he bottom fgures show that whle eventually all the states n the FIFO scheme correspond to hoverng camera poses, the states n LIFO always contan generc-moton camera poses followed by propagaton So at tme-step k + 1 the wndow of states becomes x k+1 = [ x R k+1 xc [ = x R k+1 xc k xc k N+1 (22) after whch an update s performed and the same FIFO procedure s repeated (see Fg 1) In short, the FIFO scheme sldes the wndow of camera poses forward n tme, whch s the most commonly used mage management scheme employed by sldng wndow flters 2) FIFO Durng enerc Motons and Hoverng - Observablty Analyss: In what follows, we evaluate the performance of the FIFO MSC-KF by studyng the observablty propertes of the correspondng VINS model As shown n [17, [15, when generc motons of the camera poses are nvolved, the VINS model has four unobservable drectons: three correspondng to global translatons, and one to rotatons about the gravty vector Hereafter, we study the case when the platform hovers, meanng that there s lttle or no change n the postons of the camera poses, whle the camera may rotate or stand stll Assume that hoverng starts at tme step k 0 and the sze of the sldng wndow s N hen, n ths case, because of the FIFO scheme, the camera poses n the sldng wndow of states x k, wth k k 0 + N, wll all correspond to hoverng states So for any tme step k k 0 + N, the FIFO MSC-KF model s equvalent to a VINS model wth only hoverng motons, e, no translaton between consecutve camera poses At ths pont, we state the frst man result of our observablty analyss heorem 11 he lnearzed VINS model, for the case when multple features ( 3) are observed by a sensor platform performng no translatonal moton, but wth generc rotatonal motons, has fve unobservable drectons: three for global translatons, one for rotatons around the gravty vector (yaw), and one for scale

5 heorem 12 he lnearzed VINS model, for the case when multple features ( 3) are observed by a sensor platform performng no translatonal or rotatonal moton, has seven unobservable drectons: three for global translatons, three for rotatons (roll, ptch, and yaw), and one for scale Proof: see Appendx part A and B hus, for the FIFO MSC-KF, when the platform hovers, more unobservable drectons appear besdes the nevtable four ones hs wll lead to degradaton of the system performance: when the scale s unobservable, we cannot estmate the scale of the moton or the scene Furthermore, when the roll and ptch angles are also unobservable, we cannot measure gravty n the local (sensor) frame I g, whch n turn makes t mpossble to extract the true body acceleratons a from the accelerometer readngs [see (4) B LIFO: Hoverng Mode In what follows, we ntroduce an alternatve LIFO-based scheme that wll extract the same nformaton durng hoverng as n the case of generc motons, e, the correspondng VINS model has only four unobservable drectons 1) LIFO Scheme: Assume that the sensor platform starts hoverng at tme-step k 0, wth generc motons before k 0, and the sze of the sldng wndow s N We employ the FIFO MSC-KF for the generc moton tme nterval, e, k k 0 hus, at the tme-step k 0 + 1, we have the followng sldng wndow of the states from FIFO [see (22) [ [ x k0 +1 = x Rk0 x +1 C = x Rk0+1 xc k0 xc k0 N+1 (23) Once we detect that the platform s n hoverng mode between tme-step k 0 and k (the crteron s descrbed n Sect III-C), we swtch to the LIFO scheme: nstead of droppng the oldest camera pose x Ck0 as n FIFO, we N+1 drop the newest pose x Rk0, and replace t wth the state +1 correspondng to the next tme-step x Rk0 hs procedure +2 corresponds to the MSC-KF performng propagaton only, wthout any state droppng or clonng At ths pont, the sldng wndow of states becomes [ [ x k0 +2 = x Rk0 x +2 C = x Rk0+2 xc k0 xc k0 N+1 (24) and a flter update s performed usng the camera observatons correspondng to the poses n the wndow he same process s repeated for as long as the platform contnues to hover (see Fg 1) Once the sensor platform leaves hoverng (e, t starts to perform generc motons agan), the MSC-KF swtches back to FIFO mode 2) LIFO Durng Hoverng - Observablty Analyss: In what follows, we show that the LIFO-based MSC-KF, desgned for dealng wth hoverng condtons, acqures suffcent nformaton, e, the unobservable drectons of the correspondng VINS model are the same as n the case of generc motons When followng the LIFO scheme [see (24), the sldng wndow of states for any partcular tme-step k durng hoverng s x k = [ [ x R k xc = x Rk xc k0 xc k0 N+1 (25) where only the latest camera pose s beng replaced here are two parts n ths state vector: the rght part (tme-steps k 0 N + 1,,k 0 1), corresponds to camera poses that underwent generc motons, whle the left part (tme-step k 0 and k), corresponds to hoverng poses hus, the LIFO MSC- KF for hoverng s equvalent to a VINS model where the sensor platform ntally performs generc motons and then swtches to hoverng At ths pont, we state the second man result of our observablty analyss heorem 2 he lnearzed VINS model, for the case when multple features ( 3) are observed by a sensor platform performng generc motons for at least 4 tme-steps, and then startng to hover ether wth or wthout rotatonal motons, has only four unobservable drectons: three for global translatons, and one for rotatons around the gravty vector (yaw) Proof: see Appendx part C hus, the unobservable drectons of the LIFO MSC-KF are exactly the same as the ones of VINS undergong generc motons, whch valdates our choce of the LIFO scheme for dealng wth hoverng condtons 3) LIFO-based MSC-KF Update: Assume that the sensor platform hovers from tme-step k 0 to tme-step k 0 + N H Ideally we would lke to solve a bundle adjustment problem, where the state conssts of both generc-moton poses x Ck0, j = 0,,N 1, and hoverng j poses x, s = 1,,N Rk0 +s H, wth all the feature measurements observed durng the tme nterval k 0 N + 1 to k 0 + N H However, f the hoverng perod lasts for a long tme (e, N H s large), t s computatonally challengng to solve ths optmzaton problem n a batch form Moreover, t also suffers from numercal nstablty due to the lack of suffcent baselne between the hoverng poses [8 Alternatvely, the MSC-KF provdes us wth a framework to solve ths optmzaton problem ncrementally [26, n a recursve manner Specfcally, at each tme-step k durng hoverng, we perform a state-only MSC-KF update usng the measurement model of (20) In contrast, snce whle hoverng we retan the same poses x Ck0, j = 0,,N 1, j n the state vector [see (25), the covarance should only be updated once usng all measurements, otherwse the flter wll become nconsstent In our LIFO-based MSC-KF, ths covarance update takes place at tme step k 0 + N H, rght before exstng the hoverng perod C Hoverng Detecton he method we employ for detectng whether the camera moton between two consecutve mage frames ncludes a translatonal component, plays a crucal role for swtchng n a tmely manner between the FIFO and LIFO schemes We acheve ths by approprately modfyng our exstng

6 tghtly-coupled vsual-nertal framework, for robust feature trackng Specfcally, let b k denote the unt-norm bearng measurement to a feature (e, b k = z k z k ) at tme step k 2 Between two consecutve camera poses, k and k + 1, all feature observatons that correspond to nlers satsfy the eppolar constrant [27: b k+1 I k+1 p Ik C ( I k+1 ˆ q Ik ) b k = 0 (26) where we use the flter s state estmates to evaluate C ( ) I k+1 ˆ q Ik When there s suffcent baselne between the camera poses, we employ the 2-pt RANSAC [28 to estmate the unt vector of translaton I k+1p Ik n (26) In contrast, for zerotranslatonal motons, b k+1 s (approxmately) parallel to C ( ) I k+1 ˆ q Ik b k and (26) becomes ll-condtoned In that case, we employ a 0-pt RANSAC framework, where we classfy pont correspondences as nlers or outlers by drectly usng a model provded by the state estmates: In partcular, we compute b k+1 C( I k+1 ˆ q Ik ) b k 2 = 0 (27) d k = 1 M M b k+1 C( ) I k+1 ˆ q Ik b k 2 (28) =1 and threshold d k, so as to decde whether the vehcle excted suffcent translatonal moton, between tme-steps k and k + 1, by settng the boolean varable: ξ k = 1 f d k < ε, else ξ k = 0 (29) So as to ensure smooth transtons from hoverng to nonhoverng decsons, we expect multple consecutve decsons to be the same, before classfyng the robot s moton IV EXPERIMENAL RESULS AND IMPLEMENAION DEAILS We valdated the robustness of the proposed approach usng a MAV Our expermental platform, conssts of a lowcost quadrotor, the Parrot ARDRONE, equpped wth a lowweght ( 100 gr) sensng platform [see Fg 2 (a) Specfcally, the sensng modaltes comprse a Pont rey Chameleon monochrome camera 4 wth resoluton pxels and an InterSense NavChp IMU 5 IMU sgnals were sampled at a frequency of 100 Hz whle camera mages were acqured at 75 Hz usng an ARM CPU Images and nertal measurements were streamed through the wreless module of the quadrotor and processed n realtme on a ground staton computer A sldng wndow of 12 mages was employed, wth MSC-KF updates occurrng at 375 Hz, whle the flter was provdng state estmates at the frequency of the IMU (100 Hz) n real-tme Features were extracted from the frst mage (last mage) of the FIFO (LIFO) sldng wndow of the mages, usng the Sh-omas corner detector [29 Whle n FIFO (LIFO) mode, features were tracked forward (backward) n the sldng wndow, usng the KL trackng algorthm [30 For the purpose of valdatng the robustness of the proposed algorthm, the quadrotor was commanded to perform rapd transtons from hoverng to forward moton and then agan to hoverng As t s demonstrated n Fg 2 (b) and the accompanyng vdeo, the proposed localzaton framework that employs the FIFO/LIFO MSC-KF schemes (dependng on the decson of the moton classfer), was able to robustly detect the transtons of the vehcle s moton profle, and successfully track ts pose In contrast, the regular FIFOonly sldng wndow MSC-KF, usng the same wndow sze of 12 mages, faled to track the vehcle s pose, durng sgnfcant perods of hoverng and dverged, as predcted by our observablty analyss V CONCLUSION AND FUURE WORK In ths paper, we ntroduced a robust moton classfer for detectng transtons between hoverng and generc motons Addtonally, we studed the observablty propertes of a vson-aded nertal navgaton system (VINS) undergong two dfferent types of moton: () hoverng-only (e, zerotranslaton) maneuvers, and () moton wth suffcent baselne followed by hoverng maneuvers Moreover, we leveraged the results of our observablty analyss to ntroduce a LIFO-FIFO swtchng strategy for selectng the mages processed by a sldng-wndow flter under dfferent operatng condtons Fnally, we demonstrated the robustness of the proposed strategy for dealng wth sngular moton confguratons usng a quadrotor rapdly transtonng from hoverng to forward moton wthn an ndoor envronment As part of our future work, we plan to further modfy exstng VINS frameworks so as to ncorporate knematc constrants dependng on the vehcle s moton profle A Proof of heorem 11 APPENDIX For a VINS model we have the followng state vector x = [ x R f 1 f M (30) where x R s defned n (1), f s the poston of feature 6, = 1,,M, n the global frame, and M s the number of features wth M 3 he observablty matrx M [31 of the VINS model has as ts k-th block row M k = H k Φ, for k 1, where Φ s the state transton matrx from tme-step 1 to k [see (8), and H k s the measurement Jacoban of the feature observaton model at tme-step k [see (14) From [21, we have the analytcal expresson for M k : 6 Note that for the purpose of ths observablty analyss, we nclude the feature postons n the state vector he same analyss holds for the MSC-KF as well snce the latter performs sldng wndow SLAM wth margnalzaton wth respect to the feature postons

7 (a) (b) (c) Fg 2: Experment: A Parrot ARDRONE rapdly transtonng from hoverng to forward moton maneuvers (a) Close-vew of the quadrotor testbed along wth ts onboard sensors (b) 3D vew of the overall estmated trajectory wth the hoverng perods annotated (c) On-board vew from the expermental dataset [ Γ11 [Γ12 Γ21 Γ22 Γ13 Γ23 Γ1 ΓM 2 ΓM 3 [ M δ tk δ tk Γ4 Γ4 δ tk Γ4 (31) where 0 3 N2 = C (I q ) g pi C (I q ) f1 C (I q ) fm C (I q ) [ pi = Nt,2 f1 fm Nr,2 Ns,2 (37) Γ1 = Hc,k C (Ik q ) (32) 1 Γ2 = f pi1 vi1 δ tk + g δ tk2 C (I1 q ) 2 (1,2) (5,2) Γ3 = f pik C (Ik q )Φ Φ (33) (34) (5,4) Γ4 = Φ (35) and = 1, 2,, M, s the feature ndex In the case of hoverng wth generc rotatons, snce there s no translaton and the velocty s zero, we set pi k = pi and vik = 0, for all k hen, we compute the rght nullspace of M, whch s: 0 3 N1 = C (I1 q ) g pi g f1 g fm g [ pi = Nt,1 f1 fm Nr,1 Ns,1 (36) hus, we have 5 unobservable drectons for ths model: 3 for global translatons (Nt,1 ), 1 for rotatons about gravty (Nr,1 ), and 1 for scale (Ns,1 ) B Proof of heorem 12 When no rotaton s present, compared wth the case n part A, we addtonally have I k q = I q hs brngs some further smplfcatons to the elements of M, and ts rght nullspace becomes: hus, we have 7 unobservable drectons for ths model: 3 for global translatons (Nt,2 ), 3 for rotatons (Nr,2 ), and 1 for scale (Ns,2 ) C Proof of heorem 2 We employ the batch least squares (BLS) formulaton for processng all IMU and camera measurements up to tmestep k and form the state vector [ x = xr1 xrℓ xrℓ+1 xrk f1 fm (38) where xr1,, xrℓ correspond to generc motons wth ℓ 4, and xrℓ+1,, xrk are of any moton (generc or hoverng) hen, the unobservable drectons of the lnearzed VINS model span the rght nullspace of the nformaton matrx of ths BLS under margnalzaton [32, or equvalently, span the nullspace of the correspondng Jacoban matrx he Jacoban Ak has the followng sparse structure Φ2,1 1 Hx,1 M Hx,1 I15 Φ3,2 I15 Φk,k 1 I15 H1f,1 HM f,1 H1x,k HM x,k H1f,k HM f,k (39)

8 where Φ k,k 1 s the state transton matrx from tme-step k 1 to k, and H x,k, H f,k are the measurement Jacobans for the -th feature observaton at tme-step k wth respect to x Rk and f, respectvely Now we use mathematcal nducton to show that the rght nullspace of A k s of dmenson 4, for any k l 1) Intal step: When k = l, all states x R correspond to generc motons As shown n [17, [15, a VINS undergong generc motons has only 4 unobservable drectons: 3 for global translatons and 1 for rotatons about gravty 2) Inducton step: Assume that the nullspace N k 1 of A k 1 s of dmenson 4 he Jacoban A k takes the form: [ Ak 1 0 A k = (40) B C where B, C consst of dfferent measurement Jacoban matrces So to fnd the nullspace N k of A k, we need to solve: A k N k = 0 A k 1 N 1 k = 0 and BN1 k + CN2 k = 0 (41) [ where N k = N 1 k N 2 k From (41), we have N 1 k = N k 1, and t can be shown that N 2 k s unquely determned for each soluton of N 1 k Hence, there are a total number of 4 ndependent drectons for N k, and t s easy to check that these drectons are the same as those of the VINS when undergong generc motons ACKNOWLEDEMENS he authors would lke to thank Ahmed Ahmed, Ellot Branson, and Lus Carlos Carrllo for ther help developng the software and hardware nfrastructure of the quadrotor used n our experments REFERENCES [1 A I Mourks and S I Roumelots, A mult-state constrant Kalman flter for vson-aded nertal navgaton, n Proc IEEE Int Conf Robot Autom, Rome, Italy, Apr 10 14, 2007, pp [2 E S Jones and S Soatto, Vsual-nertal navgaton, mappng and localzaton: A scalable real-tme causal approach, Int J Robot Res, vol 30, no 4, pp , Apr 2011 [3 B Wllams, N Hudson, B weddle, R Brockers, and L Matthes, Feature and pose constraned vsual aded nertal navgaton for computatonally constraned aeral vehcles, n Proc IEEE Int Conf Robot Autom, Shangha, Chna, May 9 13, 2011, pp [4 S Wess, M W Achtelk, S Lynen, M Chl, and R Segwart, Realtme onboard vsual-nertal state estmaton and self-calbraton of MAVs n unknown envronment, n Proc IEEE Int Conf Robot Autom, St Paul, MN, May 14 18, 2012, pp [5 A I Mourks, N rawny, S I Roumelots, A E Johnson, A Ansar, and L Matthes, Vson-aded nertal navgaton for spacecraft entry, descent, and landng, IEEE rans Robot, vol 25, no 2, pp , Apr 2009 [6 D C Brown, A soluton to the general problem of multple staton analytcal stereotrangulaton, Patrck Ar Force Base, Florda, RCA- MP Data Reducton echncal Report, ech Rep 43, 1958 [7 R Szelsk and S B Kang, Recoverng 3d shape and moton from mage streams usng nonlnear least squares, n Proc IEEE Conf Comp Vson Patt Recog, New York Cty, NY, Jun 15 17, 1993, pp [8 B rggs, P McLauchlan, R Hartley, and A Ftzgbbon, Bundle adjustment a modern synthess, n Vson Algorthms: heory and Practce, ser Lecture Notes n Computer Scence Sprnger-Verlag, 2000, pp [9 Klen and D Murray, Parallel trackng and mappng for small ar workspaces, n Proc 6th IEEE and ACM Int Symp on Mxed and Augmented Realty, Nara, Japan, Nov 13-16, 2007, pp [10 H Strasdat, J Montel, and A J Davson, Real-tme monocular slam: Why flter? n Proc IEEE Int Conf Robot Autom, Anchorage, Alaska, May 3 8, 2010, pp [11 S Wess and R Segwart, Real-tme metrc state estmaton for modular vson-nertal systems, n Proc IEEE Int Conf Robot Autom, Shangha, Chna, May 9 13, 2011, pp [12 V Indelman, S Wllams, M Kaess, and F Dellaert, Factor graph based ncremental smoothng n nertal navgaton systems, n Proc Intl Conf on Informaton Fuson, FUSION, Sngapore, Jul 9 12, 2012, pp [13 M Kaess, H Johannsson, R Roberts, V Ila, J Leonard, and F Dellaert, SAM2: Incremental smoothng and mappng usng the Bayes tree, Intl J Robot Res, vol 31, no 2, pp , Feb 2012 [14 R Smth, M Self, and P Cheeseman, Estmatng uncertan spatal relatonshps n robotcs, Autonomous robot vehcles, vol 1, pp , 1990 [15 J A Hesch, D Kottas, S L Bowman, and S I Roumelots, owards consstent vson-aded nertal navgaton, n Proc 10th Int Workshop Alg Found Robot, Cambrdge, MA, Jun 13 15, 2012 [16 D Kottas, J A Hesch, S L Bowman, and S I Roumelots, On the consstency of vson-aded nertal navgaton, n Proc Int Symp Exper Robot, Quebec Cty, Canada, Jun 17 21, 2012 [17 A Martnell, Vson and IMU data fuson: Closed-form solutons for atttude, speed, absolute scale, and bas determnaton, IEEE rans Robot, vol 28, no 1, pp 44 60, Feb 2012 [18 M L and A I Mourks, Vson-aded nertal navgaton for resource-constraned systems, n Proc IEEE/RSJ Int Conf on Intellgent Robots and Systems, Vlamoura, Algarve, Portugal, Oct 7 12, 2012, pp [19 S Shen, Y Mulgaonkar, N Mchael, and V Kumar, Vson-based state estmaton for autonomous rotorcraft mavs n complex envronments, n Proc IEEE Int Conf Robot Autom, Karlsruhe, ermany, May 6 10, 2013, pp [20 N rawny and S I Roumelots, Indrect Kalman flter for 3D atttude estmaton, Unversty of Mnnesota, Dept of Comp Sc & Eng, MARS Lab, ech Rep , Mar 2005 [Onlne Avalable: trawny/publcatons/quaternons 3Dpdf [21 J A Hesch, D Kottas, S L Bowman, and S I Roumelots, Observablty-constraned vson-aded nertal navgaton, Unversty of Mnnesota, Dept of Comp Sc & Eng, MARS Lab, ech Rep , Feb 2012 [Onlne Avalable: dkottas/pdfs/vns tr wnter 2012pdf [22 A I Mourks and S I Roumelots, A dual-layer estmator archtecture for long-term localzaton, n Proc IEEE Conf Comp Vson Patt Recog, Anchorage, AK, Jun 24 26, 2008, pp 1 8 [23 S I Roumelots and J W Burdck, Stochastc clonng: A generalzed framework for processng relatve state measurements, n Proc IEEE Int Conf Robot Autom, Washngton, DC, May 11 15, 2002, pp [24 J-Y Bouguet, Camera calbraton toolbox for matlab, 2006 [Onlne Avalable: [25 F M Mrzae and S I Roumelots, A Kalman flter-based algorthm for IMU-camera calbraton: observablty analyss and performance evaluaton, IEEE rans Robot, vol 24, no 5, pp , Oct 2008 [26 A H Jazwnsk, Stochastc Processes and Flterng heory Analytcal Mechancs Assocates, Inc, 1970 [27 R I Hartley and A Zsserman, Multple Vew eometry n Computer Vson Cambrdge Unversty Press, 2000 [28 L Knep, M Chl, and R Segwart, Robust real-tme vsual odometry wth a sngle camera and an mu, n Proc Brtsh Machne Vson Conf, Dundee, UK, Aug 29 Sep 2, 2011, pp [29 J Sh and C omas, ood features to track, n Proc IEEE Conf Comp Vson Patt Recog, Washngton, DC, Jun 27 Jul 2, 1994, pp [30 B Lucas and Kanade, An teratve mage regstraton technque wth an applcaton to stereo vson, n Proc Int Jont Conf on Artfcal Intellgence, Vancouver, BC, Canada, Aug 24 28, 1981, pp [31 P S Maybeck, Stochastc models, estmaton, and control New York, NY: Academc Press, 1979, vol I [32 Huang, Improvng the consstency of nonlnear estmators: Analyss, algorthms, and applcatons, PhD dssertaton, Unversty of Mnnesota, 2012