Camera-IMU-based localization: Observability analysis and consistency improvement

Abstract

This work investigates the relationship between system observability properties and estimator inconsistency for a Vision-aided Inertial Navigation System (VINS). In particular, first we introduce a new methodology for determining the unobservable directions of nonlinear systems by factorizing the observability matrix according to the observable and unobservable modes. Subsequently, we apply this method to the VINS nonlinear model and determine its unobservable directions analytically. We leverage our analysis to improve the accuracy and consistency of linearized estimators applied to VINS. Our key findings are evaluated through extensive simulations and experimental validation on real-world data, demonstrating the superior accuracy and consistency of the proposed VINS framework compared to standard approaches.

Keywords

Vision-aided inertial navigation visual-inertial odometry observability analysis estimator consistency

Get full access to this article

View all access options for this article.

References

Bar-Shalom

Kirubarajan

(2001) Estimation with Applications to Tracking and Navigation. New York: John Wiley & Sons.

Bouguet

(2006) Camera calibration toolbox for matlab. Available at: www.vision.caltech.edu/bouguetj/calibdoc/.

Boyd

Vandenberghe

(2004) Convex Optimization. Cambridge; New York: Cambridge University Press.

Bryson

Sukkarieh

(2008) Observability analysis and active control for airborne SLAM. IEEE Transactions on Aerospace and Electronic Systems 44(1): 261–280.

Carlone

Macchia

Tibaldi

Bona

(2012) Robot localization and 3D mapping: Observability analysis and applications. In: Proceedings of the International symposium on artificial intelligence, robotics and automation in space, Turin, Italy, 4–6 September 2012, pp. 7–14.

Chatfield

(1997) Fundamentals of High Accuracy Inertial Navigation. Reston, VA: AIAA.

Chiuso

Favaro

Jin

Soatto

(2002) Structure from motion causally integrated over time. IEEE Transactions on Pattern Analysis and Machine Intelligence 24(4): 523–535.

Durrie

Gerritsen

Frew

Pledgie

(2009) Vision-aided inertial navigation on an uncertain map using a particle filter. In: Proceedings of the IEEE International conference on robotics and automation, Kobe, Japan, 12–17 May 2009, pp. 4189–4194.

Ebcin

Veth

(2007) Tightly-coupled image-aided inertial navigation using the unscented Kalman filter. Technical report, Air Force Institute of Technology, Dayton, OH.

10.

Hermann

Krener

(1977) Nonlinear controllability and observability. IEEE Transactions on Automatic Control 22(5): 728–740.

11.

Hesch

Kottas

Bowman

Roumeliotis

(2012a) Observability-constrained vision-aided inertial navigation. Technical report 2012-001, University of Minnesota, Department of Computer Science and Engineering, MARS Lab. Available at: www-users.cs.umn.edu/~joel/_files/Joel_Hesch_TR12.pdf.

12.

Hesch

Kottas

Bowman

Roumeliotis

(2012b) Towards consistent vision-aided inertial navigation. In: International workshop on the algorithmic foundations of robotics, Cambridge, MA, 13–15 June 2012, pp. 559–574.

13.

Hesch

Mirzaei

Mariottini

Roumeliotis

(2010) A Laser-aided Inertial Navigation System (L-INS) for human localization in unknown indoor environments. In: Proceedings of the IEEE International conference on robotics and automation, Anchorage, AK, 3–8 May 2010, pp. 5376–5382.

14.

Huang

Mourikis

Roumeliotis

(2008) A first-estimates Jacobian EKF for improving SLAM consistency. In: Proceedings of the International symposium on experimental robotics. Athens, Greece, 14–17 July 2008, pp. 373–382.

15.

Huang

Mourikis

Roumeliotis

(2010) Observability-based rules for designing consistent EKF SLAM estimators. International Journal of Robotics Research 29(5): 502–528.

16.

Huang

Trawny

Mourikis

Roumeliotis

(2011) Observability-based consistent EKF estimators for multi-robot cooperative localization. Autonomous Robots 30(1): 99–122.

17.

Isidori

(1989) Nonlinear Control Systems. Berlin, New York: Springer-Verlag.

18.

Jones

Soatto

(2011) Visual-inertial navigation, mapping and localization: A scalable real-time causal approach. International Journal of Robotics Research 30(4): 407–430.

