An Efficient and Accurate UAV State Estimation Method with Multi-LiDAR–IMU–Camera Fusion

Highlights

What are the main findings?

• The proposed DLIC method reformulates the complex, coupled UAV state estimation problem in multi-LiDAR–IMU–camera systems as an efficient distributed subsystem optimization framework. The designed feedback mechanism effectively constrains and optimizes the UAV state using the estimated subsystem states.
• Extensive experiments demonstrate that DLIC achieves superior accuracy and efficiency on a resource-constrained embedded UAV platform equipped with only an 8-core CPU. It operates in real time while maintaining low memory usage.

What are the implications of the main finding?

• This work demonstrates that the challenging, coupled UAV state estimation problem in multi-LiDAR–IMU–camera systems can be effectively addressed through distributed optimization techniques, paving the way for scalable and efficient estimation frameworks.
• The proposed DLIC method offers a promising solution for real-time state estimation in resource-limited UAVs with multi-sensor configurations.

Abstract

State estimation plays a vital role in UAV navigation and control. With the continuous decrease in sensor cost and size, UAVs equipped with multiple LiDARs, Inertial Measurement Units (IMUs), and cameras have attracted increasing attention. Such systems can acquire rich environmental and motion information from multiple perspectives, thereby enabling more precise navigation and mapping in complex environments. However, efficiently utilizing multi-sensor data for state estimation remains challenging. There is a complex coupling relationship between IMUs’ bias and UAV state. To address these challenges, this paper proposes an efficient and accurate UAV state estimation method tailored for multi-LiDAR–IMU–camera systems. Specifically, we first construct an efficient distributed state estimation model. It decomposes the multi-LiDAR–IMU–camera system into a series of single LiDAR–IMU–camera subsystems, reformulating the complex coupling problem as an efficient distributed state estimation problem. Then, we derive an accurate feedback function to constrain and optimize the UAV state using estimated subsystem states, thus enhancing overall estimation accuracy. Based on this model, we design an efficient distributed state estimation algorithm with multi-LiDAR-IMU-Camerafusion, termed DLIC. DLIC achieves robust multi-sensor data fusion via shared feature maps, effectively improving both estimation robustness and accuracy. In addition, we design an accelerated image-to-point cloud registration module (A-I2P) to provide reliable visual measurements, further boosting state estimation efficiency. Extensive experiments are conducted on 18 real-world indoor and outdoor scenarios from the public NTU VIRAL dataset. The results demonstrate that DLIC consistently outperforms existing multi-sensor methods across key evaluation metrics, including RMSE, MAE, SD, and SSE. More importantly, our method runs in real time on a resource-constrained embedded device equipped with only an 8-core CPU, while maintaining low memory consumption.

1. Introduction

State estimation aims to determine a UAV’s position, pose, velocity, and other motion states in real time [1,2]. It serves as a critical prerequisite for perception and localization in UAV systems [2,3,4]. With advances in manufacturing technology, the cost and size of sensors have decreased sharply. Consequently, state estimation using multi-sensor systems has attracted increasing attention [5,6], as such systems can greatly enlarge the field of view (FoV) and reduce the measurement noise inherent to single-sensor configurations [7]. This facilitates more accurate navigation and mapping in complex environments. Typical sensor combinations include light detection and ranging (LiDAR), camera, and inertial measurement unit (IMU) [8]. LiDAR and cameras collect distance and texture information of the surrounding environment, respectively, while IMUs provide measurements of the UAV’s motion dynamics. Thus, the multi-sensor-based state estimation method has wide application in UAV navigation and perception.

In general, multi-sensor configurations can be categorized into three cases: single-sensor-multi-type, multi-sensor-single-type, and multi-sensor-multi-type. The first case employs multiple sensors, with one sensor per type (e.g., single LiDAR + single IMU + single camera) [6,9,10]. The second case uses multiple sensors of the same type (e.g., multi-LiDAR) [11,12,13]. The third case combines multiple sensor types, where each type includes at least one sensor (e.g., single IMU + multi-LiDAR [14,15,16,17], or single IMU + single camera + multi-LiDAR [5]).

For general multi-sensor-based state estimation, state errors mainly arise from measurement errors of individual sensors, such as 3D point position errors from LiDARs, 2D pixel intensity errors from cameras, and acceleration and angular velocity errors from IMUs. Ensuring accurate and reliable state estimation remains a critical challenge in most UAV-based applications. It is well known that multiple measurements of the same physical quantity can significantly reduce measurement errors. Based on this fact, incorporating multiple IMUs, LiDARs, and cameras can effectively suppress measurement errors compared with using a single sensor [13]. Moreover, a multi-sensor-multi-type system is a feasible and practical configuration. On the one hand, the prices and weights of LiDARs and cameras have both decreased in recent years, enabling commercial UAVs to be equipped with multiple LiDARs and cameras [18]. On the other hand, IMU sensors are commonly embedded into LiDAR units to correct motion distortions in LiDAR scans [18], making multi-IMU configurations prevalent in real-world applications. Therefore, it is essential to study state estimation methods tailored for general multi-sensor-multi-type systems.

Although state estimation has achieved notable progress in single-sensor-multi-type and multi-sensor-single-type systems, few studies have explored state estimation in general multi-sensor-multi-type systems. Existing methods [5,14,15,16,17] primarily consider multi-sensor systems with a single IMU. Consequently, state estimation on general multi-sensor-multi-type systems remains a significant challenge. The major bottleneck lies in the optimization of multiple IMUs. According to the IMU pre-integration theory [19], IMU biases have a dominant influence on pre-integration accuracy. In multi-IMU scenarios, the biases of multiple IMUs are mutually coupled, and the IMU measurements received from multiple IMUs are often unordered. These two issues lead to an extremely complex Jacobian matrix for multi-IMU biases, which seriously impedes the efficiency of mainstream state estimation frameworks such as sliding window-based methods [20,21] and error state iterated Kalman filter (ESIKF)-based methods [22]. Furthermore, in practical applications, most UAVs have limited computational resources. Hence, performing efficient and accurate state estimation for general multi-sensor-multi-type embedded systems remains an urgent yet underexplored problem.

To address the aforementioned challenge, we observe that a multi-sensor-multi-type system can be decomposed into a series of single-sensor-multi-type subsystems. In this way, the original complex problem is reformulated as a distributed and low-complexitystate estimation problem. Distributed state estimation is well-suited for embedded systems, as multi-core CPUs in such systems can process each low-complexity sub-problem in parallel. Lin et al. [13] developed a distributed state estimation method based on extended Kalman filter (EKF), but their method was typically designed for multi-LiDAR setups and does not fully exploit measurement constraints from multiple IMUs and multiple cameras. To this end, we propose an efficient and accurate distributed state estimation method with multi-LiDAR–IMU–Camera fusion. First, we construct an efficient distributed state estimation model. It decomposes the multi-LiDAR–IMU–camera system into a series of single LiDAR–IMU–camera subsystems, reformulating the complex coupling problem into multiple simple sub-state estimation problems that can be solved efficiently in parallel. Next, we analyze the inter-relations among subsystem states and derive a feedback function based on IMU pre-integration theory [19], which constrains and optimizes the UAV state using estimated subsystem states. Based on this model, we propose an efficient and accurate distributed state estimation method in a multi-LiDAR–IMU–camera system, termed DLIC. DLIC achieves robust multi-sensor data fusion through a shared feature map, effectively improving both estimation accuracy and robustness. Furthermore, to accelerate state estimation, we design an efficient LiDAR–IMU-assisted image-to-point cloud registration (A-I2P) module. This module establishes high-quality 2D–3D correspondences based on sparse LiDAR depth, thereby constructing reliable visual measurements efficiently. Extensive experiments are conducted on 18 real-world indoor and outdoor scenes from the public NTU VIRAL dataset [18]. The results demonstrate that DLIC consistently outperforms existing multi-sensor methods across key evaluation metrics, including root mean square error (RMSE), mean absolute error (MAE), standard deviation (SD), and the sum of squares due to error (SSE). Moreover, resource consumption tests on an embedded device equipped with only an 8-core CPU show that DLIC achieves real-time performance while maintaining low memory usage. In all, the core contributions are:

• We propose an efficient and accurate distributed state estimation method, DLIC, which fuses data from multiple LiDARs, IMUs, and cameras to achieve accurate UAV state estimation.
• DLIC decomposes the complex, coupled multi-LiDAR–IMU–camera system into a series of single LiDAR–IMU–camera subsystems. A feedback function is then derived to effectively constrain and optimize the global UAV state based on the estimated subsystem states.
• To further accelerate state estimation, we develop an efficient I2P module that establishes high-quality 2D–3D correspondences and constructs reliable visual measurements efficiently.

The remainder of this paper is organized as follows. First, the related works of multi-sensor-based state estimation methods are illustrated in Section 2. The bottleneck of current methods is analyzed in Section 3. After that, the proposed DLIC is discussed with details in Section 4. Extensive experiments are shown in Section 5. Finally, this work is concluded in Section 6.

2. Related Works

