Use of a Weighted ICP Algorithm to Precisely Determine USV Movement Parameters

: The purpose of this article is to present a study aimed at developing a method for the precise determination of unmanned surface vehicle (USV) movement parameters (heading (HDG), speed over ground (SOG) and rate of turn (ROT)) through appropriate processing. The technique employs a modiﬁed weighted ICP (Iterative Closest Point) algorithm and a 2D points layer arranged in the horizon plane obtained from measurements. This is performed with the help of Light Detection and Ranging (LIDAR). A new method of weighting is presented. It is based on a mean error in a given direction and the results of modiﬁed weighted ICP tests carried out on the basis of ﬁeld measurement data. The ﬁrst part of the paper characterizes LIDAR measuring errors and indicates the possibilities for their use in matching point clouds. The second part of the article deals with a method for determining the SOG and course over ground (COG), based on a modiﬁed weighted ICP algorithm. The main part of the paper reviews a test method aimed at evaluating the accuracy of determining the SOG and COG by the scan-matching method using a modiﬁed weighted ICP algorithm. The ﬁnal part presents an analysis comparing the obtained SOG and COG results with reference results of GNSS RTK measurements and the resulting generalised conclusions.


Introduction
Heading (HDG), speed over ground (SOG) and rate of turn (ROT) are basic movement parameters which characterize the way a ship's hull moves in relation to the Earth's surface. They are crucial in the decision-making process, particularly when navigating in congested areas, including harbors and rivers. Currently, these parameters are most frequently determined on a ship using a gyrocompass and a log, i.e., navigational instruments whose measuring accuracy, according to the International Maritime Organization (IMO) requirements, is not required to be very high: "Errors in the indicated speed, when the ship is operating free from shallow water effect and from the effects of wind, current and tide, should not exceed 2% of the speed of the ship, or 0.2 knots, whichever is greater" [1], "The follow-up error for different rates of turn should be: less than ±0.5 • at rates up to 10 • /s; and less than ±1.5 • between a rate of 10 • /s and 20 • /s" [2], although it determines the precision and the ability to control the ship's movement which, consequently, may affect the navigational safety level. It is therefore reasonable to carry out scientific research aimed at searching for new measuring and data processing methods in order to minimize measuring errors in a ship's movement parameters.
For many years, maritime navigation has placed increasing emphasis on the development of methods to determine position and navigational parameters that could offer an alternative to the GNSS system [3]. In the age of the development of unmanned surface vehicles (USVs) whose main tasks increasingly involve independent (autonomous) maneuvering in restricted areas, it is necessary to develop new methods to obtain precise (accurate) navigational movement parameters (in particular the HDG, SOG, course over ground (COG), and ROT) [4,5]. Basing the USV decision-making process (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.

Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR
The determination of horizontal coordinates using LIDAR is carried out based on the angle α and the distance r measured between the sensor and the target, i.e., the light signal reflection point ( Figure 1).
Appl. Sci. 2019, 9, x FOR PEER REVIEW 3 of 17 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.

Evaluation of the Accuracy of Determining the Coordinates of a Position Using LIDAR
The determination of horizontal coordinates using LIDAR is carried out based on the angle and the distance measured between the sensor and the target, i.e., the light signal reflection point ( Figure 1). The mean error M and the parameters of mean error ellipse (i.e., the lengths of semi-axes and ) of the determination of 2D horizontal coordinates (x, y) using LIDAR can be calculated by applying the law of mean error propagation [35,36]. When the functions of a single result of the following measurement are known: The mean error M and the parameters of mean error ellipse (i.e., the lengths of semi-axes a and b) of the determination of 2D horizontal coordinates (x, y) using LIDAR can be calculated by applying the law of mean error propagation [35,36]. When the functions of a single result of the following measurement are known: equations of the mean errors of positional lines can be written as: which enable the determination of mean error of position: where: θ-the angle of intersection of positional lines, which for LIDAR measurements is always equal to 90 • , σ α -mean error of the angle measurement α, σ r -mean error of the distance measurement r, r-distance between the sensor and the target, a-the length of the long semi-axis of mean error ellipse (see Figure 2), b-the length of the short semi-axis of mean error ellipse (see Figure 2).
It follows from Dependence (5) that the value of mean error M and the length of the long semi-axis a of the mean error ellipse are significantly affected by the distance r between the sensor and the target. The calculations assumed that the measurements were performed using the Scanse Sweep LIDAR [37] for which the manufacturer provides σ r = 0.004258·r (rounded value). Thereby one takes into account the fact that the error value of the angle measurement does not only depend on the beam's divergence (footprint of the beam increases as a function of divergence) and the angle of its incidence on the surface of the object, but also on the accuracy of measuring the angle change by the optical head encoder that can be affected by environmental conditions such as temperature and humidity, vibrations and imperfections in the performance of mechanical elements that cooperate with each other. This can be characteristic for entry level LIDAR devices. The spatial angle resolution can be derived from sampling frequency that is set up to 1000 Hz (when optical sensor is spinning with frequency 1 Hz the Lidar is able to sample up to 1000 point for a single spin, 5 Hz-200 points, 10 Hz-100 points). The range spatial resolution is around 0.1 cm.
Based on the known lengths of semi-axes a and b of the mean error ellipse, and based on the known direction of α measurement, one can simply generate a covariance matrix cov of the (x, y) vector coordinates: where α a -is the angle between the semi-axis a of the mean error ellipse and the axis OY (for LIDAR measurements, equal to α + 90 • ). Then, using this, to determine the mean square error σ β 2 of the determination of the position coordinates, but in the specified direction β: σ β 2 = S β ·cov(x, y)·S T β = sin β· sin β r 2 ·σ α 2 · sin 2 α a + σ r 2 · cos α a − − cos α a · cos β· sin α a σ r 2 − r 2 ·σ α 2 + + sin β· sin β· r 2 ·σ α 2 · cos 2 α a + σ r 2 · sin 2 α a − − cos α a · sin α a · sin β· σ r 2 − r 2 ·σ α 2 , where S β = cos β sin β . (calculated using the covariance matrix).

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-toplane 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 ( + 1) and the rotation matrix ( + 1) determined based on two maps of the surrounding environment ( ) and ( + 1), constructed at the moment ( ) and the moment ( + 1) following it (e.g., 2D horizontal layer of cloud points for LIDAR measurements). Let ℤ( ) and ℤ( + 1) represent two sets {( 1 ( ),  1 ( )), … , ( ( ),  ( ))} and {( 1 ( + 1),  1 ( + 1)), … , ( ( + 1),  ( + 1))} of measurements of the distance and direction , carried out in relation to field obstacles (see Figure 1). On their basis, set of points ( ) = { 1 ( ), … , n ( )} and ( + 1) = { 1 ( + 1), … , n ( + 1)} will be determined, where the coordinates of each point in the formula are calculated via the following dependence: where: = 1, … , They will be used to determine the translation vector ( + 1) and the rotation matrix ( + 1) using the singular value decomposition (SVD) method [38], which will be carried out in combination with a change in the position (in subsequent iteration steps) of the set of points from ( ) to ( + 1) using the modified weighted ICP method. In reality, there is very little probability that both sets of points ( ) and ( + 1) will contain the same number of points. Equalising the number of

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. Let us assume that a USV determines the change in its position through the translation vector T(k + 1) and the rotation matrix R(k + 1) determined based on two maps of the surrounding environment M(k) and 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 Z(k) and Z(k + 1) represent two sets (r 1 (k), α 1 (k)), . . . , (r n (k), α n (k)) and (r 1 (k + 1), α 1 (k + 1)), . . . , (r n (k + 1), α n (k + 1)) of measurements of the distance r and direction α, carried out in relation to n field obstacles (see Figure 1). On their basis, set of points M(k) = p 1 (k), . . . , p n (k) and M(k + 1) = p 1 (k + 1), . . . , p n (k + 1) will be determined, where the coordinates of each point in the formula are calculated via the following dependence: where: i = 1, . . . , n They will be used to determine the translation vector T(k + 1) and the rotation matrix R(k + 1) using the singular value decomposition (SVD) method [38], which will be carried out in combination with a change in the position (in subsequent iteration steps) of the set of points from M(k) to M(k + 1) using the modified weighted ICP method. In reality, there is very little probability that both sets of points M(k) and 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 M(k). Next, using the nearest neighbor method, the nearest apex of triangles to the points from the set M(k + 1) are found (points from M(k)) in such a way that for each point p i (k) the nearest point p id(i) (k + 1) is assigned (see Figure 3). 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 ( ). Next, using the nearest neighbor method, the nearest apex of triangles to the points from the set ( + 1) are found (points from ( )) in such a way that for each point ( ) the nearest point ( ) ( + 1) is assigned (see Figure 3). Weighting of the pairs. The modification of the ICP method will involve the application of the weighting factor 1,…, ( , + 1) for calculations for each pair of points (nearest neighbors): where ( , + 1) is the sum of mean errors of the determination of the coordinates of points ( ) and ( ) ( + 1) in the specified direction (see Figure 2): where ( ) is the mean error of the determination of the coordinates of the point ( ) in the direction of (from ( ) to ( ) ( + 1)), ( ) ( + 1) is the mean error of the determination of the coordinates of the point ( + 1) in the direction of (from ( ) ( + 1) to ( )) , or ( , + 1) is the mean error of the determination of the coordinates of the points ( ) and ( ) ( + 1) (see Figure 1): where ( ) is the mean error of the determination of the coordinates of the point ( ) , ( ) ( + 1) is the mean error of the determination of the coordinates of the point ( ) ( + 1).
Weighting step gives specific calculated value for each pair. Then in further processing heavier pairs involve more the translation and rotation parameters ( Figure 4). Weighting of the pairs. The modification of the ICP method will involve the application of the weighting factor w 1,...,n (k, k + 1) for calculations for each pair of points (nearest neighbors): where G i (k, k + 1) is the sum of mean errors of the determination of the coordinates of points p i (k) and p id(i) (k + 1) in the specified direction β (see Figure 2): where σ βi (k) is the mean error of the determination of the coordinates of the point p i (k) in the direction of β (from p i (k) to p id(i) (k + 1)), σ βid(i) (k + 1) is the mean error of the determination of the coordinates of the point p i (k + 1) in the direction of β (from p id(i) (k + 1) to p i (k)), or G i (k, k + 1) is the mean error of the determination of the coordinates of the points p i (k) and p id(i) (k + 1) (see Figure 1): where M i (k) is the mean error of the determination of the coordinates of the point p i (k), M id(i) (k + 1) is the mean error of the determination of the coordinates of the point p id(i) (k + 1).
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 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 ( + 1) and the rotation matrix ( + 1), will take the following form: Provided that points ( ) and ( ) ( + 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 identical to [47] was used. It is required to determine two weighted average coordinates from the cloud m( ) and m( + 1), dependent on point clouds ( ), ( + 1) and assigned weights in the previous stage: 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. 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 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 ( + 1) and the rotation matrix ( + 1), will take the following form: Provided that points ( ) and ( ) ( + 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 identical to [47] was used. It is required to determine two weighted average coordinates from the cloud m( ) and m( + 1), dependent on point clouds ( ), ( + 1) and assigned weights in the previous stage: Assigning an error metric. The final function minimizing the value of the matching error E, used to accurately determine the translation vector T(k + 1) and the rotation matrix R(k + 1), will take the following form: Provided that points p i (k) and 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 identical to [47] was used. It is required to determine two weighted average coordinates from the cloud m(k) and m(k + 1), dependent on point clouds (k), (k + 1) and assigned weights in the previous stage: Subsequently, we can move on to determine the weighted covariance matrix, which will be used to calculate the innovation of rotation matrix and translation (in a given iteration of the algorithm) Using the SVD decomposition method on the C matrix where matrices U, Σ, V are characteristic matrices for the SVD method. By means of decomposition carried out on designated matrices, we obtain the innovations in given iteration-it-of the R it rotation matrix and the T it translation: The obtained values in the R iter and T iter matrices are partial values developed in a given iteration of the algorithm. Based on this, the set m(k + 1) is translated at last iteration (see Figure 6) and as a result the translation and rotation is obtained: where R it=1 and T it=1 are initialized values.
Subsequently, we can move on to determine the weighted covariance matrix, which will be used to calculate the innovation of rotation matrix and translation (in a given iteration of the algorithm) Using the SVD decomposition method on the C matrix where matrices U, Σ, V are characteristic matrices for the SVD method. By means of decomposition carried out on designated matrices, we obtain the innovations in given iteration--of the R rotation matrix and the translation: The obtained values in the and matrices are partial values developed in a given iteration of the algorithm. Based on this, the set m(k + 1) is translated at last iteration (see Figure 6) and as a result the translation and rotation is obtained: ( + 1) = ( +1 • + + ) + … + ( itn • itn−1 + itn ).
where =1 and =1 are initialized values. ] thus obtained will be used to determine where ∆ is the distance in time between the moment ( ) of performing the measurements of ℤ( ), and the moment ( + 1) of performing subsequent measurements of ℤ( + 1).  The translation vector T(k + 1) = ∆x ∆y thus obtained will be used to determine where ∆t is the distance in time between the moment (k) of performing the measurements of Z(k), and the moment (k + 1) of performing subsequent measurements of Z(k + 1).
On the other hand, the final form of the rotation matrix R(k + 1) = cos ∆θ sin ∆θ − sin ∆θ cos ∆θ will be used to determine: and HDG(k + 1) = HDG 0 + arc sin(∆θ), where HDG 0 is the initial HDG value determined using other methods (devices) prior to the commencement of measurement. Figure 7 shows that the mean error value changes significantly as the function of angle β, 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.

Weighted Matching Point Clouds Using the Mean Direction Error
and HDG( + 1) = HDG 0 + arc sin( ), where HDG 0 is the initial HDG value determined using other methods (devices) prior to the commencement of measurement. Figure 7 shows that the mean error value changes significantly as the function of angle , 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 ( ) and ( + 1) cloud. In order to facilitate the interpretation of the phenomenon, it was assumed that the mean error ellipses were similar in size.  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

Weighted Matching Point Clouds Using the Mean Direction Error
and HDG( + 1) = HDG 0 + arc sin( ), where HDG 0 is the initial HDG value determined using other methods (devices) prior to the commencement of measurement. Figure 7 shows that the mean error value changes significantly as the function of angle , 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 ( ) and ( + 1) cloud. In order to facilitate the interpretation of the phenomenon, it was assumed that the mean error ellipses were similar in size.  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 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.

Weighted Matching Point Clouds Using the Mean Direction Error
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 Huber's robust function coefficient kHu = c·Median p 1,...,n (k) − p id(1,...n) (k + 1) finally took the following form:

The Study and the Analysis of the Obtained Results
The main aim of the study was to evaluate the accuracy of the determination of the HDG, SOG and ROT using a modified ICP algorithm, carried out as a result of their comparison with highaccuracy reference measurements. The study involved: • measurements carried out using LIDAR and a GNSS RTK receiver, and the synchronous recording of their results,

The Study and the Analysis of the Obtained Results
The main aim of the study was to evaluate the accuracy of the determination of the HDG, SOG and ROT using a modified ICP algorithm, carried out as a result of their comparison with high-accuracy reference measurements. The study involved: • 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 Scanse 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). The measurements were carried out on a USV sailing with a speed of approximately 1.2 m s (2.4 kts) over a trajectory of approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer wirelessly connected to AUSV (autonomous unmanned surface vehicle, see Figure 11.). While the USV was sailing, the on-board computer synchronously recorded (when generating a full scan using LIDAR) the measurement results making up the so-called data sets containing: a LIDAR scan (in the form of a measurement set Z = (r 1 , α 1 ), . . . , (r n , α n ) ), NMEA messages with the position coordinates of SOG and COG (course over ground) from the GNSS RTK receiver [50]. 865 data sets were thus collected by recording each subsequent sets every 0.2 s on average, i.e., after a change in the USV position by approx. 0.25 m.
Appl. Sci. 2019, 9, x FOR PEER REVIEW 11 of 17 • 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 Scanse 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). The measurements were carried out on a USV sailing with a speed of approximately 1.2 m s (2.4 kts) over a trajectory of approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer wirelessly connected to AUSV (autonomous unmanned surface vehicle, see Figure 11.). While the USV was sailing, the on-board computer synchronously recorded (when generating a full scan using LIDAR) the measurement results making up the so-called data sets containing: a LIDAR scan (in the form of a measurement set ℤ = {( 1 ,  1 ), … , ( n ,  n )}), NMEA messages with the position coordinates of SOG and COG (course over ground) from the GNSS RTK receiver [50]. 865 data sets were thus collected by recording each subsequent sets every 0.2 s on average, i.e., after a change in the USV position by approx. 0.25 m.   • 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 Scanse 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). The measurements were carried out on a USV sailing with a speed of approximately 1.2 m s (2.4 kts) over a trajectory of approximately 220 m, as presented in Figure 10. All data was collected via a mobile computer wirelessly connected to AUSV (autonomous unmanned surface vehicle, see Figure 11.). While the USV was sailing, the on-board computer synchronously recorded (when generating a full scan using LIDAR) the measurement results making up the so-called data sets containing: a LIDAR scan (in the form of a measurement set ℤ = {( 1 ,  1 ), … , ( n ,  n )}), NMEA messages with the position coordinates of SOG and COG (course over ground) from the GNSS RTK receiver [50]. 865 data sets were thus collected by recording each subsequent sets every 0.2 s on average, i.e., after a change in the USV position by approx. 0.25 m.     Table 1 presents statistical parameters (i.e., the mean value, standard deviation and the maximum value) which characterize the computational accuracy of each parameter. The following acronyms were used to describe ICP's variants: K_ICP for algorithm without modification, H_ICP for robust version based on Huber's function, HM_ICP for robust version based on Huber's function with mean error weighting and HD_ICP for robust version based on Huber's function with directional error weighting. The lower index ICP was used for values computed by ICP algorithm's variants and lower index R was introduced to point the values surveyed with GNSS RTK receiver.   Table 1 shows that the HD_ICP algorithm was the best in terms of the accuracy of determining the USVs movement parameters; it is followed by HM_ICP, H_ICP and K_ICP. Figure 12 shows a graph of SOG_ICP-SOG_R differences. Analysis of the Accuracy of Determining the COG and SOG Using a Modified ICP Algorithm Table 1 presents statistical parameters (i.e., the mean value, standard deviation and the maximum value) which characterize the computational accuracy of each parameter. The following acronyms were used to describe ICP's variants: K_ICP for algorithm without modification, H_ICP for robust version based on Huber's function, HM_ICP for robust version based on Huber's function with mean error weighting and HD_ICP for robust version based on Huber's function with directional error weighting. The lower index ICP was used for values computed by ICP algorithm's variants and lower index R was introduced to point the values surveyed with GNSS RTK receiver. A comparison of the parameter values (obtained based on 865 collected data packets) listed in Table 1 shows that the HD_ICP algorithm was the best in terms of the accuracy of determining the USVs movement parameters; it is followed by HM_ICP, H_ICP and K_ICP. Figure 12 shows a graph of SOG_ICP-SOG_R differences. Figure 12 shows that the accuracy of determining the SOG using the K_ICP method significantly decreases for scans in the intervals of 220, 365 and 720, 825 . These scans differ significantly from each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber's function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to −0.007 m s (−0.014 kts) and of the standard deviation, equal to 0.026 m s (0.05 kts) presented in Table 1.

