Skip to main content
SpringerLink
Log in

From Intrinsic Optimization to Iterated Extended Kalman Filtering on Lie Groups

  • Published:
Journal of Mathematical Imaging and Vision Aims and scope Submit manuscript

Abstract

In this paper, we propose a new generic filter called Iterated Extended Kalman Filter on Lie Groups. It allows to perform parameter estimation when the state and the measurements evolve on matrix Lie groups. The contribution of this work is threefold: (1) the proposed filter generalizes the Euclidean Iterated Extended Kalman Filter to the case where both the state and the measurements evolve on Lie groups, (2) this novel filter bridges the gap between the minimization of intrinsic non-linear least squares criteria and filtering on Lie groups, (3) in order to detect and remove outlier measurements, a statistical test on Lie groups is proposed. In order to demonstrate the efficiency of the proposed generic filter, it is applied to the specific problem of relative motion averaging, both on synthetic and real data, for Lie groups \(SE\left( 3\right) \) (rigid-body motions), \(SL\left( 3\right) \) (homographies), and \(Sim\left( 3\right) \) (3D similarities). Typical applications of these problems are camera network calibration, image mosaicing, and partial 3D reconstruction merging problem. In each of these three applications, our approach significantly outperforms the state-of-the-art algorithms.

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.

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

Similar content being viewed by others

Notes

  1. For several Lie groups of interest, such as \(SO\left( 3\right) \), \(SE\left( 3\right) \), \(Sim\left( 3\right) \), analytical expressions of \(\text {exp}_{G}^{\wedge }\left( \cdot \right) \) and \(\text {log}_{G}^{\vee }\left( \cdot \right) \) exist [57]. However, for \(SL\left( 3\right) \) for example, matrix exponential and logarithm have to be computed.

  2. A closed-form expression of \(\varphi _{G}\left( b\right) \) was recently derived for \(SE\left( 3\right) \) in [4].

  3. In this paper, we consider quantities that are invariant to the right action of the Lie group on itself. Similar results could be obtained by considering the left action, leading to a left concentrated Gaussian distribution on \(G\), which is the modelization used for instance in [18].

  4. Of course, this shape is emphasized by the action of \(\mu \) on \(\text {exp}{}_{G}^{\wedge }\left( \epsilon \right) \).

  5. Augmenting the size of the state during the prediction step is sometimes called “smoothing” and not “filtering” in the literature.

  6. The Matlab code is available at https://sites.google.com/site/guillaumebourmaud/

