P. Absil, R. Mahony, and R. Sepulchre, Optimization Algorithms on Matrix Manifolds, 2008.

S. Agarwal, K. Mierle, and O. Ceres,

T. Bailey and H. F. Durrant-whyte, Simultaneous localization and mapping (slam): Part ii, IEEE Robotics & Automation Magazine, vol.13, issue.3, pp.108-117, 2006.

T. Bailey, J. Nieto, and E. Nebot, Consistency of the fastslam algorithm, Proceedings 2006 IEEE International Conference on Robotics and Automation, pp.424-429, 2006.

T. Barfoot, R. James, P. T. Forbes, and . Furgale, Pose estimation using linearized rotations and quaternion algebra, Acta Astronautica, vol.68, issue.1-2, pp.101-112, 2011.

T. D. Barfoot, State Estimation for Robotics, 2017.

T. D. Barfoot and P. T. Furgale, Associating Uncertainty with Three-Dimensional Poses for Use in Estimation Problems, IEEE Transactions on Robotics, vol.30, issue.3, pp.679-693, 2014.

A. Barrau and S. Bonnabel, Extended kalman filtering with nonlinear equality constraints: a geometric approach, IEEE Transactions on Automatic Control, pp.1-1, 2019.
URL : https://hal.archives-ouvertes.fr/hal-02129552

A. Barrau, Non-linear state error based extended Kalman filters with applications to navigation. Theses, Ecole Nationale Supérieure des Mines de Paris, 2015.
URL : https://hal.archives-ouvertes.fr/tel-01247723

A. Barrau and S. Bonnabel, Intrinsic Filtering on SO(3) with Discrete-time Observations, Decision and Control (CDC), 2013 IEEE 52nd Annual Conference on, pp.3255-3260, 2013.

A. Barrau and S. Bonnabel, An EKF-SLAM algorithm with consistency properties, 2015.

A. Barrau and S. Bonnabel, Intrinsic filtering on lie groups with applications to attitude estimation, IEEE Transactions on Automatic Control, vol.60, issue.2, pp.436-449, 2015.

A. Barrau and S. Bonnabel, Navigating with highly precise odometry and noisy GPS: a case study, IFAC Symposium on Nonlinear Control Systems (NOLCOS), 2016.
URL : https://hal.archives-ouvertes.fr/hal-01267244

A. Barrau and S. Bonnabel, The invariant extended kalman filter as a stable observer, IEEE Transactions on Automatic Control, vol.62, issue.4, pp.1797-1812, 2017.
URL : https://hal.archives-ouvertes.fr/hal-01692380

A. Barrau and S. Bonnabel, Three examples of the stability properties of the invariant extended kalman filter. IFAC-PapersOnLine, vol.50, pp.431-437, 2017.
URL : https://hal.archives-ouvertes.fr/hal-01526467

A. Barrau and S. Bonnabel, Invariant kalman filtering, Robotics, and Autonomous Systems, vol.1, issue.1, 2018.
URL : https://hal.archives-ouvertes.fr/hal-01982804

A. Barrau and S. Bonnabel, A Mathematical Framework for IMU Error Propagation with Applications to Preintegration, 2019.
URL : https://hal.archives-ouvertes.fr/hal-02394774

A. Barrau and S. Bonnabel, Linear observed systems on groups, Systems Control Letters, vol.129, pp.36-42, 2019.
URL : https://hal.archives-ouvertes.fr/hal-01671724

B. M. Bell and F. W. Cathey, The iterated kalman filter update as a gauss-newton method, IEEE Transactions on Automatic Control, vol.38, issue.2, pp.294-297, 1993.

P. J. Besl and N. D. Mckay, A method for registration of 3-d shapes, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14, issue.2, pp.239-256, 1992.

J. Gerald and . Bierman, Factorization Methods for Discrete Sequential Estimation, vol.128, 1977.

S. Bonnabel, P. Martin, and P. Rouchon, Symmetry-Preserving Observers, IEEE Transactions on Automatic Control, vol.53, issue.11, pp.2514-2526, 2008.
URL : https://hal.archives-ouvertes.fr/hal-00449206

