Disparity as a Separate Measurement in Monocular SLAM

Talha Manzoor1 and Abubakr Muhammad2

Abstract— In this paper, we investigate the effects of includingdisparity as an explicit measurement (not just for initializa-tion) in addition to the projective camera measurements, inconventional monocular/bearing-only SLAM in a 2-D world.We conduct an observability analysis for a 1.5-D scenariowhich theoretically shows that adding disparity measurementsinfluences the observability rank condition, and so disparity canbe regarded as a valid measurement. As disparity is dependenton the camera’s location one time step earlier, we rederivethe EKF equations for measurement models dependent onthe previous state. It is shown that this modification resultsin an increase in the measurement noise covariance. We alsodiscuss the implications of the white noise assumption in EKFbeing violated with the inclusion of disparity measurements.In the end, we compare the results of a simulated monocularSLAM scenario in a 2-D world with and without disparitymeasurements. Here we use the magnitude of the eigenvaluesof the simulated state covariance matrix as a metric forobservability [1]. Our results suggest that including disparitymeasurements in monocular SLAM improves observability bothin the map and pose variables. Hence, this paper is an invitationto consider disparity measurements explicitly, in monocularSLAM.

I. INTRODUCTION

Simultaneous Localization and Mapping with a singlecamera, also known as Monocular SLAM, is a well knownproblem in the robotics community and extensive researchand development have been carried out in this area. Manymonocular SLAM systems have been developed to date withthe first being presented by Davison [2] who used the projec-tion of observed features or landmarks on the image plane ofthe camera as measurements for the EKF. Since then, a lotof research has been done in single camera SLAM [3], [4],[5], and in many sub-areas. Another approach introduced byPaz et al [6] includes monocular information in a stereo handheld system for SLAM, using both projective and disparitymeasurements. In this paper, we introduce a similar idea anddiscuss the possibility of including stereo information in amonocular SLAM system. Thus, we consider both projectiveand disparity measurements. However, the stereo in this caseis done in time. This type of stereo vision is commonlyknown as motion-stereo [7] or wide-baseline stereo [8].

One major concern while deploying a single camera as asensor for any mapping problem is the inherent handicapin trying to reconstruct the environment from projectedimages. This problem of observability of monocular (or

1Talha Manzoor is a PhD student in Electrical Engineering at LahoreUniversity of Management Sciences, Lahore, Pakistan 13060023 atlums.edu.pk

2Abubakr Muhammad is with the faculty of Electrical Engineering,Lahore University of Management Sciences, Lahore, Pakistan abubakrat lums.edu.pk

bearing-only) SLAM has been investigated extensively inthe past. Bonnifait and Garcia [9] carry out an observabilityanalysis of a planar robot trying to localise itself, giventhe bearing to 3 known beacons. They conclude that thesystem is observable except for the degenerate case whenthe robot is located on the circle defined by the 3 beaconsand is stationary. In a similar paper, Calleja et al [10] haveconsidered the observability of a planar robot, performingSLAM with a bearing-only sensor. Instead of expressing theSLAM system in its regular form, they do so in the errorform presented in [11]. This allows them to approximate thesystem as piecewise constant, enabling them to use linearobservability techniques as given in [12]. They prove thatfor absolute coordinates SLAM, the process is unobservableover 2 time segments when the robot is either stationary, ormoving directly towards a landmark. Introducing a knownlandmark (beacon) renders the linearly approximated processfully observable. Tribou et al [13] present an observabilityanalysis for a planar position based visual servoing system,also applicable to monocular SLAM. They focus on deter-mining the unobservable configurations for their application.They too approximate the system with a piecewise constantone and consider a constant velocity motion model whichconstitutes a larger unobservable subspace than the vehiclespecific kinematic model in [10]. The observability analysiswe present over here however, is not for the purpose ofsolving the observability problem for 2-D monocular SLAMin general. Indeed it has already been solved by Belo et alin [14], in which they show that 3 known landmarks areenough to guarantee the observability of monocular SLAM.Our analysis over here however, is for theoretically demon-strating that the observability of monocular SLAM is indeedinfluenced by the inclusion of disparity measurements. Wedo so by evaluating the observability rank criterion for non-linear systems as presented in [15]. Due to the complexityof the analytical calculation for 2-D, we assume a 1.5-Dscenario in which the robot is constrained to move along astraight line in a 2-D world and conjecture that if disparitymeasurements are useful in 1.5D, they must also be so in2-D (which we show through simulations).