References

  1. Absil, P.A., Mahony, R., Sepulchre, R.: Optimization Algorithms on Matrix Manifolds. Princeton University Press, Princeton (2009)

    MATH  Google Scholar 

  2. Agrawal, M.: A Lie algebraic approach for consistent pose registration for general Euclidean motion. In: IROS, pp. 1891–1897 (2006)

  3. Bandeira, A.S., Singer, A., Spielman, D.A.: A Cheeger inequality for the graph connection laplacian. arXiv preprint arXiv:1204.3873 (2012)

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

    Article  Google Scholar 

  5. Bell, B.M., Cathey, F.W.: The iterated Kalman filter update as a Gauss–Newton method. IEEE Trans. Autom. Control 38(2), 294–297 (1993)

    Article  MathSciNet  MATH  Google Scholar 

  6. Benhimane, S., Malis, E.: Homography-based 2D visual tracking and servoing. Int. J. Robot. Res. 26(7), 661–676 (2007)

    Article  Google Scholar 

  7. Berger, J., Lenzen, F., Becker, F., Neufeld, A., Schnörr, C.: Second-order recursive filtering on the rigid-motion lie group se (3) based on nonlinear observations. arXiv preprint arXiv:1507.06810 (2015)

  8. Bernstein, D.S.: Matrix Mathematics: Theory, Facts, and Formulas. Princeton University Press, Princeton (2009)

    Book  MATH  Google Scholar 

  9. Bertsekas, D.P.: Incremental least squares methods and the extended Kalman filter. SIAM J. Optim. 6(3), 807–822 (1996). doi:10.1137/S1052623494268522

    Article  MathSciNet  MATH  Google Scholar 

  10. Björck, A.: Numerical methods for least squares problems. Society for Industrial and Applied Mathematics, Philadelphia (1996). doi:10.1137/1.9781611971484

  11. Bonnabel, S.: Left-invariant extended Kalman filter and attitude estimation. In: IEEE Conference on Decision and Control (2007)

  12. Bonnabel, S., Martin, P., Salaun, 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)

  13. Boumal, N., Singer, A., Absil, P.A.: Robust estimation of rotations from relative measurements by maximum likelihood. In: Proceedings of the 52nd Conference on Decision and Control, CDC (2013)

  14. Boumal, N., Singer, A., Absil, P.A., Blondel, V.D.: Cramer-Rao bounds for synchronization of rotations. Inf. Inference 3(1), 1–39 (2014)

    Article  MathSciNet  MATH  Google Scholar 

  15. Bourmaud, G., Mégret, R., Arnaudon, M., Giremus, A.: Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions. J. Math. Imaging Vis. 51(1), 209–228 (2015)

    Article  MathSciNet  MATH  Google Scholar 

  16. Bourmaud, G., Mégret, R., Giremus, A., Berthoumieu, Y.: Global motion estimation from relative measurements in the presence of outliers. ACCV (2014)

  17. Bourmaud, G., Mégret, R., Giremus, A., Berthoumieu, Y.: Global motion estimation from relative measurements using iterated extended Kalman filter on matrix Lie groups. In: ICIP (2014)

  18. 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 (EUSIPCO) (2013)

  19. Caballero, F., Merino, L., Ferruz, J., Ollero, A.: Homography based Kalman filter for mosaic building. applications to UAV position estimation. In: IEEE International Conference on Robotics and Automation, pp. 2004–2009 (2007)

  20. Chatterjee, A., Govindu, V.M.: Efficient and robust large-scale rotation averaging. In: ICCV (2013)

  21. Chirikjian, G.S.: Stochastic Models, Information Theory, and Lie Groups, vol. 2. Springer, Boston (2012). doi:10.1007/978-0-8176-4944-9

    Book  MATH  Google Scholar 

  22. Crandall, D.J., Owens, A., Snavely, N., Huttenlocher, D.: Discrete-continuous optimization for large-scale structure from motion. In: CVPR, pp. 3001–3008 (2011)

  23. Crassidis, J., Markley, F.: Unscented filtering for spacecraft attitude estimation. J. Guid. Control Dynam. 26, 536–542 (2003)

    Article  Google Scholar 

  24. Davison, A.J., Reid, I.D., Molton, N.D., Stasse, O.: Monoslam: Real-time single camera slam. IEEE Trans. Paatern Anal. Mach. Intell. 29(6), 1052–1067 (2007)

    Article  Google Scholar 

  25. Enqvist, O., Kahl, F., Olsson, C.: Non-sequential structure from motion. In: ICCV Workshops, pp. 264–271 (2011)

  26. Faraut, J.: Analysis on Lie Groups: An Introduction, vol. 110. Cambridge University Press, Cambridge (2008)

    Book  MATH  Google Scholar 

  27. Fisher, R.A., Yates, F.: Statistical tables for biological, agricultural and medical research. Vet. Rec. 68, 1015 (1956)

    Google Scholar 

  28. Fredriksson, J., Olsson, C.: Simultaneous multiple rotation averaging using Lagrangian duality. In: Asian Conference of Computer Vision (2012)

  29. Govindu, V.M.: Combining two-view constraints for motion estimation. CVPR 2, 218–225 (2001)

    Google Scholar 

  30. Govindu, V.M.: Lie-algebraic averaging for globally consistent motion estimation. CVPR 1, 684–691 (2004)

    Google Scholar 

  31. Grisetti, G., Kuemmerle, R., Ni, K.: Robust optimization of factor graphs by using condensed measurements. In: IEEE International Conference on Intelligent Robots and Systems (IROS) (2012)

  32. Hall, B.: Lie Groups, Lie Algebras, and Representations An Elementary Introduction. Springer, New York (2003)

    Book  MATH  Google Scholar 

  33. Hartley, R., Aftab, K., Trumpf, J.: L1 rotation averaging using the Weiszfeld algorithm. Institute of Electrical and Electronics Engineers, pp. 3041–3048 (2011). doi:10.1109/CVPR.2011.5995745

  34. Hartley, R., Trumpf, J., Dai, Y., Li, H.: Rotation averaging. Int. J. Comput. Vis. 103(3), 267–305 (2013)

    Article  MathSciNet  MATH  Google Scholar 

  35. Haykin, S.S., et al.: Kalman filtering and neural networks. Wiley Online Library, (2001)

  36. Jeong, Y., Nister, D., Steedly, D., Szeliski, R., Kweon, I.: Pushing the envelope of modern methods for bundle adjustment. In: CVPR (2010)

  37. Jiang, N., Cui, Z., Tan, P.: A global linear method for camera pose registration. In: 2013 IEEE International Conference on Computer Vision doi:10.1109/iccv.2013.66

  38. Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J.J., Dellaert, F.: iSAM2: Incremental smoothing and mapping using the bayes tree. Int. J. Robot. Res. 31(2), 216–235 (2012)

    Article  Google Scholar 

  39. Kaess, M., Ranganathan, A., Dellaert, F.: iSAM: Incremental smoothing and mapping. IEEE Trans. Robot. (TRO) 24(6), 1365–1378 (2008)

    Article  Google Scholar 

  40. Khurd, P., Grady, L., Oketokoun, R., Sundar, H., Gajera, T., Gibbs-Strauss, S., Frangioni, J.V., Kamen, A.: Global error minimization in image mosaicing using graph connectivity and its applications in microscopy. J. Pathol. Inform. 2, S8 (2012)

    Article  Google Scholar 

  41. Klein, G., Murray, D.: Parallel tracking and mapping for small ar workspaces. In: International Symposium on Mixed and Augmented Reality (ISMAR) (2007)

  42. Konolige, K.: Sparse sparse bundle adjustment. In: BMVC, pp. 1–11 (2010)

  43. Lefferts, E., Markley, F., Shuster, M.: Kalman filtering for spacecraft attitude estimation. J Guid. Control Dynam. 5(5), 417–429 (1982)

    Article  Google Scholar 

  44. Li, G., Liu, Y., Yin, J., Shi, Z.: Newton geodesic optimization on special linear group. In: Proceedings of the 48h IEEE Conference on Decision and Control (CDC) (2009). doi:10.1109/cdc.2009.5400115

  45. Long, A.W., Wolfe, K.C., Mashner, M., Chirikjian, G.S.: The banana distribution is Gaussian: a localization study with exponential coordinates. In: Robotics: Science and Systems (2012)

  46. Lui, Y.: Advances in matrix manifolds for computer vision. Image Vis. Comput. 30, 380–388 (2011)

    Article  MathSciNet  Google Scholar 

  47. Malis, E., T.Hamel, Mahony, R., Morin, P.: Dynamic estimation of homography transformations on the special linear group of visual servo control. In: IEEE Conference on Robotics and Automation (2009)

  48. Markley, F.: Attitude error representation for Kalman filtering. J. Guid. Control Dynam. 26(2), 311–317 (2003)

    Article  MathSciNet  Google Scholar 

  49. Martinec, D., Pajdla, T.: Robust rotation and translation estimation in multiview reconstruction. In: Computer Vision and Pattern Recognition, 2007. CVPR’07. IEEE Conference on, pp. 1–8. IEEE (2007)

  50. Meidow, J.: Efficient video mosaicking by multiple loop closing. In: Photogrammetric Image Analysis, pp. 1–12. Springer (2011)

  51. Moulon, P., Monasse, P., Marlet, R.: Global fusion of relative motions for robust, accurate and scalable structure from motion. In: Computer Vision (ICCV), 2013 IEEE International Conference on, pp. 3248–3255. IEEE (2013)

  52. Persson, S., Sharf, I.: Invariant momentum-tracking Kalman filter for attitude estimation. In: Robotics and Automation (ICRA), 2012 IEEE International Conference on, pp. 592–598 (2012). doi:10.1109/ICRA.2012.6224562

  53. Ramachandra, K.: Kalman Filtering Techniques for Radar Tracking. CRC Press, Boca Raton (2000)

    Google Scholar 

  54. Ring, W., Wirth, B.: Optimization methods on riemannian manifolds and their application to shape space. SIAM J. Optim. 22(2), 596–627 (2012)

    Article  MathSciNet  MATH  Google Scholar 

  55. Roberts, R., Sinha, S.N., Szeliski, R., Steedly, D.: Structure from motion for scenes with large duplicate structures. In: IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2011, pp. 3137–3144. IEEE (2011)

  56. Rudkovskii, M.: Twisted product of Lie groups. Sib. Math. J. 38(5), 969–977 (1997)

    Article  MathSciNet  Google Scholar 

  57. Selig, J.M.: Lie groups and Lie algebras in robotics. NATO Science Series II: Mathematics, Physics and Chemistry pp. 101–125 (2005)

  58. Singer, A., Shkolnisky, Y.: Three-dimensional structure determination from common lines in cryo-em by eigenvectors and semidefinite programming. SIAM J. Imaging Sci. 4(2), 543–572 (2011)

    Article  MathSciNet  MATH  Google Scholar 

  59. Smith, P., Drummond, T., Roussopoulos, K.: Computing map trajectories by representing, propagating and combining pdfs over groups. In: ICCV, pp. 1275–1282 (2003)

  60. Strasdat, H., Davison, A., Montiel, J., Konolige, K.: Double window optimisation for constant time visual SLAM. In: IEEE International Conference on Computer Vision (ICCV) (2011)

  61. Strasdat, H., Montiel, J.M.M., Davison, A.J.: Real-time monocular slam: Why filter? In: ICRA, pp. 2657–2664 (2010)

  62. Taylor, C.J., Kriegman, D.J.: Minimization on the Lie group SO(3) and related manifolds (1994)

  63. Vercauteren, T., Pennec, X., Malis, E., Perchant, A., Ayache, N.: Insight into efficient image registration techniques and the demons algorithm. In: Information Processing in Medical Imaging, pp. 495–506. Springer (2007)

  64. Wachinger, C., Wein, W., Navab, N.: Registration strategies and similarity measures for three-dimensional ultrasound mosaicing. Acad. Radiol. 15(11), 1404–1415 (2008)

    Article  Google Scholar 

  65. Wang, L., Singer, A.: Exact and stable recovery of rotations for robust synchronization. Inf. Inference 2(2), 145–193 (2013). doi:10.1093/imaiai/iat005

    Article  MathSciNet  MATH  Google Scholar 

  66. Wang, Y., Chirikjian, G.: Error propagation on the Euclidean group with applications to manipulators kinematics. IEEE Trans. Robot. 22(4), 591–602 (2006)

    Article  Google Scholar 

  67. Wolfe, K., Mashner, M., Chirikjian, G.: Bayesian fusion on Lie groups. J. Algebr. Stat. 2, 75–97 (2011)

    Article  MathSciNet  Google Scholar 

