Using Interchangeably the Extended Kalman Filter and Geodetic Robust Adjustment Methods to Increase the Accuracy of Surface Vehicle Positioning in the Coastal Zone

Abstract

This paper presents a study to evaluate the comparative positioning accuracy of Surface Vehicle (SV) using Dead Reckoning (DR), Geodetic Least-Squares Adjustment (GLSA), Geodetic Robust Adjustment (GRA), and External Kalman Filter (EKF) methods. This involved simulating the results of navigational measurements subject to errors (including gross errors) used to position the SV swimming along a given trajectory in the vicinity of three beacons. We showed an apparent increase in the SV positioning accuracy, from approximately 9 m of Root Mean Square (RMS) obtained by DR and GLSA methods, to approximately 2 m (RMS), achieved using GRA and EKF methods. We also showed that, by interchanging GRA and EKF methods, it is still possible to increase the positioning accuracy of the SV up to 1.14 m (RMS). However, such an interchange should occur after the experimentally determined limit of the mean error of the position coordinates estimated by the GRA method has been exceeded.

1. Introduction

Automation of the process of determining Surface Vehicle (SV) position coordinates in the coastal zone using radar, Light Detection And Ranging (LiDAR) or Charge-Coupled Device (CCD), and Complementary Metal-Oxide-Semiconductor (CMOS) cameras is currently the subject of much research [1,2,3,4,5,6,7,8,9,10]. Much of it is directed towards developing algorithms for coordinate position estimation using the Extended Kalman Filter (EKF) in its classical form or its modified versions: Unscented Kalman Filter (UKF), Particle Filter (PF), Central Difference Kalman Filter (CDKF) [11,12,13,14,15,16,17,18,19], adaptive fault-tolerant extended Kalman filter (AFTKEF) [20], and resilient extended Kalman filter [21]. The last two of the mentioned algorithms work well in the case of partial loss of data from sensors—“input”. The idea of the AFTEKF algorithm is to choose a suitable so-called fading factor based on the orthogonal theory and embed it into the prior state covariance matrix. The resilient extended Kalman filter is proposed for discrete-time nonlinear stochastic systems with sensor failures and random observer gain perturbations. Resilient extended Kalman filter is intended for discrete-time nonlinear stochastic systems with sensor failures and random observer gain perturbations. During his work, he assumes that the failure mechanisms of multiple sensors are independent of each other, with different failure rates. The locally unbiased robust minimum mean square filter is designed for state estimation under these conditions.

In the case of SV positioning, the EKF input provides both parameters describing the SV’s movement vector (e.g., course and speed)—determined, for example, by Doppler Velocity Log (DVL) and Fibre-Optic Gyroscope (FOG)—as well as the results from measurements (distances and bearings) taken relative to beacons with known positions (obtained, for example, from an electronic navigation chart)—determined, for instance, by radar or LiDAR, or by the camera.

The EKF works perfectly when there is a misidentification of beacons, leading to the assignment of converted measurement results [3]. The algorithm treats such measurement results as having a gross error and, as a result, suppresses them. In particular, this problem may arise when the beacons are located very close to each other or when the movement of the SV from which the measurements are taken is disturbed (i.e., the SV tilts, causing uncontrolled changes in course and speed).

However, several position coordinate estimation methods have also been developed in the field of surveying that can detect, suppress, and eliminate gross error measurements [22,23,24,25,26]. The best known and considered among the best are methods based on the so-called equivalent weight matrix. Of these, Hampel’s Geodetic Robust Adjustment (GRA) and Danish methods are the most widely used [25,26].

However, there is an essential difference between EKF and GRA, regarding how gross error measurements are suppressed. EKF uses a gain matrix for this purpose, taking into account the historical covariance matrix. GRA, on the other hand, uses a covariance matrix built continuously, and considers the equivalent weighting matrix.

For these reasons, the accuracy of position coordinate estimation by these methods can be expected to depend to varying degrees on the number and type of measurements, the way the beacons are positioned relative to each other, and the position relative to the SV beacons.

A study is therefore warranted to allow a comparative assessment of the two methods. Since EKF is used for positioning in dynamics and GRA for positioning in statics, this type of research is not likely to be found in the literature. For these reasons, it is expected that their results may be unique, but they can indeed find their practical application in specific measurement conditions.

This article presents research into the potential for improving surface vehicle positioning accuracy through the interchangeable use of GRA and EKF methods. It is divided into four sections. The second section formulates a scientific problem related to the positioning accuracy of SV with the selected methods based on measurements made with onboard navigation equipment and a measurement system based on an omnidirectional camera. The third section describes a simulation study involving the generation of navigational measurement results with errors (including gross errors). The method is used to position an SV sailing along a given trajectory in the vicinity of three beacons. The fourth section presents an extensive analysis of the simulation results, indicating that the GRA and EKF methods as the best for positioning the SV. It provides a proposal for the index to select the most accurate one under specific measurement conditions. The final section provides generalised conclusions from the study and guidance on the interchangeable use of EKF and GRA methods.

2. Problem Formulation

Let SV be in motion (swims along a given trajectory) in the time interval $\tau$. During this time, at any moment $k\left(k\in \tau \right)$, the onboard navigation equipment determines course over ground $COG\left(k\right)$ and speed over ground $SOG\left(k\right)$. The measuring system is based on an omnidirectional camera, which determines relative bearings $R{B}_1\left(k\right),R{B}_2\left(k\right),$$\dots ,R{B}_n\left(k\right)$ and distances $D_1\left(k\right),D_2\left(k\right),\dots ,D_n\left(k\right)$ to n beacons, located in the navigation area.

At each subsequent moment $\left(k+1\right)$, based on the results of the measurements $COG\left(k\right)$ and $SOG\left(k\right)$ taken at the previous moment instant $\left(k\right)$, let the so-called state vector be estimated: $\widehat{\mathbf{x}}(k+1)=[\begin{array}{cccc}\mathrm{x}\left(k+1\right)& y\left(k+1\right)& COG(k+1)& SOG(k+1)\end{array}{]}^{\mathrm{T}}$. This is done using a function for a non-linear SV motion model (DR method) (1).

$$\begin{array}{c}\mathbf{f}(\widehat{\mathbf{x}}\left(k\right),\mathbf{u}\left(k\right),\mathbf{w}\left(k\right))=\left[\begin{array}{c}{f}_x\\ f_y\\ f_{COG}\\ f_{SOG}\end{array}\right]\\ =\left[\begin{array}{c}\mathrm{x}\left(k\right)\\ \mathrm{y}\left(k\right)\\ COG\left(k\right)\\ SOG\left(k\right)\end{array}\right]+\left[\begin{array}{c}\Delta t\left(k+1\right)·SOG\left(k\right)·\cos COG\left(k\right)\\ \Delta t\left(k+1\right)·SOG\left(k\right)·\sin COG\left(k\right)\\ COG\left(k\right)+\Delta COG\left(k\right)\\ SOG\left(k\right)+\Delta SOG\left(k\right)\end{array}\right]+\left[\begin{array}{c}{w}_x\left(k\right)\\ w_y\left(k\right)\\ w_{COG}\left(k\right)\\ w_{SOG}\left(k\right)\end{array}\right],\end{array}$$

(1)

where:

• $x\left(k\right),y\left(k\right)$—coordinates of the SV position calculated at the moment k,
• $\Delta t(k+1)$—time period from k to $(k+1)$ time moments,
• $\Delta COG\left(k\right)$, $\Delta SOG\left(k\right)$—are known control inputs at k time moment,
• $w_x\left(k\right),w_y\left(k\right),w_{COG}\left(k\right),w_{SOG}\left(k\right)$—so-called random disturbances in the determination of coordinates, course over ground and speed over ground in the form of errors with zero expectation value and normal distribution $\mathrm{N}\left[0,1\right]$ at k time moment.

In addition, based on the estimated state vector $\widehat{\mathbf{x}}\left(k+1\right)$ and the known coordinates of $x_{l_i},y_{l_i}$ of the position of the i-th beacon $l_i$, let the observation vector be estimated $\widehat{\mathbf{z}}\left(k+1\right)$ (distance and relative bearing to the beacon $l_i$) using the function (2):