S. Bonnabel, P. Martin, and P. Rouchon, Non-linear symmetry-preserving observers on Lie groups, IEEE Transactions on Automatic Control, vol.54, issue.7, pp.1709-1713, 2009.

S. Bonnabel, P. Martin, and E. Salaün, Invariant extended Kalman filter: theory and application to a velocity-aided attitude estimation problem, Proceedings of the 48th IEEE Conference on, pp.1297-1304, 2009.
URL : https://hal.archives-ouvertes.fr/hal-00494342

G. Bourmaud, R. Mégret, M. Arnaudon, and A. Giremus, Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions, Journal of Mathematical Imaging and Vision, vol.51, issue.1, pp.209-228, 2015.
URL : https://hal.archives-ouvertes.fr/hal-01311170

G. Bourmaud, R. Mégret, A. Giremus, and Y. Berthoumieu, Discrete extended Kalman filter on Lie groups, Signal Processing Conference (EUSIPCO), 2013 Proceedings of the 21st European, pp.1-5, 2013.
URL : https://hal.archives-ouvertes.fr/hal-00903252

G. Bourmaud, R. Mégret, A. Giremus, and Y. Berthoumieu, From intrinsic optimization to iterated extended kalman filtering on lie groups, J. Math. Imaging Vis, vol.55, issue.3, pp.284-303, 2016.
URL : https://hal.archives-ouvertes.fr/hal-01311169

G. Bresson, Z. Alsayed, L. Yu, and S. Glaser, Simultaneous localization and mapping: A survey of current trends in autonomous driving, IEEE Transactions on Intelligent Vehicles, vol.2, issue.3, pp.194-220, 2017.
URL : https://hal.archives-ouvertes.fr/hal-01615897

M. Brossard, A. Barrau, and S. Bonnabel, Exploiting symmetries to design ekfs with consistency properties for navigation and slam, IEEE Sensors Journal, vol.19, issue.4, pp.1572-1579, 2019.
URL : https://hal.archives-ouvertes.fr/hal-02064782

M. Brossard, S. Bonnabel, and A. Barrau, Unscented kalman filter on lie groups for visual inertial odometry, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.649-655, 2018.
URL : https://hal.archives-ouvertes.fr/hal-01735542

M. Brossard, A. Barrau, and S. Bonnabel, RINS-W: Robust Inertial Navigation System on Wheels, IROS 2019, 2019.
URL : https://hal.archives-ouvertes.fr/hal-02057117

M. Brossard and S. Bonnabel, Learning Wheel Odometry and IMU Errors for Localization, International Conference on Robotics and Automation (ICRA), 2019.
URL : https://hal.archives-ouvertes.fr/hal-01874593

M. Brossard, S. Bonnabel, and J. P. Condomines, Unscented kalman filtering on lie groups, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.2485-2491, 2017.
URL : https://hal.archives-ouvertes.fr/hal-01489204

C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza et al., Simultaneous localization and mapping: Present, future, and the robust-perception age, 2016.

N. Carlevaris-bianco, M. Kaess, and R. M. Eustice, Generic node removal for factor-graph slam, IEEE Transactions on Robotics, vol.30, issue.6, pp.1371-1385, 2014.

P. Chauchat, A. Barrau, and S. Bonnabel, Kalman filtering with a class of geometric state equality constraints, 2017 IEEE 56th Annual Conference on Decision and Control (CDC), pp.2581-2586, 2017.
URL : https://hal.archives-ouvertes.fr/hal-01580569

P. Chauchat, A. Barrau, and S. Bonnabel, Invariant Smoothing on Lie Groups, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2018.
URL : https://hal.archives-ouvertes.fr/hal-01725847

G. S. Chirikjian, Classical Results and Geometric Methods. Applied and numerical harmonic analysis, vol.1, 2009.

. Gregory-s-chirikjian, Stochastic Models, Information Theory, and Lie Groups, vol.2, 2011.

H. Chiu, S. Williams, F. Dellaert, S. Samarasekera, and R. Kumar, Robust vision-aided navigation using sliding-window factor graphs, 2013 IEEE International Conference on Robotics and Automation, pp.46-53, 2013.

