Selective Kalman Filter: When and How
to Fuse Multi-Sensor Information
to Overcome Degeneracy in SLAM

Simultaneous localization and mapping (SLAM) plays a significant role in robotics, autonomous driving, and Augmented Reality. To address challenging environments and to enhance localization robustness and environmental perception, there is an increasing focus on SLAM systems that fuse multiple sensors [1]. These systems typically integrate a combination of sensors such as LiDAR, vision, RGB-D camera, and IMU, exemplified by R³LIVE [2], FAST-LIVO [3], LVIO-Fusion [4], RIVER [5], etc.

Existing Kalman filter research [6, 7] has focused on multi-sensor information fusion, which propose using an Extended Kalman Filter (EKF) and an adaptive fuzzy logic system to fuse odometry and sonar signals. The core idea of these studies are to adaptively adjust the gain. Sun et al. [8] present a new multi-sensor optimal information fusion criterion weighted by matrices within the framework of linear minimum variance. By determining the cross-covariance, they then establish the optimal fusion matrix weights. To address the issue of occasional GPS signal loss or inaccuracies, Farhad [9] applies the KF to incorporate GPS measurements into the data fusion process, utilizing GPS data heavily only when it becomes reliably available. However, these works primarily focus on how to utilize and merge information from multiple sensors, without considering the necessity of fusion or integrating all available data. When fusing high-precision data with moderate-precision data in non-degenerative conditions, the accuracy improvement is often minimal or even negative, and it also incurs additional computational costs. We refer to this indiscriminate fusion of all measurement data as the “all-in” method. This approach faces two main issues:

First, the “all-in” method, by processing a volume of information than single-sensor SLAM, often results in significant computational burden, impacting real-time performance.

Second, while integrating the advantages of various sensors, the “all-in” method also incorporates their disadvantages. For instance, visual SLAM is frequently affected by motion blur, drastic lighting changes, and dynamic objects, leading to substantial errors or even system failure.

Accordingly, this paper addresses the two aforementioned issues by discussing when and how to selectively fuse multi-sensor information, using a LiDAR and vision-integrated SLAM system as an example. A critical consideration in multi-sensor fusion for SLAM is that individual sensors are prone to localization degeneracy, whereas incorporating different sensor types can provide additional constraints and prevent SLAM degeneracy.

For the “when” aspect, our solution is to integrate visual information only when the more accurate and robust LiDAR SLAM subsystem shows signs of degeneracy, relying solely on the LiDAR subsystem for pose estimation otherwise.

Regarding “how” to fuse, if all information is merged, the lower precision of the visual subsystem might reduce the overall accuracy of the LiDAR system. Therefore, our solution is to fuse visual information only in the directions where the LiDAR subsystem is degenerative.

The solution to “when” requires accurately determining whether degeneracy has occurred in LiDAR SLAM. Existing research in this area includes Zhang’s work [10], which assesses degeneracy by evaluating the condition number of the approximate Hessian matrix. However, due to the differing units and magnitudes of rotation and translation within the state, the eigenvalues of the approximate Hessian matrix couple rotation and translation. Consequently, the derived condition number is not dimensionless, making it challenging to accurately assess degeneracy in practical applications. Similarly, Hinduja’s approach [11] relies on comparing eigenvalues with thresholds for degeneracy detection, but this method also suffers from the issue of non-uniform dimensions in eigenvalues, complicating the establishment of fixed thresholds. To tackle these challenges, LION [12] and Switch-SLAM [13] introduce strategies that leverage submatrices of the Hessian matrix for independent degeneracy assessment in rotation and translation. The uniform dimensions within each submatrix endow the eigenvalues with a clear geometric significance, facilitating the application of fixed thresholds for degeneracy analysis. Based on this approach, X-ICP [14] also applies normalization and removes mostly noisy data with little contribution. However, these approaches do not account for the high degree of coupling between rotation and translation in point-plane constraints, leading to inaccuracies in determining the extent and direction of degeneracy. MM-LINS [15] is our previously proposed work, which includes degeneracy detection, but it does not have a dedicated analysis and experiments specifically for degeneracy detection. Julian [16] proposed a learning-based method for degeneracy detection. However, this approach is notably time-consuming and relies solely on current point clouds, which are insufficient for comprehensive degeneracy assessment without incorporating historical point cloud data. Moreover, this method suffers from a lack of generalizability.

