Exploiting Ground Plane Constraints forVisual-Inertial Navigation

Ghazaleh Panahandeh, Dave Zachariah, and Magnus JanssonACCESS Linnaeus Centre

KTH Royal Institute of TechnologySE-100 44 Stockholm, Sweden

{ghpa, dave.zachariah, magnus.jansson}@ee.kth.se

Abstract—In this paper, an ego-motion estimation approachis introduced that fuses visual and inertial information, using amonocular camera and an inertial measurement unit. The systemmaintains a set of feature points that are observed on the groundplane. Based on matched feature points between the current andprevious images, a novel measurement model is introduced thatimposes visual constraints on the inertial navigation system toperform 6 DoF motion estimation. Furthermore, feature pointsare used to impose epipolar constraints on the estimated motionbetween current and past images. Pose estimation is formulatedimplicitly in a state-space framework and is performed by aSigma-Point Kalman filter. The presented experiments, conductedin an indoor scenario with real data, indicate the ability of theproposed method to perform accurate 6 DoF pose estimation.

Index Terms—Ego-motion estimation, vision-aided INS,ground plane feature detection, epipolar geometry.

I. INTRODUCTION

Navigation based on signals from an inertial measurementunit (IMU) has the advantage of being self-contained androbust with respect to external disturbances. With the devel-opment of MEMS technology, it is possible to implementthe navigation system at low cost. The drawback of dead-reckoning based navigation, however, is that the position errorsaccumulate over time. The growth rate of the error can besignificantly reduced if the system is aided with additionalsensor information. One of the most common alternativesensors is a radio receiver, such as a global positioning system(GPS) receiver.

Integration of vision and inertial sensors is an alternativeto GPS for indoor as well as outdoor navigation when theGPS signals are not available or reliable. By proper incorpo-ration of visual information, the error growth of the inertialnavigation system (INS) can be reduced [1]. However, aidingthe INS with a vision sensor requires sensor-to-sensor relativetransformation to be known; disregarding such an offset in thesystem will introduce unmodeled biases that may grow overtime [2], [3]. Additionally, in vision-based pose analysis, acentral challenge is that information about metric distances islost in the camera projection that transforms the position of3D points onto the 2D image plane [4].

In order to get more accurate and stable pose estimates,finding a proper solution for merging the IMU informationwith the camera image data is crucial. Depending on theimage processing tools and computer vision methods used to

extract the pose information from the camera images, differentsolutions are proposed for such a framework.

The structure of this paper is as follows. A summary ofthe related works is given in Section I-A. In Section I-B,we present an overview of our proposed method. Notationsare introduced in Section I-C. In Section II, we providethe sensors models and describe the INS and the camerastates. In Section III, the fundamental geometrical constraintsof the visual inputs are derived. Measurement model of thesystem, which leads to the state-space equations, is given inSection IV. The structure of the used Sigma-Point Kalmanfilter algorithm is presented in Section V. Feature detectionand outlier rejection are discussed in Section VI. Results aredemonstrated in Section VII. Finally, the conclusion of thestudy is summarized in Section VIII.

A. Related work

Fusing inertial and vision sensors has been explored indifferent applications, such as vision-aided navigation sys-tems [5], [6], mobile robotics [7], [8], and traffic applica-tions [9]. Broadly speaking, vision-aided inertial navigationcan be divided in two approaches: vision-based SLAM [10],[11] and ego-motion estimation [6], [12], [13].

In vision-based SLAM, a 3D map of observed featurepoints in the scene is progressively estimated jointly withthe current camera pose. Using the estimated map, thesemethods are capable of loop-closing that cancels drift ofthe position estimate when returning to an already-visitedlocation. In ego-motion estimation methods, the main goal isto recover the camera pose rather than the positions of thefeature points. Hence, the computational complexity of ego-motion estimation approaches is significantly lower than thatof the SLAM-based methods.

Two commonly used image processing techniques, forextracting pose information from the images, are based onfeature point extraction [13] and optical flow computation [14].Feature point extraction is usually followed by point matchingbetween the consecutive images to obtain displacement mea-surements that are subsequently fused with inertial measure-ments. While in the optical flow based methods, changes inthe images are treated as the projection of the scene velocity.As neither technique can recover the metric scale, furtherconstraints must be imposed for motion estimation. Stereo

cameras can resolve the scale ambiguity problem by providingdepth estimates of the scene [12], [15], [16]. But the accuracyof such methods is highly dependent on the relative baselinebetween the cameras.

More information may be exploited when navigating instructured environments, e.g., man-made buildings. In theliterature, various structures have been investigated for vision-based motion estimation, e.g., horizontal or vertical lines,vanishing points and lines [4], edges of buildings or fixedobjects, and artificial visual tags [17].

