Research Papers

Developing Computationally Efficient Nonlinear Cubature Kalman Filtering for Visual Inertial Odometry

[+] Author and Article Information
Trung Nguyen

Intelligent Systems Lab,
Faculty of Engineering and Applied Science,
Memorial University of Newfoundland,
St. John's, NL A1B 3X9, Canada
e-mail: tn0432@mun.ca

George K. I. Mann

Intelligent Systems Lab,
Faculty of Engineering and Applied Science,
Memorial University of Newfoundland,
St. John's, NL A1B 3X9, Canada
e-mail: gmann@mun.ca

Andrew Vardy

Department of Computer Science;
Department of Electrical and Computer Engineering,
Memorial University of Newfoundland,
St. John's, NL A1B 3X9, Canada
e-mail: av@mun.ca

Raymond G. Gosine

Intelligent Systems Lab,
Faculty of Engineering and Applied Science,
Memorial University of Newfoundland,
St. John's, NL A1B 3X9, Canada
e-mail: rgosine@mun.ca

Contributed by the Dynamic Systems Division of ASME for publication in the JOURNAL OF DYNAMIC SYSTEMS, MEASUREMENT,AND CONTROL. Manuscript received October 12, 2018; final manuscript received February 16, 2019; published online March 27, 2019. Assoc. Editor: Richard Bearee.

J. Dyn. Sys., Meas., Control 141(8), 081012 (Mar 27, 2019) (10 pages) Paper No: DS-18-1461; doi: 10.1115/1.4042951 History: Received October 12, 2018; Revised February 16, 2019

This paper presents a computationally efficient sensor-fusion algorithm for visual inertial odometry (VIO). The paper utilizes trifocal tensor geometry (TTG) for visual measurement model and a nonlinear deterministic-sampling-based filter known as cubature Kalman filter (CKF) to handle the system nonlinearity. The TTG-based approach is developed to replace the computationally expensive three-dimensional-feature-point reconstruction in the conventional VIO system. This replacement has simplified the system architecture and reduced the processing time significantly. The CKF is formulated for the VIO problem, which helps to achieve a better estimation accuracy and robust performance than the conventional extended Kalman filter (EKF). This paper also addresses the computationally efficient issue associated with Kalman filtering structure using cubature information filter (CIF), the CKF version on information domain. The CIF execution avoids the inverse computation of the high-dimensional innovation covariance matrix, which in turn further improves the computational efficiency of the VIO system. Several experiments use the publicly available datasets for validation and comparing against many other VIO algorithms available in the recent literature. Overall, this proposed algorithm can be implemented as a fast VIO solution for high-speed autonomous robotic systems.

Copyright © 2019 by ASME
Your Session has timed out. Please sign back in to continue.


