# 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 horizontal coordinates using LIDAR is carried out based on the angle \(\alpha\) and the distance \(r\) 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 \(a\) and \(b\)) of the determination of 2D horizontal coordinates \((\mathrm{x},\mathrm{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:  
\[\alpha=\mathrm{arc}\,\mathrm{tg}\frac{x}{y}, \tag{1}\]  
Figure 1: The principle of measuring 2D horizontal coordinates \((\mathrm{x},\mathrm{y})\) using Light Detection and Ranging (LIDAR). \[r=\sqrt{x^{2}+y^{2}}, \tag{2}\]  
equations of the mean errors of positional lines can be written as:  
\[\sigma_{I(\alpha)}=\sigma_{\alpha}/\sqrt{\left(\frac{\partial\alpha}{\partial x} \right)^{2}+\left(\frac{\partial\alpha}{\partial y}\right)^{2}}, \tag{3}\]  
\[\sigma_{I(r)}=\sigma_{r}/\sqrt{\left(\frac{\partial r}{\partial x}\right)^{2}+ \left(\frac{\partial r}{\partial y}\right)^{2}}, \tag{4}\]  
which enable the determination of mean error of position:  
\[M=\frac{1}{\sin\theta}\sqrt{\sigma_{I(\alpha)}^{2}+\sigma_{I(r)}{}^{2}}=\sqrt {\left(\sigma_{\alpha}\cdot r\right)^{2}+\sigma_{r}{}^{2}}, \tag{5}\]  
\[a=\sigma_{\alpha}\cdot r, \tag{6}\]  
\[b=\sigma_{r}, \tag{7}\]  
where:  
\(\theta\)--the angle of intersection of positional lines, which for LIDAR measurements is always equal to 90\({}^{\circ}\), \(\sigma_{\alpha}\)--mean error of the angle measurement \(\alpha\),  
\(\sigma_{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 \(\sigma_{r}=0.004258\cdot 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 \(\alpha\) measurement, one can simply generate a covariance matrix \(\mathbf{cov}\) of the \(\left(\mathrm{x},\mathrm{y}\right)\) vector coordinates:  
\[\mathbf{cov}(x,\mathrm{y})=\left[ \begin{array}{cc}a^{2}\cdot\cos^{2}\alpha _{a}+b^{2}\cdot\sin^{2}\alpha_{a}&\left(a^{2}-b^{2}\right)\cdot\sin\alpha_{a} \cdot\cos\alpha_{a}\\ \left(a^{2}-b^{2}\right)\cdot\sin\alpha_{a}\cdot\cos\alpha_{a}&a^{2}\cdot\sin ^{2}\alpha_{a}+b^{2}\cdot\cos^{2}\alpha_{a}\end{array} \right], \tag{8}\]  
where \(\alpha_{a}\)--is the angle between the semi-axis a of the mean error ellipse and the axis OY (for LIDAR measurements, equal to \(\alpha\ +90^{\circ}\)).