A few approaches make use of features located on theground plane, which is a salient structure in these environ-ments. For instance, Song et al. [18] proposed an algorithm forestimating the velocity of mobile robots based on the Kalmanfilter integration using an optical flow method for a downward-looking camera. A more flexible system for pedestrian naviga-tion is introduced in [19] in which a camera is pointed towardsthe ground. Using a computer vision algorithm, 3D cameraframe translation is derived that provides the velocity of thecamera in the measurement model. The hardware structureof their system is, however, aided with GPS data in additionto the IMU and camera sensors. Moreover, in our previouswork [13], we introduced an IMU-camera ego-motion methodbased on the assumption that the camera optical axis is roughlyorthogonal to the ground plane. Then, in contrast to theexisting approaches, the scale ambiguity problem is removedand the system is capable of 5 DoF motion estimation.

However, such a configuration is restricted to a down-ward looking camera which further limits the extraction ofother structural information from the scene. By observingthe surrounding environment and the ground plane one canimpose more constraints on the camera relative position andorientation over time. Since the feature points located on theground plane are coplanar, information about their relativeposition can be recovered given a pose estimate of the nav-igation system. Additionally, selecting ground plane featuresfor motion estimation avoids regions of the image that maycontain moving objects like human beings or moving vehicles.

B. System overview

In this paper, an IMU-camera sensor fusion system isintroduced for a scenario that may find applications in mobilerobot navigation, as well as personal navigation. The systemperforms 6 DoF ego-motion estimation by maintaining a set ofobserved ground feature points of a previous image frame. Incontrast to other methods that use cameras with an optical axisapproximately orthogonal to the ground plane, such as [13],[18], [19], no specific restrictions are imposed on the cameramotion other than that the camera is continuously observingfeatures on the plane.

When the camera optical axis is fixed to be perpendicularto the ground plane, the feature points located on the planeare at a constant orthogonal distance to the image plane. Theassumption of using a camera with orthogonal optical axis tothe ground plane is relaxed by virtually transforming the imageplane as if it was captured by a virtual camera perpendicular

to the ground plane [20]. Using the virtual camera concept,we derive a novel measurement model for ground planeobservations. Additionally, by exploiting epipolar geometrybetween views, we can further constrain the direction of thetranslation vector [4], [5], [21]. Such constraints, however,cannot reduce the error growth rate along the direction ofmotion; hence, the observed ground plane features act ascomplementary constraints.

The 6 DoF ego-motion estimation problem is posed ina state-space framework and solved by a state-augmentedSigma-Point Kalman filter [22]. The maximum number ofaugmented feature points is a parameter that controls the sizeof the state vector. At each time instant when a new frameis processed, states representing past feature points are subse-quently discarded and replaced by new ones. To obtain featurepoints on the ground plane they must first be detected. Existingdetection methods are either based on ground plane textures,e.g., color cue segmentations [23], geometric assumptions onthe environment [23], [24], or regions of the image planethat reliably contain the ground plane [15]. These methodseither assume constrained camera motion or that the planarhomography is constructed only after a manual initializationphase. Similar to [15], in this paper, the ground plane featuresare selected using a predefined box in the lower part of theimage while the outliers are rejected using the filter’s estimatesover time.

C. Notation

In the following sections scalars are denoted by lowercaseletters (s), vectors by bold letters (f ), and matrices by boldcapitals (K).A⊕B denotes the direct sum between matrices A and B.

The matrix Im denotes the m×m square identity matrix.The coordinate frames are denoted {a} so that pa is the

position in that frame and Rba is the direction-cosine matrix

that rotates a vector from {a} to {b}. The navigation, IMU,and the current camera frames are denoted, {n}, {b}, and{c}, respectively. {c�} is the coordinate frame of the cameracapturing an image � lags from the current one. Then pn

c�is

the position of the camera in {n} at that time instant.

II. INS AND CAMERA STATES

Lets consider an IMU-camera sensor fusion system in whichthe camera is rigidly mounted on the IMU. The goal of theproposed algorithm is to estimate the pose of this mobilevisual-inertial system with respect to the reference navigationframe {n}.

The INS estimates the position, pnb , velocity, vn

b , andattitude, Rn

b , at a high rate given noisy accelerometer signalsf b and gyroscope signals ωb. Let dt denote the samplingperiod, then the estimates are propagated by the followingequations:

pnk+1 = pn

k + dtvnk

vnk+1 = vn

k + dtRnb,k f

bk + dtgn

Rnb,k+1 = Rn

b,k exp�dt[ωb]×

�,

(1)

IMU INS

Visualinput

f b ωb pn vn Rbn

yc

h(·)

SPKF +

δx yc

Fig. 1: System architecture.