Wong, X. I. , and Majji, M. , 2017, “ Extended Kalman Filter for Stereo Vision-Based Localization and Mapping Applications,” ASME J. Dyn. Syst. Meas. Control, 140(3), p. 030908. [CrossRef]
Rogne, R. H. , Bryne, T. H. , Fossen, T. I. , and Johansen, T. A. , 2018, “ Redundant MEMS-Based Inertial Navigation Using Nonlinear Observers,” ASME J. Dyn. Syst. Meas. Control, 140(7), p. 071001. [CrossRef]
Santoso, F. , Garratt, M. A. , and Anavatti, S. G. , 2017, “ Visual-Inertial Navigation Systems for Aerial Robotics: Sensor Fusion and Technology,” IEEE Trans. Autom. Sci. Eng., 14(1), pp. 260–275. [CrossRef]
Kottas, D. G. , and Roumeliotis, S. I. , 2015, “ An Iterative Kalman Smoother for Robust 3D Localization on Mobile and Wearable Devices,” IEEE International Conference on Robotics and Automation, Seattle, WA, May 26–30, pp. 6336–6343.
Hartley, R. , and Zisserman, A. , 2004, Multiple View Geometry in Computer Vision, Cambridge University Press, New York.
Delmerico, J. , and Scaramuzza, D. , 2018, “ A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots,” IEEE International Conference on Robotics and Automation, Brisbane, Australia, May 21–25, pp. 2502–2509.
Mourikis, A. I. , and Roumeliotis, S. I. , 2007, “ A Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation,” IEEE International Conference on Robotics and Automation, Roma, Italy, Apr. 10–14, pp. 3565–3572.
Li, M. , and Mourikis, A. I. , 2013, “ High-Precision, Consistent EKF-Based Visual-Inertial Odometry,” Int. J. Rob. Res., 32(6), pp. 690–711. [CrossRef]
Qin, T. , Li, P. , and Shen, S. , 2018, “ Vins-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE Trans. Rob., 34(4), pp. 1004–1020. [CrossRef]
Leutenegger, S. , Lynen, S. , Bosse, M. , and Furgale, P. , 2015, “ Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization,” Int. J. Rob. Res., 34(3), pp. 314–334. http://www.roboticsproceedings.org/rss09/p37.pdf
Pakki, K. , Chandra, B. , Gu, D.-W. , and Postlethwaite, I. , 2013, “ Square Root Cubature Information Filter,” IEEE Sens. J., 13(2), pp. 750–758. [CrossRef]
Forster, C. , Carlone, L. , Dellaert, F. , and Scaramuzza, D. , 2017, “ On-Manifold Preintegration for Real-Time Visual—Inertial Odometry,” IEEE Trans. Rob., 33(1), pp. 1–21. [CrossRef]
Arasaratnam, I. , and Haykin, S. , 2009, “ Cubature Kalman Filters,” IEEE Trans. Autom. Control, 54(6), pp. 1254–1269. [CrossRef]
De Silva, O. , Mann, G. K. I. , and Gosine, R. G. , 2016, “ The Right Invariant Nonlinear Complementary Filter for Low Cost Attitude and Heading Estimation of Platforms,” ASME J. Dyn. Syst. Meas. Control, 140(1), p. 011011. [CrossRef]
Yu, X. , Baker, T. , Zhao, Y. , and Tomizuka, M. , 2017, “ Fast and Precise Glass Handling Using Visual Servo With Unscented Kalman Filter Dual Estimation,” ASME J. Dyn. Syst. Meas. Control, 140(4), p. 041008. [CrossRef]
Wan, E. A. , and Merwe, R. V. D. , 2000, “ The Unscented Kalman Filter for Nonlinear Estimation,” IEEE Adaptive Systems for Signal Processing, Communications, and Control Symposium, Lake Louise, AB, Canada, Oct. 4, pp. 153–158.
Loianno, G. , Watterson, M. , and Kumar, V. , 2016, “ Visual Inertial Odometry for Quadrotors on SE(3),” IEEE International Conference on Robotics and Automation, Stockholm, Sweden, May 16–21, pp. 1544–1551.
Hu, J.-S. , and Chen, M.-Y. , 2014, “ A Sliding-Window Visual-IMU Odometer Based on Tri-Focal Tensor Geometry,” IEEE International Conference on Robotics and Automation, Hong Kong, China, May 31–June 7, pp. 3963–3968.
Mutambara, A. G. , 1998, Decentralized Estimation and Control for Multisensor Systems, CRC Press, Boca Raton, FL.
Wang, S. , Feng, J. , and Tse, C. K. , 2014, “ A Class of Stable Square-Root Nonlinear Information Filters,” IEEE Trans. Autom. Control, 59(7), pp. 1893–1898. [CrossRef]
Hesch, J. A. , Kottas, D. G. , Bowman, S. L. , and Roumeliotis, S. I. , 2014, “ Camera IMU Based Localization: Observability Analysis and Consistency Improvement,” Int. J. Rob. Res., 33(1), pp. 182–201. [CrossRef]
Peretroukhin, V. , Vega-Brown, W. , Roy, N. , and Kelly, J. , 2016, “ PROBE-GK: Predictive Robust Estimation Using Generalized Kernels,” IEEE International Conference on Robotics and Automation, Stockholm, Sweden, May 16–21, pp. 817–824.
Heo, S. , Cha, J. , and Park, C. G. , 2018, “ EKF-Based Visual Inertial Navigation Using Sliding Window Nonlinear Optimization,” IEEE Trans. Intell. Transp. Syst. (epub).
Andreff, N. , and Tamadazte, B. , 2015, “ Laser Steering Using Virtual Trifocal Visual Servoing,” Int. J. Rob. Res., 35(6), pp. 672–694. [CrossRef]
Rameau, F. , Ha, H. , Joo, K. , Choi, J. , and Kweon, I. , 2016, “ A Real-Time Vehicular Vision System to Seamlessly See-Through Cars,” European Conference on Computer Vision, Amsterdam, The Netherlands, Oct. 8–10 and 15–16, pp. 209–222. https://link.springer.com/chapter/10.1007/978-3-319-48881-3_15
Min, H.-G. , Li, X.-C. , Sun, P.-P. , Zhao, X.-M. , and Xu, Z.-G. , 2015, “ Visual Odometry for on-Road Vehicles Based on Trifocal Tensor,” IEEE First International Smart Cities Conference, Guadalajara, Mexico, Oct. 25–28 , pp. 1–5.
Chen, Y. , Yang, G. L. , Jiang, Y. X. , and Liu, X. Y. , 2018, “ Monocular Visual Odometry Based on Trifocal Tensor Constraint Monocular,” J. Phys.: Conf. Ser., 976(1), pp. 1–6. https://iopscience.iop.org/article/10.1088/1742-6596/976/1/012002
Trawny, N. , and Roumeliotis, S. I. , 2005, “ Indirect Kalman Filter for 3D Attitude Estimation: A Tutorial for Quaternion Algebra,” Department of Computing Science and Engineering, University of Minnesota, Minneapolis, MN, Report No. 2005-002.
Weiss, S. , and Siegwart, R. , 2011, “ Real-Time Metric State Estimation for Modular Vision-Inertial Systems,” IEEE International Conference on Robotics and Automation, Shanghai, China, May 9–13, pp. 4531–4537.
Arasaratnam, I. , Haykin, S. , and Hurd, T. R. , 2010, “ Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulations,” IEEE Trans. Signal Process., 58(10), pp. 4977–4993. [CrossRef]
Bay, H. , Ess, A. , Tuytelaars, T. , and Van Gool, L. , “ Speeded-Up Robust Features (SURF),” Comput. Vision Image Understanding, 110(3), pp. 346–359. [CrossRef]
Lowe, D. G. , 2004, “ Distinctive Image Features From Scale-Invariant Key points,” Int. J. Comput. Vision, 60(2), pp. 91–110. [CrossRef]
Civera, J. , Grasa, O. G. , Davison, A. J. , and Montiel, J. M. M. , 2010, “ 1-Point RANSAC for Extended Kalman Filtering: Application to Real-Time Structure From Motion and Visual Odometry,” J. Field Rob., 27(5), pp. 609–631. [CrossRef]
Geiger, A. , Lenz, P. , Stiller, C. , and Urtasun, R. , 2013, “ Vision Meets Robotics: The Kitti Dataset,” Int. J. Rob. Res., 32(11), pp. 1231–1237. [CrossRef]
Arasaratnam, I. , “ Matlab Code Examples of Cubature Kalman Filter,” The MathWorks, Natick, MA, accessed Mar. 5, 2019, https://haranarasaratnam.com/software.html
Mur-Artal, R. , and Tardos, J. D. , 2017, “ ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras,” IEEE Trans. Rob., 33(5), pp. 1255–1262. [CrossRef]
Burri, M. , Nikolic, J. , Gohl, P. , Schneider, T. , Rehder, J. , Omari, S. , Achtelik, M. W. , and Siegwart, R. , 2016, “ The Euroc Micro Aerial Vehicle Datasets,” Int. J. Rob. Res., 35(10), pp. 1157–1163. [CrossRef]
Bloesch, M. , Burri, M. , Omari, S. , Hutter, M. , and Siegwart, R. , 2017, “ Iterated Extended Kalman Filter Based Visual-Inertial Odometry Using Direct Photometric Feedback,” Int. J. Rob. Res., 36(10), pp. 1053–1072. [CrossRef]
Maybeck, P. S. , 1979, “Stochastic Models, Estimation and Control,” Academic Press, New York.
Agarwal, S. , and Mierle, K. , 2018, “ Ceres Solver,” accessed Mar. 5, 2019, http://ceres-solver.org
Lee, D.-J. , 2008, “ Nonlinear Estimation and Multiple Sensor Fusion Using Unscented Information Filtering,” IEEE Signal Process. Lett., 15(1), pp. 861–864. [CrossRef]


