# Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters
## 2 Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR
### The Determination of USV Movement Parameters Using the Scan-Matching Method
The modification of the ICP method will involve the application of the weighting factor \(w_{1,\ldots,n}(k,k+1)\) for calculations for each pair of points (nearest neighbors):  
\[w_{1}(k,k+1)=\frac{1}{\sqrt{G_{i}(k,\ k+1)}}, \tag{11}\]  
where \(G_{i}(k,\ k+1)\) is the sum of mean errors of the determination of the coordinates of points \(\mathbf{p}_{i}(k)\) and \(\mathbf{p}_{il(i)}(k+1)\) in the specified direction \(\beta\) (see Figure 2):  
\[G_{i}(k,k+1)=\sigma_{\beta i}^{2}(k)+\sigma_{\beta i(i)}^{2}(k+1), \tag{12}\]  
where \(\sigma_{\beta i}(k)\) is the mean error of the determination of the coordinates of the point \(\mathbf{p}_{i}(k)\) in the direction of \(\beta\) (from \(\mathbf{p}_{i}(k)\) to \(\mathbf{p}_{il(i)}(k+1)\)), \(\sigma_{\beta i(i)}(k+1)\) is the mean error of the determination of the coordinates of the point \(\mathbf{p}_{i}(k+1)\) in the direction of \(\beta\) (from \(\mathbf{p}_{il(i)}(k+1)\) to \(\mathbf{p}_{i}(k)\)), or \(G_{i}(k,\ k+1)\) is the mean error of the determination of the coordinates of the points \(\mathbf{p}_{i}(k)\) and \(\mathbf{p}_{il(i)}(k+1)\) (see Figure 1):  
\[G_{i}(k,k+1)=M_{i}^{2}(k)+M_{il(i)}^{2}(k+1), \tag{13}\]  
where \(M_{i}(k)\) is the mean error of the determination of the coordinates of the point \(\mathbf{p}_{i}(k)\), \(M_{il(i)}(k+1)\) is the mean error of the determination of the coordinates of the point \(\mathbf{p}_{il(i)}(k+1)\). Figure 3: Left side—2-point clouds before Dalaunay triangulation; right side—Dalaunay triangulation and nearest neighbor match (dashed lines). Weighting step gives specific calculated value for each pair. Then in further processing heavier pairs involve more the translation and rotation parameters (Figure 4). Rejection. Due to the relatively high noise level of many scans, particularly in the sectors illustrating long distance measurements, the Huber function [43] which attenuated the outlying measurement results (affected by gross errors) was also applied in the calculations. This function was arbitrarily selected from among many functions known and commonly used for this purpose [21; 31; 44; 45; 46; 47]. The robust function rejects or significantly reduces weights of the specific pairs due to robustness criterion (see Figure 5). The robust function is based on \(kHu\) factor presented in Algorithm 1. Rejecting or weight minimization is performed based on norm of two paired points. In this article the fusion of the robust function and proposed error weighting is used to derivate the final weight. Assigning an error metric. The final function minimizing the value of the matching error E, used to accurately determine the translation vector \(\mathbf{T}(k+1)\) and the rotation matrix \(\mathbf{R}(k+1)\), will take the following form:  
\[\text{E}(\mathbf{R},\ \mathbf{T})=\ \sum\
olimits_{i=1}^{n}w_{i}(k,k+1)\cdot \lVert\mathbf{R}\mathbf{p}_{i}(k)+\mathbf{T}-\mathbf{p}_{id(i)}(k+1)\rVert^{2}. \tag{14}\]  
Provided that points \(\mathbf{p}_{i}(k)\) and \(\mathbf{p}_{id(i)}(k+1)\) are located the closest to one another, i.e., are the nearest neighbors. This method is based on the point-to-point measure of error. Minimizing the metric between selected points. The method of minimizing the error between the corresponding points was carried out on the basis of the SVD (Singular Value Decomposition). The method works equally for the weighted and classical ICP algorithm. In this article a method  
Figure 4: Weighting due to proposed error factor. Bolted, dashed line equals more weight for specific pair. Figure 5: Rejecting pairs due to robust criterion. identical to [47] was used.