for each IMU sample k. Here, gn is the local gravitation vectorand [·]× denotes the skew-symmetric matrix representation ofthe cross product operation [25]. For the matrix exponential,exp(·), the Pade approximation can be used [26].

The position of the camera is related to the navigation statesby the transformation pn

c = pnb + Rn

bpbc, where pb

c denotesthe offset between the IMU and camera coordinate centers in{b} and is assumed to be known by calibration [2], [3].

As depicted in Fig. 1, we approach the estimation prob-lem using a feedback approach. Given measurements fromthe camera (Visual input), yc, the errors of the INS errorsare estimated and corrected by a Sigma-Point Kalman filter(SPKF). Hence, to estimate parameters in the SPKF, the totalsystem state vector is described as

xins ��(pn

b )� (vn

b )� (qn

b )� (f b)� (ωb)�

��, (2)

where qnb is the unit quaternion representation of the rotation

matrix Rnb , and the IMU sensor biases are represented by f b

and ωb.

A. INS error states

Let δpnb and δvn

b denote the position and velocity errors ofthe INS, respectively. Parameterizing the quaternion error byEuler angles, we denote the attitude error by δθ ∈ R

3. Finallythe IMU sensor error biases are represented by δf b and δωb.Then the errors can be concatenated into an error state vector,

δxins ��(δpn

b )� (δvn

b )� (δθ)� (δf b)� (δωb)�

��.

(3)During a short period of time the errors can be modeled by

a discrete-time linear state-space model

δxinsk+1 = Fins

k δxinsk +Gins

k wk ∈ R15. (4)

The statistical properties of the process noise, wk, is givenby the noise characteristics of the IMU. We model it as atemporally uncorrelated, zero-mean process with a diagonalcovariance matrix Q ∈ R

12×12. The system matrices Finsk and

Ginsk are given in Appendix A.

B. Camera error states

For small angle errors, the camera position error equals

δpnc = Tδxins, where T �

�I3 0 [Rn

bpbc]× 0

�.

Given a set of the L most recent images taken at differentlocations {pn

c�}L�=1, the camera position errors are augmentedinto

δxcam = [(δpnc1)

� · · · (δpncL)

�]�. (5)

gravity field gravity field

ground plane {n}

z

y

x

{ } { }

{ } { }

z z

z z

Fig. 2: The real and the virtual camera coordinate frames for the current andthe previous view. The two real camera coordinate frames, {c} and {c�}, aredepicted in black color. Their corresponding virtual camera coordinate frames,{v} and {v�}, are depicted in red color. Each virtual camera coordinate frameis collocated at the center of its corresponding camera coordinate frame. Thenavigation coordinate frame {n} is assumed to be located on the groundwhere its z axis is considered to be orthogonal to the ground plane. A samplefeature point π, located on the ground plane, is considered to be on the bothcamera’s field of view.

Since, the recorded position errors are static, the discrete-timeerror state space model of the camera position is

δxcamk+1 = δxcam

k .

III. GEOMETRY OF PROJECTED FEATURES

In this section, the essential geometrical constraints from thevisual input are given. The obtained relations are then used inSection IV to construct the state-space model of the system.Consequently, this model is also used to formulate a SPKFthat estimates the INS errors in Section V.

If π ∈ R3 is an observed feature point by a camera, its

position relative to the camera center {c} is denoted by πc andits normalized pixel coordinates and homogenous coordinateson the image plane are represented by zc and zc, respectively.Using the pinhole camera model [4], in the following, we willmake use of the relationship between a feature point and itsprojection on the image plane,

zc = (e�z πc)−1πc, (6)

andzc =

�I2 0

�(e�z π

c)−1πc, (7)

where e�z = [0 0 1]. Note that scale is lost in the projectionof πc.

A. Ground plane features

Consider an arbitrary feature point π and its positioncoordinatized in camera frames {c} and {c�}, respectively,illustrated in Fig. 2. The general transformation of the pointbetween frames can be written as,

πc = Rcc�(πc� − tc�c ) . (8)

This enables us to relate an observation of π in two differentviews to the motion between the views, parameterized by therotation matrix Rc

c� and the translation vector tc�c . Thus theprojection of πc is correlated with the motion of the systemand therefore imposes constraints on the accumulated INSerrors (3).

The feature point observed in the past frame πc� is, how-ever, an unknown nuisance parameter. Estimating the distanceof πc� is a poorly conditioned problem when it is largerelative to the translation between views. The problem canbe circumvented by exploiting the fact that the point belongsto the ground plane. Then πc� can be expressed in termsof camera position pn

