Project 3 - Phase 1: Extended Kalman Filter (EKF)

Published:

  • Keywords: Extended Kalman Filter (EKF), IMU, PnP, 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

Implement the EKF-based state estimation for the quadrotor UAV. Need to fuse the odometry obtained from the tag_detector (PnP) with the IMU measurements.

Estimated Paths in RViz

Note: RViz with rqt_plot of positions, orientations, and velocities. The red arrows are the odometry from the EKF, and the blue arrows are the odometry from the PnP.

Implementation Details

  • Strictly follow the derivations in the notes, which is shown as follows:
EKF Methodology
  • The equations of $A_{t}=\frac{\partial f}{\partial x}$ are based on the work by Gary, typically the formula derivations of $\dot{G^{-1}}$ and $\dot{R}$.
  • To get a more stable solution for $K_{t}$, the Lower-Upper decomposition is used, as MatrixXd K_t = (C_t * state_cov * C_t.transpose() + Rt).lu().solve(C_t * state_cov).transpose();
  • So far, the EKF can greatly produce a much more smooth odometry. From the rqt_plot visualization, we can obviously find that some abrupt changes from VO can be mitigated by the EKF.