Disparity as a Separate Measurement in Monocular SLAM

Transcription

1 Disparity as a Separate Measurement in Monocular SLAM Talha Manzoor and Abubakr Muhammad 2 Abstract In this paper, we investigate the eects o including disparity as an explicit measurement not just or initialization in addition to the projective camera measurements, in conventional monocular/bearing-only SLAM in a 2-D world We conduct an observability analysis or a 5-D scenario which theoretically shows that adding disparity measurements inluences the observability rank condition, and so disparity can be regarded as a valid measurement As disparity is dependent on the camera s location one time step earlier, we rederive the EKF equations or measurement models dependent on the previous state It is shown that this modiication results in an increase in the measurement noise covariance We also discuss the implications o the white noise assumption in EKF being violated with the inclusion o disparity measurements In the end, we compare the results o a simulated monocular SLAM scenario in a 2-D world with and without disparity measurements Here we use the magnitude o the eigenvalues o the simulated state covariance matrix as a metric or observability [] Our results suggest that including disparity measurements in monocular SLAM improves observability both in the map and pose variables Hence, this paper is an invitation to consider disparity measurements explicitly, in monocular SLAM I INTRODUCTION Simultaneous Localization and Mapping with a single camera, also known as Monocular SLAM, is a well known problem in the robotics community and extensive research and development have been carried out in this area Many monocular SLAM systems have been developed to date with the irst being presented by Davison [2] who used the projection o observed eatures or landmarks on the image plane o the camera as measurements or the EKF Since then, a lot o research has been done in single camera SLAM [3], [4], [5], and in many sub-areas Another approach introduced by Paz et al [6] includes monocular inormation in a stereo hand held system or SLAM, using both projective and disparity measurements In this paper, we introduce a similar idea and discuss the possibility o including stereo inormation in a monocular SLAM system Thus, we consider both projective and disparity measurements However, the stereo in this case is done in time This type o stereo vision is commonly known as motion-stereo [7] or wide-baseline stereo [8] One major concern while deploying a single camera as a sensor or any mapping problem is the inherent handicap in trying to reconstruct the environment rom projected images This problem o observability o monocular or Talha Manzoor is a PhD student in Electrical Engineering at Lahore University o Management Sciences, Lahore, Pakistan at lumsedupk 2 Abubakr Muhammad is with the aculty o Electrical Engineering, Lahore University o Management Sciences, Lahore, Pakistan abubakr at lumsedupk bearing-only SLAM has been investigated extensively in the past Bonniait and Garcia [9] carry out an observability analysis o a planar robot trying to localise itsel, given the bearing to 3 known beacons They conclude that the system is observable except or the degenerate case when the robot is located on the circle deined by the 3 beacons and is stationary In a similar paper, Calleja et al [0] have considered the observability o a planar robot, perorming SLAM with a bearing-only sensor Instead o expressing the SLAM system in its regular orm, they do so in the error orm presented in [] This allows them to approximate the system as piecewise constant, enabling them to use linear observability techniques as given in [2] They prove that or absolute coordinates SLAM, the process is unobservable over 2 time segments when the robot is either stationary, or moving directly towards a landmark Introducing a known landmark beacon renders the linearly approximated process ully observable Tribou et al [3] present an observability analysis or a planar position based visual servoing system, also applicable to monocular SLAM They ocus on determining the unobservable conigurations or their application They too approximate the system with a piecewise constant one and consider a constant velocity motion model which constitutes a larger unobservable subspace than the vehicle speciic kinematic model in [0] The observability analysis we present over here however, is not or the purpose o solving the observability problem or 2-D monocular SLAM in general Indeed it has already been solved by Belo et al in [4], in which they show that 3 known landmarks are enough to guarantee the observability o monocular SLAM Our analysis over here however, is or theoretically demonstrating that the observability o monocular SLAM is indeed inluenced by the inclusion o disparity measurements We do so by evaluating the observability rank criterion or nonlinear systems as presented in [5] Due to the complexity o the analytical calculation or 2-D, we assume a 5-D scenario in which the robot is constrained to move along a straight line in a 2-D world and conjecture that i disparity measurements are useul in 5D, they must also be so in 2-D which we show through simulations Our monocular SLAM algorithm is based on the Extended Kalman Filter As mentioned beore, we use both projective and disparity measurements As our disparity measurements are based on motion-stereo, this measurement is dependent on the current and previous states Similar systems using motion stereo have been developed in the past like [6], [7] However, these approaches apply corrections separately to the image sequence, and then use the conventional ilter or depth estimation In our work, we have incorporated