$$\begin{array}{c}\mathbf{h}(\widehat{\mathbf{x}}(k+1),\mathit{\nu }(k+1))=\left[\begin{array}{c}{f}_{D_i}\\ f_{{RB}_i}\end{array}\right]\\ =\left[\begin{array}{c}\sqrt{{\left(x_{l_i}-x\left(k+1\right)\right)}^2+{\left(y_{l_i}-y\left(k+1\right)\right)}^2}\\ {COG}^{\prime }(k+1)-\mathrm{atan}\frac{x_{l_i}-x\left(k+1\right)}{y_{l_i}-y\left(k+1\right)}\end{array}\right]+\left[\begin{array}{c}{\nu }_{D_i}(k+1)\\ {\nu }_{{RB}_i}(k+1)\end{array}\right],\end{array}$$

(2)

where:

• i-th navigational mark against which measurements have been taken $R{B}_i(k+1)$ and $D_i(k+1)$ is measured using the system based on the omnidirectional camera at the moment $(k+1)$;
• $CO{G}^{\prime }(k+1)=\left\{\begin{array}{c}COG(k+1)-360{}^{\circ },COG(k+1)\ge 180{}^{\circ }\\ COG(k+1),COG(k+1)<180{}^{\circ }\end{array}\right.$—$COG$ of the SV expressed in semicircular measure, as determined by onboard navigational equipment at the moment $(k+1)$;
• ${\nu }_{D_i}(k+1),{\nu }_{{RB}_i}(k+1)$—so-called random distortions in the determination of the distance and relative bearing to the i-th navigational mark, expressed in terms of errors with zero expectation and normal distribution $\mathrm{N}\left[0,1\right]$ at the moment $(k+1)$.

The functions, constructed as $\mathbf{f}\left(\widehat{\mathbf{x}}\left(k\right),\mathbf{u}\left(k\right),0\right)$ and $\mathbf{h}\left(\widehat{\mathbf{x}}\left(k+1\right),0\right)$, can be used to estimate SV position coordinates according to the EKF algorithm from both measurements $COG\left(k\right)$ and $SOG\left(k\right)$, as well as $R{B}_1\left(k\right),R{B}_2\left(k\right),$$\dots ,R{B}_n\left(k\right)$ and $D_1\left(k\right),D_2\left(k\right),\dots ,D_n\left(k\right)$ (Figure 1).

Figure 1. Algorithm of the estimation of the vector state $\widehat{\mathbf{x}}\left(\mathrm{k}+1\right)$ using Extended Kalman Filter.

Figure 1. Algorithm of the estimation of the vector state $\widehat{\mathbf{x}}\left(\mathrm{k}+1\right)$ using Extended Kalman Filter.

where:

• $\widehat{\mathbf{x}}(k+1)$ and $\mathbf{P}(k+1)$—the estimated state vector and its covariance matrix, fixed a posteriori at time moment $(k+1)$,
• $\widehat{\mathbf{x}}{(k+1)}^{-}$ and $\mathbf{P}{(k+1)}^{-}$—the estimated state vector and its covariance matrix, fixed a priori at time moment $(k+1)$,
• $\mathbf{F}\left(k\right)=\left[\begin{array}{cccc}\frac{\partial f_x}{\partial x}& \frac{\partial f_x}{\partial y}& \frac{\partial f_x}{\partial COG}& \frac{\partial f_x}{\partial SOG}\\ \frac{\partial f_y}{\partial x}& \frac{\partial f_y}{\partial y}& \frac{\partial f_y}{\partial COG}& \frac{\partial f_y}{\partial SOG}\\ \frac{\partial f_{COG}}{\partial x}& \frac{\partial f_{COG}}{\partial y}& \frac{\partial f_{COG}}{\partial COG}& \frac{\partial f_{COG}}{\partial SOG}\\ \frac{\partial f_{SOG}}{\partial x}& \frac{\partial f_{SOG}}{\partial y}& \frac{\partial f_{SOG}}{\partial COG}& \frac{\partial f_{SOG}}{\partial SOG}\end{array}\right]=$
• $\left[\begin{array}{cccc}1& 0& \Delta t\left(k+1\right)·SOG\left(k\right)·\cos COG\left(k\right)& \Delta t\left(k+1\right)·\sin COG\left(k\right)\\ 0& 1& -\Delta t\left(k+1\right)·SOG\left(k\right)·\sin COG\left(k\right)& \Delta t\left(k+1\right)·\cos COG\left(k\right)\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$—the so-called system matrix calculated as a Jacobi matrix of functions $\mathbf{f}\left(\mathbf{x}\left(k\right),\mathbf{u}\left(k\right),0\right)$,
• $\mathbf{Q}\left(k\right)=\left[\begin{array}{cccc}{\sigma }_x^2& {\sigma }_{xy}& 0& 0\\ {\sigma }_{yx}& {\sigma }_y^2& 0& 0\\ 0& 0& {\sigma }_{COG}^2& 0\\ 0& 0& 0& {\sigma }_{SOG}^2\end{array}\right]$—the noise matrix of state vector at the time of moment k for assumed mean error values of the following measurements: course over ground ${\sigma }_{COG}$ and speed over ground ${\sigma }_{SOG}$, where:
• ${\sigma }_x^2=\Delta t^2(k+1)·{(SOG\left(k\right)·{\sigma }_{COG}·\cos COG\left(k\right))}^2+\Delta t^2(k+1)·{({\sigma }_{SOG}·\sin COG\left(k\right))}^2$,
• ${\sigma }_y^2=\Delta t^2(k+1)·{(SOG\left(k\right)·{\sigma }_{COG}·\sin COG(k+1))}^2+\Delta t^2(k+1)·{({\sigma }_{SOG}·\cos COG\left(k\right))}^2$,
• ${\sigma }_{xy}={\sigma }_{yx}=\Delta t^2(k+1)·\sin (2·COG\left(k\right))·({\sigma }_{SOG}^2-{(SOG\left(k\right)·{\sigma }_{COG})}^2)/2$,
• $\mathbf{P}\left(k\right)$—of the state vector updated in the next time moment k,
• ${\mathbf{J}}_{\mathrm{h}}\left(k+1\right)=\left[\begin{array}{cccc}\frac{\partial f_{D_i}}{\partial x}& \frac{\partial f_{D_i}}{\partial \mathrm{y}}& \frac{\partial f_{D_i}}{\partial \mathrm{COG}}& \frac{\partial f_{D_i}}{\partial \mathrm{SOG}}\\ \frac{\partial f_{{RB}_i}}{\partial \mathrm{x}}& \frac{\partial f_{{RB}_i}}{\partial \mathrm{y}}& \frac{\partial f_{{RB}_i}}{\partial \mathrm{COG}}& \frac{\partial f_{{RB}_i}}{\partial \mathrm{SOG}}\end{array}\right]=$
• $\left[\begin{array}{cccc}\frac{x_{l_i}-x\left(k+1\right)}{{\mathrm{D}}_{l_i}\left(k+1\right)}& \frac{y_{l_i}-y\left(k+1\right)}{{\mathrm{D}}_{l_i}\left(k+1\right)}& 0& 0\\ \frac{{y\left(k+1\right)-y}_{l_i}}{{\mathrm{D}}_{l_i}\left(k+1\right){\mathrm{D}}_{l_i}\left(k+1\right)}& \frac{{x\left(k+1\right)-x}_{l_i}}{{\mathrm{D}}_{l_i}\left(k+1\right){\mathrm{D}}_{l_i}\left(k+1\right)}& 1& 0\end{array}\right]$—the Jacobi matrix of the function $\mathbf{h}\left(\widehat{\mathbf{x}}(k+1),0\right)$, generated for each pair of measurements taken up to the i-th navigation mark, with ${\mathrm{D}}_{l_i}\left(k+1\right)=\sqrt{{\left(x_{l_i}-x\left(k+1\right)\right)}^2+{\left(y_{l_i}-y\left(k+1\right)\right)}^2}$,
• $\widehat{\mathbf{z}}\left(k+1\right)=\mathbf{h}\left(\widehat{\mathrm{x}}\left(\mathrm{k}+1\right),0\right)$,
• $\mathbf{z}(k+1)=\left[\begin{array}{c}{D}_i(k+1)\\ {RB}_i(k+1)\end{array}\right]$,
• $\mathbf{R}\left(k+1\right)=\left[\begin{array}{cc}{{\sigma }_D}^2& 0\\ 0& {{\sigma }_{RB}}^2\end{array}\right]$—the noise matrix of a vector $z(k+1)$ of instantaneous observations $\left(k+1\right)$ with the assumed values of the mean errors of the distance and relative bearing measurements ${\sigma }_D$ and bearing ${\sigma }_{RB}$ to the beacons.

The SV position coordinates can also be estimated without measurement $SOG\left(k\right)$, i.e., based on measurement results $COG\left(k\right)$, $R{B}_1\left(k\right),R{B}_2\left(k\right),\dots ,R{B}_n\left(k\right)$ and $D_1\left(k\right),D_2\left(k\right),\dots ,D_n\left(k\right)$. For this purpose, the classical geodetic adjustment method using the least squares method (GLSA) or the robust adjustment method (GRA), based on the so-called equivalent weighting matrix of measurements with gross error, can be used. The functional and stochastic models and the optimisation criteria for the two indicated geodetic adjustment methods could be formulated as follows [25,26,27]:

$$\left.\begin{array}{c}{\mathbf{V}}_{\mathrm{x}}(k+1)=\mathbf{A}(k+1){\widehat{\mathbf{d}}}_{\mathrm{x}}(k+1)+\mathbf{L}(k+1)-\mathrm{functional}\mathrm{model}\\ {\mathbf{C}}_{\mathrm{x}}\left(k+1\right)={\delta }_0^2{(k+1){\mathbf{P}}_{\mathrm{x}}\left(k+1\right)}^{-1}-\mathrm{stochastic}\mathrm{model}\\ {{\mathbf{V}}_{\mathrm{x}}}^{\mathrm{T}}(k+1){\mathbf{P}}_{\mathrm{x}}(k+1){\mathbf{V}}_{\mathrm{x}}(k+1)=min-\mathrm{classic}\mathrm{adjustment}\mathrm{crit}.\\ {{\mathbf{V}}_{\mathrm{x}}}^{\mathrm{T}}(k+1){\widehat{\mathbf{P}}}_{\mathrm{x}}(k+1){\mathbf{V}}_{\mathrm{x}}(k+1)=min-\mathrm{robust}\mathrm{adjustment}\mathrm{crit}.\end{array}\right\}$$

(3)

Given the assumptions (Equation (3)), after appropriate mathematical transformations, Relations 4 and 5 can be created for the estimation of the correction vector ${\widehat{\mathbf{d}}}_{\mathrm{x}}(k+1)$ of the predicted coordinates of the SV position (state vector $\widehat{\mathbf{x}}{(k+1)}^{-}$, established a priori):

$$\begin{array}{c}{\widehat{\mathbf{d}}}_x\left(k+1\right)=-{\left({\mathbf{A}}^{\mathrm{T}}\left(\mathrm{k}+1\right){\mathbf{P}}_{\mathrm{x}}\left(\mathrm{k}+1\right)\mathbf{A}\left(\mathrm{k}+1\right)\right)}^{-1}\\ {\mathbf{A}}^{\mathrm{T}}\left(\mathrm{k}+1\right){\mathbf{P}}_{\mathrm{x}}\left(\mathrm{k}+1\right)\mathbf{L}\left(\mathrm{k}+1\right)-\mathrm{for}\mathrm{the}\mathrm{GLSA}\mathrm{method},\end{array}$$

(4)

$$\begin{array}{c}{\widehat{\mathbf{d}}}_x\left(k+1\right)=-{\left({\mathbf{A}}^{\mathrm{T}}\left(\mathrm{k}+1\right){\widehat{\mathbf{P}}}_{\mathrm{x}}\left(\mathrm{k}+1\right)\mathbf{A}\left(\mathrm{k}+1\right)\right)}^{-1}\\ {\mathbf{A}}^{\mathrm{T}}\left(\mathrm{k}+1\right){\widehat{\mathbf{P}}}_{\mathrm{x}}\left(\mathrm{k}+1\right)\mathbf{L}\left(\mathrm{k}+1\right)-\mathrm{for}\mathrm{the}\mathrm{GRA}\mathrm{method},\end{array}$$

(5)

where:

• $\mathbf{A}\left(k+1\right)=\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{cc}\frac{\partial f_{D_1}}{\partial x}& \frac{\partial f_{D_1}}{\partial \mathrm{y}}\\ \frac{\partial f_{{RG}_1}}{\partial \mathrm{x}}& \frac{\partial f_{{RG}_1}}{\partial \mathrm{y}}\end{array}\\ \begin{array}{cc}\frac{\partial f_{D_2}}{\partial x}& \frac{\partial f_{D_2}}{\partial \mathrm{y}}\\ \frac{\partial f_{{RG}_2}}{\partial \mathrm{x}}& \frac{\partial f_{{RG}_2}}{\partial \mathrm{y}}\end{array}\\ ⋮\\ \begin{array}{cc}\frac{\partial f_{D_n}}{\partial x}& \frac{\partial f_{D_n}}{\partial \mathrm{y}}\\ \frac{\partial f_{{RG}_n}}{\partial \mathrm{x}}& \frac{\partial f_{{RG}_n}}{\partial \mathrm{y}}\end{array}\end{array}\end{array}\end{array}\right]$—design matrix,
• $\mathbf{L}\left(k+1\right)=\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}\sqrt{{\left(x_{l_1}-x\left(k+1\right)\right)}^2+{\left(y_{l_1}-y\left(k+1\right)\right)}^2}-D_1(k+1)\\ CO{G}^{{}^{\prime }}(k+1)-\mathrm{atan}\frac{x_{l_1}-x(k+1)}{y_{l_1}-y(k+1)}-R{B}_1(k+1)\end{array}\\ \begin{array}{c}\sqrt{{\left(x_{l_2}-x\left(k+1\right)\right)}^2+{\left(y_{l_2}-y\left(k+1\right)\right)}^2}-D_2(k+1)\\ CO{G}^{{}^{\prime }}(k+1)-\mathrm{atan}\frac{x_{l_2}-x(k+1)}{y_{l_2}-x(k+1)}-R{B}_2(k+1)\end{array}\\ ⋮\end{array}\\ \sqrt{{\left(x_{l_n}-x\left(k+1\right)\right)}^2+{\left(y_{l_n}-y\left(k+1\right)\right)}^2}-D_n(k+1)\\ CO{G}^{{}^{\prime }}(k+1)-\mathrm{atan}\frac{x_{l_n}-x(k+1)}{y_{l_n}-y(k+1)}-R{B}_n(k+1)\end{array}\right]$—residual vector,
• ${\delta }_0^2\left(k+1\right)=\frac{{{\mathbf{V}}_{\mathrm{x}}(k+1)}^{\mathrm{T}}{\mathbf{P}}_{\mathrm{x}}(k+1){\mathbf{V}}_{\mathrm{x}}(k+1)}{2n-2}$—variance coefficient,
• ${\widehat{\mathbf{P}}}_{\mathrm{x}}\left(k+1\right)=\mathbf{T}\left({\overline{\mathbf{V}}}_{\mathrm{x}}\left(k+1\right)\right){\mathbf{P}}_{\mathrm{x}}\left(k+1\right)$—equivalent weighting matrix,
• $\mathbf{T}\left({\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right)=\left[\begin{array}{cccc}t\left({\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{1,1}\right)& 0& 0& 0\\ 0& t\left({\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{2,1}\right)& 0& 0\\ 0& 0& \ddots & 0\\ 0& 0& 0& t\left({\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{2n,1}\right)\end{array}\right]$—diagonal damping matrix,
• $t\left({\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{i,1}\right)=\left\{\begin{array}{c}1,|{\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{i,1}\in 〈-m;m〉\\ \exp \left\{-l{\left(\left|\overline{{\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{i,1}}\right|-m\right)}^g\right\},|\left|{\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{i,1}\right|>m\end{array}\right.$—Danish damping function, where:
• ${\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{i,1}=\frac{{\left[{\widehat{\mathrm{V}}}_{\mathrm{x}}\left(\mathrm{k}+1\right)\right]}_{i,1}}{\sqrt{{\left[{\widehat{\mathrm{C}}}_{{\widehat{\mathrm{V}}}_{\mathrm{x}}\left(\mathrm{ffi}\left(k+1\right)=1\right)}\left(\mathrm{k}+1\right)\right]}_{i,i}}}fori=1,2,\cdots ,2n$ and $m,l,g$—parameters controlling the gross error measurement weights,
• ${\widehat{\mathbf{C}}}_{{\widehat{\mathrm{V}}}_{\mathrm{X}}}\left(k+1\right)={\mathbf{P}}_{\mathrm{x}}^{-1}\left(k+1\right)-\mathbf{A}(k+1){\left({\mathbf{A}}^{\mathrm{T}}(k+1){\mathbf{P}}_{\mathrm{x}}(k+1)\mathbf{A}(k+1)\right)}^{-1}{\mathbf{A}}^{\mathrm{T}}(k+1)$—covariance matrix for ${\delta }_0^2\left(k+1\right)=1$,
• ${\widehat{\mathbf{V}}}_{\mathrm{x}}(k+1)=\mathbf{A}(k+1){\widehat{\mathbf{d}}}_{\mathrm{x}}(k+1)+\mathbf{L}(k+1)$,
• ${\mathbf{P}}_{\mathrm{x}}\left(k+1\right)=\left[\begin{array}{cccccccc}\frac{1}{{\delta }_{D_1}^2}& 0& 0& 0& 0& 0& 0& 0\\ 0& \frac{1}{{\delta }_{{RB}_1}^2}& 0& 0& 0& 0& 0& 0\\ 0& 0& \frac{1}{{\delta }_{D_2}^2}& 0& 0& 0& 0& 0\\ 0& 0& 0& \frac{1}{{\delta }_{{RB}_2}^2}& 0& 0& 0& 0\\ 0& 0& 0& 0& \ddots & 0& 0& 0\\ 0& 0& 0& 0& 0& \ddots & 0& 0\\ 0& 0& 0& 0& 0& 0& \frac{1}{{\delta }_{D_n}^2}& 0\\ 0& 0& 0& 0& 0& 0& 0& \frac{1}{{\delta }_{{RB}_n}^2}\end{array}\right]$—weight matrix.

Based on Formulae (4) and (5) obtained in this way, it is possible to estimate the state vector iteratively $\widehat{\mathbf{x}}(k+1)$ at each successive moment $(k+1)$, according to the algorithm shown in Figure 2 for the GLSA method, and according to the algorithm shown in Figure 3 for the GRA method. By starting the iterative computation process after the prediction has been performed, the process can be considered to be complete when the absolute values of the differences of the correction vectors $\left|{{\mathbf{V}}_{\mathbf{x}}(k+1)-\mathbf{V}}_{\mathbf{x}}{(k+1)}^{-}\right|$ will be smaller than the maximum absolute values of the vector correction increments $\Delta {\mathbf{V}}_{\mathbf{max}}$.

Assume that the calculations for values $\Delta {\mathbf{V}}_{\mathbf{max}}$ are determined arbitrarily at the time of initialisation of the algorithms, while the values ${\left[{\mathbf{V}}_{\mathbf{x}}(k+1)\right]}_{i,1}=0\mathrm{for}i=1,2,\cdots ,2n$ are determined after each successive prediction.

Figure 2. Algorithm for state vector estimation $\widehat{\mathbf{x}}\left(\mathrm{k}+1\right)$ using classical geodetic adjustment.

Figure 2. Algorithm for state vector estimation $\widehat{\mathbf{x}}\left(\mathrm{k}+1\right)$ using classical geodetic adjustment.

Let us now assume that, based on the results of the measurements $COG\left(k\right)$ and $SOG\left(k\right)$ obtained using SV’s onboard navigation equipment, e.g., the Inertial Navigation System (INS) from VectorNav ‘NS-100’ [28] and the Doppler Velocity Log (DVL) from LinkQuest ‘NavQuest 600’ [29] (ignoring the effects of current and sea drift), at any time moment $\left(k\right)$, during the time interval $\tau$, the DR method 1 is used to determine the position coordinate vector $\mathbf{x}(k+1)$.

Knowing the covariance matrix $\mathbf{P}\left(k\right)$ of the initial (previous) position coordinate vector $\mathbf{x}\left(k\right)$ and the covariance matrix $\mathbf{P}(k+1/1)$ of the coordinate vector increment $\mathbf{x}(k+1)-\mathbf{x}\left(k\right)$, it is possible to calculate the covariance matrix $\mathbf{P}(k+1)$ of the coordinate vector of the final (next) position $\mathbf{x}(k+1)$ according to the relation:

$$\mathbf{P}(k+1)=\mathbf{P}\left(k\right)+\mathbf{P}(k+1/1)=[\begin{array}{cc}{\sigma }_x^2& {\sigma }_{xy}\\ {\sigma }_{yx}& {\sigma }_y^2\end{array}].$$

(6)

In turn, the covariance matrix obtained in this way, $\mathbf{P}\left(k+1\right)$, can be used indirectly to assess the accuracy of the position vector determination, $\mathbf{x}\left(k+1\right)$, carried out with the following:

• Mean error

  $$M_{xy}=\sqrt{{{\sigma }_x}^2+{{\sigma }_y}^2},$$

  (7)
• Mean ellipse error

  $$a=\sqrt{\frac{1}{2}·\left({{\sigma }_x}^2+{{\sigma }_y}^2+\sqrt{p}\right)},$$

  (8)

  $$b=\sqrt{\frac{1}{2}·\left({{\sigma }_x}^2+{{\sigma }_y}^2-\sqrt{p}\right)},$$

  (9)

  $$\phi =\frac{1}{2}\mathrm{arc}\mathrm{tg}\frac{2·{\sigma }_{xy}}{{{\sigma }_x}^2-{{\sigma }_y}^2}.$$

  (10)

  where:
  • $p={\left({{\sigma }_x}^2-{{\sigma }_y}^2\right)}^2+4·{{\sigma }_{xy}}^2$,
  • $\phi$—directional angle of the ellipse;
  • a—length of the large half-axis of the ellipse;
  • b—length of the small half-axis of the ellipse.

Figure 3. Algorithm for state vector estimation $\widehat{\mathbf{x}}(k+1)$ using geodetic adjustment with Danish gross error suppression function.

Figure 3. Algorithm for state vector estimation $\widehat{\mathbf{x}}(k+1)$ using geodetic adjustment with Danish gross error suppression function.

Knowing the values of the measurement errors (${\sigma }_{COG}=2{}^{\circ }$ and ${\sigma }_{SOG}=1\%$) of the measured speed given by the manufacturers of INS ‘NS-100’ and DVL ‘NavQuest 600’ in the technical specifications [28,29] and assuming that the SV flows with uniform rectilinear motion $COG(k+1)=90{}^{\circ }$ and $SOG(k+1)=6\mathrm{m}/\mathrm{s}$, the positions of circles of mean error (having radius length equal to $M_{xy}$) and mean error ellipses (having semi-axis lengths equal to a and b, and a directional angle equal to $\phi$) of the positions determined by the DR method, e.g., after 200 s and after 500 s (which are illustrated in Figure 4), can be calculated.

Figure 4. Change in mean error and mean error ellipse after SV travels 1200 m and 3000 m.

Figure 4. Change in mean error and mean error ellipse after SV travels 1200 m and 3000 m.

Figure 4 shows that the mean error ellipse extends in the direction perpendicular to the $COG(k+1)$. Its half-axis length b is almost twice as short as the half-axis a. Thus, it is the measurement error ${\sigma }_{COG}$ which determines the mean error $M_{xy}$, whose value reaches 108.9 m after the SV has travelled a distance of only 3000 m.

Let us also assume that the SV can, at any subsequent time $(k+1)$, determine the coordinates of the occupied positions. This is also done using the GLSA method (Figure 2), solely based on course over ground measurements $COG\left(k\right)$ performed by, e.g., GNSS Satellite Compass ComNav [30], and relative bearings $R{B}_1(k+1),R{B}_2(k+1),R{B}_3(k+1)$ and distances $D_1(k+1),D_2(k+1),D_3(k+1)$ taken, e.g., with an omnidirectional camera relative to the three navigation marks. The mean error values of the positions determined in this way can then be calculated from the trace of the covariance matrix according to the formula:

$$M_{xy}=\sqrt{Tr\left({\mathbf{C}}_{\mathbf{X}}(k+1)\right)},$$

(11)

where:

$${\mathbf{C}}_{\mathbf{X}}(k+1)={\left({\mathbf{A}}^{\mathrm{T}}(k+1){\mathbf{P}}_{\mathbf{x}}(k+1)\mathbf{A}(k+1)\right)}^{-1},$$

(12)

assuming that ${\delta }_0^2(k+1)=1$.

Figure 5 shows the areas of the accuracy of GLSA-determined positions relative to three beacons set in two characteristic variants.

Moreover, Figure 5 shows that the two variants of beacon alignment (a) and (b) allow the GLSA method to determine position with a mean error of less than the 30 m level in many of the drawn areas of accuracy. This means that the GLSA method can outperform the DR method in terms of the accuracy of the SV position determination. This situation may occur in the case of continuous position determination by the DR method after exceeding the contractual time limit, depending on the values of ${\sigma }_{COG}$ and ${\sigma }_{SOG}$. Taking into account the results of the calculation of the positioning accuracy with the DR method presented in Figure 4, it can be stated, with certainty, that the contractual limit was already exceeded after 120 s. At this point, the mean error $M_{xy}$ reached a value of 43.6 m.

Figure 5. Position accuracy areas are determined by the GLSA method based on relative bearings and distances taken relative to three navigation marks aligned: (a) in a line, (b) in a triangle. (${\sigma }_D=20m$—arbitrarily assumed, ${\sigma }_{RG}=0.25{}^{\circ }$—assumed based on [30]; the distance between beacons is approximately 2600 m).

Figure 5. Position accuracy areas are determined by the GLSA method based on relative bearings and distances taken relative to three navigation marks aligned: (a) in a line, (b) in a triangle. (${\sigma }_D=20m$—arbitrarily assumed, ${\sigma }_{RG}=0.25{}^{\circ }$—assumed based on [30]; the distance between beacons is approximately 2600 m).

Mainly for these reasons, the augmented Kalman filter method is now often used to determine SV positions. In its operation, it can make use of both measurements of motion parameters $COG\left(k\right)$, $SOG\left(k\right)$, as well as additional measurements of relative bearings $R{B}_1(k+1),R{B}_2(k+1),R{B}_3(k+1)$.

However, it is essential to realise that the EKF is a good solution when the measurement error values are set ’perfectly’. Unfortunately, the movement of the SV is almost always disturbed by wind, sea currents, and wave action. Under their influence, the SV hull tilts uncontrollably, changing course and speed; consequently, this leads to high variability in measurement accuracy. In particular, this problem can be attributed to the data-processing specificity of camera measurements taken against land and floating beacons.

At this point, therefore, two questions of scientific interest can be asked regarding the choice of method for determining SV positions:

• Must the accuracy of EKF position determination always be greater than the accuracy of GLSA or GRA position determination?
• If the answer to the first question is no, in which situations can GLSA or GRA be indicated as a better method for determining positions. In which cases will an extended Kalman filter be better?

The article’s authors will attempt to answer the questions thusly posed in a later section. The results of simulation studies will be helpful for this purpose. These will consist of generating measurement results with errors (including gross errors), and then, estimating position coordinates on their basis using the extended Kalman filter method (according to the algorithm presented in Figure 1) and geodetic adjustment: classical and robust to gross errors (according to the algorithms presented in Figure 2 and Figure 3).

3. Research

In the simulation studies, the SV was assumed to sail along a straight section at a constant speed in the vicinity of three beacons, in the time interval of $\tau$. The onboard navigation equipment determines $COG\left(k\right)$ and $SOG\left(k\right)$. A measuring system based on an omnidirectional camera determines $R{B}_1\left(k\right),R{B}_2\left(k\right),R{B}_3\left(k\right)$ and $D_1\left(k\right),D_2\left(k\right),D_3\left(k\right)$ to the beacons with known positions, at any moment in time, $k\left(k\in \tau \right)$. All determined values are subject to a measurement error represented by a randomly varying correction $\xi$ with normal distribution $\mathrm{N}\left[0,1\right]$, generated by the computer [31]. The limits of this correction correspond to a multiple of the mean error value, characterising the accuracy of the measurement performed with a given device. In every tenth moment, the $k=\{10,20,30,\dots ,10·\lfloor k_{last}/10\rfloor \}$ value $\xi \in 〈-10;-5〉\vee 〈5;10〉$ simulates the gross error of measurement, while in other moments, the k value $\xi \in 〈-3;3〉$ simulates the mean error of measurement with $P\approx 0.99$ (with $k_{last}$ being the last moment $k\in \tau$). Calibration measurements $R{B}_1\left(k\right),R{B}_2\left(k\right),R{B}_3\left(k\right)$, and $D_1\left(k\right),D_2\left(k\right),D_3\left(k\right)$ to the beacons are calculated from the reference position SV, determined by the DR method, and based on the initial invariant values of $COG(k=0)$ and $SOG(k=0)$.

Based on the measurement results obtained in this way, at each successive moment of $(k+1)$ time interval $\tau$, the coordinates of the SV position are determined using DR, EKF, GLSA, and GRA methods in parallel. Whereby, the approximate geodetic adjustment position is the one determined at the previous time moment k (by the same geodetic method) after being shifted by a vector calculated according to Formula (1). That is, the same vector of the position determined by the EKF method in the prediction stage is shifted.

A unique software application prepared in the integrated development environment C++ Builder [32], with installed template library package for linear algebra, Eigen [33], was used to conduct the simulation studies as specified. During its operation, it performs four primary tasks in parallel:

• Simulating the next position of the reference trajectory of SV movement, using the DR method with a constant time step $\Delta t(k+1)$ based on initial, invariant (reference) values of $COG(k=0)$ and $SOG(k=0)$;
• Simulating variables amendments:

  (a)

  ${\xi }_{COG}\left(k+1\right)$—added to the model $COG(k=0)$,

  (b)

  ${\xi }_{SOG}\left(k+1\right)$—added to the model $SOG(k=0)$,

  (c)

  ${\xi }_{{RB}_1}\left(k+1\right),{\xi }_{{RB}_2}\left(k+1\right),{\xi }_{{RB}_3}\left(k+1\right)$—added to the reference relative bearings $RB{r}_1(k+1),RB{r}_2(k+1),RB{r}_3(k+1)$ calculated from the next position of the reference SV trajectory, up to three beacons, and taking into account the reference value of the $COG(k=0)$;

  (d)

  ${\xi }_{D_1}\left(k+1\right),{\xi }_{D_2}\left(k+1\right),{\xi }_{D_3}$$\left(k+1\right)$—added to the reference distances $D{r}_1(k+1),D{r}_2(k+1),D{r}_3(k+1)$, calculated from successive positions of the simulated SV reference trajectory, up to three beacons;

• DR-based estimation of SV position coordinates:

  (a)

  $COG(k+1)=COG(k=0)+{\xi }_{COG}\left(k+1\right)$,

  (b)

  $SOG(k+1)=SOG(k=0)+{\xi }_{SOG}\left(k+1\right)$;

• Estimation of SV position coordinates with EKF, GLSA, and GRA methods based on the following:

  (a)

  $COG(k+1)=COG(k=0)+{\xi }_{COG}\left(k+1\right)$,

  (b)

  $SOG(k+1)=SOG(k=0)+{\xi }_{SOG}\left(k+1\right)$,

  (c)

  $R{B}_1(k+1)=RB{r}_1(k+1)+{\xi }_{{RB}_1}\left(k+1\right),$

  (d)

  $R{B}_2(k+1)=RB{r}_2(k+1)+{\xi }_{{RB}_2}\left(k+1\right),$

  (e)

  $R{B}_3(k+1)=RB{r}_3(k+1)+{\xi }_{{RB}_3}\left(k+1\right),$

  (f)

  $D_1(k+1)=D{r}_1(k+1)+{\xi }_{D_1}\left(k+1\right),$

  (g)

  $D_2(k+1)=D{r}_2(k+1)+{\xi }_{D_2}\left(k+1\right),$

  (h)

  $D_3(k+1)=D{r}_3(k+1)+{\xi }_{D_3}\left(k+1\right);$

• Calculation of statistical parameters and generation of graphs and overview maps for analysing the test results obtained, i.e.,

  (a)

  Maximum value, arithmetic mean, and standard deviation of the distance to the reference position from the position estimated by DR, EKF, GLSA, and GRA methods;

  (b)

  Distance graphs to the reference position from the estimated position, moving average, and moving RMS, to the reference position from the estimated position, and percentage of distance to the reference point from the estimated position,

  (c)

  An overview map with the beacon alignment adopted for the study and the resulting SV positions estimated by DR, EKF, GLSA, and GRA methods relative to them.

In carrying out the tests with the developed application, it was assumed that SV flows with $COG(k=0)=90{}^{\circ }$ and $SOG(k=0)=5$ m/s. It is also assumed that measurements $COG(k+1)$, $SOG(k+1)$, $R{B}_1(k+1)$, $R{B}_2(k+1)$, $R{B}_3(k+1)$, $D_1(k+1)$, $D_2(k+1)$, and $D_3(k+1)$ are performed at the exact moment in time with a constant frequency of 1 Hz, $\Delta t\left(k+1\right)=1$ s. On the other hand, to make the measurements simulated by the application more realistic, they are performed:

• Onboard navigational equipment: it was assumed that ${\sigma }_{COG}=2{}^{\circ }$ and ${\sigma }_{SOG}=0.05$ m/s (1% of the measured speed). It was also assumed that the measurement $COG(k+1)$ was performed with the INS ‘NAV-100’ and the measurement $SOG(k+1)$ was performed with the DVL ‘NavQuest’ [28,29];
• Measurement system based on an omnidirectional camera: it was arbitrarily assumed that ${\sigma }_{RG}={\sigma }_{COG}+0.5{}^{\circ }$ [34] and ${\sigma }_D=0.5$ m [35]. It is taken into account that the measurements $R{B}_1(k+1),R{B}_2(k+1),R{B}_3(k+1)$ are relative to $COG(k+1)$.

The operation of the application thread responsible for the iterative calculations performed by the GLSA and GRA methods was considered complete when the absolute differences of the correction vectors $\left|{{\mathbf{V}}_{\mathbf{x}}(k+1)-\mathbf{V}}_{\mathbf{x}}{\left(k+1\right)}^{-}\right|$ reached values smaller than the value of the vector ${\Delta \mathbf{V}}_{\max }={\left[0.01\mathrm{m}0.001{}^{\circ }0.01\mathrm{m}0.001{}^{\circ }0.01\mathrm{m}0.001{}^{\circ }\right]}^{\mathrm{T}}$.

In the case of the GRA method, it was arbitrarily assumed that the values of the control parameters of the Danish damping function (relation no. 5): $l=0.001$, $m=2.5$, $g=1.2$. This means that the value of the standardised correction of the undamped single measurement result ${\left[{\overline{\mathbf{V}}}_{\mathrm{x}}(k+1)\right]}_{i,1}fori=1,2,\cdots ,2n$ should belong to the interval $〈-m;m〉$, with probability $P=0.988$.

The tests were conducted against beacons aligned and triangulated. They consisted of simulating a single crossing (execution of a single test trial) and 100 crossings (implementation of 100 test trials) of the SV along a straight stretch of road that was 1500 m long. Each test trial started at time $k=0$ from a position with coordinates $\left(x\right(k=0)=-2500\mathrm{m},y(k=0)=2500\mathrm{m})$ and ended when the SV navigated to the reference position with coordinates $\mathrm{x}\left(k+1\right)>750\mathrm{m}$. The beacons were aligned at positions with coordinates: $\left(-500,0\right)$, $\left(0,0\right)$, $\left(500,0\right)$, while in a triangle, by changing the coordinates of the position of the centre beacon to $\left(0,-500\right)$.

4. Results and Discussion

The result of a single test (a single SV crossing along a road section) was a dataset consisting of one series of reference positions, determined by the DR method, without taking into account measurement errors. The four series of positions estimated by the DR, EKF, GLSA, and GRA methods took measurement errors into account. In turn, 100 such datasets resulted from 100 tests (100 SV crossings along the same stretch of road). The sets of position series collected in this way were transformed into sets of distance series calculated between the reference position and the estimated position. After appropriate processing, these were used for comparative statistical analyses. The analyses were conducted to assess the accuracy of position coordinate estimation using the DR, EKF, GLSA, and GRA methods. Their results justified the necessity and provided the basis for developing a decision rule for selecting SV position coordinate estimation methods (among GRA and EKF methods) based on an accuracy criterion.

First, tables with maximum values, arithmetic mean, and standard deviation of distances (Table 1 and Table 2) were prepared based on them.

In the second instance, they were used to prepare the distance and arithmetic mean distance graphs and the percentage, moving average, and moving RMS distances (Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22 and Figure 23).

Individual arithmetic mean values were calculated from 100 distance values corresponding to the exact moment k.

Moving average ${MA}_k$ distance $D_{re}\left(k\right)$ to the reference position from the estimated position corresponding to the moment k was determined according to the formulae:

• For a single passage SV

  $${MA}_k=\frac{1}{10}\sum _{i=k-5}^{k+4}{D}_{re}\left(k=i\right),$$

  (13)
• For 100 SV crossings

  $${MA}_k=\frac{1}{1000}\sum _{j=1}^{100}\sum _{i=k-5}^{k+4}{\left\{D_{re}\left(k=i\right)\right\}}_j,$$

  (14)

  where:
  • $k=\left\{5,6,7,\cdots ,k_{last}-4\right\},$
  • j—SV crossing number.

The moving root mean square ${MRMS}_k$ distance $D_{re}\left(k\right)$ to the reference position from the estimated position, corresponding to the moment k, was determined according to the formula:

• For a single SV crossing

  $${MRMS}_k=\sqrt{\frac{{\sum }_{i=k-5}^{k+4}{\left(D_{re}\left(k=i\right){-\overline{D}}_{re}\right)}^2}{10},}$$

  (15)
• For 100 SV crossings

  $${MRMS}_k=\sqrt{\frac{{\sum }_{j=1}^{100}{\sum }_{i=k-5}^{k+4}{\left\{{\left(D_{re}\left(k=i\right){-\overline{D}}_{re}\right)}^2\right\}}_j}{1000}.}$$

  (16)

The sample size is ten in the moving average and moving RMS calculations were performed according to relations (13)–(16). That is, each sample will contain one distance from the position estimated from simulated measurements, with gross error and nine distances from positions estimated from simulated measurements with mean error. This will ensure that the statistical analysis based on trend tracking will be meaningful, despite the more frequent occurrence of simulated measurements with mean error, and the less frequent occurrence of simulated measurements with gross error.

In contrast, the use of moving RMS instead of its more commonly used moving standard deviation counterpart for statistical analysis is justified primarily by the small sample size. In statistical terms, moving RMS is the highest reliability estimator. ‘Greatest reliability’ does not refer to the results, but rather, to the random sample that will be most likely to be found with such a deviation in the population.

4.1. Tests Conducted against Aligned Beacons Aligned

As a result of the tests involving the simulation of a single crossing and 100 SV crossings, two sets of data were obtained, i.e., a set of five 300 positions and four 300 distance series and a set of five hundred 300 positions and four hundred 300 distance series. Based on these, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13 and Figure 14 and Table 1 were prepared to analyse the first group of test results.

Figure 6 shows an overview of the location of the beacons in the line and the estimated positions relative to them during single and 100 SV crossings.

Figure 6 shows a significantly increasing scatter of positions estimated by the GLSA method, before and after the SV passes the extreme beacons. It also shows a uniform and significant increase in the scatter as a function of time of the positions, estimated by the DR method. It also shows that the positions estimated by the EKF and GRA methods are the most concentrated around the reference road positions.

Figure 6. Position accuracy areas are used to determine alignment of beacons and estimated SV positions relative to them during: (a) single crossing, (b) 100 crossings.

Figure 6. Position accuracy areas are used to determine alignment of beacons and estimated SV positions relative to them during: (a) single crossing, (b) 100 crossings.

Table 1 shows the values of the statistical parameters of the distance spread to the reference position from the estimated position.

Table 1. Statistical parameters of the scatter of the distance to the reference position from the positions estimated by the GLSA, EKF, GRA, and DR methods.

Positioning Method	Maximum Distance to the Reference Position (m)		Arithmetic Mean of the Distance to the Reference Position (m)		Standard Deviation Distance to Reference Position (m)
	Single crossing	100 crossings	Single crossing	100 crossings	Single crossing	100 crossings
GLSA	195.89	247.82	5.72	5.91	17.97	19.41
EKF	16.52	15.19	2.73	2.72	3.06	2.80
GRA	18.56	15.63	2.35	2.35	2.88	2.62
DR	24.74	30.52	15.58	16.21	5.46	8.42

Table 1. Statistical parameters of the scatter of the distance to the reference position from the positions estimated by the GLSA, EKF, GRA, and DR methods.

Positioning Method	Maximum Distance to the Reference Position (m)		Arithmetic Mean of the Distance to the Reference Position (m)		Standard Deviation Distance to Reference Position (m)
	Single crossing	100 crossings	Single crossing	100 crossings	Single crossing	100 crossings
GLSA	195.89	247.82	5.72	5.91	17.97	19.41
EKF	16.52	15.19	2.73	2.72	3.06	2.80
GRA	18.56	15.63	2.35	2.35	2.88	2.62
DR	24.74	30.52	15.58	16.21	5.46	8.42

Taking into account the smallest and largest values of the arithmetic mean (as an expected value) and standard deviation (as a measure of the dispersion around the expected value) of the distance to the reference position from the estimated position presented in Table 1, a generalised conclusion can be drawn: GRA is the best and GLSA the worst (marked in blue in Table 1). The EKF method, on the other hand, is the best in terms of the smallest value of the maximum distance between the reference position and the estimated position (shown in green in Table 1).

Figure 7 and Figure 8 show plots of distance and arithmetic mean distance to the reference position from the estimated position during single and 100 SV crossings as a function of time.

Figure 7. Distance to reference position from estimated position during a single SV crossing as a function of time.

Figure 7. Distance to reference position from estimated position during a single SV crossing as a function of time.

Figure 8. Arithmetic mean distance to reference position from the estimated position during 100 SV crossings as a function of time.

Figure 8. Arithmetic mean distance to reference position from the estimated position during 100 SV crossings as a function of time.

The graphs shown in Figure 7 and Figure 8 confirm the conclusions of the analysis of Figure 6. They clearly show the largest distances at the beginning and end of the test in the case of the GLSA method and a uniform increase in the distance throughout the test in the case of the DR method. However, by focusing attention on the shape of the graphs in Figure 8, one can see a spike in the distance at the time points in which the gross errors of the measurements were simulated (every tenth moment k). This is when the largest distances to the reference positions from the positions estimated by the GLSA method occur.

Figure 9 and Figure 10 plot the moving average distance to the reference position from the estimated position during a single and 100 SV crossings as a function of time.

Figure 9. Moving average distance to reference position from the estimated position during a single SV crossing as a function of time (logarithmic scale—log base = 10).

Figure 9. Moving average distance to reference position from the estimated position during a single SV crossing as a function of time (logarithmic scale—log base = 10).

Figure 10. Moving average distance to reference position from the estimated position during 100 SV crossings as a function of time (logarithmic scale—log base = 10).

Figure 10. Moving average distance to reference position from the estimated position during 100 SV crossings as a function of time (logarithmic scale—log base = 10).

The graphs evaluated against each other in Figure 9 and Figure 10 clearly show that neither method provides an estimate of the position with the smallest moving mean distances to the reference position throughout the test. In the time interval $〈0\mathrm{s};\left.90\mathrm{s}\right)$, the DR method proved to be the best in this respect. In the time interval of $〈90\mathrm{s};900\mathrm{s}〉$, it gives way to the GRA method. In the time interval $\left.〈900\mathrm{s};1000\mathrm{s}\right)$, it gives way to the EKF method.

Figure 11 and Figure 12 show plots of moving RMS to the reference position from the estimated position during a single and 100 SV crossings as a function of time.

Figure 11. Moving RMS of the distance to the reference position from the estimated position during a single SV crossing as a function of time.

Figure 11. Moving RMS of the distance to the reference position from the estimated position during a single SV crossing as a function of time.

Figure 12. Moving RMS of distance to reference position from the estimated position during 100 SV crossings as a function of time.

Figure 12. Moving RMS of distance to reference position from the estimated position during 100 SV crossings as a function of time.

Figure 11 and Figure 12 clearly show that, throughout the test, the moving RMS value of the distance remains similar for the EKF method (equal scatter around the moving average distance) and increases monotonically uniformly for the DR method. The graphs relating to the GLSA and GRA methods are parallel to each other and, in terms of the variability of the waveform, are decreasing in the time interval $\left.〈0\mathrm{s};500\mathrm{s}\right)$ and increasing in the time interval $〈500\mathrm{s};1000\mathrm{s}〉$. It should be noted here that the GRA method obtains the smallest moving RMS distance values (smallest spread around the moving average distance) over the most extensive time interval $〈250\mathrm{s};850\mathrm{s}〉$ (for more than half of the test duration).

Figure 13 and Figure 14 present the percentage of distance to the reference position from the estimated position with values falling within the ranges: $〈0\mathrm{m};\left.1\mathrm{m}\right)$, $〈1\mathrm{m};\left.2\mathrm{m}\right)$, $〈2\mathrm{m};\left.3\mathrm{m}\right)$, and $〈3\mathrm{m};\left.4\mathrm{m}\right)$.

Figure 13. Percentage of distance to the reference position from the estimated position during a single crossing of the SV.

Figure 13. Percentage of distance to the reference position from the estimated position during a single crossing of the SV.

Figure 14. Percentage of distance to reference position from the estimated position during 100 SV crossings.

Figure 14. Percentage of distance to reference position from the estimated position during 100 SV crossings.

From the bar charts in Figure 13 and Figure 14, it can be seen that the most significant distance percentages were obtained by: the GSLA and GRA methods in the interval $〈0\mathrm{m};\left.1\mathrm{m}\right)$, the GRA method in the interval $〈1\mathrm{m};\left.2\mathrm{m}\right)$, and EKF method in the interval $〈1\mathrm{m};\left.3\mathrm{m}\right)$.