Our monocular SLAM algorithm is based on the ExtendedKalman Filter. As mentioned before, we use both projectiveand disparity measurements. As our disparity measurementsare based on motion-stereo, this measurement is dependenton the current and previous states. Similar systems usingmotion stereo have been developed in the past like [16],[17]. However, these approaches apply corrections separatelyto the image sequence, and then use the conventional filterfor depth estimation. In our work, we have incorporated

978-1-4799-2744-9/13/$31.00 ©2013 IEEE

Proceeding of the IEEEInternational Conference on Robotics and Biomimetics (ROBIO)

Shenzhen, China, December 2013

1647

disparity measurements per se in the EKF by rederiving theEKF equations through the Bayesian approach given in [18].We use the law of total probability twice, to establish theconditional dependence of the belief both on the currentand previous states. After the proof is complete, we seethat the affect of this dependence is simply an increase inmeasurement noise. As intuition suggests, this increase isdependent both on the process model and on the processnoise covariance.

Ham et al [1] have shown that the eigenvalues andeigenvectors of the state covariance matrix of the KalmanFilter can provide useful insight into the observability of thesystem. They show that the magnitude of the eigenvaluescan provide a sense of the degree of observability wherethe eigenvector with the smallest eigenvalue gives the mostobservable linear combination of the states and the eigen-vector with the largest eigenvalue giving otherwise. Hencewe use the trace of the covariance matrix, to analyze theperformance of a simulated monocular SLAM scenario bothwith and without disparity measurements.

The organization of the paper is as follows. Section II givesan observability analysis, comparing two 1.5D monocularSLAM systems with and without disparity measurements.The equations for the generalized EKF for measurementmodels dependent on the previous state are derived in SectionIII. The measurement equations and discussion about thecoloredness of the measurement noise are given in SectionIV. Results are presented in Section V with the conclusionin Section VI.

II. OBSERVABILITY ANALYSIS FOR A 1.5-DMONOCULAR SLAM SCENARIO

We show here that including disparity as a separate mea-surement does indeed effect the performance of monocularSLAM. One of the necessary conditions for the estimationerror of the discrete time Kalman Filter to remain boundedis that the system must satisfy the non-linear observabilityrank condition [19]. We first state this condition for generalnon-linear systems.

A. The Non-linear Observability Rank Condition

Consider the following discrete-time non-linear system

xk+1 = f(xk, uk); yk+1 = h(xk+1), (1)

where x ∈ Rn, u ∈ Rp and y ∈ Rm. This system isobservable if for any initial state x0 and some final time kthe initial state x0 can be uniquely determined by knowledgeof the input ui and output yi for all i ∈ [0, k].Checking the observability of a system amounts to checkingthe rank of a certain observability matrix [15], [20]. If thesystem given by Equation 1 is in control affine form suchthat f(xk, uk) = g0(xk) +

∑gi(x)uik, then it is observable

if and only if the rank of the following observability matrixis full

O =[ (

dL0f (h)

)T (dL1f (h)

)T · · ·(dLn−1f (h)

)T ]T(2)

where Lif(h) is the ith order Lie Derivative [21] of the scalarfield of the measurement h with respect to the vector field fand d denotes the gradient operator with respect to x.

B. Observability and Beacons for Monocular SLAM