2 disparity measurements per se in the EKF by rederiving the EKF equations through the Bayesian approach given in [8] We use the law o total probability twice, to establish the conditional dependence o the belie both on the current and previous states Ater the proo is complete, we see that the aect o this dependence is simply an increase in measurement noise As intuition suggests, this increase is dependent both on the process model and on the process noise covariance Ham et al [] have shown that the eigenvalues and eigenvectors o the state covariance matrix o the Kalman Filter can provide useul insight into the observability o the system They show that the magnitude o the eigenvalues can provide a sense o the degree o observability where the eigenvector with the smallest eigenvalue gives the most observable linear combination o the states and the eigenvector with the largest eigenvalue giving otherwise Hence we use the trace o the covariance matrix, to analyze the perormance o a simulated monocular SLAM scenario both with and without disparity measurements The organization o the paper is as ollows Section II gives an observability analysis, comparing two 5D monocular SLAM systems with and without disparity measurements The equations or the generalized EKF or measurement models dependent on the previous state are derived in Section III The measurement equations and discussion about the coloredness o the measurement noise are given in Section IV Results are presented in Section V with the conclusion in Section VI II OBSERVABILITY ANALYSIS FOR A 5-D MONOCULAR SLAM SCENARIO We show here that including disparity as a separate measurement does indeed eect the perormance o monocular SLAM One o the necessary conditions or the estimation error o the discrete time Kalman Filter to remain bounded is that the system must satisy the non-linear observability rank condition [9] We irst state this condition or general non-linear systems A The Non-linear Observability Rank Condition Consider the ollowing discrete-time non-linear system x k+ = x k, u k ; y k+ = hx k+, and y R m This system is observable i or any initial state x 0 and some inal time k the initial state x 0 can be uniquely determined by knowledge o the input u i and output y i or all i [0, k] Checking the observability o a system amounts to checking the rank o a certain observability matrix [5], [20] I the system given by Equation is in control aine orm such that x k, u k = g 0 x k + g i xu ik, then it is observable i and only i the rank o the ollowing observability matrix is ull [ dl O = 0 h T dl h T dl n h ] T T 2 where x R n, u R p where L i h is the ith order Lie Derivative [2] o the scalar ield o the measurement h with respect to the vector ield and d denotes the gradient operator with respect to x B Observability and Beacons or Monocular SLAM The irst ever monocular SLAM system presented in [2] was initialized by a certain number o known landmarks or beacons This empirically suggests that the observability o monocular SLAM is related to the number o beacons present in the environment Since then, work has been done on investigating the requirement on the minimum number o beacons or monocular/ bearing-only SLAM [0], [3] However this is based on the approximated picewise constant systems observability theory as presented in [2] For 2- D monocular SLAM, the rank o the observability matrix given by Equation 2 becomes too complex to compute even or a computer algebraic package Thus, to show that disparity measurements do indeed aect the observability o monocular SLAM, we carry out an observability analysis or a simple case o the general problem C Observability or 5-D Monocular SLAM Consider the problem posed in Figure A robot constrained to move on a single line, attempts to perorm SLAM in a 2-D world containing a single beacon L, and a single landmark L2 It carries a single camera and receives as measurements, u k and u2 k which are the respective projections o L and L2 on the image plane at time k The robot can only move along the x-axis and its location at time k is given by c k The location o a landmark is given by x L, d L where d L is the inverse depth o the landmark We d L x L x L2 y c 0 x c L L2 c k d L2 u2 0 u 0 u2 u u2 k Fig The robot perorming SLAM in a 2-D world with beacon Here, is the ocal length o the camera Quantities highlighted in blue are known, whereas those in black are to be estimated u k now calculate the observability matrix or a constant velocity motion model The state vector is given by X k = [ c k ċ k x L2k d L2k ] T 3 The constant velocity model is given by the ollowing vectorvalued unction = [ c k + tċ k ċ k x L2k d L2k ] T 4

