# 3D Point Cloud Generation Based on Multi-Sensor Fusion
## 2 Filtering and Smoothing Theory
### Error Equation  
In the trajectory estimation, the source of position errors mainly included the initial IMU attitude angle, drift, inertial navigation installation and odometer speed measurement errors. The IMU attitude angle error is obtained from the attitude error equation. The attitude error equation of the system is expressed, as in [22]:  
\[\dot{\Phi}^{R}=-\omega_{iR}^{R}\times\Phi^{R}+\delta\omega_{iR}^{R}-C_{b}^{R} \delta\omega_{ib}^{b} \tag{8}\]  
where \(\Phi^{R}=\begin{bmatrix}\varnothing_{N}\ \ \ \varnothing_{E}\ \ \ \varnothing_{H}\end{bmatrix}^{T}\) is the attitude error angle relative to the three axes of the navigation system, \(\delta\omega_{iR}^{R}\) is the angular velocity error vector of the rotation of the navigation system relative to the inertial system; \(\omega_{ib}^{b}\) represents the drift error of the three axial gyroscopes; \(\times\) denotes a vector cross product.  
Since the radius of curvature of the track is usually large and the slope of the track is small, the horizontal attitude angle of the track measurement trolley is usually small, so each element of the direction cosine matrix \(C_{b}^{R}\) is approximately expressed as  
\[C_{b}^{R}\approx\begin{bmatrix}cos\varphi&-sin\varphi\angles in the two directions are represented by \(\varnothing_{\gamma},\varnothing_{\theta}\) and the heading error is represented by \(\varnothing_{\varphi}\). Its conversion relationship is expressed as  
\[\begin{bmatrix}\varnothing_{\gamma}\\ \varnothing_{\theta}\\ \varnothing_{\varphi}\end{bmatrix}=\begin{bmatrix}cos\varphi&sin\varphi&0\\ -sin\varphi&cos\varphi&0\\ 0&0&1\end{bmatrix}\begin{bmatrix}\varnothing_{N}\\ \varnothing_{E}\\ \varnothing_{H}\end{bmatrix} \tag{10}\]  
Substituting Equation (8) into Equations (6) and (7) results in  
\[\begin{bmatrix}\dot{\varnothing}_{\gamma}\\ \dot{\varnothing}_{\theta}\\ \dot{\varnothing}_{\varphi}\end{bmatrix}=-\begin{bmatrix}\delta\overline{w}_{ y}^{b}\\ \delta\overline{w}_{z}^{b}\end{bmatrix}+\begin{bmatrix}\omega_{\gamma}\\ \omega_{\theta}\\ \omega_{\varphi}\end{bmatrix} \tag{11}\]  
where \(\begin{bmatrix}\delta\overline{w}_{x}^{b}&\delta\overline{w}_{y}^{b}&\delta \overline{w}_{z}^{b}\end{bmatrix}^{T}\) is the equivalent gyroscope zero bias, and \(\begin{bmatrix}\omega_{\gamma}&\omega_{\theta}&\omega_{\varphi}\end{bmatrix}^ {T}\) is the equivalent gyro angle random walk noise.  
Due to the weak mobility of the measuring trolley in the railway track surveying, there was a coupling between the systematic errors. There were few observation control points, so it was impossible to effectively estimate all systematic errors. In short-distance measurements, the roll angle of attitude errors did not cause significant position errors in three directions, and the roll angle error could be ignored when constructing the Kalman filter model. Equation (9) is  
\[\begin{bmatrix}\dot{\varphi}_{\theta}\\ \dot{\varphi}_{\varphi}\end{bmatrix}=-\begin{bmatrix}\overline{w}_{y}^{b}\\ \overline{w}_{z}^{b}\end{bmatrix}+\begin{bmatrix}w_{\theta}\\ w_{\varphi}\end{bmatrix} \tag{12}\]  
Based on the above analysis, the error term was put into Equation (5) to obtain the estimated position value \(\Delta r^{R}\) with error. According to the position update equation, the obtained position error equation is  
\[\delta\Delta r^{R}=\Delta\overline{r}^{R}-\Delta r^{R}\approx\begin{bmatrix}0& sin\varphi&cos\varphi\\ 0&-cos\varphi&sin\varphi\\ 1&0&0\end{bmatrix}\begin{bmatrix}\overline{\varnothing}_{\theta}\\ \overline{\varnothing}_{\varphi}\\ \delta k\end{bmatrix}v_{0} \tag{13}\]