↑ Autonomous Aerial Robotics

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

Date
Keywords

Extended Kalman Filter (EKF), IMU, PnP, 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 implemented EKF-based state estimation for a quadrotor by fusing PnP-based odometry from tag_detector with 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

  • The implementation follows the derivations in the notes shown below.
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();
  • Qualitatively, the rqt_plot visualizations show smoother odometry and fewer abrupt changes after EKF filtering.