State Estimation for Aggressive Flight in GPS-Denied EnvironmentsUsing Onboard Sensing

Adam Bry, Abraham Bachrach, Nicholas Roy{abry, abachrac, nickroy}@mit.edu

Massachusetts Institute of Technology, Cambridge, MA, USA

In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2012)

Abstract— In this paper we present a state estimation methodbased on an inertial measurement unit (IMU) and a planarlaser range finder suitable for use in real-time on a fixed-wing micro air vehicle (MAV). The algorithm is capable ofmaintaing accurate state estimates during aggressive flight inunstructured 3D environments without the use of an externalpositioning system. Our localization algorithm is based on anextension of the Gaussian Particle Filter. We partition the stateaccording to measurement independence relationships and thencalculate a pseudo-linear update which allows us to use 20xfewer particles than a naive implementation to achieve similaraccuracy in the state estimate. We also propose a multi-stepforward fitting method to identify the noise parameters of theIMU and compare results with and without accurate positionmeasurements. Our process and measurement models integratenaturally with an exponential coordinates representation of theattitude uncertainty. We demonstrate our algorithms experi-mentally on a fixed-wing vehicle flying in a challenging indoorenvironment.

I. I NTRODUCTION

Developing micro air vehicles that approach the maneuver-ability and speed of birds flying through urban environmentsposes a number of challenges for robotics researchers interms of planning, control, and state estimation. Recentwork has demonstrated systems that can perform impressiveacrobatics [5] and other control feats [15], [16], however suchsystems are completely reliant on extremely accurate stateestimates provided by external camera arrays. In contrast,vehicles that are capable of flight using state estimatescomputed from onboard sensor data are either confined towide open areas without obstacles, or slow-moving hoveringvehicles such as quadrotors [2], [9].

The wide disparity between what is possible in terms ofagile flight with an external positioning system and whathas been demonstrated with onboard sensing suggests thatstate estimation from onboard sensors is indeed a significantchallenge in extending the capabilities of MAVs in real worldenvironments. In addition to providing good estimates ofthe system mean, the state estimation algorithm should alsoaccurately represent uncertainty so that control and planningalgorithms can be appropriately cautious around obstaclesand other state constraints [3].

This paper presents a state estimation method that issuitable for use in real-time on a fixed wing MAV maneuver-ing through a cluttered environment. Our system leveragesan inertial measurement unit (gyros and accelerometers)and a planar laser range finder in a filtering frameworkthat provides the accuracy, robustness, and computationalefficiency required to localize a MAV within a known 3Doccupancy map.

Fig. 1. Fixed wing experimental platform flying indoors localizing usingan onboard laser range scanner and inertial measurement unit.

In order to efficiently project the nonlinear laser measure-ment update of the vehicle position back through the stateestimate, we integrate the laser range-finder measurement asa pseudo-measurement on a partition of the state space. Thepsuedo measurement is computed from a Gaussian ParticleFilter (GPF) state update [13]. This technique drasticallyreduces the number of particles required compared to avanilla implementation of a GPF, which in itself provides amarked improvement over a conventional particle filter [19].Our algorithm enables realtime performance in the face ofthe computational limitations of the flight computer. Wequantitatively validate our algorithm on a dataset collected bymanually maneuvering the sensing components in a motioncapture environment. Finally, we demonstrate the effective-ness of our approach experimentally on a fixed wing vehiclebeing piloted in a challenging GPS-denied environment.

The process model that accompanies the GPF measure-ment update is a based on an exponential-coordinates ex-tended Kalman filter that is driven by inertial measurements.We also propose a technique for estimating the uncertaintyparameters of the IMU, namely gyro and accelerometer noisevariances, that is based on a multi-step projection of the noisecompared with smoothed state estimates. When the algorithmis used with accurate position and orientation measurementsthe noise variances converge. When the method is usedwith inaccurate position-only measurements we still seeconvergence, but also show that with noisier measurements,the optimization is more sensitive to initialization.

II. PROBLEM STATEMENT

Assuming the MAV to be a rigid body and neglectinghigher-order effects such as propeller speed and time-varyingairflow over the vehicle, the state of a MAV is given by itsposition and orientation and the associated linear and angularvelocities. For control purposes it is convenient to representthe velocities in body coordinates. Thus the goal of thefilter is to estimate the quantities

[

ωTb vTb R ∆T

]T

whereωb =[

p q r]T

is the angular velocity in body

coordinates,vb =[

u v w]T

is the linear velocity inbody coordinates,R is the rigid body orientation rotationmatrix, and ∆ =

[

∆x ∆y ∆z

]Tis the translation

vector from the origin in global coordinates to the originof the body frame, expressed in global coordinates.