Download references

Acknowledgments

The research leading to these results has received funding from the European Community’s Seventh Framework Programme (FP7/2007–2013) under grant agreement 288199—Dem@Care.

Author information

Authors and Affiliations

  1. Toshiba Research Europe, Cambridge, UK

    Guillaume Bourmaud

  2. University of Bordeaux, IMS Laboratory CNRS UMR 5218, Bordeaux, France

    Rémi Mégret, Audrey Giremus & Yannick Berthoumieu

  3. Department of Mathematical Sciences, University of Puerto-Rico at Mayaguez, Mayagüez, Puerto Rico

    Rémi Mégret

Authors
  1. Guillaume Bourmaud
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Rémi Mégret
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Audrey Giremus
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Yannick Berthoumieu
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Guillaume Bourmaud.

Appendices

Derivation of the LG-IEKF

1.1 Derivation of \(\delta ^{l+1}\)

Here, we derive the expression of the \(\delta ^{l+1}\) in (53):

$$\begin{aligned} \delta ^{l+1}&=\text {log}_{G}^{\vee }\left( X^{l+1}\left( X^{l}\right) ^{-1}X^{l}\mu _{k|k-1}^{-1}\right) \nonumber \\&=\text {log}_{G}^{\vee }\left( \text {exp}{}_{G}^{\wedge }\left( \delta ^{l+1/l}\right) \text {exp}{}_{G}^{\wedge }\left( \delta ^{l}\right) \right) \nonumber \\&\simeq \varphi _{l}\delta ^{l+1/l}+\delta ^{l}\nonumber \\&=\varphi _{l}\left( H_{l}^\mathrm{{T}}Q_{k}^{-1}H_{l}+\varphi _{l}^\mathrm{{T}}P_{k|k-1}^{-1}\varphi _{l}\right) ^{-1}\nonumber \\&\quad \left\{ H_{l}^\mathrm{{T}}Q_{k}^{-1}\right. \left. \text {log}_{G'}^{\vee }\left( Z_{k}h\left( X^{l}\right) ^{-1}\right) -\varphi _{l}^\mathrm{{T}}P_{k|k-1}^{-1}\delta ^{l}\right\} +\delta ^{l}\nonumber \\&=\varphi _{l}\left( H_{l}^\mathrm{{T}}Q_{k}^{-1}H_{l}+\varphi _{l}^\mathrm{{T}}P_{k|k-1}^{-1}\varphi _{l}\right) ^{-1}\nonumber \\&\quad \left\{ H_{l}^\mathrm{{T}}Q_{k}^{-1}\text {log}_{G'}^{\vee }\left( Z_{k}h\left( X^{l}\right) ^{-1}\right) -\varphi _{l}^\mathrm{{T}}P_{k|k-1}^{-1}\delta ^{l}\right. \nonumber \\&\left. +\left( H_{l}^\mathrm{{T}}Q_{k}^{-1}H_{l}+\varphi _{l}^\mathrm{{T}}P_{k|k-1}^{-1}\varphi _{l}\right) \varPhi _{l}\delta ^{l}\right\} \nonumber \\&=\varphi _{l}\left( H_{l}^\mathrm{{T}}Q_{k}^{-1}H_{l}+\varphi _{l}^\mathrm{{T}}P_{k|k-1}^{-1}\varphi _{l}\right) ^{-1}\nonumber \\&\quad H_{l}^\mathrm{{T}}Q_{k}^{-1}\left\{ \text {log}_{G'}^{\vee }\left( Z_{k}h\left( X^{l}\right) ^{-1}\right) +H_{l}\varPhi _{l}\delta ^{l}\right\} \nonumber \\&=K_{l}\left\{ \text {log}_{G'}^{\vee }\left( Z_{k}h\left( X^{l}\right) ^{-1}\right) +H_{l}\varPhi _{l}\delta ^{l}\right\} , \end{aligned}$$
(78)

