Skip to main content
SpringerLink
Log in

Unscented Kalman filter and control on \(\mathsf {TSE(3)}\) with application to spacecraft dynamics

  • Original Paper
  • Published:
Nonlinear Dynamics Aims and scope Submit manuscript

Abstract

This paper presents a novel rigid-body navigation and control architecture within the framework of special Euclidean group \(\mathsf {SE(3)}\) and its tangent bundle \(\mathsf {TSE(3)}\) while considering stochastic processes in the system. The proposed framework combines the orbit-attitude motions of the rigid body into a single, compact set. The stochastic state filter is designed based on the unscented Kalman filter (UKF) which uses a special retraction function to encode the sigma points onto the manifold. The navigation system is then integrated and evaluated with two different control techniques on \(\mathsf {TSE(3)}\): An almost globally asymptotically stabilizing Morse–Lyapunov-based control system with backstepping and a robust sliding mode-based control system. Also, the performance of the UKF in \(\mathsf {TSE(3)}\) proposed here is compared with similar filters in the literature to demonstrate the robustness and accuracy of the proposed filter in a realistic setting. Numerical simulations are conducted to demonstrate the effectiveness of the proposed navigation filter for the full state estimation. In addition, the navigation and control systems are tested in the nonlinear gravity field of a small celestial body with an irregular shape. In particular, the performance of the closed-loop systems is studied in a tracking problem of spacecraft motion near the asteroid Bennu based on OSIRIS-REx mission data.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8

Similar content being viewed by others

Code availability

This project was coded in Matlab using Simulink, and may be made available upon request.