We assume a set of inertial measurements consisting of3-axis acceleration, 3-axis angular rate measurements, andexteroceptive measurements consisting of planar laser rangescans. Further, we assume we have access to a 3D map ofthe environment represented as an occupancy grid.

III. IMU P ROCESSMODEL

Our state estimation algorithm uses an Extended KalmanFilter (EKF) to estimate a Gaussian distribution over systemstates. The EKF process model is based on a discrete time,nonlinear discrete transition function:

xt+1 = f(xt, ut, wt) (1)

wherext is the system state vector,ut is the input vector tothe system, andwt is a random disturbance drawn from anormal distributionN(0, Q). The EKF tracks the state at timet as a Gaussian distribution with meanµt and covarianceΣt.These first two moments are propagated forward accordingto:

µt+1 = f(µt, ut, 0) (2)

Σt+1 = AtΣtATt +WtQWT

t (3)

whereµ andΣ denote predicted quantities before a measure-ment update has occurred, andAt andWt are the appropriatepartial derivatives off .

A. Exponential Coordinates Attitude Uncertainty

We track orientation uncertainty in perturbation rotationsin the body frame. If the true orientation is given by therotation matrixR, we can writeR = RR(χ) whereR is theestimated orientation andR(χ) = eχ

∧

is the error rotationmatrix. χ ∈ ℜ3 is the perturbation rotation about the bodyaxes. We use the∧ symbol to the right of a vector to denotethe skew symmetric matrix formed as:

χ∧ =

0 −χ3 χ2

χ3 0 −χ1

−χ2 χ1 0

(4)

Taking the matrix exponential of a skew symmetric matrixreturns a rotation matrix corresponding to a rotation of|χ|about the axis defined byχ whereχ is referred to as theexponential coordinates of rotation.

In our expression for the true orientation,R(χ) postmultiplies R which puts the perturbations in the body frame.

Since the error is parameterized byχ, the covariance can betracked in a3×3 matrixΣχ. The covariance can be thoughtof as cones of uncertainty surrounding the body frame axesdefined by the columns ofR. A sketch of this uncertainty isshown in figure 2 for the covariance (in degrees):

Σχ =

32 0 00 52 00 0 152

(5)

This choice of coordinates for the filter error is desirablesince fundamentally rigid body orientation, denoted mathe-matically as the special orthogonal group (SO3), has threedegrees of freedom. While any three-element representationis provably singular for some orientation, more commonly-used parameterizations (i.e., quaternions or rotation matrices)will have constraints between the elements of the representa-tion. Thus a linearized filter covariance over the parameterswill not be full rank. Numerical errors pose the constantthreat of creating negative eigenvalues, and thus causingthe estimator to diverge. Furthermore, an efficient linearizedmeasurement update as is commonly-used in Gaussian filtersdoes not respect the constraints and thus does not map ontoSO3. A renormalization scheme could be used after everyupdate, but at any given time the representation can bearbitrarily poor [20].

As we will see, the attitude uncertainty representationis agnostic to the actual underlying orientation integrationand tracking. Quaternions and rotation matrices are easy toupdate based on usingχ in the estimator state vectorµ.

Fig. 2. This figure shows the uncertainty representation in body axes. Wesee that high variance on the z axis perturbation maps into large motions forthe x and y bases. In our implementation we use a Forward-Left-Up (body),East-North-Up (global) (FLU, ENU) coordinate system as opposed to thetraditional aerospace frame of Forward-Right-Down, North-East-Down.

B. Process Equations

The equations of motion for a rigid body are given by:

ωb = J−1(−ωb × Jωb + τb) (6)

vb = −ωb × vb +RT g + ab (7)

R = Rω∧

b (8)

∆ = Rvb, (9)

whereτb is the torque applied to the body andab is the accel-eration in body coordinates. Since the IMU provides accuratemeasurements ofωb andab, we follow the commonly-usedtechnique of omittingωb from the state, neglecting equation6, and treating the IMU measurements as inputs to the filter.

For the quantities used in equation 2 we have

x =[

vb χ ∆]

(10)

u =[

ugyro uaccel]

(11)

w =[

wgyro waccel]

(12)

Combining this state representation with equations 7-9.

fc(xt, ut, wt) =

vbR

∆

(13)

=

−ωb × vb +RT g + guaccel

Ru∧

gyroRvb

(14)

Taking the appropriate partial derivatives we get:

∂vb∂x

=[

−ω∧

b (RT g)∧ 0]

(15)

∂χ

∂x=

[

0 −ω∧

b 0]

(16)

∂∆

∂x=

[

R −Rv∧b 0]

(17)

for a continuous dynamics linearization of:

Ac =∂f

∂x=

−ω∧

b (RT g)∧ 00 −ω∧

b 0R −Rv∧b 0

(18)

and for the input vector we have:

∂vb∂u

