Constrained Cubature Particle Filter for Vehicle Navigation
Abstract
1. Introduction
2. Cubature Particle Filter
3. Constrained Cubature Particle Filter
3.1. Importance Sampling
3.2. Resampling
3.3. Convergence Analysis
4. Experimental Results
4.1. GNSS/DR Vehicle Navigation System
4.2. Experimental Setup
5. Conclusions
Author Contributions
Funding
Institutional Review Board Statement
Informed Consent Statement
Data Availability Statement
Conflicts of Interest
References
- Wang, W.; Xing, C.Y.; Feng, S. State of the art and perspectives of autonomous navigation technology. Acta Aeronaut. Astronaut. Sin. 2021, 42, 525049. [Google Scholar] [CrossRef]
- Zhao, X.; Min, H.; Xu, Z.; Wang, W. An ISVD and SFFSD-based vehicle ego-positioning method and its application on indoor parking guidance. Transp. Res. Part C Emerg. Technol. 2019, 108, 29–48. [Google Scholar] [CrossRef]
- Yuan, Y.; Li, F.; Chen, J.; Wang, Y.; Liu, K. An improved Kalman filter algorithm for tightly GNSS/INS integrated navigation system. Math. Biosci. Eng. 2024, 21, 963–983. [Google Scholar] [CrossRef] [PubMed]
- Brossard, M.; Barrau, A.; Bonnabel, S. AI-IMU dead reckoning. IEEE Trans. Intell. Veh. 2020, 5, 585–595. [Google Scholar] [CrossRef]
- Hu, G.; Ni, L.; Gao, B.; Zhu, X.; Wang, W.; Zhong, Y. Model predictive based unscented Kalman filter for hypersonic vehicle navigation with INS/GNSS integration. IEEE Access 2020, 8, 4814–4823. [Google Scholar] [CrossRef]
- Min, H.; Wu, X.; Cheng, C.; Zhao, X. Kinematic and dynamic vehicle model-assisted global positioning method for autonomous vehicles with low-cost GPS/camera/in-vehicle sensors. Sensors 2019, 19, 5430. [Google Scholar] [CrossRef] [PubMed]
- Wang, W.; Min, H.; Wu, X.; Hou, X.; Li, Y.; Zhao, X. High accuracy and low complexity LiDAR place recognition using unitary invariant frobenius norm. IEEE Sens. J. 2022, 23, 11205–11207. [Google Scholar] [CrossRef]
- Gustafsson, F.; Hendeby, G. Some relations between extended and unscented Kalman filters. IEEE Trans. Signal Process. 2012, 60, 545–555. [Google Scholar] [CrossRef]
- Stepanov, O.A.; Litvinenko, Y.A.; Vasiliev, V.A.; Toropov, A.B.; Basin, M.V. Polynomial filtering algorithm applied to navigation data processing under quadratic nonlinearities in system and measurement equations. Part 1. description and comparison with Kalman type algorithms. Gyroscopy Navig. 2021, 12, 205–223. [Google Scholar] [CrossRef]
- Gao, B.; Gao, S.; Hu, G.; Zhong, Y.; Gu, C. Maximum likelihood principle and moving horizon estimation based adaptive unscented Kalman filter. Aerosp. Sci. Technol. 2018, 73, 184–196. [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. Fusion 2020, 64, 194–204. [Google Scholar] [CrossRef]
- Gao, B.; Hu, G.; Zhang, L.; Zhong, Y.; Zhu, X. Cubature Kalman filter with closed-loop covariance feedback control for integrated INS/GNSS navigation. Chin. J. Aeronaut. 2023, 36, 363–376. [Google Scholar] [CrossRef]
- Gao, B.; Li, W.; Hu, G.; Zhong, Y.; Zhu, X. Mahalanobis distance-based fading cubature Kalman filter with augmented mechanism for hypersonic vehicle INS/CNS autonomous integration. Chin. J. Aeronaut. 2022, 35, 114–128. [Google Scholar] [CrossRef]
- Kulikov, G.Y.; Kulikova, M.V. Stability analysis of extended, cubature and unscented Kalman filters for estimating stiff continuous-discrete stochastic systems. Automatica 2018, 90, 91–97. [Google Scholar] [CrossRef]
- Wei, W.; Gao, S.; Zhong, Y.; Gu, C.; Hu, G. Adaptive square-root unscented particle filtering algorithm for dynamic navigation. Sensors 2018, 18, 2337. [Google Scholar] [CrossRef] [PubMed]
- Speekenbrink, M. A tutorial on particle filters. J. Math. Psychol. 2016, 73, 140–152. [Google Scholar] [CrossRef]
- Closas, P.; Guillamon, A. Sequential estimation of intrinsic activity and synaptic input in single neurons by particle filtering with optimal importance density. EURASIP J. Adv. Signal Process. 2017, 2017, 65. [Google Scholar] [CrossRef]
- Gordon, N.J.; Salmond, D.J.; Smith, A.F.M. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proc. F Radar Signal Process. 1993, 140, 107–113. [Google Scholar] [CrossRef]
- Del Moral, P.; Doucet, A.; Jasra, A. On adaptive resampling strategies for sequential Monte Carlo methods. Bernoulli 2012, 18, 252–278. [Google Scholar] [CrossRef]
- Jia, K.; Pei, Y.; Gao, Z.; Zhong, Y.; Gao, S.; Wei, W.; Hu, G. A quaternion-based robust adaptive spherical simplex unscented particle filter for MINS/VNS/GNS integrated navigation system. Math. Probl. Eng. 2019, 2019, 8532601. [Google Scholar] [CrossRef]
- Pitt, M.; Shephard, N. Filtering via simulation: Auxiliary particle filters. J. Am. Stat. Assoc. 1999, 94, 590–599. [Google Scholar] [CrossRef]
- Liu, D.; Duan, J.; Shi, H. A strong tracking square root central difference FastSLAM for unmanned intelligent vehicle with adaptive partial systematic resampling. IEEE Trans. Intell. Transp. Syst. 2016, 17, 3110–3120. [Google Scholar] [CrossRef]
- Liu, H.T.; Lin, Y.M.; Chen, Y.H.; Zhou, E.M.; Peng, B. A study on resampling strategy of intelligent particle filter based on genetic algorithm. J. Electron. Inf. Technol. 2021, 43, 3459–3466. [Google Scholar] [CrossRef]
- Xia, B.; Sun, Z.; Zhang, R.; Lao, Z. A cubature particle filter algorithm to estimate the state of the charge of lithium-ion batteries based on a second-order equivalent circuit model. Energies 2017, 10, 457. [Google Scholar] [CrossRef]
- Shi, Q.; Liu, M.; Hang, L. A novel distribution system state estimator based on robust cubature particle filter used for non-Gaussian noise and bad data scenarios. IET Gener. Transm. Distrib. 2022, 16, 1385–1399. [Google Scholar] [CrossRef]
- Feng, H.; Cai, Z. Target tracking based on improved cubature particle filter in UWSNs. IET Radar Sonar Navig. 2019, 13, 638–645. [Google Scholar] [CrossRef]
- Zhang, Y.G.; Cheng, R.; Huang, Y.L.; Li, N. Truncated adaptive cubature particle filter. Syst. Eng. Electron. 2016, 38, 382–391. [Google Scholar] [CrossRef]
- Liu, M.; He, M.; Qiao, S.; Liu, B.; Cao, Z.; Wang, R. A high-order state-of-charge estimation model by cubature particle filter. Measurement 2019, 146, 35–42. [Google Scholar] [CrossRef]
- Xing, D.; Wei, M.; Zhao, W.; Wang, Y.; Wu, S. Vehicle state estimation based on adaptive cubature particle filtering. J. Nanjing Univ. Aeronaut. Astronaut. 2020, 52, 445–453. [Google Scholar] [CrossRef]
- Gao, Z.; Mu, D.; Zhong, Y.; Gu, C. Constrained Unscented Particle Filter for SINS/GNSS/ADS Integrated Airship Navigation in the Presence of Wind Field Disturbance. Sensors 2019, 19, 471. [Google Scholar] [CrossRef]
- Seifzadeh, S.; Khaleghi, B.; Karray, F. Distributed soft-data-constrained multi-model particle filter. IEEE Trans. Cybern. 2015, 45, 384–394. [Google Scholar] [CrossRef]
- Xu, C.H.; Wang, X.; Duan, S.; Wan, J. Spatial-temporal constrained particle filter for cooperative target tracking. J. Netw. Comput. Appl. 2021, 176, 102913. [Google Scholar] [CrossRef]
- Zhang, H.; Li, L.; Xie, W. Constrained multiple model particle filtering for bearings-only maneuvering target tracking. IEEE Access 2018, 6, 51721–51734. [Google Scholar] [CrossRef]
- Xu, A.; Zou, X.; Qing, Y.; Fang, Q.; Sui, X. The improved cubature Kalman filter in GNSS/INS tightly coupled mode. Sci. Surv. Mapp. 2022, 47, 22–28. [Google Scholar] [CrossRef]
- Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman filtering for continuous discrete systems: Theory and simulations. IEEE Trans. Signal Process. 2010, 58, 4977–4993. [Google Scholar] [CrossRef]
- Arasaratnam, I.; Haykin, S. Cubature Kalman filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [Google Scholar] [CrossRef]
- Gao, B.; Hu, G.; Zhu, X.; Zhong, Y. Robust cubature Kalman filter with abnormal observations identification using Mahalanobis distance criterion for vehicular INS/GNSS integration. Sensors 2019, 19, 5149. [Google Scholar] [CrossRef]
- Andrieu, C.; Doucet, A.; Holenstein, R. Particle markov chain Monte Carlo methods. J. R. Stat. Soc. Ser. B 2010, 72, 269–342. [Google Scholar] [CrossRef]
- Chen, J.G.; Li, J.; Gao, X.B. Particle filtering with equality state constraints. J. Univ. Electron. Sci. Technol. China 2011, 40, 596–601. [Google Scholar] [CrossRef]
- Simon, D.; Tien, L.C. Kalman filtering with state equality constraints. IEEE Trans. Aerosp. Electron. Syst. 2002, 38, 128–136. [Google Scholar] [CrossRef]
- Cui, P.Y.; Zheng, L.F.; Pei, F.J. Research on integrated navigation system based on self-adjust particle filter. Comput. Eng. 2008, 34, 185–187. [Google Scholar] [CrossRef]
- Crisan, D.; Doucet, A. A survey of convergence results on particle filtering methods for practitioners. IEEE Trans. Signal Process. 2002, 50, 736–746. [Google Scholar] [CrossRef]
- Hu, X.; Schön, T.B.; Ljung, L. A basic convergence result for particle filtering. IEEE Trans. Signal Process. 2008, 56, 1337–1348. [Google Scholar] [CrossRef]
- Fang, J.C.; Li, X.E.; Shen, G.X.; Fang, J.G.; Wang, Q.; Wan, D. Study of GPS/DR integrated navigation system for urban vehicle. China J. Highw. Transp. 1999, 12, 84–89. [Google Scholar]
- Zhao, Y.; Wang, N.; Ye, J.K. Constraints unscented particle filter and its application in vehicle navigation. J. Chang. Univ. Nat. Sci. Ed. 2020, 40, 109–116. [Google Scholar]
Filter | RMSE in East (m) | RMSE in North (m) | Error Range (m) | Overall RMSE (m) |
---|---|---|---|---|
PF | 4.1719 | 4.7618 | (−13.1, 14.0), (−14.5, 16.1) | 6.3308 |
CPF | 3.7231 | 3.7504 | (−13.0, 11.1), (−13.0, 12.1) | 5.2846 |
CCPF | 2.7061 | 2.5168 | (−8.1, 9.6), (−7.0, 7.1) | 3.6956 |
Filter | RMSE in East (m/s) | RMSE in North (m/s) | Error Range (m/s) | Overall RMSE (m/s) |
---|---|---|---|---|
PF | 0.0912 | 0.1023 | (−0.24, 0.22), (−0.25, 0.21) | 0.1371 |
CPF | 0.0892 | 0.0887 | (−0.11, 0.16), (−0.18, 0.19) | 0.1258 |
CCPF | 0.0769 | 0.0674 | (−0.14, 0.15), (−0.10, 0.15) | 0.1022 |
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content. |
© 2024 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
Xue, L.; Zhong, Y.; Han, Y. Constrained Cubature Particle Filter for Vehicle Navigation. Sensors 2024, 24, 1228. https://doi.org/10.3390/s24041228
Xue L, Zhong Y, Han Y. Constrained Cubature Particle Filter for Vehicle Navigation. Sensors. 2024; 24(4):1228. https://doi.org/10.3390/s24041228
Chicago/Turabian StyleXue, Li, Yongmin Zhong, and Yulan Han. 2024. "Constrained Cubature Particle Filter for Vehicle Navigation" Sensors 24, no. 4: 1228. https://doi.org/10.3390/s24041228
APA StyleXue, L., Zhong, Y., & Han, Y. (2024). Constrained Cubature Particle Filter for Vehicle Navigation. Sensors, 24(4), 1228. https://doi.org/10.3390/s24041228