The first ever monocular SLAM system presented in [2]was initialized by a certain number of known landmarksor beacons. This empirically suggests that the observabilityof monocular SLAM is related to the number of beaconspresent in the environment. Since then, work has been doneon investigating the requirement on the minimum numberof beacons for monocular/ bearing-only SLAM [10], [13].However this is based on the approximated picewise constantsystems observability theory as presented in [12]. For 2-D monocular SLAM, the rank of the observability matrixgiven by Equation 2 becomes too complex to computeeven for a computer algebraic package. Thus, to show thatdisparity measurements do indeed affect the observability ofmonocular SLAM, we carry out an observability analysis fora simple case of the general problem.

C. Observability for 1.5-D Monocular SLAM

Consider the problem posed in Figure 1. A robot con-strained to move on a single line, attempts to perform SLAMin a 2-D world containing a single beacon (L1), and asingle landmark (L2). It carries a single camera and receivesas measurements, u1k and u2k which are the respectiveprojections of L1 and L2 on the image plane at time k.The robot can only move along the x-axis and its location attime k is given by ck. The location of a landmark is given by(xL,

1dL

) where 1dL

is the inverse depth of the landmark. We

1dL2

u10

c0 c1yx

xL1L1

fck

xL2 L2

u20 u21 u2ku11 u1k

1dL1

Fig. 1. The robot performing SLAM in a 2-D world with 1 beacon. Here, fis the focal length of the camera. Quantities highlighted in blue are known,whereas those in black are to be estimated.

now calculate the observability matrix for a constant velocitymotion model. The state vector is given by

Xk =[ck ck xL2k dL2k

]T. (3)

The constant velocity model is given by the following vector-valued function

f =[ck +∆tck ck xL2k dL2k

]T. (4)

1648

The measurement vector is given by

Yk =

[u1k − xL1fdL1

u2k

]=

[−ckfdL1

xL2kfdL2k − ckfdL2k

].

Note that the first measurement is not exactly equal to theprojective measurement. A bias term, consisting of all knownquantities has been subtracted. Applying the definition ofEquation 2, the observability matrix of this system turns outto be full rank, hence the process is observable. Also, it canbe checked that the rank of O will not be full if we excludethe beacon. Thus at least 1 beacon is required for monocularSLAM in this setting to be observable. The exact form ofthe observability matrix for both cases can be found in [22].

D. 1.5-D Monocular SLAM with disparity measurements

We now see the effect of including disparity as a mea-surement on observability of the system in Figure 1. Herewe neglect the measurements of L1 and assume that there isno beacon. The process and state are the same as given inEquations 3 and 4. We now have a disparity measurementgiven by

y3k = u2k − u2k−1 = −fckdL2k + fck−1dL2k . (5)

Note that this measurement is dependent on the previousstate. Section III discusses the effect this dependence wouldhave on the EKF. The observability matrix in this caseturns out to have full rank [22]. Thus we see that includingdisparity as a measurement has eliminated the requirementof the beacon in order to keep SLAM observable. This showsthat disparity is indeed a valid measurement and that it alsoimproves the observability of monocular SLAM in 1.5-D.

III. EKF FOR MEASUREMENT MODELS DEPENDENT ONTHE PREVIOUS STATE

The measurement model shown in Equation 5 is de-pendent on the previous and currrent states. However, theconventional Kalman filter assumes that the measurement isdependent on the current state only. We derive the Kalmanfilter for this case from the basic Bayes Filter following themethod given in [18]. Before presenting our equations, wefirst describe the motion and measurement models. Afterpresenting the equations, we discuss the mathematical proof.

A. Functions and variables

1) Motion Model: The motion or process model is givenby Xk = f(Xk−1, µk, ωk) where ωk is the process noise atstep k. Its covariance is given by Qk. After linearization weget the following form

Xk = f(Xk−1, µk, 0) + FXk−1(Xk−1 − Xk−1) + Fωk

ωk,

where FXk−1is the Jacobian of f w.r.t Xk−1, and Fωk

isthe Jacobian of f w.r.t ωk