References

  1. Pollock, K.R.: An analysis of orbital propagators for low earth orbit rendezvous. In: Naval Postgraduate School, Monterey, CA., Provided by the SAO/NASA Astrophysics Data System, vol. OMB, no. 0704-0188 (1994)

  2. Shuster, S.P.: A survey and performance analysis of orbit propagators for LEO, GEO, and highly elliptical orbits. Utah State University, vol. OMB, no. 0704-0188 (2017)

  3. Vittaldev, V., Mooij, E., Naeije, M.C.: Unified state model theory and application in astrodynamics. Celest. Mech. Dyn. Astron. 112(3), 253–282 (2012). https://doi.org/10.1007/s10569-011-9396-5

    Article  MathSciNet  Google Scholar 

  4. Lefferts, E.J., Markley, F.L., Shuster, M.D.: Kalman filtering for spacecraft attitude estimation. J. Guid. Control Dyn. 5(5) (1982). https://doi.org/10.2514/3.56190.

  5. Carpenter, J.R., D’Souza, C.N.: Navigation filter best practices. NASA Technical Reports (2018). WBS: 869021.03.04.01.03

  6. Mayhew, C.G., Sanfelice, R.G., Teel, A.R.: On quaternion-based attitude control and the unwinding phenomenon. In: Proceedings of the 2011 American Control Conference, pp. 299–304 (2011). https://doi.org/10.1109/ACC.2011.5991127

  7. Yang, J., Stoll, E.: Adaptive sliding mode control for spacecraft proximity operations based on dual quaternions. J. Guid. Control Dyn. 42(11), 2356–2368 (2019). https://doi.org/10.2514/1.G004435

    Article  Google Scholar 

  8. Crassidis, J.L., Markley, F.L.: Unscented filtering for spacecraft attitude estimation. J. Guid. Control Dyn. 26(4), 536–542 (2003). https://doi.org/10.2514/2.5102

    Article  Google Scholar 

  9. Crassidis, J.L., Markley, F.L., Cheng, Y.: Survey of nonlinear attitude estimation methods. J. Guid. Control Dyn. 30(1), 12–28 (2007). https://doi.org/10.2514/1.22452

    Article  Google Scholar 

  10. Wen, J.T.-Y., Kreutz-Delgado, K.: The attitude control problem. IEEE Trans. Automat. Control 36(10), 1148–1162 (1991). https://doi.org/10.1109/9.90228

    Article  MathSciNet  MATH  Google Scholar 

  11. Bhat, S.P., Bernstein, D.S.: A topological obstruction to continuous global stabilization of rotational motion and the unwinding phenomenon. Syst. Control Lett. 39(1), 63–70 (2000). https://doi.org/10.1016/S0167-6911(99)00090-0

    Article  MathSciNet  MATH  Google Scholar 

  12. Bohn, J., Sanyal, A.K.: Almost global finite-time stabilization of rigid body attitude dynamics using rotation matrices. Int. J. Robust Nonlinear Control 25(4) (2015). https://doi.org/10.1002/rnc.3399

  13. Sy, L., Lovell, N.H., Redmond, S.J.: Estimating lower limb kinematics using a Lie group constrained EKF and a reduced wearable IMU count. In: 8th IEEE RAS/EMBS International Conference for Biomedical Robotics and Biomechatronics (BioRob), pp. 310–315 (2020). https://doi.org/10.1109/BioRob49111.2020.9224342

  14. Malladi, B.P., Di Cairano, S., Weiss, A.: Nonlinear model predictive control of coupled rotational-translational spacecraft relative motion. In: 2019 American Control Conference (ACC), pp. 3581–3586 (2019). https://doi.org/10.23919/ACC.2019.8814345

  15. Filipe, N., Tsiotras, P.: Rigid body motion tracking without linear and angular velocity feedback using dual quaternions. In: 2013 European Control Conference (ECC), pp. 329–334 (2013). https://doi.org/10.23919/ECC.2013.6669564

  16. Markley, F.L., Junkins, J.L.: Fundamentals of Spacecraft Attitude Determination and Control. Springer, Berlin (2014)

    Book  Google Scholar 

  17. Lee, D., Vukovich, G.: Robust adaptive terminal sliding mode control on se(3) for autonomous spacecraft rendezvous and docking. Nonlinear Dyn. 83, 2263–2279 (2016). https://doi.org/10.1007/s11071-015-2479-1

    Article  MathSciNet  MATH  Google Scholar 

  18. Nazari, M., Maadani, M., Butcher, E.A., Yucelen, T.: Morse–Lyapunov-based control of rigid body motion on TSE(3) via backstepping. In: AIAA Guidance, Navigation, and Control Conference 2018 (2018). https://doi.org/10.2514/6.2018-0602

  19. Seo, D., Nazari, M.: Rigid body adaptive stabilization on the tangent bundle of the Lie groups. In: AIAA Scitech 2019 Forum (2019). https://doi.org/10.2514/6.2019-0653

  20. Butcher, E.A., Wang, J., Lovell, T.A.: On Kalman filtering and observability in nonlinear sequential relative orbit estimation. J. Guid. Control Dyn. 40(9) (2017). https://doi.org/10.2514/1.G002702

  21. Woolsey, C.A.: Reduced Hamiltonian dynamics for a rigid body coupled to a moving point mass. J. Guid. Control Dyn. 28(1) (2005). https://doi.org/10.2514/1.54099

  22. Lam, Q., Stamatakos, N., Woodruff, C., Ashton, S.: Gyro modeling and estimation of its random noise sources

  23. Nirmal, K., Sreejith, A.G., Mathew, J., Sarpotdar, M., Suresh, A., Prakash, A., Safonova, M., Murthy, J.: Noise modeling and analysis of an IMU-based attitude sensor: improvement of performance by filtering and sensor fusion. In: Navarro, R., Burge, J.H. (eds.) Advances in Optical and Mechanical Technologies for Telescopes and Instrumentation II, vol. 9912, pp. 2138–2147. International Society for Optics and Photonics, SPIE (2016). https://doi.org/10.1117/12.2234255

  24. Lee, D., Sanyal, A.K., Butcher, E.A.: Asymptotic tracking control for spacecraft formation flying with decentralized collision avoidance. J. Guid. Control Dyn. 38(4), 587–600 (2015). https://doi.org/10.2514/1.G000101

    Article  Google Scholar 

  25. Lee, D., Vukovich, G.: Robust adaptive terminal sliding mode control on SE(3) for autonomous spacecraft rendezvous and docking. Nonlinear Dyn. (2016). https://doi.org/10.1007/s11071-015-2479-1

    Article  MathSciNet  MATH  Google Scholar 

  26. Lee, D., Sanyal, A.K., Butcher, E.A., Scheeres, D.J.: Finite-time control for spacecraft body-fixed hovering over an asteroid. IEEE Trans. Aerosp. Electron. Syst. 51(1), 506–520 (2015). https://doi.org/10.1109/TAES.2014.140197

    Article  Google Scholar 

  27. Sjøberg, A., Egeland, O.: An EKF for Lie groups with application to crane load dynamics. Model. Identif. Control Nor. Res. Bull. 40, 109–124 (2019). https://doi.org/10.4173/mic.2019.2.3

    Article  Google Scholar 

  28. Heo, S., Park, C.G.: Consistent EKF-based visual-inertial odometry on matrix Lie group. IEEE Sens. J. 18(9), 3780–3788 (2018). https://doi.org/10.1109/JSEN.2018.2808330

    Article  Google Scholar 

  29. Bourmaud, G., Megret, R., Giremus, A., Berthoumieu, Y.: Discrete extended Kalman filter on Lie groups. In: 21st European Signal Processing Conference (2013)

  30. Bourmaud, G., Megret, R., Arnauden, M., Giremus, A.: Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions. J. Math. Imaging Vis. 51, 209–228 (2015). https://doi.org/10.1007/s10851-014-0517-0

    Article  MathSciNet  MATH  Google Scholar 

  31. Brossard, M., Bonnabel, S., Condomines, J.: Unscented Kalman filtering on Lie groups. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2017). https://doi.org/10.1109/IROS.2017.8206066

  32. Brossard, M., Bonnabel, S., Barrau, A.: Unscented Kalman filter on Lie groups for visual inertial odometry. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2018). https://doi.org/10.1109/IROS.2018.8593627

  33. Loianno, G., Watterson, M., Kumar, V.: Visual inertial odometry for quadrotors on \({\sf SE(3)}\). In: IEEE International Conference on Robotics and Automation (ICRA) (2016). https://doi.org/10.1109/ICRA.2016.7487292

  34. Wan, E.A., Van Der Merwe, R.: The unscented Kalman filter for nonlinear estimation. In: Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (2000). https://doi.org/10.1109/ASSPCC.2000.882463

  35. Barfoot, T.D., Furgale, P.T.: Associating uncertainty with three-dimensional poses for use in estimation problems. IEEE Trans. Robotics 30(3), 679–693 (2014). https://doi.org/10.1109/TRO.2014.2298059

    Article  Google Scholar 

  36. Absil, P.-A., Mahony, R., Sepulchre, R.: Optimization Algorithms on Matrix Manifolds. Princeton University Press, Princeton (2008). ISBN: 978-0-691-13298-3

  37. Brossard, M., Barrau, A., Bonnabel, S.: A code for unscented Kalman filtering on manifolds (UKF-M). In: International Conference on Robotics and Automation (ICRA) (2020). arXiv:2002.00878

  38. Misra, G., Sanyal, A.K.: Analysis of orbit-attitude coupling of spacecraft near small solar system bodies. In: AIAA Guidance, Navigation, and Control Conference

  39. Wittal, M., Mangiacapra, G., Appakonam, A., Nazari, M., Capello, E.: AAS/AIAA Astrodynamics Specialist Conference, pp. AAS 20–690 (2020). https://doi.org/10.13140/RG.2.2.21502.82240

  40. Muller, A., Terze, Z.: The significance of the configuration space Lie group for the constraint satisfaction in numerical time integration of multi body systems. Mech. Mach. Theory 82, 173–202 (2014). https://doi.org/10.1016/j.mechmachtheory.2014.06.014

    Article  Google Scholar 

  41. Lee, T., McClamroch, H., Leok, M.: Optimal attitude control for a rigid body with symmetry. In: Proceedings of the American Control Conference (2007). https://doi.org/10.1109/ACC.2007.4282362

  42. Sola, J., Deray, J., Atchuthan, D.: A micro Lie theory for state estimation in robotics. arXiv preprint arXiv: 1812.01537 (2018)

  43. Gallier, J.: Basics of classical Lie groups: the exponential map, Lie groups, and Lie algebras. Texts Appl. Math. Geom. Methods Appl. 38, 367–414 (2001). https://doi.org/10.1007/978-1-4613-0137-0_14

    Article  MathSciNet  Google Scholar 

  44. Murray, R.M., Li, Z., Sastry, S.S.: A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton (1994)

    MATH  Google Scholar 

  45. Nazari, M., Butcher, E.A., Yucelen, T., Sanyal, A.: Decentralized consensus control of a rigid-body spacecraft formation with communication delay. J. Guid. Control Dyn. (2016). https://doi.org/10.2514/1.G001396

    Article  Google Scholar 

  46. Hajri, H., Said, S., Berthoumieu, Y.: Maximum likelihood estimators on manifolds. In: Geometric Science of Information, pp. 692–700 (2017). https://doi.org/10.1007/978-3-319-68445-1_80

  47. Fletcher, P.T., Joshi, C.L.S., Pizer, S.: Gaussian distributions on lie groups and their application to statistical shape analysis. In: Information Processing in Medical Imaging, pp. 450–462 (2003). https://doi.org/10.1007/978-3-540-45087-0_38

  48. Falorsi, L., de Haan, P., Davidson, T.R., Forré, P.: Reparameterizing distributions on Lie groups. In: Chaudhuri, K., Sugiyama, M. (eds.) Proceedings of the Twenty-Second International Conference on Artificial Intelligence and Statistics, vol. 89 of Proceedings of Machine Learning Research, pp. 3244–3253 (2019)

  49. Vandyke, M., Schwartz, J., Hall, C.: Unscented Kalman Filtering for spacecraft attitude state and parameter estimation. Adv. Astronaut. Sci. 119(01) (2004)

  50. Perea, L., How, J., Breger, L., Elosegui, P.: Nonlinearity in Sensor Fusion: Divergence Issues in EKF, Modified Truncated GSF, and UKF

  51. Mohammed, M., Boussadia, H., Bellar, A., Adnane, A.: Performance comparison of attitude determination, attitude estimation, and nonlinear observers algorithms. J. Phys. Conf. Ser. 783(01), 012017 (2017). https://doi.org/10.1088/1742-6596/783/1/012017

    Article  Google Scholar 

  52. Tønne, K.K.: Stability analysis of EKF-based attitude determination and control (2007)

  53. Samiei, E., Nazari, M., Butcher, E.A., Sanyal, A.K.: Robust stochastic stabilization of attitude motion. Int. J. Dyn. Control 7, 619–635 (2019). https://doi.org/10.1007/s40435-018-0456-5

    Article  MathSciNet  Google Scholar 

  54. Ghasemi, K., Ghaisari, J., Abdollahi, F.: Robust formation control of multiagent systems on the lie group se(3). Int. J. Robust Nonlinear Control 30, 1–33 (2019). https://doi.org/10.1002/rnc.4806

    Article  MathSciNet  MATH  Google Scholar 

  55. Chalanga, A., Kamal, S., Fridman, L.M., Bandyopadhyay, B., Moreno, J.A.: Implementation of super-twisting control: super-twisting and higher order sliding-mode observer-based approaches. IEEE Trans. Ind. Electron. 63(6), 3677–3685 (2016). https://doi.org/10.1109/TIE.2016.2523913

    Article  Google Scholar 

  56. Eker, İ: Sliding mode control with PID sliding surface and experimental application to an electromechanical plant. ISA Trans. 45(1), 109–118 (2006). https://doi.org/10.1016/S0019-0578(07)60070-6

    Article  Google Scholar 

  57. Lee, T., Leok, M., McClamroch, N.H.: Lie group variational integrators for the full body problem. Comput. Methods Appl. Mech. Eng. 196(8) (2005). https://doi.org/10.1016/j.cma.2007.01.017

  58. Kane, C., Marsden, J., Ortiz, M.: Symplectic-energy-momentum preserving variational integrators. J. Math. Phys. 40, 3353–3371 (1999). https://doi.org/10.1063/1.532892

    Article  MathSciNet  MATH  Google Scholar 

  59. Scheeres, D.J.: Orbital Motion in Strongly Perturbed Environments. Springer, Berlin (2012)

    Book  Google Scholar 

  60. Kaula, W.M.: Theory of Satellite Geodesy. Blaisdell, Bosto (1966). ISBN-13: 978-0486414652. ISBN-10: 0486414655

  61. Scheeres, D.J., Ostro, S.J., Hudson, R.S., DeJong, E.M., Suzuki, S.: Dynamics of orbits close to asteroid 4179 Toutatis. Icarus 132(1), 53–79 (1998). https://doi.org/10.1006/icar.1997.5870

    Article  Google Scholar 

  62. Nazari, M., Wauson, R., Critz, T., Butcher, E.A., Scheeres, D.J.: Observer-based body-frame hovering control over a tumbling asteroid. Acta Astronaut. 102, 124–139 (2014). https://doi.org/10.1016/j.actaastro.2014.05.016

  63. Misra, G., Sanyal, A., Scheeres, D.J.: Coupled orbit-attitude dynamics and relative state estimation of spacecraft near small solar system bodies. Adv. Space Res. 57(8), 1747–1761 (2015). https://doi.org/10.1016/j.asr.2015.05.023

  64. Bierhaus, E.B. et al.: The OSIRIS-REx spacecraft and the touch-and-go sample acquisition mechanism (TAGSAM). Space Sci. Rev. 214(107) (2018). https://doi.org/10.1007/s11214-018-0521-6

  65. Beshore, E. et al.: The OSIRIS-REx asteroid sample return mission. In: IEEE Aerospace Conference Proceedings (2015). https://doi.org/10.1109/AERO.2015.7118989

  66. Blau, P.: Spaceflight101.com (2020). https://spaceflight101.com/osiris-rex/osiris-rex-spacecraft-overview/. Accessed January 12, 2021

  67. Dani Hauf, L.M.: OSIRIS-REx, Discovering the Origins of the Solar System (2020). https://www.lockheedmartin.com/en-us/products/osiris-rex.html. Accessed January 12, 2021

  68. Melton, R.G.: Fundamentals of astrodynamics and applications. J. Guid. Control Dyn. 21(4), 672–672 (1998). https://doi.org/10.2514/2.4291

    Article  Google Scholar 

  69. Wie, B.: Space Vehicle Dynamics and Control, 2nd edn. American Institute of Aeronautics and Astronautics (2008). ISBN: 978-1-56347-953-3

  70. Havlík, J., Straka, O.: Performance evaluation of iterated extended Kalman filter with variable step-length. J. Phys. Conf. Ser. 659, 12–22 (2015). https://doi.org/10.1088/1742-6596/659/1/012022

    Article  Google Scholar 

  71. Miletović, I., Pool, D.M., Stroosma, O., Paassen, M.M.V., Chu, Q.: Improved Stewart platform state estimation using inertial and actuator position measurements. Control Eng. Pract. 62, 102–115 (2017). https://doi.org/10.1016/j.conengprac.2017.03.006

    Article  Google Scholar 

  72. Moor, J.B., Anderson, B.D.O.: Optimal Filtering. Information and System Sciences Series, Prentice Hall (1979)

  73. Markley, F.L., Crassidis, J.L.: Correction to: Fundamentals of Spacecraft Attitude Determination and Control. Springer, New York (2014). ISBN: 978-1-4939-0802-8

  74. Lee, K.W., Singh, S.N.: Adaptive and supertwisting adaptive spacecraft orbit control around asteroids. J. Aerosp. Eng. 32(4), 04019042 (2019). https://doi.org/10.1061/(ASCE)AS.1943-5525.0001043

    Article  Google Scholar 

