Improved Cubature Kalman Filtering on Matrix Lie Groups Based on Intrinsic Numerical Integration Error Calibration with Application to Attitude Estimation
Abstract
:1. Introduction
 (1)
 Giving a generalized system measurement model covering measurements in both Euclidean spaces and Lie groups, and developing a Bayesian estimator by utilizing BSQMT with the generalized measurement model for the state estimation problem on Lie groups.
 (2)
 Deriving improved cubature Kalman filtering on matrix Lie groups with BSQMT to calibrate numerical integration errors, and introducing a method with the maximum likelihood principle to calculate adaptive expected model variance.
 (3)
 Applying the proposed Lie group filtering to quaternion attitude estimation problems, and providing numerical simulations to validate the effectiveness of the proposed filtering.
2. Mathematical Preliminary
2.1. Introduction to Matrix Lie Groups
2.2. Uncertainty on Matrix Lie Groups
3. Bayesian Estimation Based on Bayes–Sard Quadrature Moment Transform
3.1. Bayes–Sard Quadrature Moment Transform with Cubature Points
3.2. Bayesian Estimation on Lie Groups Using BSQMT
Algorithm 1 Bayesian estimation on Lie groups using BSQMT. 

4. Proposed Filtering Algorithm
4.1. Improved Cubature Kalman Filtering on Lie Groups with BSQMT
Algorithm 2 Bayes–Sard cubature Kalman filter (BSCKFLG). 

4.2. EMV Estimation
5. Application to Attitude Estimation
Numerical Simulations
 *
 $SO\left(3\right)$based CKF ($SO\left(3\right)$ CKF) that considers the attitude embedded in a special orthogonal group ($SO\left(3\right)$) and the gyroscope bias in a vector space;
 *
 *
 *
 Bayes–Sard quadrature cubature Kalman filter ($SO\left(3\right)$ BSCKF) derived from [11] by utilizing the same Lie group action as the $SO\left(3\right)$ CKF;
 *
 our proposed right and left BSCKFs on Lie groups (RightBSCKFLG and LeftBSCKFLG) in Section 4;
 *
 the proposed adaptive right BSCKFLG with timevarying EMVs (RightBSCKFLGadaptive).