2) Inverse Motion Model: The inverse motion model isgiven by Xk−1 = g(Xk, µk, ωk). Note that for same timesteps, ωk has the same value as its instance appearing in themotion model. After linearization we get

Xk−1 = g(Xk, µk, 0) +GXk(Xk − Xk) +Gωk

ωk,

where GXkis the Jacobian of g w.r.t Xk, and Gωk

is theJacobian of g w.r.t ωk.

3) Measurement Model: The measurement model is givenby Yk = h(Xk, Xk−1)+vk where vk is the sensor noise. Itscovariance is given by Rk. After linearization we get

Yk =h(Xk, Xk−1) +HXk(Xk − Xk)

+HXk−1(Xk−1 − Xk−1) + vk,

where HXkis the Jacobian of h w.r.t Xk, and HXk−1

is theJacobian of h w.r.t Xk−1.

B. Generalized Kalman Filter Equations

Our final equations for the motion and measurementmodels just presented are given as follows:

Xk|k−1 = f(Xk−1|k−1, µk, 0), (6)

Pk|k−1 = FXk−1Pk−1|k−1FXk−1

T + FωkQkFωk

T , (7)

Wk = Rk +HXk−1Gωk

QkGωk

THXk−1

T , (8)

Kk = Pk|k−1HXk

T (HXkPk|k−1HXk

T +Wk)−1, (9)

Xk|k = Xk|k−1 +Kk(Yk − h(Xk|k−1, Xk−1|k−1)), (10)

Pk|k = (I −KkHXk)Pk|k−1, (11)

where Xk is the estimate of the state X , Pk is the covari-ance of Xk, µk is the control vector, Yk is the expectedmeasurement, Kk is the Kalman gain, Wk is the covarianceof Yk, f(Xk−1, µk−1) is the motion model, g(Xk, µk) is theinverse motion model and h(Xk, Xk−1) is the measurementmodel. All quantities are evaluated at time k.

The difference between the conventional EKF and the onegiven by Equations 6-11 lies in the calculation of Equation 8.For simple measurement models, Wk will be exactly equal tothe sensor noise Rk with the rest of the equations remainingunchanged. Note that for linear systems, the inverse motionmodel will just be equal to F−1. We assume that such aninversion exists.

C. Mathematical Derivation

The equations are derived first by extending the basicBayes Filter Algorithm [18], to include the previous state inthe measurement step. Then the probability distributions aresolved for the required models. To include the dependencyof Yk on Xk−1 we include an intermediate step in the BayesFilter through the theorem of total probability. The steps areas follows:

Step 1: bel(Xk) = p(Xk|Y0:k−1, µk) =∫p(Xk|Xk−1, Y0:k−1, µk)p(Xk−1|Y0:k−1, µk)dXk−1,

(12)

1649

Step 2: p(Yk|Xk, Y0:k−1, µk) =∫p(Yk|Xk, Y0:k−1, µk, Xk−1)

p(Xk−1|Xk, Y0:k−1, µk)dXk−1,

(13)

Step 3: bel(Xk) = ηp(Yk|Xk, Y0:k−1, µk)p(Xk|Y0:k−1, µk),(14)

where η is a normalizing constant. Note that the conventionalBayes filter [18] consists only of steps 1 and 3. We presentthe derivation, with this extended form of the Bayes filter.

1) Part 1: Prediction: We begin with step 1 of the Bayesfilter. The terms of Equation (12) are normally distributedwith mean and covariance as

p(Xk|Xk−1, Y0:k−1, µk)︸ ︷︷ ︸N∼(Xk;f(Xk−1,µk,0)+FXk−1

(Xk−1−Xk−1);FωkQkFωk

T )

and,

p(Xk−1|Y0:k−1, µk)︸ ︷︷ ︸N∼(Xk−1;Xk−1;Pk−1)

.

After evaluation of this equation, the outcome is also aGaussian with mean Xk|k−1 and covariance Pk|k−1. Theinterested reader is directed to [18] for a full proof. Themean and variance of the resulting distribution are given asXk|k−1 = f(Xk−1, µk, 0) and Pk|k−1 = (Fωk

QkFωkT ) +