3 The measurement vector is given by [ ] [ uk x Y k = L d L c = k d L u2 k x L2k d L2k c k d L2k Note that the irst measurement is not exactly equal to the projective measurement A bias term, consisting o all known quantities has been subtracted Applying the deinition o Equation 2, the observability matrix o this system turns out to be ull rank, hence the process is observable Also, it can be checked that the rank o O will not be ull i we exclude the beacon Thus at least beacon is required or monocular SLAM in this setting to be observable The exact orm o the observability matrix or both cases can be ound in [22] D 5-D Monocular SLAM with disparity measurements We now see the eect o including disparity as a measurement on observability o the system in Figure Here we neglect the measurements o L and assume that there is no beacon The process and state are the same as given in Equations 3 and 4 We now have a disparity measurement given by y 3k = u2 k u2 k = c k d L2k + c k d L2k 5 Note that this measurement is dependent on the previous state Section III discusses the eect this dependence would have on the EKF The observability matrix in this case turns out to have ull rank [22] Thus we see that including disparity as a measurement has eliminated the requirement o the beacon in order to keep SLAM observable This shows that disparity is indeed a valid measurement and that it also improves the observability o monocular SLAM in 5-D III EKF FOR MEASUREMENT MODELS DEPENDENT ON THE PREVIOUS STATE The measurement model shown in Equation 5 is dependent on the previous and currrent states However, the conventional Kalman ilter assumes that the measurement is dependent on the current state only We derive the Kalman ilter or this case rom the basic Bayes Filter ollowing the method given in [8] Beore presenting our equations, we irst describe the motion and measurement models Ater presenting the equations, we discuss the mathematical proo A Functions and variables Motion Model: The motion or process model is given by X k = X k, µ k, ω k where ω k is the process noise at step k Its covariance is given by Q k Ater linearization we get the ollowing orm X k = ˆX k, µ k, 0 + F Xk X k ˆX k + F ωk ω k, where F Xk is the Jacobian o wrt X k, and F ωk is the Jacobian o wrt ω k ] 2 Inverse Motion Model: The inverse motion model is given by X k = gx k, µ k, ω k Note that or same time steps, ω k has the same value as its instance appearing in the motion model Ater linearization we get X k = g ˆX k, µ k, 0 + G Xk X k ˆX k + G ωk ω k, where G Xk is the Jacobian o g wrt X k, and G ωk is the Jacobian o g wrt ω k 3 Measurement Model: The measurement model is given by Y k = hx k, X k + v k where v k is the sensor noise Its covariance is given by R k Ater linearization we get Y k =h ˆX k, ˆX k + H Xk X k ˆX k +H Xk X k ˆX k + v k, where H Xk is the Jacobian o h wrt X k, and H Xk is the Jacobian o h wrt X k B Generalized Kalman Filter Equations Our inal equations or the motion and measurement models just presented are given as ollows: ˆX k k = ˆX k k, µ k, 0, 6 P k k = F Xk P k k F Xk T + F ωk Q k F ωk T, 7 W k = R k + H Xk G ωk Q k G ωk T H Xk T, 8 K k = P k k H Xk T H Xk P k k H Xk T + W k, 9 ˆX k k = ˆX k k + K k Y k h ˆX k k, ˆX k k, 0 P k k = I K k H Xk P k k, where ˆX k is the estimate o the state X, P k is the covariance o ˆXk, µ k is the control vector, Ŷ k is the expected measurement, K k is the Kalman gain, W k is the covariance o Ŷk, X k, µ k is the motion model, gx k, µ k is the inverse motion model and hx k, X k is the measurement model All quantities are evaluated at time k The dierence between the conventional EKF and the one given by Equations 6- lies in the calculation o Equation 8 For simple measurement models, W k will be exactly equal to the sensor noise R k with the rest o the equations remaining unchanged Note that or linear systems, the inverse motion model will just be equal to F We assume that such an inversion exists C Mathematical Derivation The equations are derived irst by extending the basic Bayes Filter Algorithm [8], to include the previous state in the measurement step Then the probability distributions are solved or the required models To include the dependency o Y k on X k we include an intermediate step in the Bayes Filter through the theorem o total probability The steps are as ollows: Step : belx k = px k Y 0:k, µ k = px k X k, Y 0:k, µ k px k Y 0:k, µ k dx k, 2