=[

v∧b gI]

(19)

∂χ

∂u=

[

I 0]

(20)

∂∆

∂u=

[

0 0]

(21)

Wc =∂f

∂x=

∂vb

∂u∂χ∂u∂∆∂u

. (22)

While more sophisticated approximations could be used, weconstruct the discrete quantities for the filterf , At, andWt

using Euler integration:

f(xt, ut, 0) = xt + fc(xt, ut, 0)dt (23)

At = I +Acdt (24)

Wt = Wcdt. (25)

We integrate the attitude separately as

Rt+1 = RtR(u∧

gyro) (26)

IV. L ASER MEASUREMENTUPDATE

While the EKF is effective for propagating the first twomoments of the nonlinear dynamics through our IMU equa-tions of motion, it is not well-suited to integrating lasermeasurements from unstructured 3D environments. Usingsuch sensors directly in an EKF requires the extraction andcorrespondence of features such as corners, and line seg-ments from the sensor measurements, an error prone processthat limits the applicability of the algorithms to environmentswith specific structure [7]. In contrast Monte-Carlo tech-niques are widely used in laser-based localization algorithmsbecause they allow for the lidar range measurements modelto be used directly in the measurement function [19].

While particle filters are efficient enough for effectiveuse in localizing a 2D mobile robot, they require too manyparticles to be used for the estimation of a 3D MAV. For-tunately, we can obtain the best aspects of both algorithms,and a significant speedup can be realized by employing ahybrid filter that uses an IMU-driven EKF process modelwith pseudo-measurements computed from Gaussian ParticleFilter (GPF) laser measurement updates [13].

A. Gaussian Particle Filters

In its standard form, the GPF maintains a Gaussian distri-bution over the state space given a measurement history givenby P (xt|z0:t) = N(xt;µt,Σt). However, at each iterationof the filter, particles are used to incorporate nonlinearprocess and measurement models. To computeP (xt+1|z0:t)

a set of samples{x(j)t }Mj=1 is drawn fromN(µt,Σt) and

the samples are then propagated through the process modelf(xt, ut, wt). To perform the measurement update the sam-ples are weighted according to the measurement modelw

(j)t = P (zt|x

(j)t ). The updated Gaussian at the end of an

iteration of the filter is then obtained as the weighted meanand covariance of the samples

µt+1 =

∑Mj=1 w

(j)t x

(j)t

w(j)t

(27)

Σt+1 =

∑Mj=1 w

(j)t (x

(j)t − µt+1)(x

(j)t − µt+1)

T

w(j)t

. (28)

Assuming the underlying system is linear-Gaussian, the filteris shown to approximate the true distribution arbitrarily wellwith a large number of samples. The GPF filter differs froma standard particle filter by maintaining a unimodal Gaussiandistribution over the posterior state instead of the arbitrary(possibly multi-modal) distribution represented by the set ofparticles in a conventional particle filter.

A straightforward implementation of the GPF for stateestimation using a laser on a MAV is impractical andinefficient for two reasons:

1) IMU dynamics are well-approximated by linearizationas evidenced by the widespread use of EKFs in GPS-IMU state estimation.Thus, a particle process modeladds significant computational burden and samplingerror, without significantly improving the estimate ofthe posterior density.

2) The IMU filter maintains additional states to trackvelocity and IMU biases, however the laser measure-ments are only a function of the position and orienta-tion, parameterized by∆ and χ in our formulation.In fact, most of the orientation information in themeasurement exists in the plane of the laser containedin χz.

To address the first issue we only use the GPF to performthe measurement update, and instead of propagating sam-ples through the measurement function, we sample directlyfrom the prior distribution returned by the EKF after theprocess step,N(µ, Σ). To address the second issue abovewe explicitly partition the state according to independencerelationships in the measurement function. We perform astandard GPF measurement update on the partitioned stateand use this to compute a pseudo-measurement which is thenused to update the full state.

B. Partitioned State Update

The state is partitioned as,

xt =[

xmt xp

t

]

, (29)

where xmt ∈ ℜk is the part of the state that affects the

measurement, andxpt ∈ ℜn−k is independent from the

measurement. More formally we assume our measurementfunction has the form

zt = h(xmt , vt), (30)

permitting the independence factorization

P (zt|xpt , x

m) = P (z|xm). (31)

We can similarly partition the covariance

Σt =

[

Σ(m2)t Σ

(mp)t

Σ(pm)t Σ

(p2)t

]

. (32)

To perform the measurement update we draw samples{x

m(j)t }Mj=1 from N(µm

t , Σmt ). The samples are weighted

with the measurement function in equation 31. From theseweighted samples we can compute an update forP (xm

t |z0 :zt) using the conventional GPF weighted mean and covari-ance as in equations 27 and 28. The key idea is to thenuse the GPF update on the state variables that affect themeasurement to propagate a Kalman update to the rest ofthe state.