6. Conclusions
Author Contributions
Funding
Institutional Review Board Statement
Informed Consent Statement
Data Availability Statement
Conflicts of Interest
Appendix A. Proof of Lemma 1
References
 Aligia, D.A.; Roccia, B.A.; Angelo, C.; Magallán, G.; Gonzalez, G.N. An orientation estimation strategy for low cost IMU using a nonlinear Luenberger observer. Measurement 2020, 173, 108664. [Google Scholar] [CrossRef]
 Reina, G.; Messina, A. Vehicle dynamics estimation via augmented extended Kalman filtering. Measurement 2018, 133, 383–395. [Google Scholar] [CrossRef]
 Huang, Y.; Zhang, Y.; Bo, X.; Wu, Z.; Chambers, J. A new outlierrobust student’s t based Gaussian approximate filter for cooperative localization. IEEE/ASME Trans. Mechatron. 2017, 22, 2380–2386. [Google Scholar] [CrossRef] [Green Version]
 Kalman, R. A new approach to linear filtering and prediction problems. J. Basic Eng. 1960, 82, 35–45. [Google Scholar] [CrossRef] [Green Version]
 Sunahara Y, Y.K. An approximate method of state estimation for nonlinear dynamical systems with statedependent noise. Int. J. Control 1970, 11, 957–972. [Google Scholar] [CrossRef]
 Julier, S.; Uhlmann, J.; DurrantWhyte, H.F. A new method for the nonlinear transformation of means and covariances in filters and estimators. IEEE Trans. Autom. Control 2001, 45, 477–482. [Google Scholar] [CrossRef] [Green Version]
 Arasaratnam, I.; Haykin, S.; Elliott, R.J. Discretetime nonlinear filtering algorithms using Gauss–Hermite quadrature. Proc. IEEE 2007, 95, 953–977. [Google Scholar] [CrossRef]
 Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman filtering for continuousdiscrete systems: Theory and simulations. IEEE Trans. Signal Process. 2010, 58, 4977–4993. [Google Scholar] [CrossRef]
 Prüher, J.; Šimandl, M. Bayesian quadrature in nonlinear filtering. In Proceedings of the 12th International Conference on Informatics in Control, Automation and Robotics, Colmar, France, 21–23 July 2015; pp. 380–387. [Google Scholar]
 Pruher, J.; Straka, O. Gaussian process quadrature moment transform. IEEE Trans. Autom. Control 2017, 63, 2844–2854. [Google Scholar] [CrossRef] [Green Version]
 Pruher, J.; Karvonen, T.; Oates, C.J.; Straka, O.; Sarkka, S. Improved calibration of numerical integration error in Sigmapoint filters. IEEE Trans. Autom. Control 2020, 66, 1286–1296. [Google Scholar] [CrossRef]
 Bourmaud, G.; Mégret, R.; Giremus, A.; Berthoumieu, Y. From intrinsic optimization to iterated extended Kalman filtering on Lie groups. J. Math. Imaging Vis. 2016, 55, 284–303. [Google Scholar] [CrossRef] [Green Version]
 Hauberg, S.; Lauze, F.; Pedersen, K.S. Unscented Kalman filtering on Riemannian manifolds. J. Math. Imaging Vis. 2013, 46, 103–120. [Google Scholar] [CrossRef]
 Menegaz, H.; Ishihara, J.Y.; Kussaba, H. Unscented Kalman filters for Riemannian statespace systems. IEEE Trans. Autom. Control 2019, 64, 1487–1502. [Google Scholar] [CrossRef] [Green Version]
 Brossard, M.; Barrau, A.; Bonnabel, S. A code for unscented Kalman filtering on Manifolds (UKFM). In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 31 May–31 August 2020; pp. 1487–1502. [Google Scholar]
 Brossard, M.; Condomines, J.P. Unscented Kalman filtering on Lie groups. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 24–28 September 2017; pp. 2485–2491. [Google Scholar]
 Brossard, M.; Bonnabel, S.; Barrau, A. Unscented Kalman filter on Lie groups for visual inertial odometry. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 1–5 October 2018; pp. 649–655. [Google Scholar]
 Bonnabel, S. Leftinvariant extended Kalman filter and attitude estimation. In Proceedings of the 46th IEEE Conference on Decision and Control, New Orleans, LA, USA, 12–14 December 2007; pp. 1027–1032. [Google Scholar]
 Bonnabel, S.; Martin, P.; Salaun, E. Invariant extended Kalman filter: Theory and application to a velocityaided attitude estimation problem. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) Held Jointly with 28th Chinese Control Conference, Shanghai, China, 15–18 December 2009; pp. 1297–1304. [Google Scholar]
 Barrau, A.; Bonnabel, S. Intrinsic filtering on Lie groups with applications to attitude estimation. IEEE Trans. Autom. Control 2015, 60, 436–449. [Google Scholar] [CrossRef]
 Barrau, A.; Bonnabel, S. The invariant extended Kalman filter as a stable observer. IEEE Trans. Autom. Control 2017, 62, 1797–1812. [Google Scholar] [CrossRef] [Green Version]
 Barrau, A.; Bonnabel, S. Invariant Kalman filtering. Annu. Rev. Control Robot. Auton. Syst. 2018, 1, 237–257. [Google Scholar] [CrossRef]
 Gui, H.; de Ruiter, A.H.J. Quaternion invariant extended Kalman filtering for spacecraft attitude estimation. J. Guid. Control Dyn. 2018, 41, 863–878. [Google Scholar] [CrossRef]
 Condomines, J.P.; Hattenberger, G. Nonlinear state estimation using an invariant unscented Kalman filter. In Proceedings of the AIAA Guidance, Navigation, and Control, Boston, MA, USA, 19–22 August 2013; p. 4869. [Google Scholar]
 Condomines, J.P.; Seren, C.; Hattenberger, G. Invariant unscented Kalman filter with application to attitude estimation. In Proceedings of the IEEE 56th Annual Conference on Decision and Control (CDC), Melbourne, VIC, Australia, 12–15 December 2017; pp. 2783–2788. [Google Scholar]
 Condomines, J.P.; Hattenberger, G. Invariant Unscented Kalman Filtering: A Parametric Formulation Study for Attitude Estimation. 2019. hal02072456. Available online: https://halenac.archivesouvertes.fr/hal02072456 (accessed on 4 April 2022).
 Barrau, A.; Bonnabel, S. Three examples of the stability properties of the invariant extended Kalman filter. IFACPapersOnLine 2017, 50, 431–437. [Google Scholar] [CrossRef]
 Barrau, A.; Bonnabel, S. An EKFSLAM algorithm with consistency properties. arXiv 2015, arXiv:1510.06263. [Google Scholar]
 Bourmaud, G.; Mégret, R.; Giremus, A.; Berthoumieu, Y. Discrete extended Kalman filter on lie groups. In Proceedings of the 21st European Signal Processing Conference, Marrakech, Morocco, 9–13 September 2013; pp. 1–5. [Google Scholar]
 Bourmaud, G.; Mégret, R.; Arnaudon, M.; Giremus, A. ContinuousDiscrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions. J. Math. Imaging Vis. 2015, 51, 209–228. [Google Scholar] [CrossRef] [Green Version]
 Phogat, K.S.; Chang, D.E. Invariant extended Kalman filter on matrix Lie groups. Automatica 2020, 114, 108812. [Google Scholar] [CrossRef] [Green Version]
 Phogat, K.S.; Chang, D.E. Discretetime invariant extended Kalman filter on matrix Lie groups. Int. J. Robust Nonlinear Control 2020, 30, 4449–4462. [Google Scholar] [CrossRef]
 Teng, Z.; Wu, K.; Su, D.; Huang, S.; Dissanayake, G. An InvariantEKF VINS algorithm for improving consistency. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 24–28 September 2017; pp. 1578–1585. [Google Scholar]
 Brossard, M.; Bonnabel, S.; Barrau, A. Invariant Kalman filtering for visual inertial SLAM. In Proceedings of the International Conference on Information Fusion, Cambridge, UK, 10–13 July 2018; pp. 2021–2028. [Google Scholar]
 Sjberg, A.M.; Egeland, O. Lie Algebraic unscented Kalman filter for pose estimation. IEEE Trans. Autom. Control 2022, in press. [Google Scholar] [CrossRef]
 Guo, H.; Liu, H.; Zhou, Y.; Li, J. Quaternion invariant cubature Kalman filtering for attitude estimation. In Proceedings of the 2020 3rd World Conference on Mechanical Engineering and Intelligent Manufacturing (WCMEIM), Shanghai, China, 4–6 December 2020; pp. 67–72. [Google Scholar]
 Barrau, A.; Bonnabel, S. Stochastic observers on Lie groups: A tutorial. In Proceedings of the 2018 IEEE Conference on Decision and Control (CDC), Miami, FL, USA, 17–19 December 2018; pp. 1264–1269. [Google Scholar]
 Karvonen, T.; Oates, C.J.; Srkk, S. A BayesSard cubature method. arXiv 2018, arXiv:1804.03016. [Google Scholar]
 Xiong, K.; Liu, L.; Zhang, H. Modified unscented Kalman filtering and its application in autonomous satellite navigation. Aerosp. Sci. Technol. 2009, 13, 238–246. [Google Scholar] [CrossRef]
 Hu, G.; Gao, B.; Zhong, Y.; Gu, C. Unscented kalman filter with process noise covariance estimation for vehicular ins/gps integration system. Inf. Fusio 2020, 64, 194–204. [Google Scholar] [CrossRef]
 Wang, J.; Li, M. Covariance regulation based invariant Kalman filtering for attitude estimation on matrix Lie groups. IET Control Theory Appl. 2021, 15, 2017–2025. [Google Scholar] [CrossRef]
 Nagy, S.B.; Arne, J.T.; Fossen, T.I.; Ingrid, S. Attitude estimation by multiplicative exogenous Kalman filter. Automatica 2018, 95, 347–355. [Google Scholar]
