PaperlayerView exact revision on arXiv

Source-linked AI summary

Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation

Ross Hartley, Maani Ghaffari, Ryan M. Eustice, Jessy W. Grizzle

arXiv:1904.09251v2cs.RO

TL;DR

Legged robots need accurate pose and velocity estimates, but vision can be sensitive to lighting and environment, while standard EKFs have state-dependent linearizations and local convergence limitations. The paper develops a contact-aided InEKF that combines contact-inertial dynamics with forward kinematic corrections and exploits Lie-group symmetries. Simulations and Cassie-series experiments report better convergence than a quaternion-based EKF and demonstrate motion-capture evaluation and LiDAR mapping.

  • Problem

    Legged-robot pose and velocity estimation must support stability and walking, but vision is vulnerable to lighting and environmental changes and standard EKFs can diverge from poor initialization.

  • Method

    The paper formulates a contact-aided InEKF using Lie groups, combining IMU and contact dynamics with forward kinematic corrections and supporting IMU biases and multiple estimator formulations.

  • Results

    The proposed system shows better convergence performance than a commonly used quaternion-based EKF in real experiments and supports motion-capture accuracy evaluation and LiDAR-based mapping on Cassie.

  • Takeaways & Limitations

    The developed observer provides a proprioceptive state-estimation approach for legged robots and can supply pose estimates for LiDAR-based point-cloud mapping while walking.

  • Takeaways & Limitations

    Adding landmark observations can make the filter too computationally expensive for real-time operation as the number of landmarks grows.

Abstract

from arXiv · show

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 contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system.

1. Introduction

The paper develops a contact-aided invariant extended Kalman filter for proprioceptive legged-robot state estimation, addressing limitations of vision-dependent and standard EKF approaches. It derives, evaluates, and extends the observer for practical bipedal robotics.

  • Motivation: Vision-based state estimation can be disrupted by rapid lighting and environmental changes, motivating high-frequency estimation from proprioceptive sensors.
  • Findings: The authors report improved convergence and consistency properties, including better convergence than the commonly used quaternion-based EKF in real experiments.
  • Approach: The proposed InEKF fuses IMU and contact dynamics with forward kinematic corrections to estimate base pose, velocity, and contact states.
  • Extensions: The paper extends the observer with IMU biases, alternative left-invariant and right-invariant formulations, world-centric and robo-centric estimators, analytical discretization, and changing contact states.
  • Evaluation: Simulations and Cassie-series biped experiments compare the proposed observer with a quaternion-based EKF, including motion-capture accuracy and LiDAR terrain mapping.
  • Invariant formulation: System symmetries make the InEKF error dynamics log-linear and autonomous, while its linearized dynamics and observation model do not depend on the current state estimate.

4. A motivating example: 3D orientation propagation

The motivating example contrasts state-dependent Euler-angle linearization with a Lie-group orientation representation whose tangent-space error dynamics are autonomous and linearized independently of the state estimate.

  • Euler-angle representation: Euler-angle error dynamics depend nonlinearly on the error, inputs, and current state.Covariance propagation requires linearizing around the current estimate, so poor angle estimates reduce linearization accuracy and filter consistency.
  • Rotation-matrix representation: The rotation-matrix representation defines an invariant error whose dynamics are autonomous, depending directly on the error rather than the state estimate.The error is redefined on the Lie group and then represented in its tangent space.
  • Log-linear error dynamics: A first-order exponential-map approximation makes the tangent-space error dynamics linear and independent of the state estimate.Wrong state estimates therefore do not directly reduce linearization accuracy, improving filter accuracy and consistency.
  • Log-linear error dynamics: For this system, the apparently approximate exponential-map linearization is exact in the sense that known initial error permits recovery of the nonlinear error dynamics.This log-linear property motivates extending the approach from SO(3) to contact-aided inertial estimation for legged robots.

5. SEN+2(3) Continuous Right-Invariant EKF