To perform a Kalman measurement update we need toknow the measurement valuezt, the covariance of themeasurementR, and the observation matrixC. Firstly, wesetC as a selector matrix for the measurement part of thestate

C =[

Ik 0n−k

]

. (33)

A measurement update onxm would proceed as:

Km = Σmt (Cm)T (CmΣt(C

m)T +R)−1 (34)

µmt = µm

t +Km(zt − Cmµmt ) (35)

Σmt = (I −KmCm)Σm

t (36)

Plugging in the identity matrix forCm, the above equationscan be solved forRt

Σmt = Σm

t − Σmt (Cm)T (CmΣt(C

m)T +Rt)−1Σm

t (37)

Rt = (Σm−1

t − Σm−1

t Σmt Σm−1

t )−1 − Σmt (38)

= (Σm−1

t − Σm−1

t )−1 (39)

where we make use of the matrix inversion lemma betweenequations 38 and 39.

Using Rt we can now solve for the Kalman gain thatwould have produced the same change between our priorand posterior covariance using equation 34 and then recoverthe actual measurement that would have produced the samechange in the mean of prior vs. posterior distributions:

zt = Km−1

(µmt − µm

t ) + µmt . (40)

A Kalman gain for the entire state can then be computedusingRt andzt, and a standard Kalman measurement updateperformed.

The posterior distribution quantitiesµm−1

t andΣm−1

t arereadily available from the GPF measurement update on themeasurement part of the state vector. Naively one mightuse the Gaussian prior from which the samples were drawnto evaluate equations 39 and 40. However, the quantitieswe care about,Rt and zt, are obviously sensitive to thedifference between the prior and posterior mean and covari-ance. With a finite number of samples there will be someerror between the distribution described by the sample set{x

m(j)t }Mj=1 and the Gaussian prior. This error will carry over

to the weighted sample set which approximates the posterior.We can compensate by using the mean and covariance of theprior sample distribution instead of our analytic expressionsfor µm

t andΣmt . In practice, this substitution makes an enor-

mous difference, particularly with low numbers of particles(which is highly desirable in a real-time application).

Finally, we note that the solutions forRt andzt hinge onthe invertibility of Cm which is a proxy for the invertibilityof our measurement functionh in equation 30 with respectto xm

t . It can be difficult to knowa priori if the measurementis well conditioned or invertible. If it is not (i.e., if themeasurement does not actually contain information aboutsome piece ofxm

t ) then theRt matrix may not be positive-definite, leading to a filter divergence. Thus it is necessaryinpractice to perform an eigenvalue decomposition onRt andset any negative eigenvalues to a large constant (implyingno information gain along the corresponding eigenvector)and then reconstruct the matrix. This step also protectsthe algorithm from negative eigenvalues entering throughsampling errors.

C. Laser Localization

The likelihood evaluation proceeds according to standardtechniques used in 2D localization. We blur the a 3Doccupancy map stored as an OctoMap [21] using a Gaussiankernel around occupied cells. To perform particle measure-ment updates we project the current scan into the map usingthe sampled particle state, and sum the log-likelihood of thereached cells before exponentiating to obtain a probabilitywith which to weight the particles.

An interesting question is the appropriate partitioning ofthe state vector for the updates described in the previoussection. The use of planar LIDARs to localize in the planeis ubiquitous, suggesting that when working in 3D, laserrange scans should at least contain information about thexyplane andχz (orientation about the yaw axis of the vehicle).However, in general, a planar slice of a 3D environmentmay contain some information about the full orientation, butpopulating the 6D pose space parameterized byχ and∆ withparticles may produce limited extra information relative tothe computational cost incurred, especially because the directformulation for our filter based on exponential coordinates,is capable of inferring attitude from accurate position (xyz)measurements. We investigate different choices forxm inour experiments.

V. I DENTIFYING THE PROCESSNOISE PARAMETERS

Due to the symmetry of the inertial sensors in the IMU, weassume the process noise covarianceQ is a diagonal matrixpopulated as

Q =

[

qgyroI3 00 qaccelI3

]

(41)

and qaccel and qgyro are the parameters we wish to identify.Two issues lead to difficulty with finding these values.First, the way the noise projects onto the state changeswith the time-varyingWt matrix such that theQ matrixcannot be recovered in closed form simply by summing theouter product of sampled error. More importantly we cannotdepend on the availability of ground truth measurementsof the measured quantities, since even accurate positioningsystems do note directly measure acceleration and angularrate. Further, the behavior of the sensor may be differentunder actual flight conditions due to vibration and loadingeffects and thus the values obtained in a static test may nothold.