Filters  ARMEs of Roll (deg)  ARMEs of Pitch (deg)  ARMEs of Yaw (deg) 

$SO\left(3\right)$ CKF  $1.5172$  $1.5174$  $3.5564$ 
LeftCKFLG  $0.5011$  $0.8032$  $2.2217$ 
RightCKFLG  $0.4571$  $0.7298$  $1.9835$ 
LeftIEKF  $0.5103$  $0.8185$  $2.1791$ 
RightIEKF  $0.4463$  $0.7226$  $1.9114$ 
$SO\left(3\right)$ BSCKF  $0.2505$  $0.2351$  $0.7080$ 
LeftBSCKFLG  $0.2156$  $0.1701$  $0.5407$ 
RightBSCKFLG  $0.2062$  $0.1708$  $0.5277$ 
RightBSCKFLGadaptive  $0.2113$  $0.1745$  $0.4586$ 
Filters  ARMEs of Roll (deg)  ARMEs of Pitch (deg)  ARMEs of Yaw (deg) 

$SO$(3) CKF  $4.3881$  $3.2943$  $10.104$ 
LeftCKFLG  $3.7219$  $2.6936$  $8.8371$ 
RightCKFLG  $1.5143$  $1.1443$  $4.1851$ 
LeftIEKF  $4.2617$  $2.9011$  $9.2021$ 
RightIEKF  $0.7231$  $0.8164$  $2.4947$ 
$SO$(3) BSCKF  $1.9847$  $1.3373$  $5.6511$ 
LeftBSCKFLG  $1.8803$  $1.2971$  $5.4425$ 
RightBSCKFLG  $0.6037$  $0.5515$  $1.8587$ 
RightBSCKFLGadaptive  $0.5401$  $0.4604$  $1.4699$ 
Filters  ARMEs of Roll (deg)  ARMEs of Pitch (deg)  ARMEs of Yaw (deg) 