c� and observation zc� [13]. In order toderive the alternative parameterization of πc� , we first considera ‘virtual’ camera coordinate frame {v�} collocated at thecenter of the camera coordinate frame {c�}; see Fig. 2. Theorientation of {v�} is aligned with {n} and its optical axis isperpendicular to the ground plane. Then the distance to thepoint in {v�} along the optical axis, e�z π

v� , is equal to theheight of the camera center above the plane, e�z p

nc�

.As the transformation between the camera frame and the

virtual frame is related by a simple rotation, using (6), wecan reparameterize the unknown ground plane feature point’sposition πc� by

πc� = Rc�v�π

v�

= Rc�v�(e�z π

v�)zv�

= Rc�v�(e

�z p

nc�)

Rv�c�zc�

e�z Rv�c� z

c�

=e�z pn

c�

e�z Rv�c� z

c�zc� ,

(9)

where pnc� is the camera position, zc� is the observation

in homogenous coordinates and Rv�c�

is the rotation matrixbetween frames. Using (9), the general transformation (8) cannow be rewritten for the ground plane feature π as

πc = Rcc�(πc� − tc�c )

= Rcc�π

c� −RcbR

bn(p

nc − pn

c�)

= Rcc�

α

βzc� −Rc

bRbn(p

nc − pn

c�),

(10)

where α � e�z pnc� and β � e�z Rv�

c� zc� . This eliminates the

explicit dependence on the distance to the feature point, andexpresses it in terms of the camera position pn

c�and past

observation zc� .The rotation matrix Rv�

c� is factorized into Rv�c� =

Rv�n Rn

bRbcR

cc� . An estimate of the relative rotation Rc

c� isgiven by the gyroscopes provided that the duration betweenthe frames is relatively short so that the integrated errors aresmall. The IMU-camera rotation Rb

c is fixed and assumedto be calibrated. The attitude Rn

b is a navigation state (2).The rotation matrix Rv�

n ≡ Rvn = diag(1,−1,−1), since the

orientation of the virtual camera frame is fixed relative to thenavigation frame [20].

B. Epipolar constraints

Further relative constraints can be imposed between {c} and{c�}, in periods during which the matched ground plane fea-tures are few or nonexistent. All the matched points {zci , zc�i }are transformed into parallel image planes by Rc

c�, {zci , zci}.

The displacement of this pair of points provides informationabout the translation from pn

c� to pnc : The transformed matches

{zci , zci} generate lines that ideally intersect at the epipolarpoint zcepi,� on the image plane. This point is the projection ofthe translation vector Δp = pn

c� − pnc onto the image plane

of {c}; details are given in [21].The epipolar point can be observed as a noisy measurement.

Each pair of generated lines j intersect at a point cj|�. Theepipolar point is then estimated as a convex combination ofintersections using a weighted sum,

zcepi,� =�j

wjcj|�

where wj is the weight of the intersection-sum.

C. Feature point states

In order to impose the geometric constraint (10) the featurepoints on the ground plane, zc� , matched and observed in apast frame {c�}, are added as states. Hence, P� points areaugmented into the state vector

mc� = [(zc�1 )� · · · (zc�P�)�]�, (11)

that is added and removed for each incoming image dependingon the feature point matches. A total number of P points arematched from L images and represented by m.

IV. MEASUREMENT MODEL

The observations for the current frame {c} are the featurepoints on the ground plane, matched to those observed inprevious frame {c�}. Suppose a ground plane feature pointπi has been matched. Then its coordinates on the normalizedimage plane is modeled by the pinhole camera model [4],

zci =�I2 0

�(e�z π

ci )

−1πci + ni, (12)

where the pixel noise ni is assumed to be zero-mean withcovariance matrix Ci = σ2

pix × I2.The importance of parameterization of the feature points

position πci , equation (10), is appeared here, since, it relates

the observation to the previous frame zc�i and camera positionpnc�

, both of which are states.By stacking all P� feature point matches corresponding to

view �, one obtains the nonlinear measurement equation

ycpts,� = [(zc1)

� · · · (zcP�)�]�

= h(δxins, δxcam,mc�) + npts,�.(13)

The feature points not matched to � are subsequently matchedto a previous older frame � + 1, where � = 1, . . . , L, untilat most P observations are collected. The aggregate vectorof dimension 2P is denoted by yc

pts. Consequently, the errorcovariance matrix is Cpts = σ2

pix × I2P .

In addition, the epipolar point arising from view � can bewritten as

zcepi,� =�I2 0

�(e�z t

cc�)−1tcc� , (14)

where tcc� ≡ RcbR

bn(p

nc�

− pnc ). The error covariance matrix

of the observation is approximated by the dispersion of inter-sections,

Cepi,� =�j

wj(cj|� − zcepi,�)(cj|� − zcepi,�)�.

When the number of intersections is small the covariance isincreased to prevent the uncertainty ellipse from collapsing,C′