This section formulates a right-invariant EKF for legged-robot contact-inertial navigation on an extended matrix Lie group, using IMU, contact, and forward-kinematic information. Its invariant error dynamics and observation structure support state-independent linearization and an observability analysis consistent with the nonlinear system, with an explicit sensor-noise caveat.

  • Filter formulation: The RIEKF estimates a legged robot’s base pose and velocity with any finite number of contact points in a static environment.The same formulation can apply to manipulators when the stated contact assumptions hold, and IMU biases are initially omitted.
  • State representation: The state combines IMU orientation, velocity, position, and all world-frame contact-point positions in an extension of SE(3).Identical contact-point models permit derivation using a single contact point without loss of generality.
  • System dynamics and measurements: IMU angular velocity and acceleration drive the dynamics, while contact sensing assumes zero contact-point velocity with white Gaussian noise to accommodate slippage.Forward kinematics supplies contact orientation and relative-position information from encoder measurements.
  • Invariant prediction: The group-affine deterministic dynamics make left- and right-invariant error dynamics independent of the system state, with a log-linear error representation.The prediction step propagates the state through deterministic dynamics and covariance through a Riccati equation.
  • Observability analysis: The discrete RIEKF preserves the nonlinear system’s unobservable absolute position and yaw states without state-estimate-dependent rank artifacts.The observability result agrees with prior nonlinear analysis while requiring less computation.

6. Simulation Results

Simulation results compare the proposed invariant filter with a quaternion-based EKF, showing faster convergence, more accurate linearized dynamics, and better uncertainty-shape representation.

  • Convergence Comparison: The RIEKF converges considerably faster than the QEKF for all observable states in 100 randomized Cassie simulations.Both filters used identical measurements, noise statistics, initial covariance, and randomized initial orientations and velocities.
  • Accuracy of Linearized Dynamics: The QEKF linearization error grows with increasing initial error, whereas the InEKF propagated error remains exactly accurate in the deterministic case.The deterministic test propagated states and error dynamics for 1 second using randomly sampled IMU measurements.
  • Accuracy of Linearized Dynamics: With sensor noise, the InEKF log-linear dynamics are no longer exact but remain more accurate because they are less sensitive to initial state errors.This reduced sensitivity helps explain the faster convergence observed in the convergence comparison.
  • Covariance Ellipse Comparison: The InEKF represents coupled orientation-position uncertainty with curved distributions, unlike the QEKF’s Gaussian ellipse.The distributions were evaluated using 10,000 samples from filter covariances during an 8-second, 1 m/s walking simulation.
  • Covariance Ellipse Comparison: The InEKF can model complete yaw uncertainty, which cannot be captured by a standard Gaussian covariance ellipse.The complete-yaw-uncertainty experiment used an initial yaw standard deviation of 360 deg and evaluated distributions every 2 seconds.

7. IMU bias augmentation

The filter is augmented to estimate slowly varying IMU biases, but adding biases removes the exact group-affine guarantees and yields an imperfect InEKF.

  • Scope of Guarantees: Because no Lie group preserves the group-affine property after adding biases, the theoretical RIEKF properties no longer hold exactly.The paper describes the resulting estimator as an “imperfect InEKF” that can nevertheless outperform a standard EKF.
  • State Augmentation: IMU bias augmentation adds gyroscope and accelerometer biases as parameters in an augmented state on G × R6.The augmented state combines the original matrix Lie group with a six-dimensional parameter vector.
  • Bias Model: The bias parameters are modeled as slowly varying Brownian-motion processes whose derivatives are white Gaussian noise.This model captures the time-varying nature of the bias terms.
  • Augmented Dynamics: With biases, the linearized invariant error dynamics depend on the estimated trajectory through noise and bias errors.When bias errors vanish, the trajectory dependence disappears.
  • Measurement Update: Forward-kinematic position measurements do not depend on IMU biases, so the observation matrix is augmented with zero columns.The covariance and state are then propagated and corrected using the augmented equations.

8. Addition and Removal of Contact Points

