Journal of Mathematical Imaging and Vision

, Volume 51, Issue 1, pp 209–228 | Cite as

Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions

  • Guillaume Bourmaud
  • Rémi Mégret
  • Marc Arnaudon
  • Audrey Giremus
Article

Abstract

In this paper we generalize the continuous-discrete extended Kalman filter (CD-EKF) to the case where the state and the observations evolve on connected unimodular matrix Lie groups. We propose a new assumed density filter called continuous-discrete extended Kalman filter on Lie groups (CD-LG-EKF). It is built upon a geometrically meaningful modeling of the concentrated Gaussian distribution on Lie groups. Such a distribution is parametrized by a mean and a covariance matrix defined on the Lie group and in its associated Lie algebra respectively. Our formalism yields tractable equations for both non-linear continuous time propagation and discrete update of the distribution parameters under the assumption that the posterior distribution of the state is a concentrated Gaussian. As a side effect, we contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection. We also show that the CD-LG-EKF reduces to the usual CD-EKF if the state and the observations evolve on Euclidean spaces. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose filtering problem with observations on Lie group. In this application, the CD-LG-EKF significantly outperforms two constrained non-linear filters (one based on a linearization technique and the other on the unscented transform) applied on the embedding space of the Lie group.

Keywords

Extended Kalman filter Matrix Lie group Filtering on manifold Stochastic processes on manifolds Camera pose filtering 

