Single file implementation of an IEKF with Landmarks in Python, directly ported from https://github.com/RossHartley/invariant-ekf
Equation and section pointers to: Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation - please make a pull request with any corrections.
