Fast Block Kalman Filter (FBKF) for Visual-Inertial Navigation Tasks
EDN: LOLQGD
Abstract
This paper presents a Fast Block Kalman Filter (FBKF) for visual-inertial navigation. The filter recursively estimates the state vector describing the navigation parameters of a moving object and the coordinates of N visual features with reduced computational complexity, O(N), achieved through the decomposition of the estimation algorithm. It is shown that through applying the principal component analysis, the estimates of the block filter remain close to those of the Extended Kalman Filter (EKF), which, as shown previously, provides high estimation accuracy when consistent error models are used. The O(N) complexity is maintained even when all N features are observed simultaneously for an arbitrary time interval. The trade-off between computational time and FBKF accuracy is achieved by using a special procedure based on expanding the original state vector; negligible deviations from the EKF estimates are obtained for expansion dimensions that have only a minor effect on the computational burden of the proposed filter. A comparison with the EKF in terms of computational time and produced estimates is carried out by simulation of a visual aided INS. The results demonstrate the possibility of processing hundreds of features in real time in single-threaded mode.
References
1. Huang, G., Visual-Inertial Navigation: A Concise Review, 2019 International Conference on Robotics and Automation (ICRA), Montreal, 2019, рр. 9572–9582, https://doi.org/10.1109/ICRA.2019.8793604.
2. Степанов О.А. Основы теории оценивания с приложениями к задачам обработки навигационной информации. Ч. 1. Введение в теорию оценивания. Изд. 4-е, испр. и доп. СПб.: ГНЦ РФ АО «Концерн «ЦНИИ «Электроприбор», 2025. 498 с.
3. Bloesch, M., Burri, M., Omari, S., Hutter, M., Siegwart, R., Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback, The International Journal of Robotics Research, 2017, vol. 36, no. 10, рр. 1053–1072, https://doi.org/10.1177/0278364917728574.
4. Wu, K., Zhang, T., Su, D., Huang, S., Dissanayake, G., An Invariant-EKF VINS Algorithm for Improving Consistency, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 2017, pp. 1578–1585, https://doi.org/10.1109/IROS.2017.8205965.
5. Степанов О.А., Исаев А.М. Методика сравнительного анализа рекуррентных алгоритмов нелинейной фильтрации в задачах обработки навигационной информации на основе предсказательного моделирования // Гироскопия и навигация. 2023. Т. 31. №3 (122). C. 48–65. EDN: MVWKGC.
6. Болотин Ю.В., Брагин А.В., Гулевский Д.В. Исследование состоятельности расширенного фильтра Калмана в задаче навигации пешехода с БИНС, закрепленными на стопах // Гироскопия и навигация. 2021. Т. 29. № 2 (113). С. 59–77. DOI: 10.17285/0869-7035.0063.
7. Gui, J., Gu, D., Wang, S., Hu, H., A Review of Visual Inertial Odometry from Filtering and Optimisation Perspectives, Advanced Robotics, 2015, vol. 29, no. 1, рр. 1–13, https://doi.org/10.1080/01691864.2015.1057616.
8. Qin, T., Li, P., Shen, S., VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, IEEE Transactions on Robotics, 2018, vol. 34, no. 4, рр. 1004–1020, https://doi.org/10.1109/TRO.2018.2853729.
9. Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., Furgale, P., Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization, The International Journal of Robotics Research, 2014, vol. 34, no. 3, рр. 314–334, https://doi.org/10.1177/0278364914554813.
10. Mourikis, A.I., Characterization and optimization of the accuracy of mobile robot localization: PhD thesis. Minneapolis: University of Minnesota, 2008. 210 p.
11. Mourikis, A.I., Roumeliotis, S.I., A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation, Proceedings 2007 IEEE International Conference on Robotics and Automation, Rome, 2007, рр. 3565–3572, https://doi.org/10.1109/ROBOT.2007.364024.
12. Huai Z., Huang, G., Robocentric visual-inertial odometry, The International Journal of Robotics Research, 2019, vol. 41, no 7, pp. 667–689, https://doi.org/10.1177/0278364919853361.
13. Kaess, M., Ranganathan, A., Dellaert, F., iSAM: Fast Incremental Smoothing and Mapping with Efficient Data Association, Proceedings 2007 IEEE International Conference on Robotics and Automation, Rome, 2007, pp. 1670–1677, https://doi.org/10.1109/ROBOT.2007.363563.
14. Kazerouni, I. A., Fitzgerald, L., Dooly, G., Toal, D., A survey of state-of-the-art on visual SLAM, Expert Systems with Applications, 2022, vol. 205, рр. 117734, https://doi.org/10.1016/j.eswa.2022.117734.
15. Barrau, A., Bonnabel, S., The Invariant Extended Kalman Filter as a Stable Observer, IEEE Transactions on Automatic Control, 2017, vol. 62, no. 4, рр. 1797–1812, https://doi.org/10.1109/TAC.2016.2594085.
16. Carlson, N.A., Federated square root filter for decentralized parallel processors, IEEE Transactions on Aerospace and Electronic Systems, 1990, vol. 26, no. 3, рр. 517–525, https://doi.org/10.1109/7.106130.
17. Paik, B.S., Oh, J.H., Gain fusion algorithm for decentralised parallel Kalman filters, IEE Proceedings – Control Theory and Applications, 2000, vol. 147, no. 1, рр. 97–103, https://doi.org/10.1049/ip-cta:20000141.
18. Roy, S., Hashemi, R.H., Laub, A.J., Square root parallel Kalman filtering using reduced-order local filters, IEEE Transactions on Aerospace and Electronic Systems, 1991, vol. 27, no. 2, рр. 276–289, https://doi.org/10.1109/7.78303.
19. Stepanov, O., Litvinenko, Y., Research of Nonrecursive Federated Filtering Algorithms under Non-White Noise Measurement Errors, Ronzhin, A., Truong, XT., Meshcheryakov, R. (eds) Interactive Collaborative Robotics, ICR 2025, Lecture Notes in Computer Science, 2026, vol. 16304, Springer, Cham., https://doi.org/10.1007/978-3-032-11903-2_23.
20. Holmes, M., Introduction to Scientific Computing and Data Analysis, Cham: Springer, 2016, 440 p., https://doi.org/10.1007/978-3-319-30256-0.
21. Циоплиакис Н.И. Блочный фильтр Калмана с линейной вычислительной сложностью для комплексированных с техническим зрением инерциальных навигационных систем // Вестник ЮУрГУ. Сер.: Компьютерные технологии, управление, радиоэлектроника. 2024. Т. 24. № 4. С. 43–56. DOI: 10.14529/ctcr240404.
22. Civera, J., Davison, A.J., Montiel, J.M.M., Inverse Depth Parametrization for Monocular SLAM, IEEE Transactions on Robotics, 2008, vol. 24, no. 5, рр. 932–945, https://doi.org/10.1109/TRO.2008.2003276.
23. Hesch, J.A., Kottas, D.G., Bowman, S.L., Roumeliotis, S.I., Observability-Constrained Vision-Aided Inertial Navigation, University of Minnesota, Dept. of Comp. Sci. & Eng., MARS Lab., Tech. Rep., 2012, vol. 1.
24. Цыганова Ю.В., Куликова М.В. О современных ортогонализованных алгоритмах оптимальной дискретной фильтрации // Вестник ЮУрГУ. Сер.: Математическое моделирование и программирование. 2018. Т. 11. № 4. С. 5–30. DOI: 10.14529/mmp180401.
25. Афанасьев В.Н. Стохастические системы. Оценки и управление. 1-е изд. М.: ЛЕНАНД, 2018. 152 с.
26. Адамов Б.И., Маслов А.Н., Осадченко Н.В. Применение основных матричных разложений в задачах механики и робототехники. М.: МЭИ, 2019.
27. Shi, J., Tomasi, C., Good Features to Track, IEEE Conference on Computer Vision and Pattern Recognition, Seattle, 1994, рр. 593–600, https://doi.org/10.1109/CVPR.1994.323794.
28. Kalal, Z., Mikolajczyk, K., Matas, J., Forward-Backward Error: Automatic Detection of Tracking Failures, 20th International Conference on Pattern Recognition, Istanbul, 2010, рр. 2756–2759, https://doi.org/10.1109/ICPR.2010.675.
29. Rublee, E., Rabaud, V., Konolige, K., Bradski, G.R., ORB: An efficient alternative to SIFT or SURF, IEEE International Conference on Computer Vision (ICCV), Barcelona, 2011, рр. 2564–2571, https://doi.org/10.1109/ICCV.2011.6126544.
Review
For citations:
Tsiopliakis N. Fast Block Kalman Filter (FBKF) for Visual-Inertial Navigation Tasks. Giroskopiya i Navigatsiya. 2026;34(1):71-95. (In Russ.) EDN: LOLQGD
Views:
168
JATS XML
Email this article 