The observer supports changing contact configurations by marginalizing removed contacts and augmenting new contacts with forward-kinematic initialization and covariance mappings.

  • Contact Events: Contact states must be added or removed because legged-robot contacts are discrete events that change during navigation.The formulation addresses both contact creation and contact breaking.
  • Contact Removal: Removing a contact marginalizes its state by deleting the corresponding matrix-Lie-group row and column and eliminating matching covariance elements.The reduced covariance is computed through a linear transformation.
  • Contact Removal: The contact-removal marginalization matrix does not depend on whether the estimator uses right- or left-invariant error.This makes the removal operation common to both invariant formulations.
  • Contact Addition: Adding a contact augments the state and covariance, initializing the new contact mean from forward kinematics.A corresponding linear map is used to compute the augmented covariance.
  • Contact Addition: The covariance-augmentation and noise matrices depend on the chosen invariant error, with the displayed derivation using the right-invariant formulation.The left-invariant formulation produces different matrices.

9. Experimental Results on Cassie Robot

Experiments on Cassie evaluated convergence, state-estimation accuracy, long-term odometry, and LiDAR mapping using the proposed invariant filter.

  • Convergence Comparison: The RIEKF converged faster and more reliably than the QEKF in all 100 experimental runs.Both filters used identical logged measurements, noise statistics, initial covariance, and randomized initial orientations and velocities.
  • Convergence Comparison: When initialized far from the true state, the RIEKF consistently converged faster because its invariant error dynamics were less sensitive to the current estimate.With close initialization, the RIEKF and QEKF showed similar performance.
  • Motion Capture Accuracy: Motion-capture evaluation used 18 Qualisys cameras for position tracking during a 60-second, approximately 15 m untethered walk.Orientation ground truth was obtained from a VectorNav-100 because the motion-capture orientation estimates were poor.
  • Motion Capture Accuracy: The InEKF trajectory exhibited drift from unobservable position and yaw, while the final position drift was less than 5% of the distance traveled.The reported motion-capture experiment used a top-down trajectory comparison.
  • Long-Term Odometry: During a 200 m sidewalk walk lasting 7 minutes and 45 seconds, InEKF odometry kept the estimate on the sidewalk and ended within a few meters of the true position.The reported yaw drift was imperceptible, despite absolute position being unobservable.
  • LiDAR Mapping Application: The InEKF pose trajectory enabled LiDAR point-cloud mapping by transforming received data into the world frame and accumulating it into an environment map.The mapping experiment used a Velodyne VLP-32C LiDAR mounted on Cassie’s torso.

10. Alternative Left-Invariant Formulation

The filter also admits a left-invariant formulation, suitable for left-invariant observations, with exact transformations between left- and right-invariant error representations.

  • 10. Alternative Left-Invariant Formulation: The left-invariant filter is an alternative to the right-invariant derivation and may suit left-invariant observations such as GPS for a world-centric observer.The right-invariant derivation was selected because forward-kinematic measurements have a right-invariant observation form.
  • 10. Alternative Left-Invariant Formulation: The left-invariant error dynamics can be written as a log-linear system with corresponding dynamics and noise matrices.The derivation proceeds through the left-invariant error, chain-rule expansion, and first-order approximation.
  • 10. Alternative Left-Invariant Formulation: Left-invariant dynamics depend on the state through the IMU bias, and left-invariant observations use an update with the exponential map multiplied on the right.The state dependence is limited to the IMU bias in the described dynamics.
  • 10.1. Switching Between Left and Right-Invariant Errors: Forward-kinematic innovations are autonomous only with the right-invariant error, but the adjoint map enables switching between left- and right-invariant forms.This frame choice follows from the observation type associated with the measurement model.
  • 10.1. Switching Between Left and Right-Invariant Errors: The adjoint transformation exactly maps left- and right-invariant covariances, allowing temporary right-invariant updates before mapping the covariance back.This procedure applies when a right-invariant observation is received during left-invariant propagation.
  • 10.1. Switching Between Left and Right-Invariant Errors: The left- and right-invariant dynamics and noise matrices are related by explicit transformations derived from covariance propagation.The left-invariant dynamics can also be obtained by substituting the error transformation into the right-invariant covariance equation.
  • 10.2. Adding New Contact Points: Adding or removing contacts uses the same state relations as the right-invariant case, but covariance augmentation changes with the error variable.The new covariance is computed from the left-invariant error through a linear map.

11. Robo-centric Estimator