FXk−1Pk−1FXk−1

T respectively.2) Part 2: Measurement Covariance: Here we derive step

2 of the extended Bayes filter given by (13). The distributionsof the terms involved are given as follows

p(Yk|Xk, Y0:k−1, µk, Xk−1)︸ ︷︷ ︸(∼N(YK ;h(Xk,Xk−1)+Hxk

(Xk−Xk)+HXk−1(Xk−1−Xk−1);Rk))

and,

p(Xk−1|Xk, Y0:k−1, µk)︸ ︷︷ ︸(∼N(Xk−1;g(xk,µk,0)+GXk

(Xk−Xk);GωkQkGωk

T ))

.

The derivation is essentially the same as that of step 1 givenin [18]. In this case the integral in Equation 13 can beexpressed as

∫exp{−Mk}dXk−1, where

Mk =

1

2

(Yk−h(Xk,Xk−1)−Hxk

(Xk−Xk)−HXk−1(Xk−1−Xk−1)

)T

Rk−1

(Yk−h(Xk,Xk−1)−Hxk

(Xk−Xk)−HXk−1(Xk−1−Xk−1)

)

+1

2

(Xk−1−g(xk,µk,0)−GXk

(Xk−Xk)

)T(Gωk

QkGωkT

)−1

(Xk−1−g(xk,µk,0)−GXk

(Xk−Xk)

).

(15)

We can decompose Mk into two functions. These functionsare given by

Mk =M(Yk, Xk, Xk−1) +M(Yk, Xk). (16)

By differenciating twice with respect to Xk−1, we determinethe minimum and covariance of Mk so as to form a quadratic

function similar to the exponential of the normal distribution.Thus we get

M(Yk, Xk, Xk−1) =

1

2

(Xk−1−Φk

(HXk−1

TRk−1(Yk−h(Xk,Xk−1)−Hxk

(Xk−Xk)

−HXk−1(Xk−1−Xk−1))

+(GωkQkGωk

T )−1(g(xk,µk,0)−GXk(Xk−Xk))

))T

Φ−1k(

Xk−1−Φk

(HXk−1

TRk−1(Yk−h(Xk,Xk−1)−Hxk

(Xk−Xk)

−HXk−1(Xk−1−Xk−1))

+(GωkQkGωk

T )−1(g(xk,µk,0)−GXk(Xk−Xk))

)),

(17)

where Φk =(HXk−1

TRk−1HXk−1

+(GωkQkGωk

T )−1)−1