4.2. Tests Carried Out against Beacons Arranged in a Triangle

These tests simulated a single pass and 100 SV passes along a straight stretch of the road against three beacons arranged in a triangle. They resulted in two sets of analysed data, i.e., a set of five series of 300 positions and a set of five hundred series of 300 positions. Based on these, Table 2 and Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22 and Figure 23 were prepared for the analysis of the second group of test results.

Figure 15 shows an overview of the location of the beacons in the triangle and the estimated positions relative to them during single and 100 SV crossings.

Figure 15. Triangle alignment of beacons and SV positions estimated relative to them during: (a) a single crossing, (b) 100 crossings.

Figure 15. Triangle alignment of beacons and SV positions estimated relative to them during: (a) a single crossing, (b) 100 crossings.

Comparing Figure 6 and Figure 15 with each other, it can be seen that the estimated positions are scattered around the reference positions in similar manners. There is an increase in the scatter of positions estimated by the GLSA method, before and after the SV passes the extreme beacons (although it should be noted here that it is much smaller). The positions estimated by the EKF and GRA methods have the most considerable clustering in relation to the reference positions.

Table 2 shows the values of selected parameters of the statistical scatter of the distance to the reference position from the estimated position.

Table 2. Statistical parameters of the scatter of the distance to the reference position from the positions estimated by the GLSA, EKF, GRA, and DR methods.