As discussed in Section 1, most current multi-sensor-based SLAM methods can be classified into three cases: single-sensor-multi-type, multi-sensor-single-type, and multi-sensor-multi-type.

2.1. Single-Sensor-Multi-Type Case

LiDAR–IMU is the most common and convenient system in the single-sensor-multi-type case [23]. It has three reasons. At first, the mainstream LiDAR manufacturer embeds the low-cost IMU into the commercial LiDAR sensor, because the IMU eliminates motion blur in the LiDAR scan. Second, the LiDAR–IMU system does not need the strict extrinsic parameters. In most cases, only the rotation from LiDAR to IMU coordinate system needs to be calibrated [22]. Third, IMU decreases the drift (especially in the Z-axis) of LiDAR mapping [24], as IMU provides the pose prior for LiDAR scan-to-map registration. To fuse information from IMU and point cloud, loosely-coupled fusion is the naive solution. In LeGO-LOAM [24], Shan and Englot integrated the gyroscope data to calculate the coarse rotation for point cloud registration. Recently, tightly coupled fusion is the mainstream trend, for it estimates the IMU basis during the state optimization, thus achieving a more precise mapping result. Qin et al. [25] proposed a complete framework of an error state Kalman filter to estimate the IMU basis and the robot state. Based on their work [25], Xu et al. [22] found the complexity of the Kalman filter to be quadratic to the feature measurements. They designed a new formula for Kalman gain where the complexity only depends on the state dimension. Thus, Fast-LIO [22] can use more LiDAR features to estimate accurate odometry in real time. Another scheme is sliding window-based optimization. The error state Kalman filter is a special case in which the window size is only 1. Ye et al. [26] designed a fixed-lag smoother with prior marginalization. A rotational constraint is utilized to refine the global map. Shan et al. [20] presented a traditional framework, LIO-SAM. The fixed-lag smoothing problem is solved by factor graph optimization. In the factor graph, a factor of closed-loop detection or GPS can be added for accurate state estimation. In all, LiDAR–IMU-based SLAM has matured in recent years; its state estimation schemes (i.e., filter-based and sliding window-based) have a profound impact on multi-sensor fusion-based SLAM [27].

The LiDAR–IMU–Camera system has attracted attention [6]. Its core problem is to fuse the visual measurement into the tightly coupled optimization framework. In the branch of filter-based framework, Zheng et al. [6] added the visual measurement into Fast-LIO [22] and presented Fast-LIVO. Following Forster et al. [28] work, SVO, they built a visual global map, maintained by updating LiDAR points that fell into the camera field of view (FoV). Visual constraint is built by minimizing the photometric error of the feature pixels in the current image and the global map. Based on previous work [22], Lin et al. [10,29] designed a filter-based framework R3live. In their VIO subsystem, they aimed to minimize both the frame-to-frame and frame-to-map photometric error. In their current work, R3live++ [29], Lin et al. considered the covariance of measurement errors carefully, and online-calibrated time-offset of LiDAR and camera. In the branch of the sliding window-based optimization, Lin et al. [9] used both ESIKF and the sliding window-based optimization. In their VIO subsystem, a fixed-lag smoother [30] is leveraged to optimize the pose. Then, they compute the pixel reprojection error using the optimized pose. After that, ESIKF exploits the reprojection error as the visual measurement to estimate the state. Based on LIO-SAM [20], Shan et al. [21] proposed a refined version, LVI-SAM. It contains the VIO subsystem based on VINS [30]. In this system, they aligned the depth of 2D keypoints via projecting the reconstructed LiDAR map, thus enhancing the accuracy of the state estimated from VIO. Then, the odometry factor provided by the VIO system is added to the factor graph. Recently, Lv et al. [5] designed a continuous-time fixed-lag smoothing. In their factor graph optimization, they used B-spline [31] to represent the trajectory so that control points need to be estimated. Further, the visual factor built by structure from motion (SFM) is independent of the LiDAR factor. In all, following the optimization scheme of the LiDAR-IMU case, SLAM with IMU, LiDAR, and camera emphasizes the scheme of establishing visual measurements.

2.2. Multi-Sensor-Single-Type and Multi-Sensor-Multi-Type Case

To deal with SLAM with multi-sensor-single-type or multi-sensor-multi-type cases, researchers extend the method of state estimation in the single-sensor-multi-type case. Multi-LiDAR is the most common multi-sensor-single-type system. Jiao et al. [11] presented a framework, M-LOAM, for multi-LiDAR. First, they utilized a hand–eye calibration-based method to estimate the rigid translation between the auxiliary LiDAR and the primary LiDAR sensor. After initialization, they used a sliding window-based optimization scheme to estimate odometry and calibration parameters between the auxiliary and primary LiDARs. Lin et al. [13] designed an SLAM method for the decentralized LiDARs. The state of each LiDAR is estimated by one extended Kalman filter (EKF). The author claimed that multiple EKFs run in parallel. All EKFs share the robot state after the state optimization. These decentralized EKFs decrease the computation cost per onboard computer. These decentralized EKFs give us inspiration in designing DLIC. The difference between decentralized EKFs and DLIC is discussed in Section 2.3.

Recently, there have been some works for multi-sensor-multi-type-based SLAM. Wang et al. [12] leveraged sliding window-based optimization to add LiDAR, odometry, IMU, and GPS factors for SLAM on rail vehicles. Wang et al. [16] used the factor graph to process multi-LiDAR and single IMU data. The factor of LiDAR is derived by considering the occupancy probability, and the gravity factor is also added to the factor graph. Jung et al. [17] extended Fast-LIO [22] for the multi-LiDAR single-IMU case. They utilized B-spline interpolation to undistort the motion blur for multi-LiDAR. Nguyen et al. [15] designed a general sliding window-based SLAM framework, MILIOM. It contains one IMU factor and multiple LiDAR factors. The LiDAR factor is constructed by registering the current LiDAR point cloud to every keyframe. In 2023, Nguyen et al. [14] extended their previous work MILIOM [15] and presented SLICT. The major difference of SLICT is the usage of an octree-based surfel map [32]. The advantage of the surfel map is that it represents the planarity of planar features, so that it filters out low-quality 3D–3D correspondences. Moreover, the work of Lv et al. [5] can be used for the case of a single IMU, a single camera, and multi-LiDAR. With the help of B-Spline interpolation, point clouds from the other LiDARs are undistorted to build LiDAR factors directly. In all, the mainstream methods tend to utilize a sliding window-based optimization scheme, for it can be easily extended from multi-sensor-single-type to multi-sensor-multi-type.

2.3. Discussions

From the above discussion, current multi-sensor-based state estimation methods have made a lot of progress. However, there are still several open problems to be addressed. The first is the state estimation in the general multi-sensor-multi-type system with limited computational resources. Current optimization frameworks [5,14] cannot deal with the multi-IMU inputs, due to the extremely high complexity of multi-IMU biases optimization, as discussed in Section 1. The second problem is to explore an efficient and accurate way to construct the visual measurement. Current methods tend to embed a VIO-based sub-system into their state estimation frameworks [5,6,9,10,21,29]. But it suffers from the robustness and overlarge CPU computation resources in practical applications. In this paper, we propose an efficient and accurate distributed state estimation method, DLIC, to deal with the first problem, and then present a scheme, A-I2P, to address the second problem. Details are illustrated in Section 4.

3. Problem Statement

We briefly introduce the optimization framework of state estimation, and then analyze the challenge of state estimation in the general multi-sensor-multi-type case. The symbols’ notation is presented in

Table 1. Main symbols notation.

Symbol	Meaning
$\mathbf{x},\widehat{\mathbf{x}},\tilde{\mathbf{x}}$	State of ground truth, estimation, residual error
$\mathcal{I},\mathcal{W},\mathcal{A},k$	subscript: IMU coordinate system, gyroscope, accelerometer, timestamp
${\mathbf{p}}_{\mathcal{I}},{\mathbf{v}}_{\mathcal{I}},{\xi }_{\mathcal{I}}$	3D position, velocity, rotation vector
${\mathbf{b}}_{\mathcal{W}},{\mathbf{b}}_{\mathcal{A}},\mathbf{g}$	IMU basis of gyroscope and accelerometer, gravity vector
${\mathbf{J}}_k,{\widehat{\mathbf{P}}}_k$	Jacobian, covariance matrix of pre-integration on ${\widehat{\mathbf{x}}}_k$
${\mathcal{M}}_l$	Feature map of the l-th Kalman filter
${\mathcal{P}}_l$	3D point at the current LiDAR coordinate system
${}^{\mathcal{G}}\mathcal{P}_l$	3D point at the global coordinate system $\mathcal{G}$
I	2D pixel coordinate in the image
$y_i,z_j$	correspondence: LiDAR point, image point

. Overview of the problem analysis is shown in Figure 1.

3.1. Basic State Estimation Model