where \(K_{l}\) is the Lie–Kalman gain derived in Appendix 1.b.

1.2 Lie–Kalman Gain Derivation

Here, we derive the expression of the Lie–Kalman gain (54) (the superscripts and underscripts are omitted):

$$\begin{aligned}&K=\varphi \left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}H^\mathrm{{T}}Q^{-1}\nonumber \\&\quad =\varphi \left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}\nonumber \\&\quad \left( H^\mathrm{{T}}Q^{-1}\left( H\varPhi P\varPhi ^\mathrm{{T}}H^{T}+Q\right) \left( H\varPhi P\varPhi ^\mathrm{{T}}H^{T}+Q\right) ^{-1}\right) \nonumber \\&\quad =\varphi \left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}\nonumber \\&\quad \left( \left( H^\mathrm{{T}}Q^{-1}H\varPhi P\varPhi ^\mathrm{{T}}H^{T}+H^\mathrm{{T}}\right) \left( H\varPhi P\varPhi ^\mathrm{{T}}H^{T}+Q\right) ^{-1}\right) \nonumber \\&\quad =\varphi \left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}\nonumber \\&\quad \left( \left( \left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) \varPhi P\varPhi ^\mathrm{{T}}H^{T}\right) \left( H\varPhi P\varPhi ^\mathrm{{T}}H^{T}+Q\right) ^{-1}\right) \nonumber \\&\quad =P\varPhi ^\mathrm{{T}}H^{T}\left( H\varPhi P\varPhi ^\mathrm{{T}}H^{T}+Q\right) ^{-1}. \end{aligned}$$
(79)