Positioning Method	Maximum Distance to the Reference Position (m)		Arithmetic Mean of the Distance to the Reference Position (m)		Standard Deviation Distance to Reference Position (m)
	Single passage	100 crossings	Single crossing	100 crossings	Single crossing	100 crossings
GLSA	78.52	88.89	3.53	3.39	9.90	8.95
EKF	11.87	12.42	2.21	2.11	2.29	2.20
GRA	6.22	6.53	1.32	1.40	1.08	1.14
DR	31.48	30.61	15.22	16.06	8.32	8.39

Table 2. Statistical parameters of the scatter of the distance to the reference position from the positions estimated by the GLSA, EKF, GRA, and DR methods.

Positioning Method	Maximum Distance to the Reference Position (m)		Arithmetic Mean of the Distance to the Reference Position (m)		Standard Deviation Distance to Reference Position (m)
	Single passage	100 crossings	Single crossing	100 crossings	Single crossing	100 crossings
GLSA	78.52	88.89	3.53	3.39	9.90	8.95
EKF	11.87	12.42	2.21	2.11	2.29	2.20
GRA	6.22	6.53	1.32	1.40	1.08	1.14
DR	31.48	30.61	15.22	16.06	8.32	8.39

Analysing the values of the arithmetic mean and standard deviation in Table 2, it is possible to identify the GRA and EKF methods as clearly the best, the GLSA method as worse, and the DR method as clearly the worst. On the other hand, by comparing Table 1 and Table 2 with each other, it should be noted that the values of the statistical parameters relating to the GLSA and GRA methods have changed most significantly in favour (they are approximately twice as small). This is undoubtedly an effect of the non-co-linear alignment of the beacons with respect to each other (thus obtaining a better so-called ‘geometric coefficient’ calculated from the trace of the matrix ${\mathbf{C}}_{\mathbf{X}}$).