$SO$(3) CKF  $18.5315$  $2.8001$  $15.7173$ 
LeftCKFLG  $9.2549$  $1.4878$  $12.3105$ 
RightCKFLG  $15.2969$  $2.1041$  $13.0327$ 
LeftIEKF  $21.5772$  $2.6352$  $13.7434$ 
RightIEKF  $21.6621$  $2.3658$  $15.1156$ 
$SO$(3) BSCKF  $14.3881$  $2.4143$  $17.4939$ 
LeftBSCKFLG  $3.7793$  $1.2411$  $12.2423$ 
RightBSCKFLG  $8.4749$  $1.4365$  $10.6021$ 
RightBSCKFLGadaptive  $3.7641$  $0.5966$  $2.4787$ 
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. 
© 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).
Share and Cite
Guo, H.; Zhou, Y.; Liu, H.; Hu, X. Improved Cubature Kalman Filtering on Matrix Lie Groups Based on Intrinsic Numerical Integration Error Calibration with Application to Attitude Estimation. Machines 2022, 10, 265. https://doi.org/10.3390/machines10040265
Guo H, Zhou Y, Liu H, Hu X. Improved Cubature Kalman Filtering on Matrix Lie Groups Based on Intrinsic Numerical Integration Error Calibration with Application to Attitude Estimation. Machines. 2022; 10(4):265. https://doi.org/10.3390/machines10040265
Chicago/Turabian StyleGuo, Huijuan, Yan Zhou, Huiying Liu, and Xiaoxiang Hu. 2022. "Improved Cubature Kalman Filtering on Matrix Lie Groups Based on Intrinsic Numerical Integration Error Calibration with Application to Attitude Estimation" Machines 10, no. 4: 265. https://doi.org/10.3390/machines10040265