Invariant Observer Design of a RGB-D Aided Inertial System for MAV in GPS-Denied Environments

This paper presents an non-linear observer framework that uses low-cost inertial measurement units (IMU) and RGB-D sensor to provide position, attitude and velocity estimates for micro aerial vehicles (MAV) in GPS-denied environments. The data fusion of inertial measurements and RGB-D motion estimates is performed through the observer, which is based on the invariant observer approach for symmetry possessing systems. The gains of the invariant observer are computed using an adaption of the invariant Extended Kalman Filter (IEKF). In addition, a robust RGB-D odometry algorithm is proposed to estimate the relative motions using successive images captured by the RGB-D sensor, which is used as an aiding measurement for accurate state estimation. The proposed approach guarantees a simplified form of estimation error dynamics, as well as simplified calculation of observer gains. The resulting observer is implemented on a quad rotor MAV and successfully validated through indoor flight tests. Experimental results demonstrate that the proposed observer is effective in estimating the motion states of the MAV in GPS-denied environments.