Project 3 - Phase 2: Augmented State EKF

Published:

  • Keywords: Augmented State EKF, IMU, PnP, Stereo VO, Sensor Fusion, State Estimation
  • Coding language: C++, ROS (Docker with image: osrf/ros:kinetic-desktop-full-xenial, Visualization GUI: theasp/novnc:latest), RViz
  • Detailed code implementation can be found in the Github code repo

Need to extend the EKF to an Augmented State EKF, to fuse not only absolute pose measurements from PnP, but also relative pose measurements from the stereo visual odometry. The final filter should be able to account for failures in any one of the measurements, i.e., both the absolute and relative pose measurement may be unavailable sometimes.

Implementation Details

Strictly follow the derivations in the notes, which is shown as follows:

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