epi,� = Cepi,� + λI2. Here λ is a decreasing function of thenumber of intersections.

The observed epipolar points from L views are stacked,yielding the nonlinear measurement equation

ycepi = [(zcepi,1)

� · · · (zcepi,L)�]� + nepi

= h(δxins, δxcam) + nepi,(15)

where the noise covariance matrix Cepi is the block-diagonalconcatenation of each matrix Cepi,�.

V. ESTIMATION FRAMEWORK

The joint state-space model for the INS and camera errorstates, along with the measurements (13) and (15), is com-pactly written as

δxk+1 = Fkδxk +Gkwk

yck = h(δxk,mk) + nk,

(16)

where the covariance matrix of nk, Cn,k, is the concatenationof Cpts and Cepi. We model the statistics of the feature pointvector m (Section III-C) based on the following simplifyingassumptions: Its expected value is taken as the observedcoordinates on the image plane, �m, and its covariance matrixis Cm = σ2

pix × I2P . Further, m and δx are assumed to beuncorrelated.

Since the process model is linear, the predicted errors arezero with error covariance matrix propagated as

P−k+1 = FkP

−k F

�k +GkQG�

k ,

where

Fk ��Fins

k 00 I3L

�and Gk �

�Gins

k

03L×12

�.

When a new image is recorded an error state δpnc� is

augmented to δxcam, (5), and the error covariance matrix isupdated as

P′ =�P PT�

TP TPT�

�(17)

where T = [T 0]. When the buffer of L views is full, theoldest state and its corresponding rows and columns of thecovariance matrix are decimated.

The SPKF is set to perform a measurement update whenground plane features have been detected and matched. Based

on the state-space model, (16), the SPKF estimates the errorslinearly as

δxk = Kk(yck − yc

k), (18)

where yck is the measurement prediction and Kk is the

Kalman gain matrix. An overview of the algorithm is given inAlgorithm 1.

The joint statistics of δx and m are propagated as 2N + 1sigma points, where N is the total number of states N =15 + 3L + 2P . Let the joint state vector be denoted by x =[δx� m�]� and x− = [0� �m�]� be the prediction witherror covariance matrix P− ⊕ Cm. The sigma points X j =[δX�

j M�j ]

� are then generated using the matrix square-root of the joint error covariance matrix. By propagating thesigma points through the measurement equations (13) and (15),the correlations between the error states δx and observationsyc can be approximated [22]. These correlations form the basisof the Kalman gain.

Algorithm 1 Sigma-Point Kalman Filter

1: Initialize P−0

2: for k = 0, . . . do3: Update INS state estimates and {Rc

c�}

4: if new image ∃ then5: Extract and match feature points {zci}6: Detect feature points on ground plane7: Estimate epipoles and covariance matrices8: end if9: if yc

pts and/or ycepi exists then

10: %Generate sigma points and prediction:11: X j = x−

k ± η · column[(P−k ⊕Cm)1/2]j

12: Yj = h(δX j ,Mj)

13: yck =

2N

j=0wm

j Yj

14: %Measurement update:15: Ce,k =

2N

j=0wc

j(Yj − yck)(Yj − yc

k)� +Cn,k

16: Dk =2N

j=0wc

jδX j(Yj − yck)

�

17: Kk = DkC−1e,k

18: δxk = Kk(yck − yc

k)19: Pk = P−

k −KkCe,kK�k

20: Use δxk to correct state estimates21: P := Pk

22: else23: P := P−

k24: end if25: if new image ∃ then26: Record view position pn

c�27: Decimate and augment error states δpn

c�28: Update P correspondingly29: end if30: %Prediction:31: P−

k+1 = FkPF�k +GkQG�

k

32: end for

The weights in the SPKF are set as wcl = wm

l = 12(N+λ)

for l = 1, . . . , 2N . For l = 0, wc0 = λ

N+λ + (1− α2 + β) andwm

0 = λN+λ . Here λ = α2(N + κ) −N , with parameters set

to α = 0.1, β = 2 and κ = 0 that also determine the spreadof the sigma points through the weight η �

√N + λ.

Fig. 3: SURF feature extraction and matching. (a) The detected feature pointsin this image plane are marked by red circle, they are connected by yellowline to the corresponding matched features in the previous image (marked bygreen cross). (b) Virtual image plane of the red square-shaped region.

VI. FEATURE DETECTION AND OUTLIER REJECTION

The image processing module of the system consists offeature extraction and matching, i.e., identifying distinguish-able features between successive image frames. We used thespeeded up robust features (SURF) [27], which is invariantto image scale and rotation. The MATLAB Computer VisionToolbox implementation of SURF is used for feature detectionand matching. The algorithm is capable of identifying corre-sponding features in successive images, despite poor lightingconditions in the test environment.