4 Step 2: py k X k, Y 0:k, µ k = py k X k, Y 0:k, µ k, X k px k X k, Y 0:k, µ k dx k, 3 Step 3: belx k = ηpy k X k, Y 0:k, µ k px k Y 0:k, µ k, 4 where η is a normalizing constant Note that the conventional Bayes ilter [8] consists only o steps and 3 We present the derivation, with this extended orm o the Bayes ilter Part : Prediction: We begin with step o the Bayes ilter The terms o Equation 2 are normally distributed with mean and covariance as px k X k, Y 0:k, µ k N X k ; ˆX k,µ k,0+f Xk X k ˆX k ;F ωk Q k F ωk T px k Y 0:k, µ k N X k ; ˆX k ;P k and, Ater evaluation o this equation, the outcome is also a Gaussian with mean ˆX k k and covariance P k k The interested reader is directed to [8] or a ull proo The mean and variance o the resulting distribution are given as ˆX k k = X k, µ k, 0 and P k k = F ωk Q k F ωk T + F Xk P k F Xk T respectively 2 Part 2: Measurement Covariance: Here we derive step 2 o the extended Bayes ilter given by 3 The distributions o the terms involved are given as ollows py k X k, Y 0:k, µ k, X k and, NY K ;h ˆX k, ˆX k +H xk X k X ˆ k +H Xk X k ˆX k ;R k px k X k, Y 0:k, µ k NX k ;gˆx k,µ k,0+g Xk X k ˆX k ;G ωk Q k G T ωk The derivation is essentially the same as that o step given in [8] In this case the integral in Equation 3 can be expressed as exp{ M k }dx k, where M k = 2 Y k h ˆX k, ˆX k H xk X k ˆ X k H Xk X k ˆX k R k Y k h ˆX k, ˆX k H xk X k X ˆ k H Xk X k ˆX k T X k gˆx k,µ k,0 G Xk X k ˆX T k G ωk Q k G ωk + 2 X k gˆx k,µ k,0 G Xk X k ˆX k T 5 We can decompose M k into two unctions These unctions are given by M k = MY k, X k, X k + MY k, X k 6 By dierenciating twice with respect to X k, we determine the minimum and covariance o M k so as to orm a quadratic unction similar to the exponential o the normal distribution Thus we get 2 MY k, X k, X k = X k Φ k H Xk T R k Y k h ˆX k, ˆX k H xk X k ˆ X k H Xk X k ˆX k T +G ωk Q k G T ωk gˆx k,µ k,0 G Xk X k ˆX k Φ k X k Φ k H T Xk R k Y k h ˆX k, ˆX k H xk X k X ˆ k H Xk X k ˆX k +G ωk Q k G ωk T gˆx k,µ k,0 G Xk X k ˆX k, 7 where Φ k = H T Xk R k H Xk +G ωk Q k G T ωk Hence rom Equations 5, 6 and 7, we get MY k, X k As this does not have any dependence on X k, we can move it out o the integral What remains inside the integral is exp{ MY k, X k, X k } which is a valid probability density unction hence integrates to a constant Thus exp{ MY k, X k } gives the required distribution The mean and covariance o this distribution is given by the minimumm and inverse o the curvature o MY k, X k respectively Dierentiating with respect to Y k, we get the mean as h ˆX k, ˆX k + H Xk X k ˆX k, and the covariance, the inverse o the curvature as W k = R k + H Xk G ωk Q k G T ωk H T Xk This proves the correctness o 8 3 Part3: Correction / Update: The distributions o the terms involved in the 3rd step o the Bayes Filter are py k X k, Y 0:k, µ k NY k ;h ˆX k, ˆX k +H Xk X k ˆX k px k Y 0:k, µ k and, NX k ; ˆX k,µ k,0;f Xk P k F Xk T +F ωk Q k F ωk T The resulting distribution ollows rom [8] by replacing R k with W k so that we get the remaining equations ˆX k k = ˆX k k + K k Y k h ˆX k, ˆX k, K k = P k k H Xk T H Xk P k k H Xk T + W k, P k k = I K k H Xk P k k IV PLANAR MONOCULAR SLAM In this section, we setup the EKF presented in Section III or planar monocular SLAM and show through simulations, the eect o disparity measurements A System description Consider Figure 2, in which a camera perorms SLAM in a 2-D world The state vector is given by X k = [ c x k c x k c y k c y k c θ k c θ k L2 x k L2 y k ] T Here we assume a constant velocity motion model or state and covariance propagation The measurement equation