.Hence from Equations (15), (16) and (17), we getM(Yk, Xk). As this does not have any dependence onXk−1, we can move it out of the integral. What remainsinside the integral is exp{−M(Yk, Xk, Xk−1} which isa valid probability density function hence integrates to aconstant. Thus exp{−M(Yk, Xk} gives the required dis-tribution. The mean and covariance of this distribution isgiven by the minimumm and inverse of the curvature ofM(Yk, Xk) respectively. Differentiating with respect to Yk,we get the mean as h(Xk, Xk−1) + HXk

(Xk − Xk), andthe covariance, the inverse of the curvature as Wk = Rk +HXk−1

GωkQkGωk

THXk−1T . This proves the correctness of

(8).3) Part3: Correction / Update: The distributions of the

terms involved in the 3rd step of the Bayes Filter are

p(Yk|Xk, Y0:k−1, µk)︸ ︷︷ ︸(∼N(Yk;h(Xk,Xk−1)+HXk−1

(Xk−Xk)))

and,

p(Xk|Y0:k−1, µk)︸ ︷︷ ︸(∼N(Xk;f(Xk−1,µk,0);FXk−1

Pk−1FXk−1T+Fωk

QkFωkT )).

The resulting distribution follows from [18] by replacing Rkwith Wk so that we get the remaining equations.

Xk|k = Xk|k−1 +Kk

(Yk − h(Xk, Xk−1)

),

Kk = Pk|k−1HXk

T (HXkPk|k−1HXk

T +Wk)−1,

Pk|k = (I −KkHXk)Pk|k−1.

IV. PLANAR MONOCULAR SLAM

In this section, we setup the EKF presented in Section IIIfor planar monocular SLAM and show through simulations,the effect of disparity measurements.

A. System description

Consider Figure 2, in which a camera performs SLAM ina 2-D world. The state vector is given by

Xk =[cxk

cx′kcyk

cy′kcθk

cθ′kL2xk

L2yk]T.

Here we assume a constant velocity motion model forstate and covariance propagation. The measurement equation

1650

L1h0

(cx0,c y0)

(L1x,L1 y

)

(L2x,L2 y

)

L2h0

cθk−1cθk

L1hk

(cxk,c yk)

L2hk

L1hk−1

(cxk−1,c yk−1)

L2hk−1f

y

x

Fig. 2. Monocular state sequence in 2-D

requires L1x, L1y, L2x and L2y to be described in the cameracoordinate frame. Transforming the world coordinates givesus CL1 =

[L1x L1y

]Tas

CL1 =

[(L1x− cx) cos(cθ) + (L1y − cy) sin(cθ)−(L1x− cx) sin(cθ) + (L1y − cy) cos(cθ)

],

and so, the measurement model for the camera is given by

L1h = f(L1x− cx) cos(cθ) + (L1y − cy) sin(cθ)

−(L1x− cx) sin(cθ) + (L1y − cy) cos(cθ),

and disparity is given by L2dispk = f(L2Bk/

L2dk), where

L2Bk is the magnitude of the baseline and L2dk is theperpendicular distance of the landmark from the baseline.They are given as

L2dk =(cxk −c xk−1)(cyk−1 −L2 y)√(cxk −c xk−1)2 + (cyk −c yk−1)2

− (cxk−1 −L2 x)(cyk −c yk−1)√(cxk −c xk−1)2 + (cyk −c yk−1)2

, and

L2Bk =√

(cxk −c xk−1)2 + (cyk −c yk−1)2.B. Colored Measurement Noise

The disparity measurement is acheived through the fol-lowing steps: 1) Obtain the measurements Lihk−1,

Li hk forconsecutive time steps. 2) Rectify the measurements to getLih

′

k−1,Li h

′

k. 3) Get L2dispk as the difference betweenLih

′

k−1 and Lih′

k. These steps result in the following equa-tion for disparityLidispk =

f(Lihk cos(αk −c θk)− f2 sin(αk −c θk)

)

Lihk sin(αk −c θk) + f cos(αk −c θk)

−f(Lihk−1 cos(αk −c θk−1)− f2 sin(αk −c θk−1)

)

Lihk−1 sin(αk −c θk−1) + f cos(αk −c θk−1),

(18)

where αk = arctan ((cyk −c yk−1) / (cxk −c xk−1)). Equa-tion 18 shows that the noise in the disparity measurement attime step k is correlated with the projection measurementat time step k − 1. We can get the noise equation fromEquation 18 and approximate it linearly to get, ζdispk equal toHζhk

ζhk+Hζhk−1

ζhk−1, where ζ is the variable for noise and

H is the respective derivative. Once we have these matrices,we use the state augmentation technique presented in [23] todeal with colored measurement noise.

V. SIMULATION

A. Setup

The simulations for validation of our work has beenimplemented in MATLAB. In the scenario presented here,the robot follows a rhombus shaped trajectory with smoothedcorners as shown in Figure 3. Velocity has been kept higherat the sides and lower at the corners. The focal length ofthe camera has been set at 20 pixel units. The process andmeasurement models are as given in Section IV. The processmodel is a constant velocity one with process noise mod-eled as zero-mean Gaussian with variances of 0.5 (m/s2)2,0.1 deg2 and 0.001 m2 for the linear velocities, angularvelocities and landmark positions respectively. Measure-ment noise variables have also been drawn from zero-meanGaussian distributions with variances of 0.074 pixels2 forprojective and 74 pixels2 for disparity measurements. Lengthof one time step has been kept at 0.1 seconds after whichthe robot receives projective and disparity measurements ofthe landmarks and beacons. As seen in Figure 3 an incorrectinitial estimate is provided with large uncertainty.

