↑ Autonomous Aerial Robotics

Project 3 - Phase 2: Augmented State EKF

Date
Keywords

Augmented State EKF, IMU, PnP, Stereo VO, Sensor Fusion, State Estimation

Stack

C++, ROS (Docker with image: osrf/ros:kinetic-desktop-full-xenial, Visualization GUI: theasp/novnc:latest), RViz

Links
Code

I extended the EKF to fuse absolute pose measurements from PnP with relative pose measurements from stereo visual odometry. The goal was to maintain state estimation when either measurement source was intermittently unavailable.

Implementation Details

The implementation follows the derivations in the notes shown below.

aug_ekf_prediction
aug_ekf_update
  • First, initialize the filter with the first PnP frame.
  • Check the type of the next frame/state to see which function to call. Based on this frame’s timestamp, this frame will be inserted into the deque, and then all the frames in the deque later than this new frame will be repropagated.
  • After repropagation, the old states will be removed from the deque, and a new odometry will be published.

Estimations in RViz

On the small rosbag (given absolute poses from PnP & relative poses from stereo visual odometry):

On the large rosbag (no given pre-processed absolute/relative poses, all from scratch):

Note:

  • The red arrows are the odometry from the augmented state EKF
  • The blue arrows are the odometry from the PnP
  • The yellow arrows are the odometry from the stereo visual odometry
  • The green path is from the augmented state EKF estimation