Nonetheless it is desirable that the model parameters, andespecially the process noise parameters, be accurate. Forplanning purposes we must be able to predict distributionsover future states to guarantee safe trajectories. Within thecontext of state estimation and Monte-Carlo localization,aswe describe in section IV-C, it is important that an accuratecovariance of the state estimate be maintained when sensordata is sparse or absent, such that the state estimate can becan be properly distributed to obtain measurements whenthey become available.

While we do not have access to ground truth accelerationand angular rate with which to estimate the noise parameters,we can post-process data using a Kalman smoothing algo-rithm to obtain a state historyX =

[

x0 x1 . . . xT

]

with the error associated with each smoothed state estimategiven by

Γt = E[

(xt − xt)(xt − xt)T]

(42)

The key idea in our approach is in projecting the processnoise forward over multiple time steps such that the processnoise dominates the error in the smoothed estimate, thusallowing us to treat the smoothed estimate as ground truth.This works because the IMU process equations are neutrallystable and thus the perturbing noise results in unbounded

growth in covariance without position updates. The error onthe smoothed estimate (with position updates), on the otherhand, must be bounded (even if the smoothing occurs withincorrect noise parameters) since the system is observable.Additionally, by projecting the noise forward over multiplesteps, the parameters we identify will be suitable for use inplanning algorithms that require open-loop predictions [3]and the parameters will work with intermittent measurementfunctions as may be the case for laser localization in sparseenvironments.

Using the linearized dynamics from the EKF we canproject the filter covariance forwardN steps by repeatedlyapplying equations 3 3. Neglecting the error on the smoothedestimate, we obtain the expression:

E[

(xt+N − xt)(xt+N − xt))T]

= Σt,N (43)

=

N−1∑

i=0

Gt+i,NQGTt+i,N (44)

whereGt,N =∏t+N−1

j=t+1 AjWt. This is an important quantityfor our noise identification algorithm because it maps thenoise at each time step onto the state vector at timet+N .We can see that for identifying characteristics of the processnoise,At must be neutrally stable andWt must have fullcolumn rank. IfAt is highly unstable, theΣt,N will be overlysensitive to the noise valueswi for small i, whereas ifAt ishighly stable,Σt,N will be dominated by larger values ofiand thus the forward projection offers little benefit. However,many robotic systems, including our IMU dynamics, exhibitapproximately neutrally stable behavior.

For neutrally stable systems, asN gets large we expectΣt >> Γt. We can then divide up the datasetX to getM = T/N samples from prediction distributions obtainedby subtracting the state at timetend = iN + N − 1 fromthe state at timetbegin = iN for i ∈ [0,M − 1]. This givesus M samplesyi = xtend − xtbegin drawn from distributionsN(0,Σtbegin,N ) = P (xtend|xtbegin). We have a joint likelihoodfunction for our data given the parameters ofQ as:

P (Y |x0, Q) =M−1∏

i=0

P (xiN+N−1|xiN , Q). (45)

We would like to maximize this probability for which weuse the log-likelihood function,

l(Y |x0, Q) = −1

2

M−1∑

i=0

log |Σi|+ yTi Σiyi. (46)

From an intuitive standpoint we are optimizing for theqparameters that would produce the observed drift away fromthe smoothed estimate given by the samplesyi.

We setup and solve the optimization using standard nonlin-ear programming techniques. Specifically we use the interiorpoint method implemented in Matlab to solve for the maxi-mum likelihood values ofqgyro andqaccel. These new valuesare then used to obtain the Kalman smoothed trajectory, andthe process is repeated until convergence.

To identify the noise parameters of the IMU we flewour experimental vehicle (described below) outdoors witha low cost uBlox GPS unit. We also collected a dataset in

TABLE I

NOISE PARAMETER VALUES

Source Gyro Noise (deg/s) Accelerometer Noise (g)Vicon Optimization 0.35 0.0042GPS Optimization 0.34 0.0182Manufacturer 0.2 0.005

0 5 10 15 200

1

2

3

4Orientation Error

Deg

rees

Time (s)

0 5 10 15 200

10

20

30

40Position Error

Time (s)

Met

ers

viconvicon−predictedgpsgps−predicted

Fig. 3. This figure shows the predicted normed error and the actual normeddeviation from the smoothed estimates as a function of lookahead timefor the optimization run on both the vicon and GPS datasets. With theoptimized values we can accurately predict uncertainty for both estimationand planning purposes.

a high accuracy indoor motion capture system. Optimizednoise parameters for a lookahead time of 20 seconds areshown in table I with the manufacturer specified values forcomparison.