5 L x, L y L2 x, L2 y H is the respective derivative Once we have these matrices, we use the state augmentation technique presented in [23] to deal with colored measurement noise c x 0, c y 0 L2 h L 0 h 0 y x c x k, c y k L2 h k L h k c θ k Fig 2 Monocular state sequence in 2-D c x k, c y k L2 h k L h k requires L x, L y, L2 x and L2 y to be described in the camera coordinate rame Transorming the world coordinates gives us C L = [ L x L y ] T as [ ] C L = L x c x cos c θ + L y c y sin c θ L x c x sin c θ + L y c y cos c, θ and so, the measurement model or the camera is given by L h = L x c x cos c θ + L y c y sin c θ L x c x sin c θ + L y c y cos c θ, and disparity is given by L2 disp k = L2 B k / L2 d k, where L2 B k is the magnitude o the baseline and L2 d k is the perpendicular distance o the landmark rom the baseline They are given as L2 c x k c x k c y k L2 y d k = c x k c x k 2 + c y k c y k 2 c θ k A Setup V SIMULATION The simulations or validation o our work has been implemented in MATLAB In the scenario presented here, the robot ollows a rhombus shaped trajectory with smoothed corners as shown in Figure 3 Velocity has been kept higher at the sides and lower at the corners The ocal length o the camera has been set at 20 pixel units The process and measurement models are as given in Section IV The process model is a constant velocity one with process noise modeled as zero-mean Gaussian with variances o 05 m/s 2 2, 0 deg 2 and 000 m 2 or the linear velocities, angular velocities and landmark positions respectively Measurement noise variables have also been drawn rom zero-mean Gaussian distributions with variances o 0074 pixels 2 or projective and 74 pixels 2 or disparity measurements Length o one time step has been kept at 0 seconds ater which the robot receives projective and disparity measurements o the landmarks and beacons As seen in Figure 3 an incorrect initial estimate is provided with large uncertainty c x k L2 x c y k c y k c x k c x k 2 + c y k c y k 2, and L2 B k = c x k c x k 2 + c y k c y k 2 B Colored Measurement Noise The disparity measurement is acheived through the ollowing steps: Obtain the measurements Li h k, Li h k or consecutive time steps 2 Rectiy the measurements to get Li h k,li h k 3 Get L2 disp k as the dierence between Li h k and Li h k These steps result in the ollowing equation or disparity Li disp k = Li h k cosα k c θ k 2 sinα k c θ k Li h k sinα k c θ k + cosα k c θ k Li h k cosα k c θ k 2 sinα k c θ k Li h k sinα k c θ k + cosα k c, θ k 8 where α k = arctan c y k c y k / c x k c x k Equation 8 shows that the noise in the disparity measurement at time step k is correlated with the projection measurement at time step k We can get the noise equation rom Equation 8 and approximate it linearly to get, ζ dispk equal to H ζhk ζ hk +H ζhk ζ hk, where ζ is the variable or noise and Fig 3 A robot perorming SLAM in 2-D The cameras depict the true pose o the robot at selected time steps, equally distributed in time The beacons are drawn in yellow whereas the estimated landmark is in white The ellipses show the 95% conidence regions An incorrect estimate with large uncertainty is provided B Results The results o the proposed ramework can be seen in Figures 3 & 4 In Figure 3, the true robot location is almost always captured by the respective uncertainty ellipse Due to the incorrect initialization, there is a little inconsistency or some time ater the ilter is initialized Overall, the ilter shows consistent behavior This can also be seen or the uncertainty ellipse o the estimated landmark Figure 4 compares the perormance between the conventional EKF and the modiied EKF o Section III The irst two graphs show the trace o the position and map covariance

