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. 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.

2 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. 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.

3 However,the high volume of data also poses a significant challengefor estimation algorithm design. 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). 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.

4 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. 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. In these meth-ods, the current IMU pose, as well as the 3D positionsof all visual landmarks are jointly estimated [1] [4].

5 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. 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.

6 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. 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.

7 On the other hand, in [14],allcamera posesare simultaneously estimated. In both of these algorithms,pairwiserelative-pose measurements are derived from theimages, and used for state updates. The drawback of thisapproach is that when a feature is seen in multiple images,the additional constraints between the multiple poses arediscarded, thus resulting in loss of information. Further-more, when the same image measurements are processedfor computing several displacement estimates, these are notstatistically independent, as shown in [15].One algorithm that, similarly to the method proposedin this paper, directly uses the landmark measurementsfor imposing constraints between multiple camera poses ispresented in [16]. This is a visual odometry algorithm thattemporarily initializes landmarks, uses them for imposingconstraints on windows of consecutive camera poses, andthen discards them.

8 This method, however, does not in-corporate inertial measurements. Moreover, the correlationsbetween the landmark estimates and the camera trajectoryare not properly accounted for, and as a result, the algorithmdoes not provide any measure of the covariance of the window of camera poses is also maintained in theVariable State Dimension Filter (VSDF) [17]. The VSDFis a hybrid batch/recursive method, that (i) usesdelayedlinearizationto increase robustness against linearization in-accuracies, and (ii) exploits the sparsity of the informationmatrix, that naturally arises whenno dynamic motion modelis used. However, in cases where a dynamic motion modelis available (such as in Vision-aided inertial navigation) thecomputational complexity of the VSDF is at bestquadraticin the number of features [18].In contrast to the VSDF, the Multi-State Constraint filterthat we propose in this paper is able to exploit the benefits ofdelayed linearization while having complexity onlylinearinthe number of features.

9 By directly expressing the geometricconstraints between multiple camera poses it avoids thecomputational burden and loss of information associated withpairwise displacement estimation . Moreover, in contrast toSLAM-type approaches, it does not require the inclusionof the 3D feature positions in the Filter state vector, butstill attainsoptimalpose estimation . As a result of theseproperties, the described algorithm is very efficient, and asshown in Section IV, is capable of high-precision Vision-aided inertial navigation in real ESTIMATORDESCRIPTIONThe goal of the proposed EKF-based estimator is to trackthe 3D pose of the IMU-affixed frame{I}with respect toaglobal frameof reference{G}. In order to simplify theAlgorithm 1 Multi-State Constraint FilterPropagation: For each IMU measurement received,propagate the Filter state and covariance (cf. Section III-B).

10 Image registration: Every time a new image is recorded, augment the state and covariance matrix with a copy ofthe current camera pose estimate (cf. Section III-C). image processing module begins : When the feature measurements of a given imagebecome available, perform an EKF update (cf. Sections III-Dand III-E).treatment of the effects of the earth s rotation on the IMUmeasurements (cf. Eqs. (7)-(8)), the global frame is chosen asan Earth-Centered, Earth-Fixed (ECEF) frame in this overview of the algorithm is given in Algorithm 1. TheIMU measurements are processed immediately as they be-come available, for propagating the EKF state and covariance(cf. Section III-B). On the other hand, each time an imageis recorded, the current camera pose estimate is appendedto the state vector (cf. Section III-C). State augmentationis necessary for processing the feature measurements, sinceduring EKF updates the measurements of each trackedfeature are employed for imposing constraints between allcamera poses from which the feature was seen.