Figure 16 and Figure 17 show plots of distance and arithmetic mean distance to the reference position from the estimated position during single and 100 SV crossings as a function of time.

Figure 16. Distance to reference position from estimated position during a single SV crossing as a function of time.

Figure 16. Distance to reference position from estimated position during a single SV crossing as a function of time.

Figure 17. Arithmetic mean distance to the reference position from the estimated position during 100 SV crossings as a function of time.

Figure 17. Arithmetic mean distance to the reference position from the estimated position during 100 SV crossings as a function of time.

Comparing the graphs in Figure 7 and Figure 8 with the graphs in Figure 17 and Figure 18, there is an apparent reduction in the distance to the reference position from the GLSA-estimated position from about 100 m to about 40 m in the initial and final phases of the test. The curves of the other graphs are similar to each other.

Figure 18 and Figure 19 plot the moving mean distance to the reference position from the estimated position during a single and 100 SV crossings as a function of time.

The graphs plotted against each other in Figure 18 and Figure 19 clearly show that the GRA method estimates the position with the smallest moving mean distance to the reference position; this occurs almost throughout the entire test. The DR and EKF methods are only better at the beginning of the test and in a very short time frame $〈0\mathrm{s};18\mathrm{s}〉$.

Figure 18. Moving average distance to the reference position from the estimated position during a single SV crossing as a function of time (logarithmic scale—log base = 10).