6 Fig 4 Perormance comparison between the conventional EKF and the generalized EKF presented in Section III From top-let in counter clockwise order: Trace o covariances or robot location, 2 Trace o covariances or landmark location, 3 SSD or robot location, 4 SSD or landmark location matrices They represent the sum o the eigenvalues and thus the degree o observability o the system [] We see that there is considerable improvement with respect to the robot position variables Although the steady state behavior or the landmark remains the same, the ilter run with inclusion o disparity measurements, has a smaller transient Thus we observe an improvement both or the robot and map variables Also, we see rom Figure 4 that the steady state accuracy with respect to the sum o squared errors is almost the same or both cases, with the ilter run with inclusion o disparity measurements having smaller transients VI CONCLUSION The idea o including disparity measurements in monocular SLAM presented in this paper has been tested irst theoretically through the observability rank condition and then through simulations During this process, we have also generalized the EKF or measurement models dependent on the previous state which may have applications other than that o disparity measurements in monocular SLAM We have seen through simulations that the degree o observability is indeed improved both or robot pose and landmark variables It must be noted here that disparity is already included implicitly in Monocular SLAM, and that the process is inherently unobservable especially in the scale direction Thus the inclusion o an explicit disparity measurement cannot convert this unobservable process into an observable one However as described in this paper, it does improve the degree o observability or all states and hence provides more conident estimates or the EKF Although we have not conducted a mathematically rigorous proo or our proposition, we hope that this work will move theoreticians to look in this direction REFERENCES [] F M Ham and R G Brown, Observability, eigenvalues, and kalman iltering, Aerospace and Electronic Systems, IEEE Transactions on, no 2, pp , 983 [2] A J Davison, Real-time simultaneous localisation and mapping with a single camera, in Computer Vision, 2003 Proceedings Ninth IEEE International Conerence on IEEE, 2003, pp [3] A J Davison, I D Reid, N D Molton, and O Stasse, Monoslam: Real-time single camera slam, Pattern Analysis and Machine Intelligence, IEEE Transactions on, vol 29, no 6, pp , 2007 [4] L A Clemente, A J Davison, I Reid, J Neira, and J D Tardós, 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 drit-aware large scale monocular slam, in Proceedings o Robotics: Science and Systems RSS, vol 2, no 3, 200, p 5 [6] L M Paz, P Piniés, J D Tardós, and J Neira, Large-scale 6-do slam with stereo-in-hand, Robotics, IEEE Transactions on, vol 24, no 5, pp , 2008 [7] J Huber and V Graee, Motion stereo or mobile robots, in Industrial Electronics, 993 Conerence Proceedings, ISIE 93-Budapest, IEEE International Symposium on IEEE, 993, pp [8] P Pritchett and A Zisserman, Wide baseline stereo matching, in Computer Vision, 998 Sixth International Conerence on IEEE, 998, pp [9] P Bonniait and G Garcia, Design and experimental validation o an odometric and goniometric localization system or outdoor robot vehicles, Robotics and Automation, IEEE Transactions on, vol 4, no 4, pp , 998 [0] T Vidal-Calleja, M Bryson, S Sukkarieh, A Saneliu, and J Andrade-Cetto, On the observability o bearing-only slam, in Robotics and Automation, 2007 IEEE International Conerence on IEEE, 2007, pp [] J Kim and S Sukkarieh, Improving the real-time eiciency o inertial slam and understanding its observability, in Intelligent Robots and Systems, 2004IROS 2004 Proceedings 2004 IEEE/RSJ International Conerence on, vol IEEE, 2004, pp 2 26 [2] D Goshen-Meskin and I Bar-Itzhack, Observability analysis o piece-wise constant systems i theory, Aerospace and Electronic Systems, IEEE Transactions on, vol 28, no 4, pp , 992 [3] M J Tribou, D W Wang, and W J Wilson, Observability o planar combined relative pose and target model estimation using monocular vision, in American Control Conerence ACC, 200 IEEE, 200, pp [4] F A Belo, P Salaris, and A Bicchi, 3 known landmarks are enough or solving planar bearing slam and ully reconstruct unknown inputs, in Intelligent Robots and Systems IROS, 200 IEEE/RSJ International Conerence on IEEE, 200, pp [5] R Hermann and A Krener, Nonlinear controllability and observability, Automatic Control, IEEE Transactions on, vol 22, no 5, pp , 977 [6] L Matthies, T Kanade, and R Szeliski, Kalman ilter-based algorithms or estimating depth rom image sequences, International Journal o Computer Vision, vol 3, no 3, pp , 989 [7] C F Olson and H Abi-Rached, Wide-baseline stereo vision or terrain mapping, Machine Vision and Applications, vol 2, no 5, pp , 200 [8] S Thrun, W Burgard, D Fox, et al, Probabilistic robotics MIT press Cambridge, MA, 2005, vol [9] K Rei, S Gunther, E Yaz, and R Unbehauen, Stochastic stability o the discrete-time extended kalman ilter, Automatic Control, IEEE Transactions on, vol 44, no 4, pp , 999 [20] M Anguelova, Observability and identiiability o nonlinear systems with applications in biology Chalmers University o Technology, 2007 [2] J-J E Slotine, W Li, et al, Applied nonlinear control Prentice-Hall Englewood Clis, NJ, 99, vol 99, no [22] T Manzoor, Observability and disparity in monocular slam: A study, Master s thesis, Lahore University o Management Sciences, 203 [23] R S Bucy and P D Joseph, Filtering or stochastic processes with applications to guidance American Mathematical Society, 987, vol 326