Observability-constrained EKF implementation of the IMU-RGBD camera navigation using point and plane features

I. ALGORITHM DESCRIPTION In this report, we present our IMU-RGBD camera navigation algorithm employing the observability constrained (OC)-EKF, which seeks to maintain the original system’s observability properties in the linearized implementation (EKF). In particular, we first describe the implementation of OC-EKF for processing point feature measurements. Then, we prove that once the OC-EKF is employed for point feature measurements, the observability constraint is automatically satisfied for the plane feature measurements. A system’s observability Gramian [1], M, is defined as M