For clarity, we first illustrate the state estimation in a general multi-sensor-single-type system (i.e., IMU+Camera+LiDAR). Two coordinate systems are used, like the UAV coordinate system (marked as $\mathcal{I}$) and the world coordinate system (marked as $\mathcal{G}$). More specifically, $\mathcal{I}$ is the coordinate system of the main IMU in the UAV. $\mathcal{G}$ is the coordinate system of LiDAR at the initial time $k=0$. State estimation is to estimate the UAV state $\mathbf{x}$ by fusing the measurements of all sensors. State $\mathbf{x}$ is defined as:

$$\mathbf{x}\triangleq {[{}^{\mathcal{G}}\mathbf{p}_{\mathcal{I}}^T,{}^{\mathcal{G}}\mathbf{v}_{\mathcal{I}}^T,{}^{\mathcal{G}}\xi _{\mathcal{I}}^T,{\mathbf{b}}_{\mathcal{W}}^T,{\mathbf{b}}_{\mathcal{A}}^T,{}^{\mathcal{G}}\mathbf{g}^T]}^T\in {\mathbb{R}}^{18}$$

(1)

where ${}^{\mathcal{G}}\mathbf{p}_{\mathcal{I}}^T\in {\mathbb{R}}^3$, ${}^{\mathcal{G}}\mathbf{v}_{\mathcal{I}}^T\in {\mathbb{R}}^3$, and ${}^{\mathcal{G}}\xi _{\mathcal{I}}^T\in \mathbf{so}\left(3\right)$ represent 3D position, velocity, rotation vector of UAV on the world coordinate system. ${\mathbf{b}}_{\mathcal{W}}^T\in {\mathbb{R}}^3$ and ${\mathbf{b}}_{\mathcal{A}}^T\in {\mathbb{R}}^3$ are gyroscope and accelerometer bias of IMU. ${}^{\mathcal{G}}\mathbf{g}^T\in {\mathbb{R}}^3$ is the gravity vector on the world coordinate system. In the k-th time, the state of UAV is marked as ${\mathbf{x}}_k$. The mainstream methods estimate the UAV state with Kalman filter [6,10,16,22,29]. As the core of Kalman filter is the Maximum A Posteriori (MAP), the computation of ${\mathbf{x}}_k$ contains two steps. The first step is to calculate the initial state, marked as ${\widehat{\mathbf{x}}}_k$. ${\widehat{\mathbf{x}}}_k$ is computed with ${\mathbf{x}}_{k-1}$ via IMU pre-integration from the $(k-1)$-th to k-th time. The second step is to estimate the error of the initial state, marked as ${\tilde{\mathbf{x}}}_k$. ${\tilde{\mathbf{x}}}_k$ is optimized by minimizing the cost function $\mathbf{F}\left({\tilde{\mathbf{x}}}_k\right)$. In all, the computation of ${\mathbf{x}}_k$ is represented by the following equations:

$${\mathbf{x}}_k={\widehat{\mathbf{x}}}_k+{\tilde{\mathbf{x}}}_k,$$

(2)

$$\begin{gathered} \underset{{\tilde{\mathbf{x}}}_k}{min}\mathbf{F}\left({\tilde{\mathbf{x}}}_k\right)= \\ {\parallel {\mathbf{J}}_k{\tilde{\mathbf{x}}}_k\parallel }_{{\widehat{\mathbf{P}}}_k^{-1}}^2+\sum _{i=1}\parallel y_i^k+{\mathbf{H}}_i^k{\tilde{\mathbf{x}}}_k{\parallel }_{{\mathbf{R}}_i^{-1}}^2+\sum _{j=1}{\parallel z_j^k+{\mathbf{L}}_j^k{\tilde{\mathbf{x}}}_k\parallel }_{{\mathbf{Q}}_j^{-1}}^2 \end{gathered}$$

(3)

where ${\mathbf{J}}_k$ denotes the Jacobian matrix of pre-integration on ${\widehat{\mathbf{x}}}_k$; ${\widehat{\mathbf{P}}}_k$ means a pre-integration covariance matrix on ${\widehat{\mathbf{x}}}_k$. $y_i^k$ and $z_j^k$ are the measurements of point-to-point correspondence in the LiDAR point cloud and image, respectively. ${\mathbf{H}}_i^k$ and ${\mathbf{L}}_j^k$ are Jacobian matrices of LiDAR and visual measurements on ${\widehat{\mathbf{x}}}_k$. ${\mathbf{R}}_i$ and ${\mathbf{Q}}_j$ mean the covariance matrices of the corresponding measurements. The analytic formulas of ${\mathbf{J}}_k$, ${\widehat{\mathbf{P}}}_k$, $y_i^k$, $z_j^k$, ${\mathbf{R}}_i$, and ${\mathbf{Q}}_j$ can refer to previous works [6,22,29]. In Equation (3), ${\tilde{\mathbf{x}}}_k$ can be estimated via $\partial \mathbf{F}\left({\tilde{\mathbf{x}}}_k\right)/\partial {\tilde{\mathbf{x}}}_k=0$. It means that ${\tilde{\mathbf{x}}}_k$ is solved by a linear equation:

$$\left(\begin{array}{c}{\mathbf{A}}_{\mathit{imu}}^k\\ {\mathbf{A}}_{\mathit{lidar}}^k\\ {\mathbf{A}}_{\mathit{cam}}^k\end{array}\right){\tilde{\mathbf{x}}}_k=\left(\begin{array}{c}{\mathbf{b}}_{\mathit{imu}}^k\\ {\mathbf{b}}_{\mathit{lidar}}^k\\ {\mathbf{b}}_{\mathit{cam}}^k\end{array}\right)\Rightarrow {\mathbf{A}}_k{\tilde{\mathbf{x}}}_k={\mathbf{b}}_k$$

(4)

where $({\mathbf{A}}_{\mathit{imu}}^k,{\mathbf{b}}_{\mathit{imu}}^k)$, $({\mathbf{A}}_{\mathit{lidar}}^k,{\mathbf{b}}_{\mathit{lidar}}^k)$, $({\mathbf{A}}_{\mathit{cam}}^k,{\mathbf{b}}_{\mathit{cam}}^k)$ are computed from ${\mathbf{J}}_k$, (${\mathbf{H}}_i^k$, $y_i^k$), and (${\mathbf{L}}_j^k$, $z_j^k$) via $\partial \mathbf{F}\left({\tilde{\mathbf{x}}}_k\right)/\partial {\tilde{\mathbf{x}}}_k=0$. The analytic formulae of them can refer to previous works [6,22,29]. In the actual application, ${\tilde{\mathbf{x}}}_k$ is solved iteratively via Equation (2).

3.2. Challenge in Multi-Sensor-Multi-Type Case

State estimation in Equation (3) faces a challenge when it comes to the multi-sensor-multi-type case. Suppose that this system has multiple IMUs, cameras, and LiDARs, where the number of IMUs is L. State $\mathbf{x}$ of this system is rewritten as:

$$\mathbf{x}\triangleq {[{}^{\mathcal{G}}\mathbf{p}_{\mathcal{I}}^T,{}^{\mathcal{G}}\mathbf{v}_{\mathcal{I}}^T,{}^{\mathcal{G}}\xi _{\mathcal{I}}^T,{\mathbf{b}}_{\mathcal{W},1}^T,{\mathbf{b}}_{\mathcal{A},1}^T,\dots ,{\mathbf{b}}_{\mathcal{W},L}^T,{\mathbf{b}}_{\mathcal{A},L}^T{}^{\mathcal{G}}\mathbf{g}^T]}^T$$

(5)

where ${\mathbf{b}}_{\mathcal{W},i}^T$ and ${\mathbf{b}}_{\mathcal{A},i}^T$ mean the gyroscope and accelerometer bias of the i-th IMU. $\mathbf{x}$ is a $(12+3L)\times 1$ vector. Specifically, Equation (3) suffers from two problems. Firstly, as IMU data flows into the system with unknown data sources, the Jacobian matrix of multi-IMU ${\mathbf{J}}_k$ has the unknown and complex analytic structure [19]. Second, as ${\mathbf{J}}_k$ is a matrix with a complicated structure, there is a complex coupled relation between the UAV state and multi-IMU biases, causing the difficulty of optimizing Equation (3) in a multi-sensor-multi-type case. Also, the practical embedded system has limited computation resources, which makes it difficult to optimize the state in Equation (5) with lots of coupled variables. Thus, it is essential to explore an efficient state estimation method on the general multi-sensor-multi-type-based embedded system.

4. Proposed Method DLIC

In this paper, we observe that a multi-sensor-multi-type system can be decomposed into a series of multi-sensor-single-type subsystems. For example, a system with L LiDARs, L cameras, and L IMUs can be decomposed as L subsystems with a single LiDAR, a single camera, and a single IMU ($L\ge 2$). Then, state estimation in multi-sensor-multi-type system can be relaxed as a distributed state estimation problem in multiple multi-sensor-single-type subsystems. Based on this observation, we propose an efficient distributed state estimation method, DLIC.

4.1. Efficient Distributed State Estimation Model

We develop an efficient distributed state estimation model on a general multi-sensor-multi-type system. Unlike in Lin et al. [13], the proposed DLIC method is suitable for a general multi-sensor system, and not limited to multi-LiDAR sensors.