Our proposed solution to “how” entails accurately determining the direction of degeneracy in LiDAR SLAM, incorporating visual measurements exclusively in these degenerative directions. Conversely, the fusion approach by LION [12] ceases the use of LiDAR odometry upon any LiDAR degeneracy, switching instead to other odometries. This method is somewhat rudimentary, as relying on lower accuracy and robustness sensor information in non-degenerative directions can actually diminish system performance. The methodologies of Zhang [10], Hinduja [11], and X-ICP [14] advocate for fusing information from other sensors only in degenerative directions, a strategy we consider to be optimal. However, these methods, which are optimization-based, do not address a filtering-based framework, nor can they precisely identify the specific directions of degeneracy. This can result in the true degenerative directions not being adequately constrained following by other sensor measurements.

Therefore, this paper introduces the Selective Kalman Filter to concretely implement the “when” and “how” solutions in selective data fusion. “Selective” here refers to the careful selection of the appropriate timing and data for fusion. We employ the advanced vision-LiDAR fusion scheme R³LIVE as a baseline for deploying the Selective Kalman Filter algorithm. The comparative results of the experiment are shown in Fig. 1. The specific contributions of this paper are:

• •

  We introduce a novel multi-sensor fusion approach, named the Selective Kalman Filter, designed to address when and how to selectively fuse data. This method integrates necessary visual information only when LiDAR data is degenerate, significantly enhancing real-time performance while ensuring robustness and accuracy.

• •

  We introduce an innovative and efficient method for degeneracy detection in LiDAR odometry, which also enables the determination of the degenerative direction. This approach fully considers the coupled relationship between rotational and translational constraints.

• •

  Through experiments, we demonstrate the accuracy of the proposed degeneration detection method and the efficiency of the Selective Kalman Filter.

• •

  We open-source the code for broader accessibility and community engagement.

In this study, we simply define the state

where $\mathbf{R}$ and $\mathbf{t}$ represent rotation and translation, respectively. Within the framework of Kalman Filter, we define the measurement equation for point-to-plane constraints derived from LiDAR [17], following its linearization, as

where $H$ is the Jacobin matrix of dimensions $m\times 6$, with $m$ representing the number of point-to-plane measurements. $z$ is the measurement vector, and $v\in\mathcal{N}\left(\mathbf{0},R\right)$ comes from the raw LiDAR measurements.

Similarly, the linearized measurement equation for visual data is

where $J$ is the Jacobin matrix of dimensions $n\times 6$, and $n$ denotes the number of visual measurements. $b$ is the measurement vector, and $w\in\mathcal{N}\left(\mathbf{0},Q\right)$ comes from the raw visual measurements. In the following sections, we adopt the convention of using uppercase letters to denote matrices and lowercase letters for vectors.

First, we introduce the “all-in” method for LiDAR-inertial-vusial SLAM, as exemplified in methodologies like FAST-LIVO [3], R³LIVE [2]. In the absence of visual or LiDAR measurements, similar to FAST-LIO2 [18], the system uses the IMU’s acceleration and angular velocity for integration to propagate the state, serving as the prediction step in Kalman Filter. When LiDAR or visual measurements come, the system proceeds with data fusion updates in the Kalman Filter.

