# Multi-Sensor Fusion for Aerial Robots in Industrial GNSS-Denied Environments
## 2 Materials and Methods
### MCL Algorithm  
As previously stated, our algorithm is based on a Monte Carlo localization algorithm that was previously developed by the authors [25], which was based on an RGB-D camera as the main sensor for obtaining 3D point clouds of the environment. Taking into account the redesign of the aerial robot and the new tentative applications for industrial scenarios, there are several differences. The odometry input for the particle filter is no longer based on vision but on LiDAR. The 3D point clouds that the robot acquires are very different from the ones acquired by an RGB-D camera since now they have less density and a higher  
Figure 2: Hardware architecture. range. In essence, the filter behavior is similar: it compares the map point cloud with the robot point clouds, taking also into account the radio-based measurements, to compute the weights of particles. In such an environment, the particles have different weights according to each computed probability, so the weighted average of all particles would give the estimated current robot pose (position and orientation). Each particle \(i\) has its pose \(p_{i}\) and an associated weight \(w_{i}\) as follows:  
\[p_{i}= \begin{pmatrix}x\\ y\\ z\\ y\end{pmatrix} ,\sum_{i=1}^{N}w_{i}=1 \tag{1}\]  
where \(x,y,\) and \(z\) represent the particle position, and \(\psi\) the yaw orientation. The main algorithm is shown in Figure 3, and the three main parts of the particle filter loop of our implementation are explained below. Prediction is responsible for moving the particles according to the increment of odometry in the time interval. Thus, it generates the new particles at time \(t+1\) according to the pose (position and yaw angle) indicated by the current aerial robot pose at time \(t\):  
\[\Delta p_{i}= \begin{bmatrix}\Delta x\\ \Delta y\\ \Delta z\\ \Delta\psi\end{bmatrix}  \tag{2}\]  
\[x_{i}^{t+1}=x_{i}^{t}+\Delta x*cos(\psi_{i}^{t})-\Delta y*sin(\psi_{i}^{t}) \tag{3}\]  
\[y_{i}^{t+1}=y_{i}^{t}+\Delta x*sin(\psi_{i}^{t})+\Delta y*cos(\psi_{i}^{t}) \tag{4}\]  
\[z_{i}^{t+1}=z_{i}^{t}+\Delta z \tag{5}\]  
\[\psi_{i}^{t+1}=\psi_{i}^{t}+\Delta\psi \tag{6}\]  
The update is responsible for computing the weights of each particle \(w_{i}\). It verifies the correctness of the pose of each particle by using the 3D point cloud from the LiDAR and the radio-based measurements of the UWB sensors. The weight \(w^{ LiDAR}\) is calculated by using the LiDAR sensor and the map of the environment provided as an input to the algorithm. The goal is to see how well the point cloud provided by the sensor aligns to the  
Figure 3: Complete scheme of the algorithm. map from each particle pose. It should be noted that an initial grid of the input 3D map is created at the beginning, as an efficient measure to reduce the run time of the algorithm. On the other hand, there is the weight \(w^{UWB}\) that involves the radio sensors installed in the environment, whose positions are known. This weight represents how well each particle pose accommodates the received radio-based distance measurements from the UWB sensor onboard the aerial robot to the fixed sensors \((d_{ik})\). This weight represents how well each particle pose fits the radio-based distance measurements received from the UWB sensor on board the aerial robot to the fixed sensors using a Gaussian distribution (mean \(r_{k}\) and standard deviation \(\sigma\) where k is the corresponding beacon). Additionally, there is a parameter \(\alpha\) that is used to assign higher or lower relative importance to each of the two sensing modalities, according to the particularities of the scenario; for example, in environments with a high density of metallic structures, it is likely that radio sensors provide a higher number of incorrect distance measurements.