S. Choudhary, V. Indelman, H. I. Christensen, and F. Dellaert, Information-based reduced landmark slam, 2015 IEEE International Conference on Robotics and Automation (ICRA), pp.4620-4627, 2015.

R. Clark, M. Bloesch, J. Czarnowski, S. Leutenegger, and A. Davison, Learning to solve nonlinear least squares for monocular stereo, Proceedings of the European Conference on Computer Vision (ECCV), pp.284-299, 2018.

J. L. Crassidis, F. L. Markley, and Y. Cheng, Survey of nonlinear attitude estimation methods, Journal of Guidance, Control, and Dynamics, vol.30, issue.1, pp.12-28, 2007.

D. Crisan and A. Doucet, A survey of convergence results on particle filtering methods for practitioners, IEEE Transactions on Signal Processing, vol.50, issue.3, pp.736-746, 2002.

F. Dellaert and M. Kaess, Square root sam: Simultaneous localization and mapping via square root information smoothing, The International Journal of Robotics Research, vol.25, issue.12, pp.1181-1203, 2006.

F. Dellaert and M. Kaess, Factor graphs for robot perception, Foundations and Trends R in Robotics, vol.6, issue.1-2, pp.1-139, 2017.

J. Deyst and C. Price, Conditions for asymptotic stability of the discrete minimum-variance linear estimator, IEEE Transactions on Automatic Control, vol.13, issue.6, pp.702-705, 1968.

G. Dissanayake, P. Newman, H. F. Durrant-whyte, S. Clark, and M. Csobra, A solution to the simultaneous localisation and mapping (slam) problem, IEEE Trans. Robot. Automat, vol.17, pp.229-241, 2001.

F. Hugh, T. Durrant-whyte, and . Bailey, Simultaneous localization and mapping: part i, IEEE robotics & automation magazine, vol.13, issue.2, pp.99-110, 2006.

I. Josip?esi?, M. Markovi?, I. Bukal, and . Petrovi?, Extended information filter on matrix lie groups, Automatica, vol.82, pp.226-234, 2017.

N. Filipe, M. Kontitsis, and P. Tsiotras, Extended kalman filter for spacecraft pose estimation using dual quaternions, 2015 American Control Conference (ACC), pp.3187-3192, 2015.

J. R. Forbes and D. E. Zlotnik, Sigma point kalman filtering on matrix lie groups applied to the slam problem, Geometric Science of Information, pp.318-328, 2017.

C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, On-manifold preintegration for real-time visual-inertial odometry, IEEE Transactions on Robotics, vol.33, issue.1, pp.1-21, 2017.

C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, Svo: Semidirect visual odometry for monocular and multicamera systems, IEEE Transactions on Robotics, vol.33, issue.2, pp.249-265, 2017.

J. Gallier, Notes on differential geometry and lie groups, 2016.

C. L. Gentil, T. Vidal-calleja, and S. Huang, In2lama: Inertial lidar localisation and mapping, 2019 International Conference on Robotics and Automation (ICRA), pp.6388-6394, 2019.

J. S. Goddard and M. A. Abidi, Pose and motion estimation using dual quaternion-based extended Kalman filtering, Three-Dimensional Image Capture and Applications, vol.3313, pp.189-200, 1998.

G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard, A tutorial on graphbased slam, IEEE Intelligent Transportation Systems Magazine, vol.2, issue.4, pp.31-43, 2010.

J. Gutmann and K. Konolige, Incremental mapping of large cyclic environments, Computational Intelligence in Robotics and Automation, 1999. CIRA '99. Proceedings. 1999 IEEE International Symposium on, pp.318-325, 1999.

T. Haarnoja, A. Ajay, S. Levine, and P. Abbeel, Backprop kf: Learning discriminative deterministic state estimators, Advances in Neural Information Processing Systems, pp.4376-4384, 2016.

R. Hartley, G. Maani, R. M. Jadidi, J. W. Eustice, and . Grizzle, Contact-aided invariant extended kalman filtering for robot state estimation, 2019.

R. Hartley, J. Maani-ghaffari-jadidi, R. M. Grizzle, and . Eustice, Contact-aided invariant extended kalman filtering for legged robot state estimation, Proceedings of Robotics: Science and Systems, 2018.