To the best of our knowledge, under conditions where neither system degenerates, LiDAR-inertial odometry (LIO) systems demonstrate higher accuracy and greater robustness compared to visual-inertial odometry (VIO) SLAM [19, 20]. In integrating the Selective Kalman Filter into the LiDAR-inertial-visual odometry (LIVO) system, we prioritize the LIO subsystem as the core and the VIO subsystem as supplementary. The system’s workflow is illustrated in Fig. 2. Unlike the “all-in” method, our approach performs degeneracy detection and analysis based on LiDAR measurements:

If LIO subsystem is not degenerative, we opt not to fuse visual measurements. This approach helps prevent the introduction of errors caused by visual issues such as motion blur, overexposure, and incorrect feature matching. Additionally, it reduces computational load, thereby enhancing real-time performance.

If LIO subsystem is degenerative, to minimally introduce visual information and prevent deterioration in state estimation, we analyze the LiDAR’s degenerative direction. We then selectively fuse visual data pertinent to this degenerative direction and discard data from non-degenerative directions, which helps prevent a decline in the accuracy of the LiDAR’s non-degenerative dimensions.

We refer to this method of selectively updating information fusion as the “Selective Kalman Filter”.

First, we transform the LiDAR measurement Equation (2) using a weighted linear least squares method, resulting in

where $\hat{\mathbf{x}}$ is the estimated value of state $\mathbf{x}$. For simplification, we have made the following definitions:

So Equation (5) can be simplified to

Due to noise in real-world scenarios, the matrix $\mathbf{H}_{\mathbf{I}}$ is invariably a full rank matrix. Consequently, it follows that $\mathbf{H}_{\mathbf{I}}$ is a $6\times 6$ real symmetric positive-definite matrix.

Hinduja’s method [11] for degenerative detection involves comparing the smallest eigenvalue of $\mathbf{H}_{\mathbf{I}}$ with a predefined threshold. However, it is important to note that, since the first three dimensions of $\mathbf{H}_{\mathbf{I}}$ represent rotation and the last three represent translation, the eigenvalues obtained from the eigenvalue decomposition of the $\mathbf{H}_{\mathbf{I}}$ matrix represent values coupled with both rotation and translation. These values lack a clear physical meaning, as their units are related to both rotation and translation. Furthermore, even within the same eigenvalue decomposition of $\mathbf{H}_{\mathbf{I}}$, the eigenvalues have different units. Therefore, it becomes challenging to set a reasonable threshold for degeneracy detection. Moreover, the eigenvectors corresponding to these eigenvalues do not represent any actual directions in the real world. Consequently, this method is often impractical for deployment.

To address this issue, LION [12] proposes that degeneracy and its direction can be determined by analyzing the diagonal elements of the $\mathbf{H}_{\mathbf{I}}$. Let us first express the $\mathbf{H}_{\mathbf{I}}$ matrix in the form of a block matrix

LION separately performs eigenvalue decomposition on the $\mathbf{H}_{rr}$ and $\mathbf{H}_{tt}$:

It then sets two thresholds to assess whether there is degeneration in rotation and translation. It’s worth mentioning that this method effectively resolves the issue in Hinduja’s approach where threshold values lack physical significance and cannot be fixed. However, by overlooking the coupled terms $\mathbf{H}_{rt}$ and $\mathbf{H}_{tr}$, it neglects the mutual influence between rotational and translational constraints. Consequently, the degenerative level and direction estimated by this method are often inaccurate in most scenarios. As illustrated in Fig. 3 (a), according to LION’s perspective, the estimation in the $y$-direction is solely dependent on the Jacobians of point-plane residuals in the $y$-direction involving points A, leading to poor estimation in the $y$-direction, as shown by $y_{1}$. However, in reality, point-plane residuals in the $y$-direction involving points B can improve the estimation of rotation $\theta$ and consequently enhance the $y$-estimation, as indicated by $y_{2}$. It can be inferred that this latter approach, by considering translation and rotation in a coupled manner, provides higher accuracy for uncertain estimations.

