# 3D Point Cloud Generation Based on Multi-Sensor Fusion
## 2 Filtering and Smoothing Theory
### RTS Smoothing Algorithm  
Since the position of the control point measured by the total station and the estimated trajectory error of the original data were used as the observation update for the filter in the generation of the 3D point cloud based on multi-sensor fusion, when there was an observation update, the error of the measured position estimate and its covariance are both very small. However, the position measurement estimated errors and their covariances between the two observation updates increased over time owing to residual systematic errors. To obtain a high-precision position estimation in the whole process, as is necessary to introduce an appropriate estimation method in the measurement interval without an observation update so as to fully use the observation information before and after the observation time point. This was a typical fixed interval smoothing problem.  
The commonly used methods to address the issues of fixed-interval smoothing include two-filter and RTS smoothing. Although both algorithms are theoretically equivalent, the RTS smoothing algorithm was relatively simple to implement. The two-filter smoothing algorithm included two independent Kalman filtering processes, which made it more complicated to implement and required more variables to be stored. The RTS smoothing algorithm combined the reverse filter and the smoothing process into a single reverse recursive form, including a forward Kalman filtering process and reverse smoothing process. The structure was relatively simple, the implementation was convenient, and the calculation efficiency was relatively high. The RTS smoothing algorithm, jointly proposed by [PERSON], [PERSON] and [PERSON], was a classic fixed interval filtering smoothing algorithm. In this study, we used the RTS smoothing algorithm to process the track measurement interval data.  
The RTS smoother consists of a typical forward Kalman filter and a reverse smoother. After each system propagation and observation update, the system state vector, error covariance matrix and state transition matrix were recorded. After the data reached the end, the data were smoothed in reverse from the end to the starting point. The calculation flow of the RTS smoothing algorithm is shown in Figure 1.  
The forward filtering of the RTS smoothing algorithm is a typical Kalman filter, and the smooth estimated value of the state vector was a weighted combination of two filtering estimated values. The calculation is as follows:  
\[\begin{split} X_{t,f}^{-}&=F_{t}X_{t-1,f}\\ P_{t,f}^{-}&=F_{t}P_{t-1,f}F_{t}^{T}+\Gamma_{t}Q_{t }{F_{t}}^{T}\\ K_{t,f}&=P_{t,f}^{-}{H_{t}}^{T}\Big{[}H_{t}P_{t,f}^{- }H_{t}^{T}+R_{t}\Big{]}^{-1}\\ X_{t,f}&=X_{t,f}^{-}+K_{t}\Big{[}Z_{t}-H_{t}X_{t,f} ^{-}\Big{]}\\ P_{t,f}&=[I-K_{t}H_{t}]P_{t,f}^{-}\end{split} \tag{14}\]  
where \(X_{t,f}\) and \(P_{t,f}\) represent the optimal filter estimate and its covariance matrix of the forward filter state vector at time \(t\); \(X_{t,f}^{-}\) and \(P_{t,f}^{-}\) represent the optimal one-step prediction value of the forward filter state vector at time \(t\) and its covariance matrix. \(H_{t}\) is the observation matrix at time \(t\); \(K_{t,f}\) is the optimal gain matrix of forward filtering at time \(t\); \(F_{t}\) represents the system state transition matrix and can be obtained from the systematic error matrix \(A_{t}\). \(\Gamma_{t}\) is the discrete noise matrix at time \(t\) for forward filtering.  
The inverse smoothing process can be expressed as  
\[\begin{split} K_{t,b}&=P_{t,f}F_{t}^{T}\Big{(}P_{t +1,f}^{-}\Big{)}^{-1}\\ X_{t,b}&=X_{t,f}+K_{t,b}\Big{[}X_{t+1,b}-X_{t+1,f} ^{-}\Big{]}\\ P_{t,b}&=P_{t,f}+K_{t,b}\Big{[}P_{t+1,b}-P_{t+1,f} ^{-}\Big{]}K_{t,b}{}^{T}\end{split} \tag{15}\]  
where \(X_{t,b}\) and \(P_{t,b}\) are the optimal smoothed estimates at time \(t\) of the RTS smoothing algorithm and its error covariance matrix, \(K_{t,b}\) is the optimal smoothing gain matrix of the RTS smoother at time \(t\).