The optimization on the vicon dataset converges quicklyand consistently. However, when the optimization is per-formed on the GPS dataset the optimization is more sensitiveto initial conditions and window size. The vicon systemmeasures attitude directly, thus the smoothed attitude es-timate is dominated by the actual measurement. With theGPS dataset, attitude must be inferred from position updateswhich means the attitude estimate will be more stronglycorrelated with the IMU noise, thus making it more difficultfind the underlying noise parameter. Additionally, the GPSmeasurements are subject to time-varying bias which is notmodeled in our filter. Nonetheless, the optimization for viconand GPS converge to nearly identical values for the gyronoise at at 20 second window. The relative sensitivity to thewindow size for the GPS optimization can be seen in figure4.

The noise parameters in table I were used to generate thepredicted error lines in figure 3. We can see that the predictederror for GPS and vicon are very close for orientationas we would expect from the tabular values. In position,the deviation is also small which is surprising given thelarge difference in optimized accelerometer noise values.Thereason for this is that the positional uncertainty is largely afunction of angular uncertainty resulting in the gravity vectorbeing misinterpreted as lateral acceleration. This highlightsanother difficulty in teasing apart the relative affect of the

0 5 10 15 200

0.2

0.4

q gy

ro (

deg/

s)

Optimal IMU Noise Parameters vs. Lookahead Time

vicongps

0 5 10 15 200

0.01

0.02

q ac

cel (

g)

Optimization Lookahead time (seconds)

Fig. 4. This figure shows values forqgyro andqaccel obtained by optimizingequation 46 for different lookahead times (values ofN scaled by samplingfrequency) for both GPS and vicon. For small time the optimal noiseparameters obtained with GPS are dominated by the error in the smoothedestimates,Γt, but we see for largeN consistent values are reached. Thevicon dataset is less susceptible to this issue. It is interesting to note thatas lookahead time increases fewer “samples” are available from a datasetof fixed size, and thus the computed noise values have higher variance,implying some optimal lookahead window to identify the parameters.

noise parameters.

VI. EXPERIMENTAL RESULTS

Our experimental platform consists of a custom builtfixed-wing vehicle carrying a payload of a Hokuyo UTM-30LX laser rangefinder, a Microstrain 3DM-GX3-25 IMU,and a 1.6GHz Intel Atom base flight computer. We con-ducted a number of flight tests in the indoor environmentshown in Figure 5(a). While we did not have access toany sort of ground truth state estimates, we were able totest our algorithms on real flight data. The accuracy ofour state estimates are validated qualitatively by lookingat the accurate reconstruction of the 3D environment byreprojecting the laser points using our state estimates. Onesuch 3D point cloud is shown in Figure 5(b). To get a betterfeel for the experiments, we invite the interested reader toview the videos of the experiment available on our website:http://groups.csail.mit.edu/rrg/icra12-agile-flight

To quantify the error of the state estimator, we aggressivelymaneuvered the sensing payload in a high accuracy viconmotion capture studio. While the motion of the sensingpayload will certainly be very different when the vehicle isflying, the data allows us to evaluate our algorithms with aground truth comparison. These ground truth state estimatesallow us to evaluate the properties of our state estimationalgorithm. Results for different number of particles anddifferent partitions of the state vector are summarized infigure 6. We can see that by not partitioning the stateand performing standard GPF updates we incur significantcomputational cost in terms of number of particles to achievethe same level of accuracy. This increase in the number ofparticles is to be expected given that we are using particlesto capture the same correlations that are well capturedanalytically by the Kalman pseudo-measurement update.

The experiments demonstrate the ability of our algorithmto maintain accurate state estimates in the face of fast motion,with linear velocities up to9m/s, and angular rates of up to

(a) (b)

Fig. 5. A picture of the indoor space (a) where we flew our fixed wing vehicle. The space is roughly 12 meters by 20 meters and our vehicle fliesbetween 6 and 10 m/s, thus aggressive maneuvering and tight turning is required to stay airborne. The trajectory flown by thevehicle is shown by thered, green, and blue axes in (b). The quality of the state estimates is evident in the (height colored) point cloud renderedusing the state estimates of ouralgorithm. The floor and ceiling were cropped for visual clarity.

360 degrees per second. While a naive implementation ofthe GPF measurement update correctly estimates the stateof the vehicle with a sufficient number of particles, therequired number of particles is dramatically larger than forthe partitioned state version. The naive GPF implementationwould not be able to run in realtime on board the vehiclegiven the computation power available.

VII. R ELATED WORK

State estimation using Kalman filtering techniques hasbeen extensively studied for vehicles flying outdoors whereGPS is available. A relevant example of such a state esti-mation scheme developed by Kingston et al. [12] involvestwo Kalman filters where roll and pitch are determined bya filter driven by gyro readings as system inputs while theaccelerometer measurements are treated as a measurement ofthe gravity vector, assuming unaccelerated flight. A separatefilter estimates position and yaw using GPS measurements.