To establish this model, we first define a distributed sensor system. A multi-sensor-multi-type system consists of L groups of subsystems with a single LiDAR, IMU, and camera. And each subsystem maintains state ${\mathbf{x}}^l$$(l=1,\dots ,L)$ where ${\mathbf{x}}^l$ is defined in Equation (3). The rotation, position, and velocity in ${\mathbf{x}}^l$ are on the UAV coordinate system. In the actual application, we use the position in ${\mathbf{x}}^1$ to represent the UAV trajectory. Extending to Equations (2) and (3), an efficient distributed state estimation model is established as:

$${\mathbf{x}}_k^l={\widehat{\mathbf{x}}}_k^l+{\tilde{\mathbf{x}}}_k^l,l=1,\dots ,L,$$

(6)

$$\underset{{\tilde{\mathbf{x}}}_k^l}{min}\left\{\mathbf{F}\left({\tilde{\mathbf{x}}}_k^l\right)+\sum _{z=1,z\ne l}^L\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)·\mathbf{1}(\mathrm{\Delta }{t}_z^l\in (-\tau ,\tau ))\right\}.$$

(7)

We illustrate the Equations (6) and (7) with details. Extending from Equation (2), ${\mathbf{x}}_k^l={\widehat{\mathbf{x}}}_k^l+{\tilde{\mathbf{x}}}_k^l$ denotes the state computation of the l-the subsystem. ${\widehat{\mathbf{x}}}_k^l$ is computed by IMU pre-integration on the l-th subsystem where the detail is similar to Equation (2). ${\tilde{\mathbf{x}}}_k^l$ is the error of initial state ${\widehat{\mathbf{x}}}_k^l$. A naive way of computing ${\tilde{\mathbf{x}}}_k^l$ is to minimize the cost function of $\mathbf{F}\left({\tilde{\mathbf{x}}}_k^l\right)$. However, we find that ${\mathbf{x}}_k^l$ has the strong constraint with ${\mathbf{x}}_k^z$ where $z=1,\dots ,L$ and $z\ne l$. In the ideal situation, the rotation, position, and velocity in ${\mathbf{x}}_k^l$ are equal to ${\mathbf{x}}_k^z$. Based on this observation, we construct a self-defined feedback function $\mathbf{G}$ in Equation (7) to constrain ${\mathbf{x}}_k^l$ with states from the other subsystems. In all, Equation (7) provides an efficient way to optimize the residual error by fusing the measurements from the l-th subsystem (i.e., $\mathbf{F}\left({\tilde{\mathbf{x}}}_k^l\right)$, defined in Equation (3)), and measurements from other subsystems (i.e., $\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)$).

From the system viewpoint, $\mathbf{G}$ in Equation (7) serves as a feedback to correct ${\mathbf{x}}_k^l$ with ${\tilde{\mathbf{x}}}_k^z$ ($z\in [1,L],z\ne l$). In this context, we name $\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)$ as a self-defined feedback function. $\mathrm{\Delta }{t}_z^l$ is the time offset between the time of optimizing ${\tilde{\mathbf{x}}}_k^z$ and the time of optimizing ${\tilde{\mathbf{x}}}_k^l$. To ensure the model stability, only the measurements received in a short time interval $(-\tau ,\tau )$ are used for the estimation of ${\tilde{\mathbf{x}}}_k^l$. To achieve this goal, we use an indicator function $\mathbf{1}(\mathrm{\Delta }{t}_z^l\in (-\tau ,\tau ))$ to filter the measurements from other subsystems.

We then illustrate the theoretical solution of Equation (7). Similar to Equation (4), we first linearize the function $\mathbf{F}\left({\tilde{\mathbf{x}}}_k^l\right)$ as ${\mathbf{A}}_k{\tilde{\mathbf{x}}}_k^l={\mathbf{b}}_k$ by Taylor expansion on ${\tilde{\mathbf{x}}}_k^l=\mathbf{0}$. In the same way, we can linearize the function $\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)$ by Taylor expansion on ${\tilde{\mathbf{x}}}_k^l=\mathbf{0}$, and obtain that ${\mathbf{G}}_z^l{\tilde{\mathbf{x}}}_k^l={\mathbf{g}}_z^l$. Hence, Equation (7) can be linearized as the below equation of ${\tilde{\mathbf{x}}}_k^l$:

$$\left(\begin{array}{c}{\mathbf{A}}_k\\ {\alpha }_1^l{\mathbf{G}}_1^l\\ ⋮\\ {\alpha }_L^l{\mathbf{G}}_L^l\end{array}\right){\tilde{\mathbf{x}}}_k=\left(\begin{array}{c}{\mathbf{b}}_k\\ {\alpha }_1^l{\mathbf{g}}_1^l\\ ⋮\\ {\alpha }_L^l{\mathbf{g}}_L^l\end{array}\right)\Rightarrow {\tilde{\mathbf{A}}}_k{\tilde{\mathbf{x}}}_k={\tilde{\mathbf{b}}}_k$$

(8)

where ${\alpha }_1^l=\mathbf{1}(\mathrm{\Delta }{t}_z^l\in (-\tau ,\tau ))$. From Equation (8), ${\tilde{\mathbf{x}}}_k={\left({\tilde{\mathbf{A}}}_k^T{\tilde{\mathbf{A}}}_k\right)}^{-1}{\tilde{\mathbf{A}}}_k^T{\tilde{\mathbf{b}}}_k$. As ${\tilde{\mathbf{A}}}_k^T{\tilde{\mathbf{A}}}_k$ is a $18\times 18$ matrix, the complexity of Equation (7) is $\mathcal{O}\left(L\right)$. With the multi-thread technique in the modern CPU, all states ${\mathbf{x}}^l$$(l=1,\dots ,L)$ in Equation (7) can be solved in a distributed manner. Thus, the complexity of the theoretical distributed state estimation model is $\mathcal{O}\left(L\right)$, friendly to the efficient multi-sensor-multi-type system.

4.2. Feedback Function in Distributed State Estimation

$\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)$ leads an important role in Equation (7). To ensure the stability of optimizing Equation (7), it should be an accurate and robust feedback function. Before designing $\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)$, we need to study the relationship between ${\tilde{\mathbf{x}}}_k^l$ and ${\tilde{\mathbf{x}}}_k^z$. As IMU in the different subsystems are independent of each other, IMU biases in ${\tilde{\mathbf{x}}}_k^l$ and ${\tilde{\mathbf{x}}}_k^z$ are independent. While other parameters, including the rotation, position, and velocity in ${\tilde{\mathbf{x}}}_k^l$ and ${\tilde{\mathbf{x}}}_k^z$ are correlated, because they all reflect the UAV on the world coordinate system. It means that we can leverage the IMU pre-integration theory [19] to propagate the state error ${\tilde{\mathbf{x}}}_k^z$ to constrain ${\tilde{\mathbf{x}}}_l^z$. Based on this observation, we design $\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)$ as:

$$\mathbf{G}\left({\tilde{\mathbf{x}}}_k^l\right|{\tilde{\mathbf{x}}}_k^z,\mathrm{\Delta }{t}_z^l)={\parallel \mathbf{E}{\tilde{\mathbf{x}}}_k^l-{\mathbf{A}}_k^z{\tilde{\mathbf{x}}}_k^z\parallel }_{{\mathbf{A}}_k^z{\left({\mathbf{P}}_k^z\right)}^{-1}{\left({\mathbf{A}}_k^z\right)}^T}^2,$$

(9)

$$\mathbf{E}=\left(\begin{array}{ccc}{\mathbf{I}}_{9\times 9}& & \\ & {\mathbf{0}}_{6\times 6}& \\ & & {\mathbf{I}}_{3\times 3}\end{array}\right),$$

(10)

$$\begin{array}{cc}\hfill & {\mathbf{A}}_k^z=\hfill \\ \hfill & \left(\begin{array}{cccccc}\mathbf{I}& \mathbf{I}\mathrm{\Delta }{t}_z^l& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{I}& -{\mathbf{R}}_a{\mathbf{V}}_a\mathrm{\Delta }{t}_z^l& \mathbf{0}& -{\mathbf{R}}_a\mathrm{\Delta }{t}_z^l& \mathbf{I}\mathrm{\Delta }{t}_z^l\\ \mathbf{0}& \mathbf{0}& {\mathbf{R}}_w& -\mathbf{I}\mathrm{\Delta }{t}_z^l& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{I}\end{array}\right),\hfill \end{array}$$

(11)

$${\mathbf{V}}_a={(\mathbf{a}-{\mathbf{b}}_{\mathcal{A}}^z)}^{\wedge },$$

(12)

$${\mathbf{R}}_a=exp\left({\left({\xi }_k^z\right)}^{\wedge }\right),{\mathbf{R}}_w=exp(-{(\mathbf{w}-{\mathbf{b}}_{\mathcal{W}}^z)}^{\wedge }\mathrm{\Delta }{t}_k^l)$$

