On the Consistency of EKF-SLAM: Focusing on the Role of Observation Model

1. On the Consistency of EKF-SLAM:Focusing on the Role of Observation Model Amir Tamjidi and Hamid Taghirad Electrical and Computer Engineering Department, K.N. Toosi University of Technology Ali Agha Computer Science and Engineering Department, Texas A&M University

2. Outline Consistency in SLAM and Motivation Focusing on Three Different Observation models Consistency Map Results Conclusion

3. EKF-SLAM • EKF-SLAM Framework: • System State • Prediction stage • Update stage • Consistency • Pessimistic covariance is OK (but not too pessimistic) • Optimistic covariance = Inconsistency = Filter divergence Robot Pose features Unbiasedness CovarianceMatching

4. Main Root of Inconsistency • Nice “convergence” properties for linear case (Dissanayake et al. 2001): • Landmark covariance decreases monotonically • In the limit, landmarks become fully correlated • In the limit, landmark covariance reaches a lower bound related to the initial vehicle covariance • Nonlinear case (S. Huang et al. 2007) • Convergence properties still hold true for “nonlinear case” provided that Jacobians are calculated at true states • In real situations where we do not have true states, The inherent approximations due to linearization can lead to divergence (inconsistency) of the EKF • Some Efforts to improve Consistency • Robot heading observation (Bailey et al. 2006) • Robocentric map joining (Castellanous et al. 2007) • First Estimate Jacobian EKF (G. Huang et al. 2008)

5. LV-SLAM Fusing the information of an LRF and a monocular camera to perform more robust SLAM algorithm • Filter inconsistency leads to optimistic covariances, which spuriously decrease the search region (in active search) in featureassociation stage.

6. Three Possible variants for EKF-SLAM Robot is equipped with a bearing-range sensor We can adopt three observing Strategies BR-EKF-SLAM BO-EKF-SLAM RO-EKF-SLAM In all cases, the inverse of Bearing-Range observation model is utilized for feature initialization

7. Stationary Robot Scenario Configuration (Julier et al. 2001) Robot initial uncertainty Information Form of Kalman filter (Updating stage) • General form of the covariance matrix

8. Observability Analysis in SLAM • This result is consistent with the existing results on observability analyses for SLAM problem. • Local Nonlinear Observability (G. Huang et al. 2008) • Nonlinear observability matrix in SLAM problem is rank deficient by 3 and robot Pose is unobservable in SLAM problem in general case. • Local Linear Observability (G. Huang et al. 2008) • When linearization is applied, robot’s orientation becomes observable • Thus, EKF is allowed to spuriously inject information about robot’s orientation. • Bailey showed (and also we observed in our experiments) thatif we have inconsistency in robot’s orientation, the features’ states are also become inconsistent. (Baily et al.2006) • Thus, we focus on , which causes the uncertainty in robot’s orientation.

9. Range Only Observation model Almost Radial Symmetry

10. Bearing Only Observation model

11. Bearing Range Observation model

12. Consistency Map Visualizing the amount of inconsistency in robot’s neighborhood using a color map (here in 10th step)

13. Consistency Map Evolution of consistency map in different steps for BO, BR, and RO EKF-SLAM in stationary robot scenario.

14. Stationary Robot BR-EKF-SLAM: Maximum estimation error for feature’s x and y coordinates are emx = 9.27m and emy =6.63m which both occur at 19th time step. Ideal 2 uncertainty bound is obtained by caclulating Jacobians at true state vector. Estimated and ideal covariances do not match. RO-EKF-SLAM: Maximum estimation error for feature’s x and y coordinates are emx = 2.58m and emy = 1.87 m which both occur at 179th time step. Ideal 2 uncertainty bound is obtained by caclulating Jacobians at true state vector. Estimated and ideal covariances do not match. BO-EKF-SLAM: Maximum estimation error for feature’s x and y coordinates are emx = 1.52m and emy =1.25m which both occur at 3rd time step. Ideal 2 uncertainty bound is obtained by caclulating Jacobians at true state vector. Estimated and ideal covariances are nearly identical.

15. Moving Robot Scenario - Real Scenario • RO-EKF has a similar result to BO-EKF relative distance of landmarks is such that they reside in consistent region of both BO-EKF and RO-EKF • Overall, the estimation error of BR is greater than the other variants [courtesy by E. Nebot]

16. Moving Robot Scenario - Simulation • Configuration (Bailey et al. 2006) • Consistency is tested by calculating the average normalized estimation error squared (NEES): Robot Path and Map Robot State Vector NEES BO-EKF NEES increases with the smallest slope compared to other variants At First BR-EKF outperforms other variants BO-EKF undergoes the least amount of inconsistency First Loop Second Loop Features’ State NEES

17. Conclusion • Comparing different observation models in some well-known scenarios, we showed that in stationary robot scenario, Bearing-only observation model exhibits superior results in term of consistency. • Trade off: • Gain Consistency • Throwing out some information leads to higher uncertainty • Larger areas for search in data association stage (in active search type methods) • Introduced consistency map helps us to visualizing the extent of inconsistency the observation models inject to estimation process. • Manipulating the measurement data before injecting them into filter, can avoid spurious updates. • Future direction: Proving these results also for moving robot scenario can have great effects on improving SLAM consistency. • Future direction: Analyzing consistency map shows that in SR scenario BO and RO models are good complements for each other, and we can combine these methods to make the system consistent by loosing less information.

18. Thank you!Questions?