This approach is representative of many IMU-based es-timators that assume zero acceleration and thus use theaccelerometer reading as a direct measurement of attitude(many commercially available IMUs implement similar tech-niques on board using a complementary filter). While thisapproach has practical appeal and has been successfully usedon a number of MAVs, the zero acceleration assumption doesnot hold for general flight maneuvering and thus the accuracyof the state estimate degrades quickly during aggressiveflight.

Van der Merwe et al. use a sigma-point unscentedKalman filter (UKF) for state estimation on an autonomoushelicopter[20]. The filter utilizes another typical approachwhereby the accelerometer and gyro measurements are di-rectly integrated to obtain position and orientation and arethus treated as noise perturbed inputs to the filter. Ourfilter utilizes this scheme in our process model, howeverwe use an EKF with exponential coordinates based attituderepresentation instead of the quaternions used by Van derMerwe et al.

Techniques to identify the noise parameters relevant forthe Kalman filter emerged not long after the original filter,however the most powerful analytical techniques assume

steady state behavior of a linear time invariant system andare thus unsuitable for the time varying system that resultsfrom linearizing a nonlinear system [14]. More recent workoptimizes the likelihood of a ground-truth projection of thestate over the noise parameters but thus requires the systembe fitted with a sensor capable of providing ground-truthfor training. [1]. Our algorithm does not require the use ofadditional sensors, or external ground truth.

Laser rangefinders combined with particle filter basedlocalization is widely used in ground robotic systems [19].While planar lidars are commonly used to estimate motionin the 2D plane, they have also proved useful for localizationin 3D environments. Prior work in our group [2], as well asothers [18], [6] leveraged a 2D laser rangefinder to performSLAM from a quadrotor in GPS-denied environments. Thesystems employ 2D scan-matching algorithms to estimatethe position and heading, and redirect a few of the beamsin a laser scan to estimate the height. While the systemshave demonstrated very good performance in a numberof realistic environments, they must make relatively strongassumptions about the motion of the vehicle, and the shapeof the environment. Namely, they require walls that are leastlocally vertical, and a mostly flat floor for height estimation.As a result, the algorithms do not extend to the aggressiveflight regime targeted in this paper. Scherer et al. use laserrangefinders to build occupancy maps, and avoid obstacleswhile flying fast through obstacles [17], however they relyon accurate GPS measurements for state estimation, and donot focus on state estimation.

In addition to the laser based systems for GPS-deniedflight, there has been a significant amount of research onvision based control of air vehicles. This includes both fixedwing vehicles [11], as well as larger scale helicopters [4],[10], [8]. While vision based approaches warrant furtherstudy, the authors do not address the challenge of agileflight. This is likely to be particularly challenging for visionsensors due to the induced motion blur, combined with thecomputational complexity of vision algorithms.

Recently, Hesch et al. [7] developed a system that issimilar in spirit to ours to localize a laser scanner and INSfor localizing people walking around in indoor environments.

101

102

103

104

0.0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1.0

Number of Particles

Fra

ctio

n D

iver

ged

∆∆, χ

z

∆,χx

(a)

101

102

103

104

0.128

0.13

0.132

0.134

0.136

0.138

0.14

0.142

0.144

0.146

0.148

Number of Particles

Mea

n V

eloc

ity E

rror

(m

/s)

∆∆, χ

z

∆,χx

(b)

Fig. 6. This figure shows the percentage of trials where the filter diverged(a) and the mean velocity error verses the number of particles used in theGPF (b) for different state partitions in the measurement. As expected,the more states we use in the measurement function the more particlesare required to obtain satisfactory estimates. In a naive implementationwhere the full state is used in the measurement and thus a standard GPFupdate performed, we require 2000 particles to get similar performance toa measurement update in∆ using only 100 particles. Thus our algorithmyields an effective 20x speedup.

They make a number of simplifying assumptions such aszero velocity updates, that are not possible for a micro airvehicle. Furthermore, they model the environment as a setof planar structures aligned with one of 3 principle axes,which severly limits the types of environments in which theirapproach is applicable. Our system uses a general occupancygrid representation which provides much greater flexibilityof environments.

VIII. C ONCLUSION

In this paper we presented a state estimation algorithmfor a fixed wing vehicle based on an IMU and laser rangescanner. Our algorithm provides a novel extension of theGaussian particle filter and an exponential coordinates lin-earization of the IMU dynamics equations. We have demon-strated the performance of our algorithms on two challengingdatasets. The quantitative analysis in motion capture clearlyshows the advantages of our extensions to the Gaussianparticle filter algorithm, while the accurate map generatedduring the flight tests demonstrate the absolute accuracy ofour algorithms.

Integrating the state estimation algorithm with planningand control algorithms to perform closed-loop flight indoorsremains for future work. We are particularly interested inusing the state estimates in our previously developed partiallyobservable planning frameworks.