(13)

where ∧ is a function of generating the anti-symmetric matrix from a vector. ${\mathbf{I}}_{n\times n}$ and ${\mathbf{0}}_{n\times n}$ denote $n\times n$ identity and zero matrices. $\mathbf{I}={\mathbf{I}}_{3\times 3}$ and $\mathbf{0}={\mathbf{0}}_{3\times 3}$. ${\xi }_k^z$, ${\mathbf{b}}_{\mathcal{W}}^z$, and ${\mathbf{b}}_{\mathcal{A}}^z$ belong to state ${\mathbf{x}}_k^z$, where ${\mathbf{x}}_k^z$ is obtained via Equation (6). ${\mathbf{P}}_k^z$ is the covariance matrix of ${\mathbf{x}}_k^z$. As $\tau$ is very small (set as $0.05s$) and the UAV is not moving violently in this short time, $\mathbf{a}$ and $\mathbf{w}$ are the average acceleration and angular velocity of the z-th IMU in $\mathrm{\Delta }{t}_z^l$. The detail of ${\mathbf{A}}_k^z$ refers to work [19]. For better understanding, we provide an intuitive explanation of ${\mathbf{A}}_k^z$ here. The computations of ${\tilde{\mathbf{x}}}_k^l$ and ${\tilde{\mathbf{x}}}_k^z$ are performed at different times, where the time interval is $\mathrm{\Delta }{t}_z^l$. Thus, we need to propagate the state error ${\tilde{\mathbf{x}}}_k^z$ with $\mathrm{\Delta }{t}_z^l$. As the position, rotation, and velocity satisfy the kinematic constraint, the first three rows in ${\mathbf{A}}_k^z$ are designed as shown in Equation (11). More derivation of Equations (12) and (13) refer to the classic literature [19]. As the IMU intrinsic parameters are independent of each other, the fourth and fifth rows in ${\mathbf{A}}_k^z$ are zero matrices. As the error of gravity is constant with time, the last row in ${\mathbf{A}}_k^z$ contains a $3\times 3$ identity matrix.

4.3. Distributed State Estimation in a Multi-LiDAR–IMU–Camera System

To implement the optimization model in Equation (7), we propose a distributed state estimation method, DLIC, in a multi-LiDAR–IMU–camera system. The DLIC pipeline is provided in Figure 2. The core is to optimize ${\mathbf{x}}_k^l$ with Equation (8). It mainly has three steps: (i) preparation of Equation (8), (ii) solving Equation (8), and (iii) post-processing.

The first is the preparation of Equation (8). As IMU has a higher publishing rate (≥100 Hz) than other sensors, we follow the previous work [6] to package the LiDAR–IMU–camera data in a time interval. IMU pre-integration is to calculate the prior state ${\widehat{\mathbf{x}}}_k^l$ and ${\widehat{\mathbf{P}}}_k$ from ${\mathbf{x}}_{k-1}^l$. With ${\widehat{\mathbf{x}}}_k^l$, LiDAR measurements contain 3D–3D correspondences between LiDAR scan and the shared LiDAR feature map ${\mathcal{M}}_{\mathrm{lid}}$. They construct $(y_i^k,{\mathbf{H}}_i^k)$ in Equation (3). Visual measurements denote 2D–3D correspondences from image keypoints to the shared camera feature map ${\mathcal{M}}_{\mathrm{cam}}$ (details will be shown in Section 4.4). They construct $(z_j^k,{\mathbf{L}}_j^k)$ in Equation (3). We retrieve the optimized state from other subsystems with the condition $\mathrm{\Delta }{t}_z^l\in (-\tau ,\tau )$, and construct the feedback constraint via Equation (9).

The second step is to solve Equation (8). With the help of C++ matrix operation library Eigen, ${\tilde{\mathbf{x}}}_k^l$ is quickly solved, and ${\mathbf{x}}_k^l$ is obtained via Equation (6).

The third step is the post-processing. ${\mathbf{x}}_k^l$ is added into the shared state information with its timestamp. With ${\mathbf{x}}_k^l$, we can transform the LiDAR scan into the world coordinate system as ${\mathcal{S}}_k^l$ and back-projected image keypoints with LiDAR depth (detail is in Section 4.4) into the world coordinate system as ${\mathcal{V}}_k^l$. Then, ${\mathcal{M}}_{\mathrm{lid}}$ and ${\mathcal{M}}_{\mathrm{cam}}$ is updated as:

$${\mathcal{M}}_{\mathrm{lid}}\leftarrow {\mathcal{M}}_{\mathrm{lid}}\cup \mathrm{Down}-\mathrm{sample}\left({\mathcal{S}}_k^l\right),{\mathcal{M}}_{\mathrm{cam}}\leftarrow {\mathcal{M}}_{\mathrm{cam}}\cup {\mathcal{V}}_k^l$$

(14)

where $\mathrm{Down}-\mathrm{sample}(·)$ is a downsampling operation with voxel resolution of $0.1$ m. With the shared feature maps, correspondences of 3D–3D and 2D–3D are stable and precise, ensuring an accurate state estimation result. For better understanding, we illustrate Equation (14) with details. ${\mathcal{M}}_{\mathrm{lid}}\cup \mathrm{Down}-\mathrm{sample}\left({\mathcal{S}}_k^l\right)$ means that the LiDAR feature map is enlarged by merging the point cloud ${\mathcal{S}}_k^l$ measured by the current scan. To avoid the LiDAR feature map being overly large, the common trick is to downsample ${\mathcal{S}}_k^l$. ${\mathcal{M}}_{\mathrm{cam}}\cup {\mathcal{V}}_k^l$ means that the camera feature map is merged with the new keypoints ${\mathcal{V}}_k^l$. As the number of visual keypoints (nearly ${10}^2$) is smaller than the LiDAR points (nearly ${10}^4$), it is not essential to downsample ${\mathcal{V}}_k^l$.

4.4. Assisted Image-to-Point-Cloud Registration

From the discussions in Section 2.3, the current approach exploits the VIO subsystem to establish the visual measurement. However, this system fails to work if the initialization is not successful or keypoint tracking fails. Also, the VIO sub-system needs a much greater computational burden for the extra state optimization.

To deal with this issue, we present A-I2P without initialization and extra state optimization. A-I2P needs a shared feature map ${\mathcal{M}}_{\mathrm{cam}}$ with the lightweight maintaining cost. A-I2P aims to extract 2D–3D correspondences from image ${\mathcal{I}}_l$ and ${\mathcal{M}}_{\mathrm{cam}}$ with the aid of ${\mathcal{P}}_l$ and IMU data. The procedure is shown in Figure 3. It contains four steps: keypoint detection, depth generation, kNN search, and map update. First, Shi–Tomasi keypoint detector [33] is used to extract ${\left\{{\mathrm{I}}_k\right\}}_{k=1}^K$ keypoints, as presented as red points in Figure 3c. Second, depth generation aims to project the LiDAR scan, undistorted by IMU data, into the image plane. It obtains the sparse depth, shown as the green points ${\left\{{\mathrm{L}}_s\right\}}_{s=1}^S$ in Figure 3c. Each pixel ${\mathrm{L}}_s$ has depth.

The third step, 2D–3D kNN search, aims to establish 2D–3D correspondences from ${\left\{{\mathrm{I}}_k\right\}}_{k=1}^K$, ${\left\{{\mathrm{L}}_s\right\}}_{s=1}^S$, ${\mathcal{M}}_{\mathrm{cam}}$. It is a crucial step of A-I2P. For each ${\mathrm{I}}_k$, we search ${\mathrm{L}}_k^{\star }\in {\left\{{\mathrm{L}}_s\right\}}_{s=1}^S$ with the nearest distance to ${\mathrm{I}}_k$. We align the depth of ${\mathrm{I}}_k$ from ${\mathrm{L}}_k^{\star }$, when two constraints are satisfied: (i) pixel distance of ${\mathrm{I}}_k$ and ${\mathrm{L}}_k^{\star }$ is below ${\tau }_{2D}$ where ${\tau }_{2D}$ is empirically set as 5 pixels in the actual applications; (ii) gray-scale intensity of ${\mathrm{I}}_k$ and ${\mathrm{L}}_k^{\star }$ is below ${\tau }_I$ where ${\tau }_I$ is empirically set as 24. Keypoints with the aligned depths are marked as ${\left\{{\mathrm{I}}_a\right\}}_{a=1}^A$, as presented as the yellow points in Figure 3c. For each ${\mathrm{I}}_a$, we back-project it with its depth to 3D space as 3D point ${\mathrm{P}}_a$. With the initial estimated state ${\widehat{\mathbf{x}}}_k^l$, we transform ${\mathrm{P}}_a$ from the camera coordinate system to $\mathcal{G}$ as ${}^{\mathcal{G}}\mathrm{P}_a$. Then, we search a 3D point ${}^{\mathcal{G}}\mathrm{Q}_a^{\star }\in {\mathcal{M}}_{\mathrm{cam}}$ with the closest distance to ${}^{\mathcal{G}}\mathrm{P}_a$ using iKd-Tree [34]. If the distance of ${}^{\mathcal{G}}\mathrm{P}_a$ and ${}^{\mathcal{G}}\mathrm{Q}_a^{\star }$ is below ${\tau }_{3D}$ (${\tau }_{3D}$ is empirically set as 5 cm for optimal performance), pair $\langle {\mathrm{I}}_a,{}^{\mathcal{G}}\mathrm{Q}_a^{\star }\rangle$ is a valid 2D–3D correspondence, and can be used as the visual measurement. As the final estimated state ${\mathbf{x}}_k^l$ has a slight difference from the initial estimated state ${\widehat{\mathbf{x}}}_k^l$, the above correspondence construct scheme with ${\widehat{\mathbf{x}}}_k^l$ is relatively accurate. After the state optimization, the fourth step is the feature map update, shown in Equation (14). From the above discussion, A-I2P acts like a plug-and-play module. To ensure the correctness of 2D–3D correspondences, it leverages the LiDAR depth to convert 2D–3D correspondence to 3D–3D correspondence, and uses the 3D distance threshold to filter out incorrect 2D–3D correspondences. A-I2P only maintains a feature map with 3D keypoints. It exploits the current LiDAR scan with IMU data, so that A-I2P has a low computational burden. More analysis of DLIC is provided in the Appendix A.

