Invariant Kalman Filter Application to Optical Flow Based Visual Odometry for UAVs

Optical flow based visual odometry for UAVs has become akin to wheel encoders for ground based robots. While sensors such as laser rangefinders and Global Positioning System (GPS) receivers can provide measurements of a UAV's position, these sensors typically have a low bandwidth and can become degraded (e.g. GPS in urban canyons). Optical flow sensors provide a robust high bandwidth pseudo-velocity measurement by tracking the movement of a feature through a camera image and measuring the distance to that feature, typically using a sonar or a lidar sensor. Optical flow based visual odometry thus compliments low bandwidth UAV position measurements. We have previously used a simple linear measurement equation to approximate the optical flow as a pseudo-velocity measurement and were able to achieve fully autonomous mission flights without GPS both indoors and outdoors. This estimator, known as Local Position Estimator (LPE), is now part of the open source PX4 autopilot. In this work, we seek to improve the UAV's performance in terms of maximum speed and robustness by deriving an estimator using the full nonlinear measurement equations and by basing the estimator on the Invariant Extended Kalman Filter (IEKF). Through intelligent choice of the frame in which the estimator dynamics and measurement equations are linearized, the IEKF is able to reduce the fluctuations in the Kalman filter along typical vehicle trajectories and produce a more optimal estimate. We compare our previous algorithm, LPE, with our new algorithm, IEKF, using the PX4 gazebo based software in the loop simulator.