Fig. 3a shows an example of SURF feature detection andmatching in the our test environment. Depending on thehardware structure, different vision-based ground plane featuredetection approaches are studied in the literature [15], [23],[23], [24]. In this paper, for simplicity, a method similarto [15] is used for the ground plane feature detection. That is,among all the detected and matched features, the ground planefeatures are sampled from a square-shaped region of interestin the lower part of the image. For the sake of illustration, thesquare-shaped region together with the selected and matchedfeatures are transformed in the virtual image plane in Fig. 3b,which can be interpreted as the top view image of the groundplane, Section III-A.

In order to detect and reject mismatches or very noisyobservations, we employ a residual-based outlier rejectionbefore using the observed points on the image plane in themeasurement update (18). For each observed point zcj thefollowing test was performed,

(zcj − zcj)�C−1

e,j(zcj − zcj) < γ, (19)

where zcj is the prediction and Ce,j is the correspondingsubmatrix of Ce. Points that fail the test are rejected andremoved from the observation vector.

(a) System-hardware (b) Test environment

Fig. 4: (a) An AVT Guppy monochrome camera, with resolution 752 × 480that is rigidly mounted on top of a MicroStrain 3DMGX2 IMU. (b) Somesample images from the test environment.

VII. RESULTS

The described algorithm in the preceding sections has beenevaluated both in simulations and real experiments. Both thesimulation and real experiments have verified the 6 DoFmotion estimations, however, due to the space limitations, onlythe results of one real experiment are given in this section.

A. Setup

The hardware used for this experiment is shown in Fig. 4a.The camera is an AVT Guppy monochrome camera that isrigidly mounted on top of a MicroStrain 3DMGX2 IMU withsampling rate 250 Hz. Images are captured with resolution752×480 pixels, where the camera sampling rate is 10 Hz. Thecamera internal calibration parameters and the IMU-cameracalibration parameters are both estimated using [28].

To evaluate the performance of the proposed method withreal data, a controlled test environment was set up in anunderground hall at the university of KTH. Some of the sampleimages from the test envinronment are given in Fig. 4b. Theunderground location of the test environment provides a con-trolled level of radio interference for a reference commercialUWB system [29] used as a ground truth for evaluating theresults. The accuracy of the UWB system is about 15 cmunder line-of-sight and favorable geometrical configurationsof the tags and sensors. When line-of-sight is lost, however,the UWB position estimates become unreliable. During theexperiments all data were stored on a computer and processingwas done off-line.

B. Experimental validation

In this experiment 3081 image frames were recorded forabout 5 minutes. The IMU and camera were placed at thebottom of a trolley, moving in a closed loop trajectory. Thesystem was initialized at an estimated height of 21 cm abovethe ground. The roll and pitch were estimated using thegravitational force during the first samples when the systemwas stationary [25].

In the implementation, the number of views in memory wasset to L = 5. The measurement noise level σpix was set to

be equivalent to 10 pixels. The outlier rejection level γ = 6and any measured epipole outside of the visible region of theimage plane was rejected. The maximum number of groundplane feature point states was set to P = 8.

The estimated trajectory along with the UWB data, used asthe ground truth, is plotted in Fig. 5a over the map of the testenvironment. Although, the UWB sensors were not in line-of-sight during a substantial part of the trajectory, the resultsdo show that the SPKF is capable of drastically reducing thegrowth rate of the accumulated INS errors. The trajectoryfollows the UWB position estimates and the system stopsapproximately 50 cm from the reference.

Furthermore, the estimated height of the mobile system isshown in Fig. 5b, which is crucial in the measurement modelof the ground plane feature points. The result indicates thatwith sufficient matches and translation the height errors canbe tracked by the SPKF during this period of time.

The 3σ level for the estimated error in the IMU position,orientation, and velocity along the three axis are shown inFig. 6. The σ values are the square root of the computedcorresponding diagonal elements of the states error covariancematrix that provides a representation of the SPKF’s estimateuncertainty. According to Fig. 6a, the filter consistency is alsoverified by the height error, approximately 5 cm error, at theend of the experiment. Given that no height update was donein the estimation process, the vision-based error correction ofthe INS is significant. Expectedly, the positional error alongthe x and y axis grows slowly as the system has no access tothe absolute position update.

The provided results demonstrate the potential applicationof the proposed method in both the pedestrians and roboticsapplications for 3D pose estimations. Since, in many roboticsapplications the height is fixed, the height information canbe added as a pseudomeasurement to the SPKF, which willdramatically improve the 2D pose estimation for long termapplications. Alternatively, a barometer could provide suchinformation for pedestrians to maintain the stability of longertrajectories.