Grahic Jump Location
Fig. 3

Illustration of the 3D feature-point reconstruction

Grahic Jump Location
Fig. 4

Trifocal tensor incidence relation (point-line-point) for three views I1, I2, and I3 with camera viewpoints O1, O2, and O3

Grahic Jump Location
Fig. 2

System architecture of CKF implementation

Grahic Jump Location
Fig. 7

Structure of TTG-based and 3D feature-point approach

Grahic Jump Location
Fig. 10

Cubature Kalman filter estimation presented in Google map: (a) 2011_09_26_0095, (b) 2011_09_26_0036, and (c) 2011_09_30_0033

Grahic Jump Location
Fig. 1

System coordinates

Grahic Jump Location
Fig. 5

Illustration of the TTG based approach. The arrow is point transfer using TTG while the thin line is feature-tracking pipeline.

Grahic Jump Location
Fig. 6

Example of point transfer using TTG with KITTI dataset. The point-line-point relation is established between the feature m1 in I1, the line l2 in I2 and the feature m3 in I3.

Grahic Jump Location
Fig. 8

The predicted measurements of two approaches

Grahic Jump Location
Fig. 9

Average processing time to predict one feature

Grahic Jump Location
Fig. 11

Processing time evaluation of CKF and EKF

Grahic Jump Location
Fig. 12

Experiments with EuRoC dataset

Grahic Jump Location
Fig. 13

Cubature information filter system architecture in the general case of multiple measurement updates

Grahic Jump Location
Fig. 15

Experimental results of dataset 2011_09_30_0020

Grahic Jump Location
Fig. 16

Experimental results of dataset 2011_09_30_0033

Grahic Jump Location
Fig. 17

RMSEpos evaluation of dataset 2011_09_30_0034

Grahic Jump Location
Fig. 18

RMSEpos evaluation of dataset 2011_09_30_0020

Grahic Jump Location
Fig. 19

RMSEpos evaluation of dataset 2011_09_30_0033

Grahic Jump Location
Fig. 20

Processing time evaluation of CKF and CIF

Grahic Jump Location
Fig. 14

Experimental results of dataset 2011_09_30_0034



Some tools below are only available to our subscribers or users with an online account.

Related Content

Customize your page view by dragging and repositioning the boxes below.

Related Journal Articles
Related eBook Content
Topic Collections

Sorry! You do not have access to this content. For assistance or to subscribe, please contact us:

  • TELEPHONE: 1-800-843-2763 (Toll-free in the USA)
  • EMAIL: asmedigitalcollection@asme.org
Sign In