5. Experiments and Discussions

5.1. Dataset Configuration

To investigate the performance of multi-sensor-multi-type SLAM methods, we conduct extensive experiments on the public NTU VIRAL [18] dataset, as shown in Figure 4. The NTU VIRAL dataset is selected for three main reasons. First, to the best of our knowledge, it is the first real-world dataset that provides multi-LiDAR–IMU–camera data collected on a single UAV platform equipped with multiple LiDARs, IMUs, and cameras. Second, it includes 18 diverse indoor and outdoor scenes, enabling a comprehensive performance evaluation under varying conditions. Third, this dataset has been widely adopted in previous studies for comparison; however, most existing works utilize only one IMU and conduct evaluations on the first 9 scenes [5,6,11]. In contrast, our experiments demonstrate that by effectively fusing data from multiple sensors, the proposed DLIC achieves superior performance across all 18 scenarios. Notably, we further evaluate DLIC on the remaining 9 scenes for a more complete and rigorous assessment.

Specifically, in the VIRAL dataset, the UAV is equipped with two 16-beam Ouster LiDARs (refresh rate is 10 Hz) and two monocular camera sensors (refresh rate is 10 Hz). Each LiDAR unit integrates an IMU (refresh rate is 100 Hz). The cameras capture $640\times 480$ gray-scale images, and their intrinsic parameters are carefully calibrated in advance. Consequently, the UAV constitutes a typical multi-sensor-multi-type system. To obtain the ground truth (GT) position of the UAV, the dataset employs ultra wide band (UWB) technique and a 3D laser tracking system [18]. All sensor data in the NTU VIRAL dataset are recorded using the robot operating system (ROS, an open-source C++ robot platform) and saved as ROS bag files. During the evaluation of state estimation methods, these ROS bags are replayed to reproduce real-time data streams from all sensors. To ensure accurate and synchronized data acquisition, we construct C++ queues that store incoming sensor measurements with their corresponding timestamps.

For the proposed DLIC method, two LiDAR-camera-IMU subsystems are configured ($L=2$). The first subsystem consists of the horizontal LiDAR, its associated IMU, and the left camera; the second subsystem includes the vertical LiDAR, its IMU, and the right camera. IMU calibration is performed offline using the parameters provided by the NTU VIRAL dataset, including the extrinsic transformations between the IMU and the LiDAR/camera, as well as intrinsic gyroscope and accelerometer biases. For each subsystem, IMU initialization follows the classical open-source work, FAST-LIO [22]. Data synchronization among LiDAR, IMU, and camera also follows the FAST-LIO, using a first-in-first-out (FIFO) queue strategy. The hyperparameters in the ESIKF are kept consistent with those in Fast-LIVO [6]. To simulate the condition of a commercial UAV onboard system, DLIC is compiled with C++ on an Ubuntu 20.04 Linux system, and tested on a CPU with 8 cores and 8 GB memory.

To evaluate the accuracy of the estimated UAV position, we mainly use RMSE, MAE, SD, and SSE metrics. RMSE and MAE are evaluated in the following manner:

$$\mathrm{RMSE}=\sqrt{{\displaystyle \frac{1}{U}}\sum _i^U{\parallel T_{\mathrm{gt},i}-T_i\parallel }_2^2},\mathrm{MAE}={\displaystyle \frac{1}{U}}\sum _i^U{\parallel T_{\mathrm{gt},i}-T_i\parallel }_2$$

(15)

