Invariant EKF - umrover/mrover-ros2 GitHub Wiki

Overview: Extended Kalman Filter

kalman-filters

Please read over the Kalman Filter and Sensor Fusion section of the Localization page and view the additional resources before starting here. The Extended Kalman Filter (EKF) is a version of the Kalman filter adapted for nonlinear systems, and is the standard used in navigation systems today. Instead of representing process dynamics through a constant matrix, which is only suitable for linear processes, the EKF uses the Jacobian of the process dynamics model. Evaluated at the current state, it provides a local linear approximation for an otherwise nonlinear process. This approximation is then used in the regular Kalman filter as matrix $A$. You can think of this as similar to a tangent line approximation you may have done in calculus.

Highly recommend checking out these resources for a more detailed explanation and algorithm equations:

Invariant EKF

Although the EKF is powerful, the dependence of the error covariance $P$ on the state estimate sometimes leads to faulty convergence. Since the error covariance and state estimate depend on each other, inaccuracies in one lead to inaccuracies in the other, causing a compounding error to be generated by the filter's feedback loop. To solve this problem, the invariant EKF uses properties of Lie groups to recast the error dynamics to a linear model. This means that the error covariance no longer depends on the state estimate, which ultimately leads to better convergence.

I-EKF resources:

Goals and steps

Derive measurement and process dynamics models for our system, implement a proof of concept

We are currently developing a right-invariant EKF algorithm in MATLAB in the loc/iekf branch. Our goal is to fuse information from a variety of sensors in the RI-EKF by incorporating their data in the filter's prediction and correction steps.

Sensors used in the prediction step typically don't provide a direct measurement of the rover's pose. Instead, information about the rover's pose can be derived using what we know about physics and kinematics.

  • gyroscope
  • accelerometer
  • horizontal/vertical velocity

Sensors used in the correction step are usually direct measurements of the rover's pose.

  • RTK position
  • magnetometer
  • accelerometer (can provide roll/pitch)
  • RTK heading/pitch

Test in simulation and on rover

After implementing a proof of concept, we would like to test it using some drive data and in simulation before uploading it onto the real rover.