A. Joel, D. G. Hesch, S. L. Kottas, S. Bowman, and . Roumeliotis, Camera-imu-based localization: Observability analysis and consistency improvement, The International Journal of Robotics Research, vol.33, issue.1, pp.182-201, 2014.

J. Hsiung, M. Hsiao, E. Westman, R. Valencia, and M. Kaess, Information sparsification in visual-inertial odometry, IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems, 2018.

B. Huang, J. Zhao, and J. Liu, A survey of simultaneous localization and mapping, 2019.

G. Huang, Visual-inertial navigation: A concise review, International Conference on Robotics and Automation (ICRA), pp.9572-9582, 2019.

P. Guoquan, A. I. Huang, S. I. Mourikis, and . Roumeliotis, Observability-based rules for designing consistent ekf slam estimators, Int. J. Rob. Res, vol.29, issue.5, pp.502-528, 2010.

K. Huang, J. Xiao, and C. Stachniss, Accurate direct visual-laser odometry with explicit occlusion handling and plane detection, 2019 International Conference on Robotics and Automation (ICRA), pp.1295-1301, 2019.

S. Huang, A review of optimisation strategies used in simultaneous localisation and mapping, Journal of Control and Decision, vol.6, issue.1, pp.61-74, 2019.

S. Huang and G. Dissanayake, A critique of current developments in simultaneous localization and mapping, International Journal of Advanced Robotic Systems, vol.13, issue.5, p.1729881416669482, 2016.

J. Simon, . Julier, and . Jeffrey-k-uhlmann, A counter example to the theory of simultaneous localization and map building, ICRA. IEEE International Conference on, vol.4, pp.4238-4243, 2001.

M. Kaess and F. Dellaert, Covariance recovery from a square root information matrix for data association, Robotics and autonomous systems, vol.57, issue.12, pp.1198-1210, 2009.

M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard et al., iSAM2: Incremental smoothing and mapping using the Bayes tree, The International Journal of Robotics Research, vol.31, pp.217-236, 2012.

R. E. Kalman, A new approach to linear filtering and prediction problems, Journal of basic Engineering, vol.82, issue.1, pp.35-45, 1960.

M. Kobilarov, D. Ta, and F. Dellaert, Differential dynamic programming for optimal estimation, 2015 IEEE International Conference on Robotics and Automation (ICRA), pp.863-869, 2015.

R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, g2o: A general framework for graph optimization, 2011 IEEE International Conference on Robotics and Automation, pp.3607-3613, 2011.

K. Lenac, I. Josip?esi?, I. Markovi?, and . Petrovi?, Exactly sparse delayed state filter on lie groups for long-term pose graph slam, The International Journal of Robotics Research, vol.37, issue.6, pp.585-610, 2018.

H. Liu, M. Chen, G. Zhang, H. Bao, and Y. Bao, Ice-ba: Incremental, consistent and efficient bundle adjustment for visual-inertial slam, The IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2018.

H. Loeliger, L. Bruderer, H. Malmberg, F. Wadehn, and N. Zalmai, On sparsity by nuv-em, gaussian message passing, and kalman smoothing, 2016 Information Theory and Applications Workshop (ITA), pp.1-10, 2016.

F. Lu and E. Milios, Globally consistent range scan alignment for environment mapping, Autonomous robots, vol.4, issue.4, pp.333-349, 1997.

T. Lupton and S. Sukkarieh, Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions, IEEE Transactions on Robotics, vol.28, issue.1, pp.61-76, 2012.

W. Ma, S. Wang, R. Hu, Y. Xiong, and R. Urtasun, Deep rigid instance scene flow, CVPR, 2019.

J. Maley and G. Huang, Unit quaternion-based parameterization for point features in visual navigation, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.6880-6886, 2018.

P. Martin and E. Salaün, Design and implementation of a low-cost observer-based attitude and heading reference system, Control Engineering Practice, vol.18, issue.7, pp.712-722, 2010.
URL : https://hal.archives-ouvertes.fr/hal-00494325