1.3 Covariance Update Derivation

Here, we derive the expression of the \(P_{k|k}\) in (61) (the superscripts and underscripts are omitted):

$$\begin{aligned}&P_{k|k}=\left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}\nonumber \\&\quad =\left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}\nonumber \\&\quad \quad \left\{ \left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) \varPhi P\varPhi ^\mathrm{{T}}-H^{T}Q^{-1}H\varPhi P\varPhi ^\mathrm{{T}}\right\} \nonumber \\&\quad =\varPhi P\varPhi ^\mathrm{{T}}-\left( H^\mathrm{{T}}Q^{-1}H+\varphi ^\mathrm{{T}}P^{-1}\varphi \right) ^{-1}H^\mathrm{{T}}Q^{-1}H\varPhi P\varPhi ^\mathrm{{T}}\nonumber \\&\quad =\varPhi P\varPhi ^\mathrm{{T}}-\varPhi KH\varPhi P\varPhi ^\mathrm{{T}}\nonumber \\&\quad =\varPhi \left( Id-KH\varPhi \right) P\varPhi ^\mathrm{{T}}, \end{aligned}$$
(80)

where K is defined in (79).

Relative Motion Averaging

1.1 Derivation of \(F_{k}\)

From (38) and (72), we have

$$\begin{aligned}&\text {log}_{G_{k}}^{\vee }\left( f\left( \mu _{k-1|k-1}\right) f\left( \text {exp}_{G_{k-1}}^{\wedge }\left( \delta \right) \mu _{k-1|k-1}\right) ^{-1}\right) \nonumber \\&\quad = \text {log}_{G_{k}}^{\vee }\left( \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \left[ \begin{array}{ll} \left( \mu _{k-1|k-1}\right) _{kR} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad \mu _{k-1|k-1} \end{array}\right] \right. \nonumber \\&\qquad \left. \left( \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \left[ \begin{array}{ll} \text {exp}_{G'}^{\wedge }\left( \delta _{kR}\right) \left( \mu _{k-1|k-1}\right) _{kR} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad \text {exp}_{G_{k-1}}^{\wedge }\left( \delta \right) \mu _{k-1|k-1} \end{array}\right] \right) ^{-1}\right) \nonumber \\&\quad = \text {log}_{G_{k}}^{\vee }\left( \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \left[ \begin{array}{ll} \left( \mu _{k-1|k-1}\right) _{kR} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad \mu _{k-1|k-1} \end{array}\right] \right. \nonumber \\&\qquad \left. \left( \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \text {exp}_{G_{k}}^{\wedge }\left( \left[ \begin{array}{c} \delta _{kR}\\ \delta \end{array}\right] \right) \left[ \begin{array}{ll} \left( \mu _{k-1|k-1}\right) _{kR} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad \mu _{k-1|k-1} \end{array}\right] \right) ^{-1}\right) \nonumber \\&\quad = \text {log}_{G_{k}}^{\vee }\left( \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \text {exp}_{G_{k}}^{\wedge }\left( -\left[ \begin{array}{ll} \delta _{kR}\\ \delta \end{array}\right] \right) \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k}^{-1} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \right) \nonumber \\&\quad = \text {log}_{G_{k}}^{\vee }\left( \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \text {exp}_{G_{k}}^{\wedge }\left( -\left[ \begin{array}{lll} \varvec{0} &{}\quad Id&{}\quad \varvec{0}\\ &{}\quad Id\end{array}\right] \delta \right) \left[ \begin{array}{ll} Y_{\left( k+1\right) \, k}^{-1} &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \right) \nonumber \\&\quad = -\left[ \begin{array}{ll} \text {Ad}_{G'}\left( Y_{\left( k+1\right) \, k}\right) &{}\quad \varvec{0}\\ \varvec{0} &{}\quad Id\end{array}\right] \left[ \begin{array}{lll} \varvec{0} &{}\quad Id&{}\quad \varvec{0}\\ &{}\quad Id\end{array}\right] \delta , \end{aligned}$$
(81)

where we introduced the adjoint representation \(\text {Ad}_{G'}\left( \cdot \right) \subset \mathbb {R}^{q\times q}\) of \(G'\) on \(\mathbb {R}^{q}\) that enables us to transform an increment \(\epsilon _{ij}^{i}\in \mathbb {R}^{q}\), that acts onto an element \(Y_{ij}\) through left multiplication, into an increment \(\epsilon _{ij}^{j}\in \mathbb {R}^{q}\), that acts through right multiplication:

$$\begin{aligned} \text {exp}{}_{G'}^{\wedge }\left( \epsilon _{ij}^{i}\right) Y_{ij}=Y_{ij}\text {exp}{}_{G'}^{\wedge }\left( \text {Ad}_{G'}\left( Y_{ij}^{-1}\right) \epsilon _{ij}^{i}\right) . \end{aligned}$$
(82)

Consequently, from (81), we obtain

(83)

1.2 Derivation of \(H_{l}\)

From (48) and (73), we have

$$\begin{aligned}&\text {log}_{G'}^{\vee }\left( Z_{k}h\left( \text {exp}_{G}^{\wedge }\left( \delta \right) X^{l}\right) ^{-1}\right) \nonumber \\&\quad = \text {log}_{G'}^{\vee }\left( Y_{i\,\left( k+1\right) }\text {exp}{}_{G'}^{\wedge }\left( \delta _{\left( k+1\right) R}\right) \left( X^{l}\right) _{\left( k+1\right) R}\left( X^{l}\right) _{iR}^{-1}\text {exp}{}_{G'}^{\wedge }\left( -\delta _{iR}\right) \right) \nonumber \\&\quad = \text {log}_{G'}^{\vee }\left( Y_{i\,\left( k+1\right) }\left( X^{l}\right) _{\left( k+1\right) R}\left( X^{l}\right) _{iR}^{-1}\right. \nonumber \\&\qquad \left. \text {exp}{}_{G'}^{\wedge }\left( \text {Ad}_{G'}\left( \left( X^{l}\right) _{iR}\left( X^{l}\right) _{\left( k+1\right) R}^{-1}\right) \delta _{\left( k+1\right) R}\right) \text {exp}{}_{G'}^{\wedge }\left( -\delta _{iR}\right) \right) \nonumber \\&\quad \simeq \text {log}_{G'}^{\vee }\left( Y_{i\,\left( k+1\right) }\left( X^{l}\right) _{\left( k+1\right) R}\left( X^{l}\right) _{iR}^{-1}\right) \nonumber \\&\qquad -\delta _{iR}+\text {Ad}_{G'}\left( \left( X^{l}\right) _{iR}\left( X^{l}\right) _{\left( k+1\right) R}^{-1}\right) \delta _{\left( k+1\right) R}, \end{aligned}$$
(84)

where we approximated \(\varphi \left( \cdot \right) \) (see (10)) by \(Id\).

Consequently, from (84), we obtain

(85)

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Bourmaud, G., Mégret, R., Giremus, A. et al. From Intrinsic Optimization to Iterated Extended Kalman Filtering on Lie Groups. J Math Imaging Vis 55, 284–303 (2016). https://doi.org/10.1007/s10851-015-0622-8

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s10851-015-0622-8

Keywords

Search

Navigation