To address the aforementioned issue, we propose a new, elegantly simple, and physically meaningful degeneracy detection method based on covariance information. Initially, we invert the $\mathbf{H}_{\mathbf{I}}$ matrix, thereby obtaining the covariance matrix $\Sigma$ for state $\mathbf{x}$:

It should be noted that

From Equation (17) and (18), it is apparent that our method is not equivalent to LION’s approach using a local Hessian matrix. Our method takes into account the coupled relationship between translation and rotation. It is evident from Equations (17) and (18) that our method takes into account the coupled relationship between rotation and translation. Additionally, this method has practical physical significance: the diagonal blocks of the covariance matrix represent the variances of the state, while the off-diagonal blocks indicate the correlations between different states. Variance serves as an intuitive measure of degeneracy level. For instance, a high variance indicates significant uncertainty in the estimation of that direction, signifying degeneracy.

We perform eigenvalue decomposition on $\Sigma_{rr}$ and $\Sigma_{tt}$:

where the units for $\lambda_{ri}\quad(i=1,2,3)$ are squared angular units ($\text{rad}^{2})$, while the units for $\lambda_{tj}\quad(j=1,2,3)$ are squared length units ($\text{m}^{2}$).

The columns in $V_{r}$ and $V_{t}$ corresponding to $\lambda$ represent respective directions. At this point, both $V_{r}$ and $V_{t}$ are rotation matrices with practical physical significance, capable of rotating the original coordinate system to align with these characteristic directions. Consequently, we set two variance thresholds $\theta_{r}$ and $\theta_{t}$ based on practical requirements and experience. If $\lambda_{ri}>\theta_{r}$, it indicates rotational degeneracy, with the corresponding eigenvectors representing the degenerative direction. The same principle applies to translation. It should noted that

which indicates that the principal directions derived from our covariance-based solution differ from those obtained through Hessian-based methods.

In summary, this section introduces a degeneracy detection method that has practical physical significance and takes into account coupled constraints. Theoretically, it is capable of more accurately determining the degree and degenerative direction.

Initially, we apply a linear weighted least squares approach to the visual measurement Equation (3):

For simplification, we have made the following definitions:

So Equation (22) can be simplified to

Note that $\mathbf{J}_{\mathbf{I}}$ is always invertible according to the noise in the real world. Therefore, we obtain the estimated state $\hat{\mathbf{x}}$

Using the eigenvector matrices of the covariance matrices ${V}_{r}$ and ${V}_{t}$, we define a new rotation matrix, denoted as

It can rotate $\mathbf{x}$’s coordinate system to align with the principal component directions of both translation and rotation:

Given the known dimensions corresponding to LiDAR subsystem, we define a selective matrix $S$ to select information in the visual measurement Equation (27) that aligns with the degenerative direction, thereby filtering out extraneous data:

For instance, if there is degeneracy in one translational and one rotational dimension within the LiDAR subsystem, such as when $\lambda_{r2}>\theta_{r}$ and $\lambda_{t3}>\theta_{t}$, we set the selective matrix $S$ as

In this matrix, diagonal elements set to 1 correspond to degenerative state dimensions, while those set to 0 correspond to non-degenerative dimensions. We revert the state to its original coordinate system and simultaneously restore the original measurement equation:

To preserve the symmetry of the information matrix, we undertake further processing, resulting in

which can be implified to

where $\mathbf{J}^{\prime}_{\mathbf{I}}={V}{S}{V}^{\top}\mathbf{J}_{\mathbf{I}}{V}{S}{V}^{\top}$, $\mathbf{b}^{\prime}={V}{S}{V}^{\top}\mathbf{J}_{\mathbf{I}}{V}{S}{V}^{\top}\mathbf{J}_{\mathbf{I}}^{-1}\mathbf{b}$.

At this point, we have completed the selection of visual data, retaining only the information pertinent to the direction of LiDAR degeneracy.

