# Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters
## 1 Introduction
On the other hand, SLAM has become very popular in determining the position of autonomous underwater vehicles (AUV). A review of the methods applied in determining the positions of AUVs can be found in [18]. Since SLAM is already a relatively well-studied method, many approaches to solving a particular position-determining problem have emerged. One of the most common solutions to the problem is the so-called scan-matching. This set of techniques involves matching the subsequent scans of the surrounding environment which are obtained while exploring a particular area [19]. There are numerous algorithms variants used to match scans obtained by mapping devices. These include the ICP algorithm [20; 21], ICL [22] and algorithms based on matching histograms of the distance function [23]. However, given the different methods of matching point clouds, the ICP algorithm proposed for the first time in [24] outperforms other algorithms in its simplicity and the possibility for its continuous development. Considering the use of the SLAM technique to determine the parameters of the vessel's movement, it is worth looking at the 2011 article by [PERSON] et al. [25]. In this case, the authors use the object approach (landmark) based on the SIFT algorithm and the extended Kalman filter (EKF), and images from the navigation radar. The results of the work are promising; however, in this case there is a significant drift of coordinates of the position and parameters of the vessels movement. Another publication describing the implementation of the SLAM task at sea [17]. The approach presented here assumes that the map of the environment will be explored on a simultaneous basis by more than one vehicle/ship. This assumption allows obtaining a much more accurate model of the map of the environment (global map) by combining many sub-maps obtained by vehicles. This mapping is used to position each of them. The approach presented in the article is a scan matching without odometry information. A similar solution, but based on another scan matching algorithm, was presented in [26]. It involves adjusting concurrent scans one after another without using dead reckoning data in the alignment algorithm. This article uses the ICP algorithm with the error metric in the point-to-point version [27]. Other versions of the metrics are: point-to-line [28], point-to-plane [29], and plane-to-plane [30]. The point-to-point metric was chosen because of the intuitive way of attaching importance to individual correspondences(based solely on the coordinate error of the position of a pair of points), without the need to consider the uncertainty of the position of the line or plane (for cases of three-dimensional point clouds). In this article, the emphasis is put on the methods of weighting on the basis of estimated errors of the position coordinates of a pair of corresponding points. The problem of weighting of pairs in a navigational approach is presented in [31], and for other applications in [32; 33]. This publication assumes that pairs with a higher distance value between points will be given lower weights compared to those at a smaller distance from one another. Weighting based on color was also accepted. Another approach presented in [21] is the weighting depending on the uncertainty of the position of points in the scan and the uncertainty of the position of the straight line (in the point-to-line variant). It was assumed that more weight will be put on pairs that have less influence on the error metric. A similar approach is presented in [34]. The authors carry out tests on three variants of the error measure: point, line and plane. The approach proposed in this article is similar to this; however, it assumes a different, more universal distribution of coordinates error. They can be easily implemented not only in the case of a lidar sensor, but also in the case of navigation radar.