# A Distributed Indoor Mapping Method Based on Control-Network-Aided SLAM: Scheme and Analysis
## 2 Method
### IMU-Aided Scan-to-Map Matching  
In the scan-to-map matching process, which is also called the front end, high-precision initial pose estimates can improve the accuracy of matching, which is calculated by the IMU mechanization algorithm in this paper. The main task of IMU mechanization is to use the output values of the gyroscope and accelerometer to calculate the navigation parameter through the position, velocity, and attitude update algorithms, which is a process of recursive calculation. The speed differential equation under the navigation coordinate system can be expressed as  
\[\dot{V}_{en}^{n}=C_{b}^{n}f^{b}+g^{n}-\left(2\omega_{ie}^{n}+\omega_{en}^{n} \right)\times V_{en}^{n}, \tag{2}\]  
where \(V_{en}^{n}\) is the projected velocity under the coordinate system \(n\) of the coordinate system \(e\) (that is, the ground speed) relative to the coordinate system \(n\); \(f^{b}\) is the specific force outputted by the accelerometer; \(2\omega_{ie}^{n}\times V_{en}^{n}\) is the Coriolis acceleration; \(\omega_{en}^{n}\times V_{en}^{n}\) is the centripetal acceleration of the carrier relative to the Earth; and \(g^{n}\) is the acceleration of gravity. The quaternion equation for positional update can be expressed as  
\[q_{n(k)}^{e(k)}=q_{e(k-1)}^{e(k)}q_{n(k-1)}^{e(k-1)}q_{n(k)}^{n(k-1)} \tag{3}\]  
where the position quaternion \(q_{n(k-1)}^{e(k-1)}\) at time \(t_{k-1}\) is known information and \(q_{n(k)}^{n(k-1)}\), \(q_{e(k-1)}^{e(k)}\) can be calculated as  
\[q_{n(k)}^{n(k-1)}=\left[\begin{array}{c}\cos\left\|0.5\zeta_{k}\right\|\\ \frac{\sin\left\|0.5\zeta_{k}\right\|}{\left\|\zeta_{k}\right\|}\zeta_{k} \end{array}\right] \tag{4}\]  
\[q_{e(k-1)}^{e(k)}=\left[\begin{array}{c}\cos\left\|0.5\zeta_{k}\right\|\\ -\frac{\sin\left\|0.5\zeta_{k}\right\|}{\left\|\zeta_{k}\right\|}\xi_{k} \end{array}\right] \tag{5}\]  
where \(\zeta_{k},\xi_{k}\) is the rotation vector of the ECEF (Earth-Centered, Earth-Fixed) coordinate system and navigation coordinate system from time \(k-1\) to time \(k\). Thus, we can get a relatively correct position from the IMU mechanization algorithm, which can narrow the search window of the LiDAR scan-to-map matching. Using this method, we can properly solve the problem of data association and there is no need for an exhaustive pose search.  
Figure 1: The relationship between the scanning point and the grid points.  
The Gauss-Newton method is utilized to find the optimal match between the current LiDAR scan data and the map that has been constructed. Given an occupancy grid map obtained from previous LiDAR scan data and the initial pose (provided by IMU) of the current LiDAR scan data, we seek to find the optimal rigid transformation \(T_{\xi}=\left(p_{x},p_{y},\psi\right)\) to minimize the occupancy probability residual:  
\[\underset{\xi}{\text{argmin}}\sum_{k=1}^{K}[1-M_{smooth}(T_{\xi}h_{k})]^{2} \tag{6}\]  
where \(T_{\xi}\) transforms point \(h_{k}\) from the LiDAR scan coordinate system to the map coordinate system; \(M_{smooth}(T_{\xi}h_{k})\) returns the occupancy probability at the coordinate \(T_{\xi}h_{k}\) and is a smooth version of the occupancy probability values in the map. We use bilinear interpolation here to calculate the occupancy probability value \(M_{smooth}(T_{\xi}h_{k})\), and the gradient is computed as  
\[\forall M_{smooth}(T_{\xi}h_{k})=(\frac{\partial M_{smooth}}{\partial x}(T_{ \xi}h_{k}),\frac{\partial M_{smooth}}{\partial y}(T_{\xi}h_{k})). \tag{7}\]  
The four closest integer coordinates around the position \(T_{\xi}h_{k}\) are needed to calculate the gradient during the calculation because the discontinuous nature of the occupancy grid map makes it impossible to calculate the derivative directly and will limit the calculation accuracy.