First, we describe the conventional form of Kalman Filter. For the visual subsystem, it is assumed that there are pre-existing prior estimates, denoted as $\hat{\mathbf{x}}$ and covariance $P$. Following the methodology used in FAST-LIO [17], the Kalman gain $K$ is calculated as

leading to the update equation

By substituting Equation (33) into the update Equation (34), we obtain

According to Equation (23), we can simplify Equation (35) to

where $\bar{\mathbf{x}}$ is posterior estimated state. Regarding the posterior covariance $\bar{P}$:

Substituting in $K$ (Equation (33)) and simplifying, we obtain

By observing the changes in the measurement equation for visual information before (Equation (24)) and after (Equation (32)), we can derive the new Selective Kalman Filter formula by simply replacing $\mathbf{J}_{\mathbf{I}}$ to $\mathbf{J}^{\prime}_{\mathbf{I}}$ and $\mathbf{b}$ to $\mathbf{b}^{\prime}$:

Remark: In this paper, matrices subjected to eigenvalue decomposition are exclusively real symmetric matrices, thereby rendering the results of eigenvalue decomposition and singular value decomposition equivalent. Our primary goal is to identify the principal directions of these matrices, which essentially aligns with the physical implications associated with singular values. To streamline the presentation and reduce the number of variables, we uniformly describe the process using eigenvalue decomposition throughout the paper.

Building upon the excellent LIVO framework R³LIVE, we have integrated the Selective Kalman Filter, resulting in the creation of SKF-Fusion, as illustrated in Fig. 4. The original R³LIVE framework comprises two parallel subsystems: a direct method for LiDAR-Inertial Odometry (LIO) and a direct method for Visual-Inertial Odometry (VIO). Aspects identical to R³LIVE are not reiterated in this document. Based on this foundation, we conduct degeneracy assessments for the LIO subsystem. If no degeneration is detected, we solely utilize visual information for coloring, while concurrently preserving the map’s color information for use during degenerative states. In cases of degeneration, sequential updates between frames and between frames and the map are performed, focusing exclusively on the degenerative state components. In subsequent experiments, we evaluate SKF-Fusion using the R³LIVE dataset.

Due to the limited availability of datasets containing LiDAR, vision, and IMU information compatible with R³LIVE, this study exclusively utilizes datasets [2, 21, 22] provided by R³LIVE for experimental analysis. The handheld device used to record the datasets includes a FLIR Blackfly BFS-u3-13y3c global shutter camera, Livox AVIA LiDAR, and an integrated IMU. Experiments were conducted on a computer equipped with an AMD 7840H CPU and running Ubuntu 20.04. We divided the experiments into two groups: degenerative datasets and normal datasets (the latter may also contain instances of weak degeneracy). For the degenerative datasets, we primarily assess the accuracy of our degeneracy detection method compared to other existing degeneracy detection methods, by measuring the impact on the precision and mapping quality of SKF-Fusion applied SLAM; for the non-degenerative datasets, we evaluate the improvements in accuracy and real-time performance of our SKF-Fusion compared to R³LIVE. The same configuration parameters, including voxel resolution and degeneracy threshold, were used throughout the experiments.

Building on the traditional foundation of “all-in” fusion SLAM involving LiDAR, vision, and IMU, this study introduces an innovative degeneracy detection module based on covariance information, along with a Selective Kalman Filter for the selective updating of degenerative states. Compared to other existing degeneracy detection methods, empirical evidence demonstrates that our proposed method significantly enhances the accuracy of degeneracy detection, thereby improving both the precision and robustness of localization. Notably, our approach substantially augments the real-time performance of the visual subsystem compared to the “all-in” method. It is important to mention that the current threshold selection does not robustly adapt to LIO across different voxel resolutions, highlighting an area for further research.

The Selective Kalman Filter extends beyond the fusion of LiDAR and vision, being applicable to a broad array of combinations, such as LiDAR with wheel encoders, Pedestrian Dead Reckoning (PDR), GPS, Ultra-Wideband (UWB), and more.