# 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
### Weighted Matching Point Clouds Using the Mean Direction Error  
Figure 7 shows that the mean error value changes significantly as the function of angle \(\beta\), and the gradient of these changes will increase while the eccentricity of the ellipse is approaching 1 (the ellipse will be becoming flat, which occurs where measurements are carried out using LIDAR). Owing to these particular properties, this error can usefully determine the value of translation and rotation established while minimalizing the error metric between the points from subsequent measurements (e.g., point clouds from LIDAR measurements). Figure 8 presents, in the graphical form, how the positions matched in pairs of points based on the corrections (e.g., those added to the components of the rotation matrix and translation vector) change; the value of the rotation and translation was made depending on the values of the sums of mean errors (Equation (9)) calculated along lines connecting the matched points from \((k)\) and \((k+1)\) cloud. In order to facilitate the interpretation of the phenomenon, it was assumed that the mean error ellipses were similar in size. Figure 8: A graphic presentation the corrected position using the sums of mean errors CEP (circular error probable) calculated for the matched points. Figure 7: A graphic presentation of the corrected position of the points matched in pairs using the sums of mean errors calculated along the lines connecting the matched points. As can be seen in Figure 7, greater weight is introduced for the points whose mean errors in the specified direction (point-to-point direction) is lower-pairs marked as 2 and 3 in Figure 7. The heavier weight influences the rotation and translation more and that is why pair 2 and 3 are better aligned comparing with 1 and 4. The use of such a weighting coefficient allows taking into account the spatial distribution of errors according to the selected direction. For comparison, Figure 8 presents, in a graphical form, how the positions matched in pairs of points based on the corrections whose values depended on the values of the sums of mean errors (Equation (5)) calculated for the matched points. As can be seen each pair (1, 2, 3, 4) influences the rotation and translation in similar manner. HDG, SOG, and ROT were determined by processing LIDAR scans archived in data packets in accordance with the methodology presented in Section 2.1. The pseudo-code of the program used for the calculations using the authors original weighting factor \(w(k,k+1)\) and [PERSON]'s robust function coefficient \(kHu=c\cdot\text{Median}\|\mathbf{p}_{1,\ldots,n}(k)-\mathbf{p}_{id(1,\ldots,n)} (k+1)\|\) finally took the following form:  
``` Input: \(\mathbf{M}(k),\mathbf{M}(k+1),\sigma_{r},\sigma_{\alpha},\mathbf{R}=\mathbf{ I},\mathbf{T}=0,old_{res}=0,\



* measurements carried out using LIDAR and a GNSS RTK receiver, and the synchronous recording of their results,
* the determination of the HDG, SOG and ROT, based on LIDAR measurement results, by the scan-matching method using both classic and modified versions of the ICP algorithm,
* an analysis comparing the HDG, SOG, and ROT with reference results of GNSS RTK measurements, based on the accuracy criterion. The yacht port basin in Gdynia (Poland) was selected for the measurement area. Its boundaries include high, concrete wharves and a breakwater. Most of the time, good hydro-meteorological conditions prevail in the area (Figure 9). To carry out the measurements, a small USV (with a length of 1.62 m, width of 0.40 m, and a draught of 0.11 m) equipped with Scarse Sweep Lidar, a GNSS RTK receiver Leica Viva CS 15 [48], and an on-board computer connected to them with a RS-422/232C conversion cable [49], were used to record the measurement results (Figure 10).