A. I. Mourikis, S. I. Roumeliotis, and J. W. Burdick, Sc-kf mobile robot localization: A stochastic cloning kalman filter for processing relative-state measurements, IEEE Transactions on Robotics, vol.23, issue.4, pp.717-730, 2007.

I. Anastasios, S. I. Mourikis, and . Roumeliotis, A multi-state constraint kalman filter for vision-aided inertial navigation, Proceedings 2007 IEEE International Conference on Robotics and Automation, pp.3565-3572, 2007.

R. Mur-artal and J. D. Tardós, Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras, IEEE Transactions on Robotics, vol.33, issue.5, pp.1255-1262, 2017.

J. Peter and . Olver, Classical Invariant Theory, 1999.

G. Parlangeli, P. Pedone, and G. Indiveri, Relative pose observability analysis for 3d nonholonomic vehicles based on range measurements only, 9th IFAC Conference on Manoeuvring and Control of Marine Craft, vol.45, pp.182-187, 2012.

M. Pilté, S. Bonnabel, and F. Barbaresco, Tracking the frenet-serret frame associated to a highly maneuvering target in 3d, 2017 IEEE 56th Annual Conference on Decision and Control (CDC), pp.1969-1974, 2017.

P. Tong-qin, S. Li, and . Shen, Vins-mono: A robust and versatile monocular visual-inertial state estimator, IEEE Transactions on Robotics, vol.34, issue.4, pp.1004-1020, 2018.

C. T. Herbert-e-rauch, F. Striebel, and . Tung, Maximum likelihood estimates of linear dynamic systems, AIAA journal, vol.3, issue.8, pp.1445-1450, 1965.

T. Sandy, L. Stadelmann, S. Kerscher, and J. Buchli, Confusion: Sensor fusion for complex robotic systems using nonlinear optimization, IEEE Robotics and Automation Letters, vol.4, issue.2, pp.1093-1100, 2019.

W. Shao, S. Vijayarangan, C. Li, and G. Kantor, Stereo visual inertial lidar simultaneous localization and mapping. CoRR, abs, 1902.

D. Simon, Kalman filtering with state constraints: a survey of linear and nonlinear algorithms, IET Control Theory Applications, vol.4, issue.8, pp.1303-1318, 2010.

A. Singandhupe and H. La, A review of slam techniques and security in autonomous driving, vol.02, p.2019

C. Stachniss, J. John, S. Leonard, and . Thrun, Simultaneous localization and mapping, Springer Handbook of Robotics, pp.1153-1176, 2016.

J. Uhlmann, Unit consistency, generalized inverses, and effective system design methods, 2016.

. Jeffrey and . Uhlmann, A generalized matrix inverse that is consistent with respect to diagonal transformations, SIAM Journal on Matrix Analysis and Applications, vol.39, issue.2, pp.781-800, 2018.

J. Vallvé, J. Solà, and J. Andrade-cetto, Graph slam sparsification with populated topologies using factor descent optimization, IEEE Robotics and Automation Letters, vol.3, issue.2, pp.1322-1329, 2018.

F. Wadehn, L. Bruderer, V. Sahdeva, and H. Loeliger, New square-root and diagonalized kalman smoothers, 54th Annual Allerton Conference on Communication, Control, and Computing (Allerton), pp.1282-1290, 2016.

A. Walsh, J. Arsenault, and J. R. Forbes, Invariant sliding window filtering for attitude and bias estimation, 2019 American Control Conference (ACC), pp.3161-3166, 2019.

K. Wu, A. Ahmed, A. Georgios, S. Georgiou, and . Roumeliotis, A square root inverse filter for efficient vision-aided inertial navigation on mobile devices, Robotics: Science and Systems, vol.2, 2015.

J. Zhang and S. Singh, Loam: Lidar odometry and mapping in real-time, Proceedings of Robotics: Science and Systems Conference, 2014.

L. Zhao, S. Huang, and G. Dissanayake, Linear slam: A linear solution to the feature-based and pose graph slam based on submap joining, IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.24-30, 2013.

S. Zhao, Y. Chen, H. Zhang, and J. A. Farrell, Differential gps aided inertial navigation: a contemplative realtime approach. IFAC Proceedings Volumes, vol.47, pp.8959-8964, 2014.