Figure 18. Moving average distance to the reference position from the estimated position during a single SV crossing as a function of time (logarithmic scale—log base = 10).

Figure 19. Moving average distance to reference position from the estimated position during 100 SV crossings as a function of time (logarithmic scale—log base = 10).

Figure 19. Moving average distance to reference position from the estimated position during 100 SV crossings as a function of time (logarithmic scale—log base = 10).

Figure 20 and Figure 21 show plots of moving RMS to the reference position from the estimated position during a single and 100 SV crossings as a function of time.

Figure 20. Moving RMS of the distance to the reference position from the estimated position during a single SV crossing as a function of time (logarithmic scale—log base = 10).

Figure 20. Moving RMS of the distance to the reference position from the estimated position during a single SV crossing as a function of time (logarithmic scale—log base = 10).

Figure 21. Moving RMS of distance to reference position from the estimated position during 100 SV crossings as a function of time (logarithmic scale—log base = 10).

Figure 21. Moving RMS of distance to reference position from the estimated position during 100 SV crossings as a function of time (logarithmic scale—log base = 10).

Comparing Figure 20 and Figure 21 and Figure 11 and Figure 12 with each other, it is clear that the graphs relating to the DR and EKF methods have the same course and that the graphs relating to the GLSA and GRA methods are also parallel to each other. However, it is different for the GRA method. In Figure 11 and Figure 12, it obtains the smallest moving RMS distance values (smallest scatter around the moving average distance) over a larger time interval $〈200\mathrm{s};1000\mathrm{s}〉$.