Fig. 3. A robot performing SLAM in 2-D. The cameras depict the truepose of the robot at selected time steps, equally distributed in time. Thebeacons are drawn in yellow whereas the estimated landmark is in white.The ellipses show the 95% confidence regions. An incorrect estimate withlarge uncertainty is provided.

B. Results

The results of the proposed framework can be seen inFigures 3 & 4. In Figure 3, the true robot location is almostalways captured by the respective uncertainty ellipse. Due tothe incorrect initialization, there is a little inconsistency forsome time after the filter is initialized. Overall, the filtershows consistent behavior. This can also be seen for theuncertainty ellipse of the estimated landmark.Figure 4 compares the performance between the conventionalEKF and the modified EKF of Section III. The first twographs show the trace of the position and map covariance

1651

Fig. 4. Performance comparison between the conventional EKF and thegeneralized EKF presented in Section III. From top-left in counter clockwiseorder: 1) Trace of covariances for robot location, 2) Trace of covariances forlandmark location, 3) SSD for robot location, 4) SSD for landmark location.

matrices. They represent the sum of the eigenvalues and thusthe degree of observability of the system [1]. We see thatthere is considerable improvement with respect to the robotposition variables. Although the steady state behavior forthe landmark remains the same, the filter run with inclusionof disparity measurements, has a smaller transient. Thuswe observe an improvement both for the robot and mapvariables. Also, we see from Figure 4 that the steady stateaccuracy with respect to the sum of squared errors is almostthe same for both cases, with the filter run with inclusion ofdisparity measurements having smaller transients.

VI. CONCLUSION

The idea of including disparity measurements in monoc-ular SLAM presented in this paper has been tested firsttheoretically through the observability rank condition andthen through simulations. During this process, we have alsogeneralized the EKF for measurement models dependent onthe previous state which may have applications other thanthat of disparity measurements in monocular SLAM. Wehave seen through simulations that the degree of observabil-ity is indeed improved both for robot pose and landmarkvariables.

It must be noted here that disparity is already includedimplicitly in Monocular SLAM, and that the process is inher-ently unobservable especially in the scale direction. Thus theinclusion of an explicit disparity measurement cannot convertthis unobservable process into an observable one. Howeveras described in this paper, it does improve the degree ofobservability for all states and hence provides more confidentestimates for the EKF. Although we have not conducteda mathematically rigorous proof for our proposition, wehope that this work will move theoreticians to look in thisdirection.

REFERENCES

[1] F. M. Ham and R. G. Brown, “Observability, eigenvalues, and kalmanfiltering,” Aerospace and Electronic Systems, IEEE Transactions on,no. 2, pp. 269–273, 1983.

[2] A. J. Davison, “Real-time simultaneous localisation and mapping witha single camera,” in Computer Vision, 2003. Proceedings. Ninth IEEEInternational Conference on. IEEE, 2003, pp. 1403–1410.

[3] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “Monoslam:Real-time single camera slam,” Pattern Analysis and Machine Intelli-gence, IEEE Transactions on, vol. 29, no. 6, pp. 1052–1067, 2007.

[4] L. A. Clemente, A. J. Davison, I. Reid, J. Neira, and J. D. Tardos,“Mapping large loops with a single hand-held camera,” in Robotics:Science and Systems, 2007.

[5] H. Strasdat, J. Montiel, and A. J. Davison, “Scale drift-aware largescale monocular slam,” in Proceedings of Robotics: Science andSystems (RSS), vol. 2, no. 3, 2010, p. 5.

