Kalman filter
Invariant extended Kalman filter.