Figure 12.
A graph of SOG ICP − SOG R differences. Figure 12 shows that the accuracy of determining the SOG using the K_ICP method significantly decreases for scans in the intervals of 〈220, 365〉 and 〈720, 825〉. These scans differ significantly from each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber's function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to  Table   1.
The histogram representing the frequency of occurrence of SOG ICP − SOG R differences, presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the SOG, the HD_ICP method is better than the others. Most of the values of SOG ICP − SOG R differences, determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. Figure 13. Histogram representing the frequency of occurrence of SOG ICP − SOG R differences. Figure 14 shows that the accuracy of determination of the ΔCOG using the K_ICP method significantly decreases, as in the case of the SOG, for scans in the intervals of 〈220, 365〉 and 〈720, 825〉. The HD_ICP method also proved to be the best in determining the ΔCOG in this case. This was also confirmed by the lowest values of the mean error, equal to 0.03°, and of the standard deviation, equal to 0.11°, presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of ΔCOG ICP − ΔCOG R increment differences. The histogram representing the frequency of occurrence of SOG ICP − SOG R differences, presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the SOG, the HD_ICP method is better than the others. Most of the values of SOG ICP − SOG R differences, determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. Figure 12. A graph of SOG ICP − SOG R differences. Figure 12 shows that the accuracy of determining the SOG using the K_ICP method significantly decreases for scans in the intervals of 〈220, 365〉 and 〈720, 825〉. These scans differ significantly from each other in rotation angles and include a greater number of erroneous measurement results than the others. This is undoubtedly due to the turns made by the USV at that time. The accuracy of determining the SOG using the other methods is, at the same time, considerably higher. These methods probably achieve that through the attenuation of measurements with gross errors using Huber's function. The HD_ICP method proved to be the best in determining the SOG in a significant part of the graph course. This was also confirmed by the lowest values of the mean error, equal to  Table   1.
The histogram representing the frequency of occurrence of SOG ICP − SOG R differences, presented in Figure 13, shows even more clearly that in terms of the accuracy of determining the SOG, the HD_ICP method is better than the others. Most of the values of SOG ICP − SOG R differences, determined using the HD_ICP, HM_ICP, and H_ICP methods, fall within the interval of −0.05~0.05. Figure 13. Histogram representing the frequency of occurrence of SOG ICP − SOG R differences. Figure 14 shows that the accuracy of determination of the ΔCOG using the K_ICP method significantly decreases, as in the case of the SOG, for scans in the intervals of 〈220, 365〉 and 〈720, 825〉. The HD_ICP method also proved to be the best in determining the ΔCOG in this case. This was also confirmed by the lowest values of the mean error, equal to 0.03°, and of the standard deviation, equal to 0.11°, presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of ΔCOG ICP − ΔCOG R increment differences.  Figure 14 shows that the accuracy of determination of the ∆COG using the K_ICP method significantly decreases, as in the case of the SOG, for scans in the intervals of 220, 365 and 720, 825 . The HD_ICP method also proved to be the best in determining the ∆COG in this case. This was also confirmed by the lowest values of the mean error, equal to 0.03 • , and of the standard deviation, equal to 0.11 • , presented in Table 1. Figure 15 shows a histogram representing the frequency of occurrence of ∆COG ICP − ∆COG R increment differences.
The histogram representing the frequency of the occurrence of differences ∆COG ICP − ∆COG R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ∆COG. Its advantage over the others can be clearly seen in the interval of differences ∆COG ICP − ∆COG R −0.05, 0.05 . Given that the ∆ROT results are identical with the ∆COG results, their presentation and analysis have been omitted. Differences in the algorithms' accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between HD_ICP and HM_ICP versions are barely visible, so they were omitted.  The histogram representing the frequency of the occurrence of differences ΔCOG ICP − ΔCOG R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval of differences ΔCOG ICP − ΔCOG R 〈−0.05˚, 0.05˚〉. Given that the ΔROT results are identical with the ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms' accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between HD_ICP and HM_ICP versions are barely visible, so they were omitted.  The histogram representing the frequency of the occurrence of differences ΔCOG ICP − ΔCOG R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval of differences ΔCOG ICP − ΔCOG R 〈−0.05˚, 0.05˚〉. Given that the ΔROT results are identical with the ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms' accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between HD_ICP and HM_ICP versions are barely visible, so they were omitted.   The histogram representing the frequency of the occurrence of differences ΔCOG ICP − ΔCOG R shown in Figure 15 enables the conclusion that the HD_ICP method is also the best in terms of the accuracy of determining the ΔCOG. Its advantage over the others can be clearly seen in the interval of differences ΔCOG ICP − ΔCOG R 〈−0.05˚, 0.05˚〉. Given that the ΔROT results are identical with the ΔCOG results, their presentation and analysis have been omitted. Differences in the algorithms' accuracy are shown at Figure 16 (presenting results of H_ICP and HD_ICP). Differences between HD_ICP and HM_ICP versions are barely visible, so they were omitted.

Conclusions
As the study attempted to prove, an ICP algorithm that matches point clouds from a laser scanner can effectively generate USV movement parameters. The proposed weighting factor based on the mean error in the specified direction of the determination of the coordinates of the positions of scan points enables more realistic (corresponding to the actual movement) matching of subsequent maps of the surrounding environment. This helps to determine the USV movement parameters with a higher accuracy. This was confirmed by the study results-inter alia the values of statistical indices in the form of mean SOG ICP − SOG R = 0.007 m s (−0.014 kts) and standard deviation SOG ICP − SOG R = 0.0025 m s (0.05 kts) and ∆COG ICP − ∆COG R = 0.03 • and standard deviation ∆COG ICP − ∆COG R = 0.11 • . However, it should be noted that the authors' original weighting factor should be used in combination with a robust criterion function to reduce measurements with gross errors (e.g., based on Huber's function), as only such a solution can be fully applicable. The indicated mean error values lead us to a generalized statement that the developed method allows measuring SOG and COG with the accuracy required by the IMO. It should be noted, however, that the test was carried out in confined waters (harbors, channels, berths, marinas), beyond which the method would not be effective due to the low LIDAR range. All data used in publication is available at [51].