[6] L. M. Paz, P. Pinies, J. D. Tardos, and J. Neira, “Large-scale 6-dofslam with stereo-in-hand,” Robotics, IEEE Transactions on, vol. 24,no. 5, pp. 946–957, 2008.

[7] J. Huber and V. Graefe, “Motion stereo for mobile robots,” in Indus-trial Electronics, 1993. Conference Proceedings, ISIE’93-Budapest.,IEEE International Symposium on. IEEE, 1993, pp. 263–270.

[8] P. Pritchett and A. Zisserman, “Wide baseline stereo matching,” inComputer Vision, 1998. Sixth International Conference on. IEEE,1998, pp. 754–760.

[9] P. Bonnifait and G. Garcia, “Design and experimental validation ofan odometric and goniometric localization system for outdoor robotvehicles,” Robotics and Automation, IEEE Transactions on, vol. 14,no. 4, pp. 541–548, 1998.

[10] T. Vidal-Calleja, M. Bryson, S. Sukkarieh, A. Sanfeliu, andJ. Andrade-Cetto, “On the observability of bearing-only slam,” inRobotics and Automation, 2007 IEEE International Conference on.IEEE, 2007, pp. 4114–4119.

[11] J. Kim and S. Sukkarieh, “Improving the real-time efficiency of inertialslam and understanding its observability,” in Intelligent Robots andSystems, 2004.(IROS 2004). Proceedings. 2004 IEEE/RSJ Interna-tional Conference on, vol. 1. IEEE, 2004, pp. 21–26.

[12] D. Goshen-Meskin and I. Bar-Itzhack, “Observability analysis ofpiece-wise constant systems. i. theory,” Aerospace and ElectronicSystems, IEEE Transactions on, vol. 28, no. 4, pp. 1056–1067, 1992.

[13] M. J. Tribou, D. W. Wang, and W. J. Wilson, “Observability of planarcombined relative pose and target model estimation using monocularvision,” in American Control Conference (ACC), 2010. IEEE, 2010,pp. 4546–4551.

[14] F. A. Belo, P. Salaris, and A. Bicchi, “3 known landmarks areenough for solving planar bearing slam and fully reconstruct unknowninputs,” in Intelligent Robots and Systems (IROS), 2010 IEEE/RSJInternational Conference on. IEEE, 2010, pp. 2539–2545.

[15] R. Hermann and A. Krener, “Nonlinear controllability and observ-ability,” Automatic Control, IEEE Transactions on, vol. 22, no. 5, pp.728–740, 1977.

[16] L. Matthies, T. Kanade, and R. Szeliski, “Kalman filter-based al-gorithms for estimating depth from image sequences,” InternationalJournal of Computer Vision, vol. 3, no. 3, pp. 209–238, 1989.

[17] C. F. Olson and H. Abi-Rached, “Wide-baseline stereo vision forterrain mapping,” Machine Vision and Applications, vol. 21, no. 5,pp. 713–725, 2010.

[18] S. Thrun, W. Burgard, D. Fox, et al., Probabilistic robotics. MITpress Cambridge, MA, 2005, vol. 1.

[19] K. Reif, S. Gunther, E. Yaz, and R. Unbehauen, “Stochastic stabilityof the discrete-time extended kalman filter,” Automatic Control, IEEETransactions on, vol. 44, no. 4, pp. 714–728, 1999.

[20] M. Anguelova, Observability and identifiability of nonlinear systemswith applications in biology. Chalmers University of Technology,2007.

[21] J.-J. E. Slotine, W. Li, et al., Applied nonlinear control. Prentice-HallEnglewood Cliffs, NJ, 1991, vol. 199, no. 1.

[22] T. Manzoor, “Observability and disparity in monocular slam: A study,”Master’s thesis, Lahore University of Management Sciences, 2013.

[23] R. S. Bucy and P. D. Joseph, Filtering for stochastic processes withapplications to guidance. American Mathematical Society, 1987, vol.326.

1652