References

  1. 1.
    Abbaspour, H., Moskowitz, M.: Basic Lie Theory. World Scientific, Hackensack (2007)CrossRefMATHGoogle Scholar
  2. 2.
    Absil, P.A., Mahony, R., Sepulchre, R.: Optimization Algorithms on Matrix Manifolds. Princeton University Press, Princeton (2009)Google Scholar
  3. 3.
    Ado, I.: The representation of Lie algebras by matrices. Uspekhi Matematicheskikh Nauk 2(6), 159–173 (1947)MathSciNetGoogle Scholar
  4. 4.
    Arasaratnam, I., Haykin, S., Hurd, T.: Cubature Kalman filtering for continuous-discrete systems: theory and simulations. IEEE Trans. Signal Process. 58, 4977–4993 (2010)CrossRefMathSciNetGoogle Scholar
  5. 5.
    Arnaudon, M.: onnexions et martingales dans les groupes de Lie. Séminaire de Probabilités XXVI, pp. 146–156. Springer, Berlin (1992)CrossRefGoogle Scholar
  6. 6.
    Arsigny, V., Commowick, O., Pennec, X., Ayache, N.: A log-euclidean framework for statistics on diffeomorphisms. Medical Image Computing and Computer-Assisted Intervention-MICCAI 2006, pp. 924–931. Springer, Berlin (2006)CrossRefGoogle Scholar
  7. 7.
    Barfoot, T.D., Furgale, P.T.: Associating uncertainty with three-dimensional poses for use in estimation problems. IEEE Trans. Robot. 30(3), 679–693. doi:10.1109/tro.2014.2298059 (2014).
  8. 8.
    Bayro-Corrochano, E., Zhang, Y.: The motor extended Kalman filter: a geometric approach for rigid motion estimation. J. Math. Imaging Vis. 13(3), 205–228 (2000)CrossRefMATHMathSciNetGoogle Scholar
  9. 9.
    Bonnabel, S.: Left-invariant extended Kalman filter and attitude estimation. In: IEEE Conference on Decision and Control (2007).Google Scholar
  10. 10.
    Bonnabel, S., Martin, P., Salan, E.: Invariant extended Kalman filter : theory and application to a velocity-aided attitude estimation problem. In: IEEE Conference on Decision and Control and 28th Chinese Control Conference (2009).Google Scholar
  11. 11.
    Bourmaud, G., Mégret, R., Giremus, A., Berthoumieu, Y.: Discrete extended Kalman filter on Lie groups. In: Signal Processing Conference (EUSIPCO), Proceedings of the 21st European (2013).Google Scholar
  12. 12.
    Chirikjian, G.: Information-theoretic inequalities on unimodular Lie groups. J. Geom. Mech. 2, 119–158 (2010)CrossRefMATHMathSciNetGoogle Scholar
  13. 13.
    Chirikjian, G.: Stochastic Models, Information Theory, and Lie Groups. Springer, Berlin (2012)CrossRefMATHGoogle Scholar
  14. 14.
    Crassidis, J., Markley, F.: Unscented filtering for spacecraft attitude estimation. J. Guid. Control. Dyn. 26, 536–542 (2003)CrossRefGoogle Scholar
  15. 15.
    Davison, A.J., Reid, I.D., Molton, N.D., Stasse, O.: Monoslam: real-time single camera slam. IEEE Trans. Pattern Anal. Mach. Intell. 29(6), 1052–1067 (2007)CrossRefGoogle Scholar
  16. 16.
    Doucet, A., de Freitas, N., Gordon, N.: Sequential Monte Carlo Methods in Practice. Springer, New York (2001)CrossRefMATHGoogle Scholar
  17. 17.
    Gilmore, R.: Lie Groups, Physics, and Geometry: An Introduction for Physicists, Engineers and Chemists. Cambridge University Press, Cambridge (2008)CrossRefGoogle Scholar
  18. 18.
    Hall, B.: Lie Groups, Lie Algebras, and Representations An Elementary Introduction. Springer, New York (2003)CrossRefMATHGoogle Scholar
  19. 19.
    Hall, J., Knoebel, N., McLain, T.: Quaternion attitude estimation for miniature air vehicles using a multiplicative extended Kalman. In: Position, Location and Navigation Symposium, pp. 1230–1237 (2008).Google Scholar
  20. 20.
    Hauberg, S., Lauze, F., Pedersen, K.S.: Unscented Kalman filtering on Riemannian manifolds. J. Math. Imaging Vis. pp. 1–18 (2013). http://link.springer.com/article/10.1007/s10851-012-0372-9
  21. 21.
    Julier, S.J., Uhlmann, J.K.: New extension of the Kalman filter to nonlinear systems. Signal Process., Sensor Fusion, Target Recognit. VI 3068, 182–193 (1997). doi:10.1117/12.280797 CrossRefGoogle Scholar
  22. 22.
    Kovk, I., Michor, P., Slovak, J.: Natural Operations in Differential Geometry. Springer, Berlin (1993)Google Scholar
  23. 23.
    Kraft, E.: A quaternion-based unscented Kalman filter for orientation tracking. In: International Conference on Information Fusion, vol. 1, pp. 47–54 (2003)Google Scholar
  24. 24.
    Lefferts, E., Markley, F., Shuster, M.: Kalman filtering for spacecraft attitude estimation. J. Guid. Control Dyn. 5(5), 417–429 (1982)CrossRefGoogle Scholar
  25. 25.
    Lui, Y.: Advances in matrix manifolds for computer vision. Image Vis. Comput. 30, 380–388 (2011)CrossRefMathSciNetGoogle Scholar
  26. 26.
    Markley, F.: Attitude error representation for Kalman filtering. J. Guid. Control Dyn. 26(2), 311–317 (2003)CrossRefMathSciNetGoogle Scholar
  27. 27.
    Martin, P., Salaun, E.: Generalized multiplicative extended Kalman filter for aided attitude and heading reference system. In: AIAA Guidance, Navigation and Control Conference (2010).Google Scholar
  28. 28.
    Maybeck, P.: Stochastic Models, Estimation, and Control. Academic Press, New York (1979)MATHGoogle Scholar
  29. 29.
    Moreno-Noguer, F., Lepetit, V., Fua, P.: Accurate non-iterative o(n) solution to the pnp problem. In: IEEE International Conference on Computer Vision (2007). Google Scholar
  30. 30.
    Mourikis, A., Trawny, N., Roumeliotis, S., Johnson, A., Ansar, A., Matthies, L.: Vision-aided inertial navigation for spacecraft entry, descent, and landing. IEEE Trans. Robot. 25(2), 264–280 (2009)CrossRefGoogle Scholar
  31. 31.
    Park, W., Liu, Y., Zhou, Y., Moses, M., Chirikjian, G.S.: Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map. Robotica 26(4), 419–434 (2008)CrossRefMATHGoogle Scholar
  32. 32.
    Rentmeesters, Q., Absil, P., Van Dooren, P., Gallivan, K., Srivastava, A.: An efficient particle filtering technique on the Grassmann manifold. In: IEEE International Conference on Acoustics Speech and Signal Processing (2010).Google Scholar
  33. 33.
    Rudkovskii, M.: Twisted product of Lie groups. Sib. Math. J. 38(5), 969–977 (1997)CrossRefMathSciNetGoogle Scholar
  34. 34.
    Saccon, A., Aguiar, A., Hauser, J.: Lie group projection operator approach : Optimal control on tso(3). In: IEEE Conference on Decision and Control and European Control Conference (2011).Google Scholar
  35. 35.
    Said, S., Manton, J.H.: Extrinsic mean of Brownian distributions on compact Lie groups. IEEE Trans. Inf. Theory 58(6), 3521–3535 (2012)CrossRefMathSciNetGoogle Scholar
  36. 36.
    Sarkka, S.: On unscented Kalman filtering for state estimation of continuous-time nonlinear systems. IEEE Trans. Autom. Control 52(9), 1631–1641 (2007)CrossRefMathSciNetGoogle Scholar
  37. 37.
    Selig, J.M.: Lie groups and Lie algebras in robotics. In: Computational Noncommutative Algebra and Applications, NATO Science Series II: Mathematics, Physics and Chemistry. vol. 136, pp. 101–125. Springer (2004). http://dx.doi.org/10.1007/1-4020-2307-3_5
  38. 38.
    Simon, D.: Kalman filtering with state constraints: a survey of linear and nonlinear algorithms. IET Control Theory Appl. 4(8), 1303–1318 (2010)CrossRefMathSciNetGoogle Scholar
  39. 39.
    Singer, H.: Continuous-discrete unscented Kalman filtering. Technical Report, (2006)Google Scholar
  40. 40.
    Sipos, B.: Application of the manifold-constrained unscented Kalman filter. In: Position, Location and Navigation Symposium, pp. 30–43 (2008).Google Scholar
  41. 41.
    Snoussi, H., Mohammad-Djafari, A.: Particle filtering on Riemannian manifolds. In: AIP Conference, vol. Issue 1, p219, vol. 872 (2006).Google Scholar
  42. 42.
    Tompkins, F., Wolfe, P.J.: Bayesian filtering on the Stiefel manifold. In: IEEE International Workshop on Computational Advances in Multi-Sensor Adaptive Processing (2007).Google Scholar
  43. 43.
    Trawny, N., Mourikis, A., Roumeliotis, S., Johnson, A., Montgomery, J.: Vision-aided inertial navigation for pin-point landing using observations of mapped landmarks. J. Field Robot. 24, 357–378 (2007)CrossRefGoogle Scholar
  44. 44.
    Trawny, N., Roumeliotis, S.: Indirect Kalman filter for 3d attitude estimation. Technical report, University of Minnesota, (2005)Google Scholar
  45. 45.
    Wang, Y., Chirikjian, G.: Error propagation on the Euclidean group with applications to manipulators kinematics. IEEE Trans. Robot. 22, 591–602 (2006)CrossRefGoogle Scholar
  46. 46.
    Wang, Y., Chirikjian, G.S.: Nonparametric second-order theory of error propagation on motion groups. Int. J. Rob. Res. 27(11–12), 1258–1273 (2008)CrossRefGoogle Scholar
  47. 47.
    Wannous, H., Dovgalecs, V., Mégret, R., Daoudi, M.: Place recognition via 3d modeling for personal activity lifelog using wearable camera. In: Advances in Multimedia Modeling, Lecture Notes in Computer Science, vol. 7131, pp. 244–254. Springer (2012). http://dx.doi.org/10.1007/978-3-642-27355-1_24
  48. 48.
    Wolfe, K., Mashner, M., Chirikjian, G.: Bayesian fusion on Lie groups. J. Algebr. Stat. 2, 75–97 (2011)MathSciNetGoogle Scholar

Copyright information

© Springer Science+Business Media New York 2014

Authors and Affiliations

  • Guillaume Bourmaud
    • 1
  • Rémi Mégret
    • 1
  • Marc Arnaudon
    • 2
  • Audrey Giremus
    • 1
  1. 1.IMS Laboratory CNRS UMR 5218University of BordeauxTalence CedexFrance
  2. 2.IMB Laboratory CNRS UMR 5251University of BordeauxTalence CedexFrance

Personalised recommendations