Since the image of three non-collinear points determines aunique affine transformation in the plane, at least three non-collinear points are necessary to be selected in images to definethe ground plane. Although increasing the number of featurepoints improves the stability of the estimated trajectory, thenumber of points must be kept reasonably low to reduce thecomplexity of the filter.

Video clips and supplemental materials of the experimentare available at http://www.ee.kth.se/∼ghpa/plans2012.

VIII. CONCLUSION

An IMU-camera sensor fusion approach has been used toconstruct a vision-aided inertial navigation system by trackingsalient features of planar terrain. In the proposed algorithm, anew measurement model has been derived based on the virtualimage of a downward-looking camera where the real cameramovement is not restricted. The proposed algorithm is notonly capable of velocity estimation comparable to optical flow

x[m]

y[m

]

−2 0 2 4 6 8 10 12

8

10

12

14

16

18

20UWB trajectoryEstimated trajectoryUWB stopping point Starting pointEstimated stopping point

(a) Estimated position along the x and y axes

0 50 100 150 200 250 3000

0.1

0.2

0.3

0.4

0.5

t[s]

z[m

]

(b) Estimated height along the z axis

Fig. 5: The estimated trajectory of the IMU along with the UWB referencedata, plotted over the map of the test environment.

methods but also accurate 3D pose estimation. The problem ofnonlinearity of the state-space model of the system has beenhandled by the state-augmented Sigma-Point Kalman filter.The presented results from the real experiments indicate thatthe method is capable of correcting the INS solution even witha few number of features while the position error growth rateis significantly reduced. As a future work, instead of using afixed shaped region in the image plane for ground plane featuredetection, we are investigating a more flexible and reliablemethod for ground plane feature detection, outlier rejection,and obstacle removal.

APPENDIX A

Finsk =

�����

I3 dtI3 03 03 03

03 I3 dt[Rnb,k f

bk]× dtRn

b,k 03

03 03 I3 03 −dtRnb,k

03 03 03 I3 03

03 03 03 03 I3

�����

and

Ginsk =

�����

03 03 03 03

dtRnb,k 03 03 03

03 −dtRnb,k 03 03

03 03 dtI3 03

03 03 03 dtI3

����� .

0 50 100 150 200 250 3000

2

4

x[m

]

3σ level

0 50 100 150 200 250 3000

5

y[m

]

0 50 100 150 200 250 3000

0.2

0.4

z[m

]

t[s]

(a) Position error

0 50 100 150 200 250 3000

0.1

0.2

v x[m/s

]

3σ level

0 50 100 150 200 250 3000

0.1

0.2

v y[m/s

]

0 50 100 150 200 250 3000

0.02

0.04

v z[m/s

]

t[s]

(b) Velocity error

0 50 100 150 200 250 3000

0.5

1

1.5

Rol

l[ °

]

3σ level

0 50 100 150 200 250 3000

0.5

1

1.5

Pitc

h[ °

]

0 50 100 150 200 250 3000

10

20

Hea

ding

[ ° ]

t[s]

(c) Attitude error

Fig. 6: The 3σ levels for the error in position (a), velocity (b), and attitude (c). The σ values are the square root of the corresponding diagonal elements ofthe states error covariance matrix.

ACKNOWLEDGMENT

The authors would like to thank Dr. Alessio De Angelis forhis assistance with data collections and installation of the thereference system.

REFERENCES

[1] P. Corke, J. Lobo, and J. Dias, “An introduction to inertial and visualsensing,” The International Journal of Robotics, vol. 26, pp. 519–535,2007.

[2] F. Mirzaei and S. Roumeliotis, “A kalman filter-based algorithm for imu-camera calibration: Observability analysis and performance evaluation,”IEEE Transactions on Robotics, vol. 24, pp. 1143 –1156, oct. 2008.

[3] G. Panahandeh and M. Jansson, “IMU-camera self-calibration usingplanar mirror reflection,” in IEEE Int. Conf. on Indoor Positioning andIndoor Navigation (IPIN), May. 2011.

[4] R. I. Hartley and A. Zisserman, Multiple View Geometry in ComputerVision. Cambridge University Press, ISBN: 0521623049, 2000.

[5] D. D. Diel, P. DeBitetto, and S. Teller, “Epipolar constraints for vision-aided inertial navigation,” in Seventh IEEE Workshops on Applicationof Computer Vision, vol. 2, pp. 221 –228, Jan. 2005.

[6] A. Mourikis and S. Roumeliotis, “A multi-state constraint kalman filterfor vision-aided inertial navigation,” in IEEE Int. Conf. on Robotics andAutomation, pp. 3565 –3572, Apr. 2007.

[7] A. Milella and R. Siegwart, “Stereo-based ego-motion estimation usingpixel tracking and iterative closest point,” in IEEE Int. Conf. onComputer Vision Systems, p. 21, Jan. 2006.

