Invariant smoothing on Lie Groups

Chauchat, Paul; Barrau, Axel; Bonnabel, Silvère

doi:10.1109/iros.2018.8594068

Cited by 24 publications

(16 citation statements)

References 33 publications

Supporting

Mentioning

Contrasting

Order By: Relevance

“…Future work includes developing an invariant smoother based on this filter. This would utilize the framework developed by Chauchat et al (2018) to potentially improve IMU preintegration (Eckenhoff et al, 2018; Forster et al, 2017; Lupton and Sukkarieh, 2012) as well as contact preintegration (Hartley et al, 2018a) to perform SLAM. One interesting extension would be online estimation of kinematic parameters, which may help remove biases in the forward kinematic measurements.…”

Section: Discussionmentioning

confidence: 99%

Contact-aided invariant extended Kalman filtering for robot state estimation

Hartley

Ghaffari

Grizzle

et al. 2020

The International Journal of Robotics Research

195

156

View full text Add to dashboard Cite

Legged robots require knowledge of pose and velocity in order to maintain stability and execute walking paths. Current solutions either rely on vision data, which is susceptible to environmental and lighting conditions, or fusion of kinematic and contact data with measurements from an inertial measurement unit (IMU). In this work, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. This filter combines contact-inertial dynamics with forward kinematic corrections to estimate pose and velocity along with all current contact points. We show that the error dynamics follows a log-linear autonomous differential equation with several important consequences: (a) the observable state variables can be rendered convergent with a domain of attraction that is independent of the system's trajectory; (b) unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which (c) leads to improved convergence properties and (d) a local observability matrix that is consistent with the underlying nonlinear system. Furthermore, we demonstrate how to include IMU biases, add/remove contacts, and formulate both world-centric and robo-centric versions. We compare the convergence of the proposed InEKF with the commonly used quaternion-based EKF though both simulations and experiments on a Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture, while a LiDAR mapping experiment provides a practical use case. Overall, the developed contactaided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system. changes in lighting as well as the operating environment. It is therefore beneficial to develop a low-level state estimator that fuses data only from proprioceptive sensors to form accurate high-frequency state estimates. This approach was taken by Bloesch et al. [15] when developing a quaternion-based extended Kalman filter (QEKF) that combines inertial, contact, and kinematic data to estimate the robot's base pose, velocity, and a number of contact states. In this article, we expand upon these ideas to develop an invariant extended Kalman filter (InEKF) that has improved convergence and consistency properties allowing for a more robust observer that is suitable for long-term autonomy.The theory of invariant observer design is based on the estimation error being invariant under the action of a matrix Lie group [1,20], which has recently led to the development of the InEKF 1 [18,8,10,11] with successful applications and promising results in simultaneous localization and mapping [8,86] and aided inertial navigation systems [4,5,8,83]. The invariance of the estimation error with respect to a Lie group action is referred to as a symmetry of the system [5]. Summarized briefly, Barrau and Bonnabel [10] showed that if the state is defined on a Lie group, and the dynamics satisfy a particular "group affin...

show abstract

Section: Discussionmentioning

confidence: 99%

Contact-aided invariant extended Kalman filtering for robot state estimation

Hartley

Ghaffari

Grizzle

et al. 2020

The International Journal of Robotics Research

195

156

View full text Add to dashboard Cite

show abstract

“…After solving (8)-(9), the linearization point is updated as χ (1) = χ (0) + δ χ * and serves as a new linearization point until convergence. Remark 1: This easily generalises for smoothing onmanifold, see for instance [9], [24], [18], by simply changing the prior factor and update definitions, and adapting the jacobians accordingly. To save space, we omit it herein.…”

Section: Resolution Of the Nonlinear Optimization Problemmentioning

confidence: 99%

“…We see at each step the algorithm is faced with the resolution of the linearized optimization problem (8)- (9). This is a standard least squares problem, and the solution comes in closed form.…”

Section: E Resolution Of the Linearized Optimization Problemmentioning

confidence: 99%

“…Remark 1: This easily generalises for smoothing onmanifold, see for instance [9], [24], [18], by simply changing the prior factor and update definitions, and adapting the jacobians accordingly. To save space, we omit it herein.…”

Section: Resolution Of the Nonlinear Optimization Problemmentioning

confidence: 99%

“…But a large discrepancy between the most accurate and least accurate sensor leads to ill conditioned information matrices. This may also impact linearization errors [18], [9]. Numerical issues may thus degrade the solvers' computed solution, and in turn the state estimate accuracy and consistency.…”

Section: Introductionmentioning

confidence: 99%

See 2 more Smart Citations

Factor Graph-Based Smoothing Without Matrix Inversion for Highly Precise Localization

Chauchat¹,

Barrau

Bonnabel

2021

IEEE Trans. Contr. Syst. Technol.

Self Cite

View full text Add to dashboard Cite

We consider the problem of localizing a manned, semi-autonomous, or autonomous vehicle in the environment using information coming from the vehicle's sensors, a problem known as navigation or simultaneous localization and mapping (SLAM) depending on the context. To infer knowledge from sensors' measurements, while drawing on a priori knowledge about the vehicle's dynamics, modern approaches solve an optimization problem to compute the most likely trajectory given all past observations, an approach known as smoothing. Improving smoothing solvers is an active field of research in the SLAM community. Most work is focused on reducing computation load by inverting the involved linear system while preserving its sparsity. The present paper raises an issue which, to the knowledge of the authors, has not been addressed yet: standard smoothing solvers require explicitly using the inverse of sensor noise covariance matrices. This means the parameters that reflect the noise magnitude must be sufficiently large for the smoother to properly function. When matrices are close to singular, which is the case when using high precision modern inertial measurement units (IMU), numerical issues necessarily arise, especially with 32-bits implementation demanded by most industrial aerospace applications. We discuss these issues and propose a solution that builds upon the Kalman filter to improve smoothing algorithms. We then leverage the results to devise a localization algorithm based on fusion of IMU and vision sensors. Successful real experiments using an actual car equipped with a tactical grade high performance IMU and a LiDAR illustrate the relevance of the approach to the field of autonomous vehicles.

show abstract

State estimation of hydraulic quadruped robots using invariant-EKF and kinematics with neural networks

Yang

Zhu

et al. 2023

Neural Comput & Applic

View full text Add to dashboard Cite

Invariant smoothing on Lie Groups

Cited by 24 publications

References 33 publications

Contact-aided invariant extended Kalman filtering for robot state estimation

Contact-aided invariant extended Kalman filtering for robot state estimation

Factor Graph-Based Smoothing Without Matrix Inversion for Highly Precise Localization

State estimation of hydraulic quadruped robots using invariant-EKF and kinematics with neural networks

Contact Info

Product

Resources

About