A Multi-State Constraint Kalman Filter for Vision-aided ...

1 A Multi-State Constraint Kalman Filterfor Vision-aided Inertial NavigationAnastasios I. Mourikis and Stergios I. RoumeliotisAbstract In this paper, we present an Extended KalmanFilter (EKF)-based algorithm for real-time Vision-aided inertialnavigation. The primary contribution of this work is thederivation of a measurement model that is able to expressthe geometric constraints that arise when a static feature isobserved from multiple camera poses. This measurement modeldoes not require including the 3D feature position in the statevector of the EKF and is optimal, up to linearization Vision-aided inertial navigation algorithm we propose hascomputational complexity onlylinearin the number of features,and is capable of high-precision pose estimation in large-scalereal-world environments.

2 The performance of the algorithmis demonstrated in extensive experimental results, involving acamera/IMU system localizing within an urban INTRODUCTIONIn the past few years, the topic ofvision-aided inertialnavigationhas received considerable attention in the re-search community. Recent advances in the manufacturing ofMEMS-based inertial sensors have made it possible to buildsmall, inexpensive, and very accurate Inertial MeasurementUnits (IMUs), suitable for pose estimation in small-scalesystems such as mobile robots and unmanned aerial systems often operate in urban environments whereGPS signals are unreliable (the urban canyon ), as well asindoors, in space, and in several other environments whereglobal position measurements are unavailable.

3 The low cost,weight, and power consumption of cameras make them idealalternatives for aiding inertial navigation, in cases where GPSmeasurements cannot be relied important advantage of visual sensing is that imagesare high-dimensional measurements, with rich informationcontent. Feature extraction methods can typically detect andtrack hundreds of features in images, which, if properlyused, can result is excellent localization results. However,the high volume of data also poses a significant challengefor estimation algorithm design.

4 When real-time localizationperformance is required, one is faced with a fundamentaltrade-off between the computational complexity of an algo-rithm and the resulting estimation this paper we present an algorithm that is able tooptimallyutilize the localization information provided bymultiple measurements of visual features. Our approach ismotivated by the observation that, when a static feature isviewed from several camera poses, it is possible to defineThis work was supported by the University of Minnesota (DTC), theNASA Mars Technology Program (MTP-1263201), and the National Sci-ence Foundation (EIA-0324864, IIS-0643680).

5 The authors would like tothank Faraz Mirzaei for his invaluable help with the authors are with the Dept. of Computer Science & Engi-neering, University of Minnesota, Minneapolis, MN 55455. constraintsinvolving all these poses. The primarycontribution of our work is a measurement model thatexpresses these constraintswithoutincluding the 3D featureposition in the Filter state vector, resulting in computationalcomplexity onlylinearin the number of features.

6 After abrief discussion of related work in the next section, the de-tails of the proposed estimator are presented in Section III. InSection IV we describe the results of a large-scale experimentin an uncontrolled urban environment, which demonstratethat the proposed estimator enablesaccurate, real-timeposeestimation. Finally, in Section V the conclusions of this workare RELATEDWORKOne family of algorithms for fusing inertial measurementswith visual feature observations follows the SimultaneousLocalization and mapping (SLAM) paradigm.

7 In these meth-ods, the current IMU pose, as well as the 3D positionsof all visual landmarks are jointly estimated [1] [4]. Theseapproaches share the same basic principles with SLAM-based methods for camera-only localization ( , [5], [6],and references therein), with the difference that IMU mea-surements, instead of a statistical motion model, are usedfor state propagation. The fundamental advantage of SLAM-based algorithms is that they account for the correlations thatexist between the pose of the camera and the 3D positions ofthe observed features.

8 On the other hand, the main limitationof SLAM is its high computational complexity; properlytreating these correlations is computationally costly, andthus performing vision-based SLAM in environments withthousands of features remains a challenging algorithms exist that, contrary to SLAM, estimatethe pose of the cameraonly( , do not jointly estimatethe feature positions), with the aim of achieving real-timeoperation. The most computationally efficient of these meth-ods utilize the feature measurements to derive constraintsbetween pairs of images.

9 For example in [7], an image-based motion estimation algorithm is applied to consecutivepairs of images, to obtain displacement estimates that aresubsequently fused with inertial measurements. Similarly,in [8], [9] constraints between current and previous image aredefined using the epipolar geometry, and combined with IMUmeasurements in an Extended Kalman Filter (EKF). In [10],[11] the epipolar geometry is employed in conjunction witha statistical motion model, while in [12] epipolar constraintsare fused with the dynamical model of an airplane.

10 The use offeature measurements for imposing constraints betweenpairsof images is similar in philosophy to the method proposed inthis paper. However, one fundamental difference is that ouralgorithm can express constraints betweenmultiplecameraposes, and can thus attain higher estimation accuracy, incases where the same feature is visible in more than constraints are also employed in algorithms thatmaintain a state vector comprised of multiple camera [13], an augmented-state Kalman Filter is implemented,in which a sliding window of robot poses is maintained inthe Filter state.