[8] L. Armesto, J. Tornero, and M. Vincze, “Fast ego-motion estimationwith multi-rate fusion of inertial and vision,” Int. J. Rob. Res., vol. 26,pp. 577–589, Jun. 2007.

[9] B. Southall, T. Hague, J. A. Marchant, and B. F. Buxton, “Vision-aidedoutdoor navigation of an autonomous horticultural vehicle,” in Proc. ofthe Int. Conf. on Computer Vision Systems (ICVS), pp. 37–50, Springer-Verlag, 1999.

[10] J. Kim and S. Sukkarieh, “Improving the real-time efficiency of inertialslam and understanding its observability,” in IEEE/RSJ Int. Conf. onIntelligent Robots and Systems (IROS), vol. 1, pp. 21–26 vol.1, Sep.2004.

[11] P. Pinies, T. Lupton, S. Sukkarieh, and J. Tardos, “Inertial aiding ofinverse depth slam using a monocular camera,” in IEEE Int. Conf. onRobotics and Automation, pp. 2797–2802, Apr. 2007.

[12] M. Agrawal and K. Konolige, “Rough terrain visual odometry,” IEEEInt. Conf. on Robotics and Automation, 2007.

[13] G. Panahandeh and M. Jansson, “Vision-aided inertial navigation usingplanar terrain features,” in IEEE Int. Conf. on Robot, Vision and SignalProcessing, Sep. 2011.

[14] M. Bosse, W. Karl, D. Castanon, and P. DeBitetto, “A vision augmentednavigation system,” in IEEE Conf. on Intelligent Transportation System,pp. 1028 –1033, Nov. 1997.

[15] P. Lombardi, M. Zanin, and S. Messelodi, “Unified stereovision forground, road, and obstacle detection,” in IEEE Int. Conf. on IntelligentVehicles Symposium, pp. 783–788, Jun. 2005.

[16] X. Song, K. Althoefer, and L. Seneviratne, “A robust downward-lookingcamera based velocity estimation with height compensation for mobilerobots,” in Int. Conf. on Control Automation Robotics Vision (ICARCV),,pp. 378–383, Dec. 2010.

[17] D. Wagner and D. Schmalstieg, “Artoolkitplus for pose tracking onmobile devices,” 2007.

[18] X. Song, L. Seneviratne, and K. Althoefer, “A kalman filter-integratedoptical flow method for velocity sensing of mobile robots,” IEEE/ASMETransactions on Mechatronics, vol. 16, pp. 551–563, Jun. 2011.

[19] C. Hide, T. Botterill, and M. Andreotti, “Low cost vision-aided imu forpedestrian navigation,” in Proc. of UPINLBS, pp. 1 –7, Oct. 2010.

[20] L. G. B. Mirisola, J. Dias, and A. T. de Almeida, “Trajectory recoveryand 3d mapping from rotation-compensated imagery for an airship,” inEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS).

[21] D. Zachariah and M. Jansson, “Camera-aided inertial navigation usingepipolar points,” in Proc. IEEE/ION Position Location and NavigationSymposium (PLANS), pp. 303–309, May 2010.

[22] S. J. Julier and J. K. Uhlmann, “A new extension of the kalman filter tononlinear systems,” in Proceedings of Signal Processing, Sensor fusion,and Target Recognition, vol. 4, pp. 182–193, Apr. 1997.

[23] N. Pears and B. Liang, “Ground plane segmentation for mobile robotvisual navigation,” in IEEE/RSJ Int. Conf. on Intelligent Robots andSystems (IROS), vol. 3, pp. 1513–1518 vol.3, 2001.

[24] D. Conrad and G. DeSouza, “Homography-based ground plane detectionfor mobile robot navigation using a modified em algorithm,” in IEEEInt. Conf. on Robotics and Automation, pp. 910–915, May. 2010.

[25] J. A. Farrell and M. Barth, Global Positioning System, Inertial Naviga-tion and Integration. McGraw-Hill Companies, 1999.

[26] H. Qi and J. Moore, “Direct kalman filtering approach for GPS/INSintegration,” IEEE Transactions on Aerospace and Electronic Systems,vol. 38, pp. 687–693, Apr. 2002.

[27] H. Bay, T. Tuytelaars, and L. V. Gool, “Surf: Speeded up robustfeatures,” in ECCV, pp. 404–417, 2006.

[28] http://www.vision.caltech.edu/bouguetj/calib doc/index.html, last ac-cessed Mar.2012.

[29] Ubisense Ltd. The Ubisense Precise Real-time Location System -Series 7000 Sensor. Available: http://www.ubisense.net/, last accessedMar.2012.