Invariant EKF

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:

public ROB 530 lectures: https://www.youtube.com/watch?v=3ZBzZigz8GQ, https://www.youtube.com/watch?v=-woCjTB9Blo
github repo of ROB 530 resources (code examples): https://github.com/UMich-CURLY-teaching/UMich-ROB-530-public
https://kth.diva-portal.org/smash/get/diva2:1632658/FULLTEXT01.pdf

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.