Figure 22 and Figure 23 present the percentage of distance to the reference position from the estimated position with values falling within the ranges: $〈0\mathrm{m};\left.1\mathrm{m}\right)$, $〈1\mathrm{m};\left.2\mathrm{m}\right)$, $〈2\mathrm{m};\left.3\mathrm{m}\right)$, and $〈3\mathrm{m};\left.4\mathrm{m}\right)$.

The bar charts in Figure 22 and Figure 23 show that the largest distance percentages were obtained by the GSLA and GRA methods in the interval $〈0\mathrm{m};\left.1\mathrm{m}\right)$, the GRA method in the interval $〈1\mathrm{m};\left.2\mathrm{m}\right)$ (similar to Figure 13 and Figure 14), and the GRA method in the interval $〈1\mathrm{m};\left.3\mathrm{m}\right)$.

Figure 22. Percentage of distance to the reference point from the estimated point during a single SV crossing.

Figure 22. Percentage of distance to the reference point from the estimated point during a single SV crossing.

Figure 23. Arithmetic mean of the percentages of the distance to the reference point from the estimated point during 100 SV crossings.

Figure 23. Arithmetic mean of the percentages of the distance to the reference point from the estimated point during 100 SV crossings.

4.3. Selection of the Best Method for Estimating Position Coordinates

