# 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 taxonomy of the ICP algorithm is carried out in the following successive stages of data processing [21]:  
* Selection (selection of points which are suitable for the alignment);
* Matching (matching points from model cloud to reference points cloud);
* Weighting (giving weight to corresponding points);
* Rejecting (deleting some points based on the robust criterion function);
* Assigning an error metric (choosing point-to-point, point-to-line, point-to-plane or plane-to-plane error metric);
* Minimizing the metric between selected points. Selection. Let us assume that a USV determines the change in its position through the translation vector \(\mathbf{T}(k+1)\) and the rotation matrix \(\mathbf{R}(k+1)\) determined based on two maps of the surrounding environment \(\mathbf{M}(k)\) and \(\mathbf{M}(k+1)\), constructed at the moment \((k)\) and the moment \((k+1)\) following it (e.g., 2D horizontal layer of cloud points for LIDAR measurements). Let \(\mathbb{Z}(k)\) and \(\mathbb{Z}(k+1)\) represent two sets \(\{(r_{1}(k),\alpha_{1}(k))\),..., \((r_{n}(k),\alpha_{n}(k))\}\) and \(\{(r_{1}(k+1),\alpha_{1}(k+1))\),..., \((r_{n}(k+1),\alpha_{n}(k+1))\}\) of measurements of the distance \(r\) and direction \(\alpha\), carried out in relation to \(n\) field obstacles (see Figure 1). On their basis, set of points \(\mathbf{M}(k)=\big{\{}\mathbf{p}_{1}(k)\),...,\(\mathbf{p}_{n}(k)\big{\}}\) and \(\mathbf{M}(k+1)=\big{\{}\mathbf{p}_{1}(k+1)\),...,\(\mathbf{p}_{n}(k+1)\big{\}}\) will be determined, where the coordinates of each point in the formula are calculated via the following dependence:  
\[ \begin{array}{l}\mathbf{p}_{i}=\left[\begin{array}{c}x_{i}\\ y_{i}\end{array}\right],\\ x_{i}=r_{i}\cdot\cos(\alpha_{i}),\\ y_{i}=r_{i}\cdot\sin(\alpha_{i}),\end{array}  \tag{10}\]  
where:  
\[i=1,\ldots,n\]  
They will be used to determine the translation vector \(\mathbf{T}(k+1)\) and the rotation matrix \(\mathbf{R}(k+1)\) using the singular value decomposition (SVD) method [38], which will be carried out in combination  
Figure 2: Mean error of the determination of position coordinates in the specified direction. (calculated using the covariance matrix). with a change in the position (in subsequent iteration steps) of the set of points from \(\mathbf{M}(k)\) to \(\mathbf{M}(k+1)\) using the modified weighted ICP method. In reality, there is very little probability that both sets of points \(\mathbf{M}(k)\) and \(\mathbf{M}(k+1)\) will contain the same number of points. Equalising the number of points is required for the family of ICP algorithms with point-to-point error metric and therefore, in the calculations they were carried out by reducing the number of points. From a more numerous set, the points were randomly removed so that their number would be equal to the number of the points from a smaller set. Matching. Literature shows that there are many ways to match points in pairs. One of them is the k-d tree method presented in [39; 40]. Also considered are methods based on the heuristic search of pairs to reduce the complexity of calculations [41]. In this article, correspondence between points is created based on the method presented in [42]. It is based on the Delaunay triangulation. The method generates a two-dimensional grid of triangles based on the reference set \(\mathbf{M}(k)\). Next, using the nearest neighbor method, the nearest apex of triangles to the points from the set \(\mathbf{M}(k+1)\) are found (points from \(\mathbf{M}(k)\)) in such a way that for each point \(\mathbf{p}_{i}(k)\) the nearest point \(\mathbf{p}_{il(i)}(k+1)\) is assigned (see Figure 3). Weighting of the pairs.