# 3D Point Cloud Generation Based on Multi-Sensor Fusion
## 2 Filtering and Smoothing Theory
### Subsection Location Update Algorithm  
The attitude update adopted the quaternion algorithm, and the quaternion algorithm is expressed as follows:  
\[Q(q_{0},q_{1},q_{2},q_{3})=q_{0}+q_{1}i+q_{2}j+q_{3}k \tag{1}\]  
where \(q_{0},\ \ q_{1},\ q_{2},\ \ q_{3}\) are real numbers, \(i,j\) and \(k\) are mutually orthogonal unit vectors and imaginary units \(\surd(-1)\). According to the quaternion algorithm, the position can be updated as  
\[r^{R}=C_{iB}^{R}r^{b} \tag{2}\]  
The \(R\) system is the coordinate navigation system, and the local geographic coordinate system is used as the coordinate navigation system. The origin is at the location of the navigation system, and the three coordinate axes point to the North East High (NEH) direction. The \(i\) system is the IMU coordinate system, and there is an installation deviation between the car body coordinate system, \(b\) system and IMU coordinate system. The car body coordinate system is recorded as the \(b\) system, and the three axes of the car body coordinate system point to the front left and upper direction of the trolley, respectively. In Equation (2), \(C_{iB}^{R}=C_{b}^{R}C_{i}^{b}\), and \(C_{i}^{b}\) is the direction cosine matrix from the car system to the IMU coordinate system, representing the installation relationship between the IMU and car body. \(C_{b}^{R}\) is the direction cosine matrix between the IMU and the navigation system, and the relationship between it and the attitude quaternion is \[C_{b}^{R}=\begin{bmatrix}q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}&2(q_{1}q_{2}-q_{0} q_{3})&2(q_{1}q_{3}+q_{0}q_{2})\\ 2(q_{1}q_{2}+q_{0}q_{3})&q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2}&2(q_{2}q_{3}-q_ {0}q_{1})\\ 2(q_{1}q_{3}-q_{0}q_{2})&2(q_{2}q_{3}+q_{0}q_{1})&q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+ q_{3}^{2}\end{bmatrix} \tag{3}\]  
which equals  
\[C_{b}^{R}=\begin{bmatrix}cos\gamma cos\varphi+sin\gamma sin\varphi sin\theta &sin\varphi cos\theta&sin\gamma cos\varphi-cos\gamma sin\varphi sin\theta\\ -cos\gamma sin\varphi+sin\gamma cos\varphi sin\theta&cos\varphi cos\theta&-sin \gamma sin\varphi-cos\gamma cos\varphi sin\theta\\ -sin\gamma cos\varphi&sin\theta&cos\gamma cos\theta\end{bmatrix} \tag{4}\]  
where \(\varphi,\theta,\ \gamma\) represent three Euler angles, the heading, pitch and roll angles, respectively.  
The trajectory estimation method was based on the combination of the uses of the IMU and odometer. The attitude information measured by IMU and the speed information measured by odometer for trajectory estimation and its position update equation can be expressed as  
\[\Delta\vec{r}^{R}=C_{b}^{R}C_{i}^{b}v^{b} \tag{5}\]  
where \(\Delta\vec{r}^{R}=\left[\Delta r_{N}\ \ \Delta r_{E}\ \ \Delta r_{H}\right]^{T}\) is the relative displacement vector of the track measurement car relative to the last observation position under the navigation system. The speed of the vector car was \(v^{b}=\left[\widetilde{v}_{0}\ \ 0\ \ 0\right]^{T}\) under the car system, and \(\widetilde{v}_{0}\) is the forward speed of the car. When the installation error angle is small, the matrix \(C_{i}^{b}\) can be expressed as  
\[C_{i}^{b}=\begin{bmatrix}1&\varepsilon_{z}&-\varepsilon_{y}\\ -\varepsilon_{z}&1&\varepsilon_{x}\\ \varepsilon_{y}&-\varepsilon_{x}&1\end{bmatrix} \tag{6}\]  
where \(\varepsilon_{x},\varepsilon_{y},\varepsilon_{z}\) is the axial installation error of the IMU relative to the X-axis, Y-axis and Z-axis of the trolley. When the odometer had a measurement error, the true speed \(\widetilde{v}_{0}\) can be expressed as  
\[\widetilde{v}_{0}=(1+\delta k)v_{0} \tag{7}\]  
where \(\delta k\) is the scale factor error of the odometer, \(v_{0}\) is the output speed of the odometer. In trajectory measurement, the product of inertial navigation attitude angle error and odometer error \(\delta k\), and IMU installation error \(\varepsilon_{x},\varepsilon_{y},\varepsilon_{z}\) is a high-order small quantity and can be ignored.