The robo-centric InEKF represents states in the robot’s base frame, swaps the invariant observation and error formulations, and retains the world-centric filter’s properties.

  • Robo-centric Formulation: The robo-centric estimator measures the state in the robot’s base or IMU frame, making forward-kinematic measurements left-invariant.Its left- and right-invariant error dynamics correspond to swapped versions of the world-centric formulation.
  • Robo-centric Formulation: The robo-centric filter has the same properties as the right-invariant estimator but directly estimates body-frame velocity useful for legged-robot control.The formulation may therefore be preferred when body-frame control states are desired.
  • State Representation: For N contact points, the robo-centric state is represented as the inverse of the world-centric matrix state.The formulation estimates the corresponding body-frame states and contact positions.
  • State Representation: Inverting the world-centric state negates body velocity, while position-vector signs are removed by swapping the start and end points.The sign convention follows directly from the state inversion and frame redefinition.
  • Robot-centric Dynamics: The derivation can assume a single contact point and redefine the shorthand notation for body-centric states without loss of generality.Bias-corrected IMU measurements are used to derive the robot-centric dynamics in matrix form.
  • Robot-centric Dynamics: The deterministic robot-centric dynamics satisfy the group-affine property, so invariant error dynamics depend solely on the invariant error.Without IMU bias, the body-centric left-invariant linearization matches the world-centric right-invariant dynamics.
  • Robot-centric Dynamics: With IMU bias, the correspondence remains but bias terms are negated, and the body-centric noise covariances are swapped versions of the world-centric matrices.The covariance relation accounts for redefining the estimated state as its inverse.
  • Robot-centric Measurement Model: Forward kinematics measures contact position relative to the body, producing a left-invariant observation model whose linearization is the world-centric model with H_t negated.The noise-related matrix remains identical after accounting for the inverse-state redefinition.

12. Additional Sensor Observations

The InEKF framework accommodates additional observations and connects contact estimation to landmark-based SLAM, while experiments demonstrate odometry and LiDAR mapping applications.

  • Additional Sensor Observations: The framework relates contact-aided InEKF measurements to other robotics sensors, including magnetometer and acceleration measurements.These observations are discussed as fitting invariant observation models.
  • Landmark-based SLAM: Contact-aided InEKF and landmark-based SLAM have analogous formulations because contact positions can play the role of static landmarks measured relative to the robot.The analogy concerns jointly estimating the robot state and fixed environmental positions.
  • Landmark-based SLAM: Contact states can be replaced by landmark states, or combined with landmark positions, to form an observer containing no unobservable states.This extension follows from the structural similarity between contacts and landmarks.
  • Landmark-based SLAM: If the number of landmarks grows too large, the resulting filter can become too computationally expensive for real-time operation.This is identified as a limitation analogous to the computational cost of EKF-SLAM.
  • Summary of Formulations: The paper summarizes world-centric and robo-centric equations and provides invariant observation forms for several sensors in Tables 2 and 3.The tables assume a single contact and landmark position and define shorthand for true landmark and magnetic-field vectors.
  • Experimental Demonstrations: Cassie experiments demonstrated invariant-EKF accuracy through motion capture, long-term odometry overlaid on Google Earth, and LiDAR point-cloud maps while walking.These experiments connect the estimator to feedback signals, odometry, and environment mapping.
  • Future Work: Future work includes invariant smoothing, online kinematic-parameter estimation, mode-change detection, visual-inertial-contact odometry, and prior terrain information.These directions are presented as possible extensions for improving estimation and mapping.
  • Implementation: The implemented filters use discretized continuous-time equations with a zero-order hold on inertial measurements and analytical integration.Analytical integration was important for preserving the required error-dynamics property.

A.1. World-centric Dynamics

The world-centric continuous dynamics are analytically integrated into discrete rotation, velocity, and position updates, assuming constant IMU measurements over each interval. Auxiliary SO(3) functions provide computable integral expressions and closed-form implementations.

  • A.1. World-centric Dynamics: The discrete world-centric rotation, velocity, and position dynamics are obtained by analytically integrating the continuous-time system.The derivation includes the orientation update, velocity integral, and double integral for position.
  • A.1. World-centric Dynamics: Under a zero-order hold, orientation is updated with the SO(3) exponential map over ∆t.The interval is defined as ∆t ≜ tk+1−tk.
  • A.1. World-centric Dynamics: Closed-form expressions are available for these quantities, enabling fast computation.The closed forms are presented as an implementation alternative to the series-based expressions.
  • A.1. World-centric Dynamics: The exact discrete dynamics assume IMU measurements remain constant between tk and tk+1.This assumption is the stated condition for exact integration of the continuous-time system.