In addition to the planning and control extensions, in-vestigation of other sensing modalities such as vision areof great interest. We believe that the filtering frameworkdeveloped for the laser rangefinder will extend to incorporateadditional measurement types, thereby further improving thecapabilities of our system.

IX. A CKNOWLEDGEMENTSThis work was supported by ONR under MURI N00014-

09-1-1052, MURI N00014-10-1-0936, and Science of Au-tonomy grant N00014-10-1-0936. Their support is gratefullyacknowledged.

REFERENCES

[1] P. Abbeel, A. Coates, M. Montemerlo, A. Y. Ng, and S. Thrun.Discriminative training of Kalman filters. InProceedings of Robotics:Science and Systems, Cambridge, USA, June 2005.

[2] A. Bachrach, S. Prentice, R. He, and N. Roy. RANGE: Robustautonomous navigation in GPS-denied environments.Journal of FieldRobotics, 28(5):644–666, 2011.

[3] A. Bry and N. Roy. Rapidly-exploring random belief treesformotion planning under uncertainty. InIEEE Int. Conf. Robotics andAutomation, May 2011.

[4] G. Buskey, J. Roberts, P. Corke, and G. Wyeth. Helicopterautomationa using a low-cost sensing system.Computing Control EngineeringJournal, 15(2):8 – 9, April-May 2004.

[5] A. Coates, P. Abbeel, and A.Y. Ng. Learning for control from multipledemonstrations. InInt. Conference of Machine Learning, 2008.

[6] S. Grzonka, G. Grisetti, and W. Burgard. Towards a navigationsystem for autonomous indoor flying. InIEEE Int. Conf. Roboticsand Automation, pages 2878–2883, May 2009.

[7] J.A. Hesch, F.M. Mirzaei, G.L. Mariottini, and S.I. Roumeliotis. Alaser-aided inertial navigation system L-INS for human localizationin unknown indoor environments. InIEEE Int. Conf. Robotics andAutomation, pages 5376–5382. IEEE.

[8] Stefan Hrabar and Gaurav Sukhatme. Vision-based navigation throughurban canyons.Journal of Field Robotics, 26(5):431–452, 2009.

[9] A. S. Huang, A. Bachrach, P. Henry, M. Krainin, D. Maturana, D. Fox,and N. Roy. Visual odometry and mapping for autonomous flightusing an RGB-D camera. InInt. Symp. Robotics Research, Flagstaff,Arizona, USA, Aug. 2011.

[10] J. Kelly, S. Saripalli, and G. Sukhatme. Combined visual and inertialnavigation for an unmanned aerial vehicle. pages 255–264. 2008.

[11] J. Kim and S. Sukkarieh. SLAM aided GPS/INS navigation inGPS denied and unknown environments. InThe 2004 InternationalSymposium on GNSS/GPS, Sydney, pages 6–8, 2004.

[12] D.B. Kingston and R.W. Beard. Real-time attitude and positionestimation for small UAVs using low-cost sensors.American Instituteof Aeronautics and Astronautics, 2000.

[13] J. H. Kotecha and P. M. Djuric. Gaussian particle filtering. IEEETrans. Signal Processing, 51(10):2592–2601, October 2003.

[14] R. Mehra. On the identification of variances and adaptive kalmanfiltering. IEEE Trans. Automatic Control, 15(2):175 – 184, apr 1970.

[15] D. Mellinger and V. Kumar. Minimum snap trajectory generation andcontrol for quadrotors. InIEEE Int. Conf. Robotics and Automation,May 2011.

[16] J.W. Roberts, R. Cory, and R. Tedrake. On the controllability of fixed-wing perching. InAmerican Control Conference, pages 2018–2023.IEEE, 2009.

[17] S. Scherer, S. Singh, L. Chamberlain, and M. Elgersma. Flying fastand low among obstacles: Methodology and experiments.Int. Journalof Robotics Research, 27(5):549–574, May 2008.

[18] S. Shen, N. Michael, and V. Kumar. Autonomous multi-floor indoornavigation with a computationally constrained mav. InIEEE Int. Conf.Robotics and Automation, pages 20–25. IEEE.

[19] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Robust Monte Carlolocalization for mobile robots.Artificial Intelligence Journal, 2001.

[20] R. van der Merwe and E. Wan. Sigma-point Kalman filters forintegrated navigation. InProc. Institute of Navigation (ION), Dayton,OH, June 2004.

[21] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W.Bur-gard. OctoMap: A probabilistic, flexible, and compact 3D maprepresentation for robotic systems. InProc. of the ICRA 2010Workshop on Best Practice in 3D Perception and Modeling for MobileManipulation, Anchorage, AK, USA, May 2010.