Section 4.1 and Section 4.2 analyse the test results. They showed that the GRA and EKF methods are interchangeably the best in terms of the accuracy of SV position coordinate estimation. On the one hand, they confirm the effectiveness of the algorithms of both methods (see Figure 1 and Figure 3) in suppressing the results of measurements with gross error. On the other hand, they show the interchangeability of the two methods between each other in obtaining the highest accuracy of estimated position coordinates (see Figure 10 and Figure 19). This mainly results from the way beacons are positioned and the location of the SV in relation to them (see Figure 6 and Figure 15).

Given the above, it made sense to find an objective indicator to select the most accurate positioning method depending on the SV’s location in relation to the beacons and the measurement errors of the navigation equipment.

The authors of the article propose to use, for this purpose, the mean error of the $M_{xy}$ of the coordinates of the positions estimated by the GRA method. However, the value $M_{xy}$ is determined on the assumption that the mean error values of the measurements have been determined ’ideally’, i.e., the coefficient of variation (VC) has a value equal to one (see relation (13)). Determination of the limit value $M_{xy}$, beyond which the EKF method should be changed to GRA, should be carried out as a result of simulation tests performed for a specific navigation system.

Figure 24 and Figure 25 show graphs of the mean error of the $M_{xy}$ of the GRA-estimated position coordinates over the course of 100 SV crossings relative to the aligned and triangular marks.

Figure 24. Graph of mean error $M_{xy}$ of GRA-estimated position coordinates over the course of 100 SV crossings, relative to aligned marks as a function of time.

Figure 24. Graph of mean error $M_{xy}$ of GRA-estimated position coordinates over the course of 100 SV crossings, relative to aligned marks as a function of time.

Figure 25. Graph of mean error $M_{xy}$ of GRA-estimated position coordinates during 100 SV crossings, relative to triangle marks as a function of time.

Figure 25. Graph of mean error $M_{xy}$ of GRA-estimated position coordinates during 100 SV crossings, relative to triangle marks as a function of time.

The graphs in Figure 24 and Figure 25 show that the value of the $M_{xy}$ depends not only on the iteratively determined values of the equivalent weight matrix ${\widehat{\mathbf{P}}}_{\mathrm{x}}$ (see Figure 3), but also, and mainly, on the spacing of the beacons relative to each other and the position of the SV relative to them (becoming then, so to speak, the so-called dilution of precision factor [36]).

However, by comparing Figure 24 and Figure 25 with Figure 10, Figure 12, Figure 19, and Figure 21, it can be tentatively concluded that the GRA method should be used when the value of the $M_{xy}$ will be less than 1.6 m.

Nevertheless, it is essential to realise that the limit value $M_{xy}$ was estimated based on simulation tests carried out under straightforward assumptions, e.g., SV only flows along a line.

Therefore, the authors of the article recommend setting a limit value $M_{xy}$ depending on the number of marks and the type of measurements performed (e.g., direction only or distance only, direction and distance) and simulating the SV of a mow-the-lawn path along the area around the beacons within the achievable measurement range.

These generalised conclusions give rise to the hypothesis that the proposed methodology has great potential in supporting SV positioning in coastal and harbour waters. Along with the continuous development of measurement systems (vision, acoustic, or radar) and the high computing power, it will undoubtedly be used on a larger scale in the future. Its first implementation is planned as part of the research project entitled “Hydroacoustic positioning system for unmanned autonomous underwater vehicles” (no. DOB-SZAFIR/01/B/003/04/2021), co-financed by the Polish National Center for Research and Development (NCBR).

5. Conclusions

The EKF and GRA methods use the gain matrix and the covariance matrix in their operations. This makes them robust in their function to measurements subject to gross error. However, the EKF gain matrix is created considering the historical covariance matrix. In contrast, the GRA covariance matrix, using an equivalent weighting matrix, suppresses the results of measurements of relative bearings and distances subject to gross error. The differences indicated result in an inconsistency in the accuracy of coordinate position estimation by both methods, based on the same measurement results taken relative to the beacons. This inconsistency becomes apparent as the number of measurements (relative bearings and distances) increases and the so-called position lines with favourable (accuracy) crossing angles appear.

Therefore, these methods should be used interchangeably to improve the accuracy of position estimation. The mean limiting error of the GRA estimation of position coordinates may be used for this purpose. Its value should be estimated as a result of experimental simulation studies conducted for a specific navigation system using the information on the distribution of beacons, e.g., obtained from an electronic navigation chart.