# Application of Kalman Filter to Improve 3D LiDAR Signals of Autonomous Vehicles in Adverse Weather
## 2 Research Methodology  
The Kalman filter was first described in 1960 in a doctoral dissertation entitled New Approach to Linear Filtering and Prediction Problems published by Hungarian emigre [PERSON] [21]. Kalman filters are widely used for orbit calculation, target tracking, and navigation due to their real-time and anti-interference advantages as well as rapid action and high efficiency. In addition, they are also applied in the fields of integrated navigation and dynamic positioning, sensor data fusion, microeconomics, and so forth, especially for digital image processing and the currently hot research fields of pattern recognition, image segmentation, and image edge detection [22]. The Kalman filter is basically a recursive algorithm, with each recursion containing two steps. First, an estimated value is calculated, and then a weighted summation of the estimated value and the measured value is obtained to get the best resultant state. The process comprises five steps with five corresponding formulas: (1) predicts the prior state \(x_{k}^{\prime}\) using the corrected estimate \(\hat{x}_{k-1}\) and using the prior state \(\hat{x}_{k-1}\) predicted at time k-1 to calculate the estimated value \(x_{k}^{\prime}\) at time \(k\); (2) uses the noise \(P_{k-1}\) of the resulting state at time k-1 to calculate the estimated noise \(\hat{P_{k}}\) at time \(k\); (3) uses the estimated value of the noise \(\hat{P_{k}}\) at time \(k\) and the measured value of noise \(r\) at time \(k\) to calculate the Kalman gain \(k_{k}\) at time \(k\); \(\hat{x}_{k}\) is the posterior estimate of the state \(k_{k}\) after correction of the prior estimate \(x_{k}^{\prime}\) with the current measurement \(z_{k}\) obtained using (4); \(P_{k}\) is the error covariance of the estimated value \(\hat{x}_{k}\), which is computed by correcting the previous estimated value \(P_{k}^{\prime}\) with (5), where \(H\) is the parameter of the measurement system \(Q\) and \(R\) the process and the measurement noise covariance. The Kalman filter model assumes that the true state at time \(k\) is evolved from the state at time \((k-1)\), which conforms to the following:  
\[x_{k}^{\prime}=A\hat{x}_{k-1}+Bu_{k-1} \tag{1}\]  
\[P_{k}^{\prime}=AP_{k-1}A^{T}+Q \tag{2}\]  
\[k_{k}=P_{k}^{\prime}H^{T}\Big{(}HP_{k}^{\prime}H^{T}+R\Big{)}^{-1} \tag{3}\]  
\[\hat{x}_{k}=x_{k}^{\prime}+k_{k}\big{(}z_{k}-Hx_{k}^{\prime}\big{)} \tag{4}\]  
\[P_{k}=(1-k_{k}H)~{}P_{k}^{\prime}. \tag{5}\]  
Here, \(A\) and \(B\) are system parameters. Initially, \(x_{0}\) is a random vector, with an average value \(u_{0}=E[x_{0}]\) and covariance \(P_{0}=E\big{[}(x_{0}-u_{0})(x_{0}-u_{0})^{T}\big{]}\). Here, \(w_{k}\) and \(v_{k}\) are the process noise and observation noise, both of which are assumed to be zero-mean multivariate Gaussian noise; \(u_{k}\) is the control vector. The function f can be used to estimate the previously predicted value, and the function h can be used to calculate the measured value from the predicted value. The covariances are, respectively, \(Q_{k}\) and \(R_{k}\) expressed as follows: \(E\big{[}w_{k}w_{k}^{T}\big{]}=Q_{k}\) and \(E\big{[}v_{k}v_{k}^{T}\big{]}=R_{k}\). The nearest neighbor segmentation algorithm is described below. This filtering method uses statistical filtering and radius filtering to remove noise. Statistical filtering refers to statistical analysis of the distance between the query point and the neighboring point cloud, and removes some noise that is not within the set range [23]. The \(n\)-th point in the LiDAR point cloud model is marked \(q_{n}\), n = 1, 2, 3,..., S. Assuming that the distance from \(q_{n}\) to any point is \(d_{i}\), then the average distance from \(q_{n}\) to all its k neighboring points \(D_{avr-n}\) is represented by the Gaussian distribution (the mean is \(\mu\), the standard deviation is \(\sigma\)) as in [24]:  
\[D_{avr-n}=\frac{\sum_{i=1}^{k}d_{i}}{k}. \tag{6}\]  
Set the standard range \(S_{pa-n}\) to determine whether the point cloud in the model is noise.