where $T_{\mathrm{gt},i}$ and $T_i$ are the GT and estimated UAV 3D positions at the i-th time. U is the total time. $\parallel T_{\mathrm{gt},i}-T_i{\parallel }_2^2$ is the 3D position error at the i-th time. To evaluate the overall trajectory error, RMSE and MAE compute the position error with the different manners. Following the guidance [18], we adjust the temporal difference between the estimated and GT trajectories for the real metric estimation. Metrics SD and SSE are computed from the set $\{\parallel T_{\mathrm{gt},i}-T_i{{\parallel }_2\}}_{i=1}^U$. Units of RMSE and MAE are meter. In the following experiments, we utilize EVO (https://github.com/MichaelGrupp/evo (accessed on 24 November 2025)) to compute the above metrics.

5.2. Comparison Results

This experiment investigates the performance of DLIC and existing methods across 18 real-world scenes. The compared approaches contain visual-inertial methods (SVO [28], VINS-Fusion [30]), LiDAR-inertial methods (Fast-LIO [22], D-EKF [13], IGE-LIO [7]), multi-LiDAR method M-LOAM [11], LiDAR-inertial-visual methods (Fast-LIVO [6], R2live [9]). Considering the restricted computational resource of UAVs, only the above methods are included in the comparison, as they have relatively lightweight computational complexity. Since the proposed method can also work in a multi-LiDAR–IMU configuration, we mark it as DLI in this special case. The main quantitative comparison results on the first nine scenes are presented in

Table 2. RMSE results of the compared methods on the first nine scenes in the NTU VIRAL dataset. L, I, C means the usage of LiDAR, IMU, and camera. L1 and L2 mean the usage of single LiDAR and double LiDAR. Red bold is 1st, and blue bold is 2nd.

RMSE Metric	Sensor Usage	EEE_01	EEE_02	EEE_03	NYA_01	NYA_02	NYA_03	SBS_01	SBS_02	SBS_03
Fast-LIO [22]	L1+I1	0.540	0.220	0.250	0.240	0.210	0.230	0.250	0.260	0.240
Fast-LIVO [6]	L1+I1+C1	0.280	0.170	0.230	0.190	0.180	0.190	0.290	0.220	0.220
DVL-SLAM [35]	L1+C1	2.880	1.650	3.080	2.090	1.450	1.820	1.080	2.310	2.230
SVO [28]	I1+C1	Fail	Fail	4.120	2.290	2.910	3.320	7.840	Fail	Fail
VINS-Fusion [30]	I1+C1	0.608	0.506	0.494	0.397	0.424	0.787	0.508	0.564	0.878
R2Live [9]	L1+I1+C1	0.450	0.210	0.970	0.190	0.630	0.310	0.560	0.240	0.440
M-LOAM [11]	L2	0.249	0.166	0.232	0.123	0.191	0.226	0.173	0.147	0.153
D-EKF [13]	L2+I2	0.269	0.164	0.220	0.229	0.178	0.207	0.208	0.220	0.244
IGE-LIO [7]	L1+I1	0.209	0.197	0.217	0.231	0.195	0.194	0.207	0.219	0.212
R3Live [10]	L1+I1+C1	1.690	−	0.640	0.630	0.350	0.230	0.400	0.270	0.210
SR-LIVO [36]	L1+I1+C1	0.210	0.230	0.220	0.180	0.190	0.200	0.120	0.220	0.210
DLI (Our)	L2+I2	0.256	0.152	0.211	0.196	0.166	0.183	0.185	0.181	0.182
DLIC (Our)	L2+I2+C2	0.237	0.146	0.208	0.166	0.143	0.170	0.162	0.160	0.173

. It can be observed that visual-inertial methods [28,30] exhibit unsatisfactory performance compared to other approaches. With the addition of Lidar sensor, the performance improves notably. This contrast is primarily attributed to the high noise and instability of visual measurements in complex environments, which are less reliable than LiDAR data. In particular, the NYA_03 sequence, an indoor scene with weak texture features, further highlights this limitation. Similarly, without IMU assistance, DVL-SLAM [35] suffers from the largest localization errors, as its state estimation is highly sensitive to outliers in visual keypoint matching. These results collectively indicate that leveraging multi-sensor data can significantly enhance overall performance. In contrast, our proposed methods, DLI and DLIC, achieve superior robustness and accuracy by effectively fusing measurements from multiple sensors. This enables them to maintain stable estimation even in noisy or texture-degraded environments, achieving significantly lower RMSE values than competing approaches.

Then, we analyze the performance of state estimation methods with LiDAR-based multi-sensor. From Table 2, the overall RMSE metrics of proposed DLIC and DLI are smaller than current methods, such as R2live [9], R3live [10], M-LOAM [11], IGE-LIO [7], and SR-LIVO [36]. The reasons are illustrated in depth. First, the proposed method utilizes multi-LiDAR, so that it can receive LiDAR measurements with the larger FoVs. Second, DLIC is more robust to IMU measurement noise, for it leverages the multi-IMU to decrease acceleration and angular velocity noises. Third, the proposed method can construct the visual measurement by matching the image and the local feature map. It increases the accuracy and efficiency of visual measurement construction.

From Table 2, it is observed that some methods, including Fast-LIO [22], Fast-LIVO [6], and D-EKF [13], achieve competitive performance with the proposed DLI and DLIC methods. These methods are further compared where the results are provided in

Table 3. Other metrics of the compared methods on the first nine scenes in the NTU VIRAL dataset. Red bold is 1st, and blue bold is 2nd.

MAX RMSE Metric	EEE_01	EEE_02	EEE_03	NYA_01	NYA_02	NYA_03	SBS_01	SBS_02	SBS_03
Fast-LIO [22]	0.633	0.732	0.638	0.649	0.463	0.542	0.654	0.587	0.513
Fast-LIVO [6]	0.586	0.628	0.582	0.641	0.459	0.488	0.588	0.556	0.492
D-EKF [13]	1.012	0.621	0.438	0.588	0.469	0.630	0.709	0.492	0.504
DLI (Our)	0.973	0.541	0.411	0.520	0.545	0.466	0.439	0.435	0.491
DLIC (Our)	0.519	0.407	0.364	0.608	0.443	0.455	0.420	0.415	0.464
MAE Metric	EEE_01	EEE_02	EEE_03	NYA_01	NYA_02	NYA_03	SBS_01	SBS_02	SBS_03
Fast-LIO [22]	0.277	0.152	0.231	0.210	0.191	0.197	0.231	0.254	0.231
Fast-LIVO [6]	0.248	0.136	0.219	0.215	0.184	0.175	0.261	0.201	0.205
D-EKF [13]	0.252	0.141	0.208	0.190	0.161	0.181	0.198	0.207	0.232
DLI (Our)	0.242	0.129	0.199	0.167	0.134	0.150	0.165	0.159	0.163
DLIC (Our)	0.224	0.126	0.196	0.135	0.113	0.144	0.134	0.142	0.151

and

Table 4. Results of the compared methods on the last nine scenes in the NTU VIRAL dataset. Red bold is 1st, and blue bold is 2nd.

RMSE Metric	RTP_01	RTP_02	RTP_03	TNP_01	TNP_02	TNP_03	SPMS_01	SPMS_02	SPMS_03
Fast-LIO [22]	0.402	0.240	0.636	0.138	0.159	0.174	0.635	2.216	1.595
Fast-LIVO [6]	−	−	−	0.114	0.107	0.195	0.975	1.211	2.043
D-EKF [13]	0.298	0.165	0.633	0.096	0.109	0.178	0.363	2.047	1.505
DLI (Our)	0.216	0.157	0.572	0.095	0.104	0.144	0.301	1.898	0.684
DLIC (Our)	−	−	−	0.088	0.098	0.111	0.285	1.198	1.328
MAX RMSE Metric	RTP_01	RTP_02	RTP_03	TNP_01	TNP_02	TNP_03	SPMS_01	SPMS_02	SPMS_03
Fast-LIO [22]	1.667	0.741	1.991	0.392	0.328	0.422	4.376	7.046	5.250
Fast-LIVO [6]	−	−	−	0.304	0.421	0.377	2.165	4.607	4.631
D-EKF [13]	1.902	0.498	2.003	0.332	0.485	0.604	1.957	8.055	5.695
DLI (Our)	0.782	0.339	1.976	0.310	0.401	0.421	1.112	5.432	3.305
DLIC (Our)	−	−	−	0.251	0.295	0.511	1.062	4.161	3.574
MAE Metric	RTP_01	RTP_02	RTP_03	TNP_01	TNP_02	TNP_03	SPMS_01	SPMS_02	SPMS_03
Fast-LIO [22]	0.318	0.196	0.578	0.126	0.148	0.143	0.489	1.641	1.405
Fast-LIVO [6]	−	−	−	0.096	0.097	0.181	0.897	0.989	1.779
D-EKF [13]	0.287	0.150	0.563	0.078	0.121	0.152	0.287	1.554	1.318
DLI (Our)	0.252	0.143	0.517	0.075	0.093	0.102	0.260	1.388	0.464
DLIC (Our)	−	−	−	0.068	0.079	0.076	0.251	0.947	1.240

. In Table 3, we analyze the maximum RMSE and MAE metrics of these methods in the first nine scenes on the NTU VIRAL dataset [18]. Maximum RMSE is an important metric to reflect the stability of state estimation methods. The maximum RMSE of DLIC is always smaller than that of others. It suggests that the proposed distributed state estimation method is robust to multi-sensor measurement noise. MAE is another common metric to evaluate the UAV positional error. MAE of DLI and DLIC is smaller than that of others. Combining the results in Table 2 and Table 3, as well as the visualization of estimated trajectories in Figure 5, the proposed DLIC method has achieved an accurate state estimation result in the various scenes.

In Table 4, we further investigate the generalization ability of these methods in the last nine scenes of the NTU VIRAL dataset [18]. Most methods do not report their performance in these scenarios [6,13,22]. For a fair comparison, we fix the hyper-parameters of all methods and test their generalization ability in unseen scenarios. In the RTP scene, camera images are not saved during the dataset collection, so Fast-LIVO and DLIC cannot work. The results in Table 4 indicate that the proposed DLI and DLIC methods outperform existing methods in the overall metrics (i.e., RMSE, max RMSE, and MAE). The reason is analyzed in-depth. Compared with Fast-LIO and Fast-LIVO, the proposed DLI and DLIC can make use of the measurements from multiple LiDARs, multiple cameras, and multiple IMUs in a distributed estimation manner, thus increasing the robustness to sensor measurement noise. Compared with D-EKF, the proposed method considers the fine-grained multi-IMU measurements. It makes DLI and DLIC more accurate than D-EKF in most scenes. In all, from the results in Table 4, the proposed method achieves a higher generalization ability than existing methods.

Furthermore, to better evaluate the error turbulence caused by multi-sensor-multi-type noise during UAV state estimation, we use other metrics, such as standard deviation (SD) and the sum of squares due to error (SSE), and the result is provided in

Table 5. State error turbulence evaluation of current state estimation methods on EEE_03 scene. Red bold is 1st.

Metrics	Fast-LIO [22]	Fast-LIVO [6]	D-EKF [13]	DLIC
SD	0.101	0.071	0.068	0.062
SSE	12.338	10.212	10.075	9.241

. SD and SSE of DLIC are smaller than those of others, for the proposed method makes full use of the multi-sensor-multi-type measurements.

After that, we aim to evaluate the robustness of the proposed DLIC method under different noise levels. Specifically, the sensor noise (i.e., Gaussian random noise with the standard deviation ${\sigma }_{\mathrm{LiDAR}}$) is mainly added into the LiDAR point cloud, which is used to test the stability of multi-sensor-based state estimation methods. ${\sigma }_{\mathrm{LiDAR}}\in [0,0.5]$ is divided into five noise levels. RMSE results are provided in

Table 6. Robustness evaluation of the current state estimation methods on EEE_01 scene. Red bold is 1st.

Noise Levels	Fast-LIO [22]	Fast-LIVO [6]	D-EKF [13]	DLIC
0	0.540	0.280	0.269	0.237
1	0.548	0.285	0.272	0.241
2	0.552	0.288	0.278	0.242
3	0.563	0.291	0.280	0.245
4	0.578	0.293	0.284	0.247
5	0.583	0.298	0.290	0.256

. It is observed that, with the proposed distributed state estimation method, the estimated state is more robust to the sensor noise than other methods.

From the above experiments in the 18 scenes, it is concluded that the proposed DLIC makes full use of the multi-LiDAR–IMU–camera measurements, so that it achieves a more accurate and robust localization performance than current methods in most scenes.

5.3. Visualization Verifications

This experiment investigates the reconstruction performance of the proposed method. As the NTU VIRAL dataset [18] does not contain the GT of reconstruction results, we compare the mapping detail as well as visualize the trajectory difference of each approach. As Fast-LIVO [6], DLI, and DLIC have the closest performance in Table 2 and Table 4, these methods are compared in this experiment. The overall mapping results are shown in Figure 6. It is found that the proposed method has a scanning area larger than Fast-LIVO, for the proposed method utilizes the multi-sensor-multi-type information. From Figure 7, it is observed that the trajectory of DLIC is relatively smoother than D-EKF [13], so that DLIC has a smaller RMSE than D-EKF. From Figure 8, the local mapping details show that the reconstruction result by DLIC has less noise than D-EKF [13]. Mapping results of Fast-LIVO [6] and DLIC are provided in Figure 9. The reason why the left image is more blurred than the right one is that the point cloud reconstructed by Fast-LIVO has much greater noise than the proposed method. It indicates that the UAV trajectory estimated by the proposed DLIC method is more accurate than Fast-LIVO. After discussing the mapping results, it is concluded that the proposed DLIC method uses the multi-LiDAR–IMU–camera measurements effectively to obtain the fine-grained 3D mapping results.

5.4. Ablation Studies

This experiment investigates the performance of each module inside DLIC, with the result provided in

Table 7. Ablation study of DLIC on EEE_03 scene. Red bold is 1st.

Method	RMSE Metric	MAE Metric
Baseline-1	0.250	0.210
Baseline-2	0.284	0.233
Baseline-1+A-I2P	0.219	0.207
Baseline-2+A-I2P	0.231	0.211
Baseline + Equation (8) (DLI)	0.211	0.199
Baseline + Equation (8) + A-I2P (DLIC)	0.208	0.196

. Baseline means Fast-LIO [22]. Baseline-1 or Baseline-2 means the usage of the horizontal or vertical LiDAR. In the outdoor scene, the horizontal LiDAR sensor has a wide FoV, so that Baseline-1 is more accurate than Baseline-2. Baseline+A-I2P is to add visual measurements in the ESIKF framework. With the usage of A-I2P, the localization accuracy of Baseline-1 and 2 is enhanced, because the 2D–3D correspondences can decrease the negative impact of the noisy 3D–3D correspondences in state estimation. On the other hand, it is found that DLIC also improves the performance of Baseline-1, for it makes use of the observations from Baseline-1 and 2. With the usage of Equation (9) and the A-I2P module, the proposed DLIC has achieved a significant improvement over Baseline-1. In all, the experiment demonstrates that the proposed modules are effective for multi-sensor-multi-type-based SLAM.

To further investigate the robustness of the methods in Table 7, we follow the experimental setting in Table 6 and conduct the noise experiments in Figure 10. With the proposed A-I2P and distributed state estimation model in Equation (8), the proposed method is more robust to sensor noise.

Further, we conduct an experiment to investigate the hyper-parameters ${\tau }_{2D}$, ${\tau }_I$, and ${\tau }_{3D}$ of A-I2P on the performance of Baseline+A-I2P. The results are presented in

Table 8. Ablation study of hyper-parameters of A-I2P on EEE_03 scene. Red bold is 1st.

${\tau }_{2D}$	1	3	5	7	9
RMSE	0.248	0.245	0.244	0.251	0.254
${\tau }_I$	6	12	18	24	30
RMSE	0.244	0.241	0.234	0.221	0.238
${\tau }_{3D}$	2.5	5.0	7.5	10.0	12.5
RMSE	0.221	0.219	0.228	0.231	0.234

. ${\tau }_{2D}\in [1,9]$, ${\tau }_I\in [6,30]$, ${\tau }_{3D}\in [2.5,12.5]$. At first, we check ${\tau }_{2D}$, while ${\tau }_I$ and ${\tau }_{3D}$ are set to default values of 6 and $2.5$ cm, respectively. If ${\tau }_{2D}\ge 5$, RMSE begins to increase, for the large 2D search region causes the inaccuracy of keypoint depth. ${\tau }_{2D}=5$ has the best RMSE. Then, we fix ${\tau }_{2D}=5$ and check ${\tau }_I$, while ${\tau }_{3D}$ is at the default value. When ${\tau }_I\ge 30$, the alignment error of LiDAR point and keypoint is increasing, causing the large RMSE error. ${\tau }_I=30$ is optimal. After that, we fix ${\tau }_{2D}$ and ${\tau }_I$, and then study ${\tau }_{3D}$. If ${\tau }_{3D}$ is set too large, 3D–3D correspondence searched by kNN is not accurate, causing inaccuracy of mapping. Thus, ${\tau }_{2D}=5$, ${\tau }_I=24$, ${\tau }_{3D}=5.0$ cm are the best hyper-parameters in A-I2P.

Moreover, we conduct the runtime analysis, and the result is provided in

Table 9. Runtime analysis of DLIC on EEE_03 scene.

Method	Average Runtime per Loop
D-EKF	18.89 ms
Baseline	18.32 ms
Baseline + A-I2P	24.31 ms
Baseline + Equation (9) (DLI)	19.56 ms
Baseline + Equation (9) + A-I2P (DLIC)	25.86 ms

. Baseline is Fast-LIO [22]. Compared with Fast-LIO [22], the additional cost of DLIC is mainly the updating of the sharing feature map. Due to the multi-threading technique, solving Equation (8) does not need double the runtime, so that DLI has the closest runtime to Fast-LIO [22]. As A-I2P is an efficient module, the runtime of DLIC is ≈25 ms. Although the state-of-the-art D-EKF method [16] is faster than the proposed methods, it does not fully use the multi-LiDAR–IMU–camera sensor data, so that its state estimation accuracy is less then DLIC. The core of DLIC is the distributed state estimation in Equation (7) and A-I2P. From Table 9, it is found that the Equation (7) costs nearly 1.24 ms and A-I2P costs nearly 5.99 ms. This means that the proposed method is efficient in the actual applications.

We also analyze the peak memory usage of the proposed method. In this test, the memory of visualization from ROS RVIZ is not included. In an SLAM system, it is noted that the memory usage is related to the mapping area, because the feature map storage is the dominant factor in memory usage. In the EEE_03 scene, the result of peak memory usage is presented in

Table 10. Peak memory usage of DLIC on EEE_03 scene. ^† denotes different sensor configurations.

Method	Sensor Usage	Peak Memory Usage
Fast-LIVO [6]	L1+C1+I1	1821 MB
Fast-LIVO † [6]	L2+C2+I2	3277 MB
DLIC	L2+C2+I2	2741 MB

. For a fair comparison, Fast-LIVO ^† denotes the combination of two groups of LiDAR–camera–inertial system, in the meantime, as DLIC does. As DLIC can filter the overlapped 3D keypoints during the updating of iKd-Tree [34], DLIC saves nearly 0.5 GB of memory compared to Fast-LIVO ^†. Further, we also investigate the effect of the voxel size of the proposed method and the results are shown in

Table 11. RMSE and peak memory usage of DLIC on EEE_03 scene under different voxel size.

Voxel Size	0.025 m	0.050 m	0.100 m
RMSE	0.201 m	0.208 m	0.352 m
Peak memory usage	8421 MB	2741 MB	723 MB

. The default map voxel size is 0.050 m in the experiments. We test the performance of DLIC if the voxel size is 0.025 m and 0.100 m. When the voxel size is decreased to 0.025 m, the mapping resolution is enlarged with the overlarge CPU memory cost. If the voxel size is enlarged as 0.100 m, most mapping details are lost, such that the RMSE error is larger than 0.35 m. Thus, it is recommended to set the proper voxel size in the actual applications. From the above experiments, DLIC is a real-time state estimation method with light memory usage.

5.5. Limitations and Future Works

From the above method comparisons, visualization analysis, and ablation studies, it is found that DLIC has made a certain amount of progress in the various scenarios. However, there are still some potential improvements for DLIC. First, since DLIC is not equipped with a loop closure detection and optimization module, this issue may limit the performance of DLIC in long-range state estimation. We will design a specific loop closure module for the multi-sensor-multi-type system. Second, moving objects have a certain negative impact (i.e., motion blur) in the proposed method, as shown in Figure 11. We will address this issue in future work. Third, although our current fusion strategy demonstrates reasonable performance, it remains handcrafted. Exploring deep learning or AI-driven adaptive fusion techniques may better handle complex environments and further enhance both robustness and efficiency. We will pursue this direction in future work. Fourth, as there are few datasets that support state estimation on multi-sensor-multi-type-based systems, it is essential to establish a simulation environment on ROS to collect the multi-sensor-multi-type measurements in future work. It will greatly benefit the algorithm development of state estimation on multi-sensor-multi-type-based systems.

6. Conclusions

We propose an efficient and accurate distributed state estimation method, DLIC, which effectively fuses data from multi-LiDAR–IMU–camera sensors and estimates the UAV’s state. At first, we develop a theoretical state estimation model. It decomposes the original complex estimation problem into multiple low-complexity optimization sub-problems, which can be easily solved with a modern embedded system with a multi-core CPU. The proposed distributed state estimation model has a feedback function that serves as a bridge to interact with state estimation results from all subsystems. After that, we implement the above theoretical model and propose a method, DLIC, for a system with multi-LiDAR–IMU–camera sensors. In DLIC, we propose an A-I2P module to construct accurate 2D–3D correspondences with the aid of LiDAR depth. Extensive experiments on the public real-world NTU VIRAL dataset demonstrate the effectiveness of the proposed DLIC method. Thus, we believe that the proposed DLIC method benefits the development of multi-sensor autonomous systems.