# 3D Point Cloud Generation Based on Multi-Sensor Fusion
## 3 A Method of 3D Point Cloud Generation by Multi-Sensor Fusion
### Trajectory Correction Algorithm
Original data estimation error According to the position update algorithm, the IMU attitude angle and odometer speed were used to calculate the line trajectory, and according to the position update equation, the approximate displacement increment \(\Delta S\) of the time \(t+\Delta t\) relative to the time \(t\) was calculated, and the time \(t+\Delta t\) coordinates were  
\[r_{t+\Delta t}=r_{t}+\Delta S \tag{22}\]  
We obtained the track center point at the same time as the target center point, and translated the original data to estimate the trajectory, so that the previous target calculated track center point coincided with the corresponding time estimated trajectory point, and the error \(\left[\delta\Delta r_{N}\ \ \delta\Delta r_{E}\ \ \delta\Delta r_{H}\right]^{T}\) between the trajectory coordinates and the target calculated track center point at the next moment could be calculated. 3. RTS filter correction In the system, the IMU zero bias instability was 0.01/h, the angle random walk noise was 0.02/h, the odometer measurement accuracy was 0.45/1000, the target center point coordinate measurement was measured by Leica high-precision optical total station, the angle measurement accuracy was 0.5\({}^{\prime\prime}\), and the ranging accuracy was 1 mm + 2 \(\times\) 10\({}^{-6}\). The trajectory estimation error of the control point corresponding to the track center was used as the observation update and the optimal estimation of the IMU attitude angle error and the odometer velocity error was obtained by filtering, which was used to correct the original data and solve the track center line. The discrete Kalman filter equations in the horizontal and vertical directions were:  
Figure 14: Displacement vector from the center of the target to the center of the track in the same section. \[X_{l}(t)=F_{l}X_{l}(t-1)+w_{l}(t-1) \tag{23}\] \[Z_{l}(t)=H_{l}X_{l}(t)+\mu_{l}(t)\] \[X_{v}(t)=F_{v}X_{v}(t-1)+w_{v}(t-1)\] \[Z_{v}(t)=H_{v}X_{v}(t)+\mu_{v}(t)\]  
Since there was no coupling between the horizontal and vertical directions, Kalman filter models could be established for the horizontal and vertical directions, respectively. We selected the pitch angle, heading angle attitude error \(\varnothing_{\theta}\) and \(\varnothing_{\varphi}\), gyro offset sum \(\delta\overline{w}_{y}^{b}\) and \(\overline{\delta w}_{z}^{b}\), position error \(\delta\Delta r_{N,\delta}\Delta r_{E},\delta\Delta r_{H}\), and odometer scale factor \(\delta k\) as Kalman filter state quantities. The system state variables in the horizontal and vertical directions were  
\[X_{l}(t)= \begin{bmatrix}\delta\Delta r_{N}\\ \delta\Delta r_{E}\\ \delta k\\ \varnothing_{\varphi}\\ \delta\overline{w}_{z}^{b}\end{bmatrix} ,\qquad X_{v}(t)= \begin{bmatrix}\delta \Delta r_{H}\\ \varnothing_{\theta}\\ \delta\overline{w}_{y}^{b}\end{bmatrix}  \tag{24}\]  
The displacement error observation was used as the filter update, the position observation value was provided by the total station, and the difference between the position value and the position value measured by dead reckoning was used as the observation update of the Kalman filter, namely,  
\[Z_{l}(t)= \begin{bmatrix}\delta\Delta r_{N}\\ \delta\Delta r_{E}\end{bmatrix} ,\quad\ Z_{v}(t)=\delta\Delta r_{H} \tag{25}\]  
where the system transition matrix \(F_{l}\) is  
\[F_{l}= \begin{bmatrix}1&0&v_{0}tcos\varphi&v_{0}tsin\varphi&-\frac{1}{2}v_{0}t^ {2}sin\varphi\\ 0&1&v_{0}tsin\varphi&v_{0}tcos\varphi&\frac{1}{2}v_{0}t^{2}cos\varphi\\ 0&0&1&0&0\\ 0&0&0&1&-t\\ 0&0&0&0&1\end{bmatrix}  \tag{26}\]  
\[F_{v}= \begin{bmatrix}1&v_{0}t&\frac{1}{2}v_{0}t^{2}\\ 0&1&-t\\ 0&0&1\end{bmatrix}  \tag{27}\]  
Horizontal system noise \(w_{l}(t)= \begin{bmatrix}w_{\delta k}&w_{\varphi}\end{bmatrix} \), \(w_{l}(t)\sim N(0,Q_{l}(t))\), vertically \(w_{v}(t)=w_{\theta}\), \(w_{v}(t)\sim N(0,\ Q_{v}(t))\), where \(Q_{l}(t)=diag\Big{(}\Big{[}\sigma_{w_{\delta k}}^{2}\ \sigma_{w_{\varphi}}^{2}\Big{]}\Big{)}\), \(Q_{v}(t)=\sigma_{w_{\theta}}^{2}\) is the system noise power spectral density.