19.

Kelly

Sukhatme

(2011) Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. International Journal of Robotics Research 30(1): 56–79.

20.

Kim

Sukkarieh

(2007) Real-time implementation of airborne inertial-SLAM. Robotics and Autonomous Systems 55(1): 62–71.

21.

Kneip

Chli

Siegwart

(2011) Robust real-time visual odometry with a single camera and an IMU. In: Proceedings of the British machine vision conference, Dundee, UK, 29 August-2 September, 2011, pp. 16.1-16.11.

22.

Mourikis

(2012) Improving the accuracy of EKF-based visual-inertial odometry. In: Proceedings of the IEEE International conference on robotics and automation, Minneapolis, MN, 14–18 May 2012, pp. 828–835.

23.

Mourikis

(2013) High-precision, consistent EKF-based visualinertial odometry. International Journal of Robotics Research 32(6): 690–711.

24.

Lucas

Kanade

(1981) An iterative image registration technique with an application to stereo vision. In: Proceedings of the International joint conference on artificial intelligence, Vancouver, Canada, August 1981, pp. 674–679.

25.

Lupton

Sukkarieh

(2012) Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions. IEEE Transactions on Robotics 28(1): 61–76.

26.

Martinelli

(2012) Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination. IEEE Transactions on Robotics 28(1): 44–60.

27.

Maybeck

(1979) Stochastic models, estimation, and control, Vol. 1. New York: Academic Press.

28.

Meyer

(2000) Matrix Analysis and Applied Linear Algebra. Philadelphia, PA: SIAM.

29.

Mirzaei

Roumeliotis

(2008) A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation. IEEE Transactions on Robotics 24(5): 1143–1156.

30.

Mourikis

Roumeliotis

(2007) A multi-state constraint Kalman filter for vision-aided inertial navigation. In: Proceedings of the IEEE International conference on robotics and automation, Rome, Italy, 10–14 April, pp. 3565–3572.

31.

Nistér

(2003) An efficient solution to the five-point relative pose problem. In: Proceedings of the IEEE conference on computer vision and pattern recognition, Madison, WI, 16–22 June 2003, pp. 195–202.

32.

Roumeliotis

Burdick

(2002) Stochastic cloning: A generalized framework for processing relative state measurements. In: Proceedings of the IEEE International conference on robotics and automation, Washington DC, 11–15 May 2002, pp. 1788–1795.

33.

Shen

Michael

Kumar

(2011) Autonomous multi-floor indoor navigation with a computationally constrained MAV. In: Proceedings of the IEEE International conference on robotics and automation, Shanghai, China, 9–13 May 2011, pp. 20–25.

34.

Shi

Tomasi

(1994) Good features to track. In: Proceedings of the IEEE conference on computer vision and pattern recognition, Washington DC, 21–23 June 1994, pp. 593–600.

35.

Shuster

(1993) A survey of attitude representations. Journal of the Astronautical Sciences 41(4): 439–517.

36.

Strelow

(2004) Motion estimation from image and inertial measurements. PhD Thesis, Carnegie Mellon University, Pittsburgh, PA.

37.

Trawny

Roumeliotis

(2005) Indirect Kalman filter for 3D attitude estimation. Technical Report 2005-002, University of Minnesota, Department of Computer Science and Engineering, MARS Lab.

38.

Weiss

(2012) Vision based navigation for micro helicopters. PhD Thesis, Swiss Federal Institute of Technology (ETH), Zurich, Switzerland.

39.

Weiss

Achtelik

Chli

Siegwart

(2012) Versatile distributed pose estimation and sensor self-calibration for an autonomous MAV. In: Proceedings of the IEEE International conference on robotics and automation. Minneapolis, MN, 14–18 May 2012, pp. 957–964.

40.

Williams

Hudson

Tweddle

Brockers

Matthies

(2011) Feature and pose constrained visual aided inertial navigation for computationally constrained aerial vehicles. In: Proceedings of the IEEE International conference on robotics and automation, Shanghai, China, 9–13 May 2011, pp. 431–438.

41.

Yap

Jr. Li

Mourikis

Shelton

(2011) A particle filter for monocular vision-aided odometry. In: Proceedings of the IEEE International conference on robotics and automation, Shanghai, China, 9–13 May 2011, pp. 5663–5669.