Download references

Funding

Support from Faculty Innovative Research in Science and Technology (FIRST) is gratefully acknowledged.

Author information

Authors and Affiliations

  1. Politecnico di Torino, Torino, 10129, Italy

    Gennaro Mangiacapra

  2. Navigation System Engineer, Northrop Grumman Italia, Pomezia, RM, 00040, Italy

    Gennaro Mangiacapra

  3. Embry-Riddle Aeronautical University, 1 Aerospace Blvd., Daytona Beach, FL, 32114, USA

    Matthew Wittal & Morad Nazari

  4. NASA Kennedy Space Center, Merritt Island, FL, 32899, USA

    Matthew Wittal

  5. Politecnico di Torino, CNR-IEIIT, Corso Duca degli Abruzzi 24, Torino, 10129, Italy

    Elisa Capello

Authors
  1. Gennaro Mangiacapra
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Matthew Wittal
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Elisa Capello
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Morad Nazari
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Matthew Wittal.

Ethics declarations

Conflict of interest

The authors have no relevant financial or non-financial interests to disclose.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Mangiacapra, G., Wittal, M., Capello, E. et al. Unscented Kalman filter and control on \(\mathsf {TSE(3)}\) with application to spacecraft dynamics. Nonlinear Dyn 108, 2127–2146 (2022). https://doi.org/10.1007/s11071-022-07293-x

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s11071-022-07293-x

Keywords

Search

Navigation