A.2. Covariance Propagation

Covariance propagation uses analytical solutions for the state transition and discrete noise covariance matrices, with different computational properties for left- and right-invariant errors. In practice, the article uses an approximation for the discrete noise matrix.

  • A.2. Covariance Propagation: Covariance propagation requires solving a continuous-time Riccati equation and its associated state-transition dynamics.The state-transition matrix satisfies d/dt Φ(t, tk) = AtΦ(t, tk) with Φ(tk, tk) = I.
  • A.2. Covariance Propagation: For world-centric left-invariant errors, constant IMU inputs and estimated biases allow the state-transition matrix to be computed with a matrix exponential.The left-invariant error dynamics matrix depends only on those inputs and bias terms.
  • A.2. Covariance Propagation: The left-invariant transition terms have analytical solutions involving integrals parameterized by φ ≜ ∥¯ωk∥ and θ ≜ φ∆t.The matrices Ψ1 and Ψ2 require more complicated but still analytically solvable integrals.
  • A.2. Covariance Propagation: Right-invariant state-transition dynamics depend on the changing state estimates ¯Rt, ¯vt, and ¯pt, preventing the same simple matrix-exponential computation.This contrasts with the left-invariant case under constant inputs and biases.
  • A.2. Covariance Propagation: Left- and right-invariant transition and discrete-noise matrices are related through the group adjoint, enabling an alternative right-invariant computation.The alternative can simplify implementation when software libraries provide efficient matrix-exponential routines.
  • A.2. Covariance Propagation: Although the discrete noise covariance has an analytical solution, the article uses an approximation for all reported results.The approximation is identified as the practical implementation choice.

B. Useful Lie Group Expressions

The paper formulates useful expressions on SEK(3), the matrix Lie group of K direct isometries, including its group action, adjoint, Lie-algebra hat operator, and exponential map.

  • B. Useful Lie Group Expressions: SEK(3) consists of a rotation R ∈ SO(3) and K vectors p1, · · ·, pK in R3.An element is represented as a (3 + K) × (3 + K) matrix.
  • B. Useful Lie Group Expressions: The group action is matrix multiplication, while the adjoint maps vectors between tangent spaces of group elements.The paper gives the matrix representation of the SEK(3) adjoint.
  • B. Useful Lie Group Expressions: The Lie-algebra hat operator represents ξ ≜ vec(φ, ξ1, · · ·, ξK) as a matrix.The rotational block uses the skew-symmetric matrix (φ)×.
  • B. Useful Lie Group Expressions: The exponential map sends Lie-algebra vectors to SEK(3), using Γ0(φ) for rotation and Γ1(φ) for the vector components.The resulting matrix contains Γ0(φ) and Γ1(φ)ξi blocks.

C. Error-state Conversions

The paper relates invariant, quaternion-based EKF, and Euclidean orientation errors so estimates and covariances can be compared, initialized, and plotted consistently. These relations use exact adjoint mappings or first-order approximations depending on the error pair.

  • C. Error-state Conversions: The document uses left/right-invariant errors, QEKF error states, and Euclidean orientation errors for filtering, initialization, comparison, and covariance plotting.Conversions are needed to provide fair comparisons when filters start with identical covariance matrices.
  • C. Error-state Conversions: The QEKF local- and global-frame orientation errors correspond to the left- and right-invariant SO(3) errors, respectively.The document chooses the local-frame orientation error for the QEKF.
  • C. Error-state Conversions: Euclidean orientation error is defined as δφt ≜ φt − ¯φt and approximates the right-invariant orientation error when errors are small.This mapping is used when plotting individual axes of the orientation error covariance hull.
  • C. Error-state Conversions: First-order approximations also relate right-invariant errors to QEKF velocity errors and position errors.The same conversion process is repeated for the position states.
Loading 1904.09251v2…