Observability Analysis of Aided INS With Heterogeneous Features of Points, Lines, and Planes

In this article, we perform a thorough observability analysis for linearized inertial navigation systems (INS) aided by exteroceptive range and/or bearing sensors (such as cameras, LiDAR, and sonars) with different geometric features (points, lines, planes, or their combinations). In particular, by reviewing common representations of geometric features, we introduce two sets of unified feature representations, i.e., the quaternion and closest point (CP) parameterizations. While the observability of vision-aided INS (VINS) with point features has been extensively studied in the literature, we analytically show that the general aided INS with point features preserves the same observability property, i.e., four unobservable directions, corresponding to the global yaw and the global translation of the sensor platform. We further prove that there are at least five (or seven) unobservable directions for the linearized aided INS with a single line (plane) feature, and, for the first time, analytically derive the unobservable subspace for the case of multiple lines or planes. Building upon this analysis for homogeneous features, we examine the observability of the same system but with combinations of heterogeneous features, and show that, in general, the system preserves at least four unobservable directions, while if global measurements are available, as expected, the unobservable subspace will have lower dimensions. We validate our analysis in Monte-Carlo simulations using both EKF-based visual-inertial SLAM and visual-inertial odometry (VIO) with different geometric features.