Open Access
This article is

- freely available
- re-usable

*Sensors*
**2016**,
*16*(8),
1279;
https://doi.org/10.3390/s16081279

Article

The Positioning Accuracy of BAUV Using Fusion of Data from USBL System and Movement Parameters Measurements

^{1}

Faculty of Navigation and Naval Weapons, Polish Naval Academy, Smidowicza 69, Gdynia 81-103, Poland

^{2}

Faculty of Civil and Environmental Engineering, Department of Geodesy, Gdansk University of Technology, Narutowicza 11/12, Gdansk 80-233, Poland

^{*}

Author to whom correspondence should be addressed.

Academic Editors:
Xue-Bo Jin,
Feng-Bao Yang,
Shuli Sun
and
Hong Wei

Received: 16 May 2016 / Accepted: 2 August 2016 / Published: 15 August 2016

## Abstract

**:**

The article presents a study of the accuracy of estimating the position coordinates of BAUV (Biomimetic Autonomous Underwater Vehicle) by the extended Kalman filter (EKF) method. The fusion of movement parameters measurements and position coordinates fixes was applied. The movement parameters measurements are carried out by on-board navigation devices, while the position coordinates fixes are done by the USBL (Ultra Short Base Line) system. The problem of underwater positioning and the conceptual design of the BAUV navigation system constructed at the Naval Academy (Polish Naval Academy—PNA) are presented in the first part of the paper. The second part consists of description of the evaluation results of positioning accuracy, the genesis of the problem of selecting method for underwater positioning, and the mathematical description of the method of estimating the position coordinates using the EKF method by the fusion of measurements with on-board navigation and measurements obtained with the USBL system. The main part contains a description of experimental research. It consists of a simulation program of navigational parameter measurements carried out during the BAUV passage along the test section. Next, the article covers the determination of position coordinates on the basis of simulated parameters, using EKF and DR methods and the USBL system, which are then subjected to a comparative analysis of accuracy. The final part contains systemic conclusions justifying the desirability of applying the proposed fusion method of navigation parameters for the BAUV positioning.

Keywords:

underwater positioning; estimation of coordinates; autonomous biomimetic underwater vehicle## 1. Introduction

Determining the position coordinates with high accuracy during the process of conducting marine underwater navigation is becoming a significant problem. This is mainly due to the fact that the Global Navigational Satellite System (GNSS) cannot be used. Sometimes using satellite navigation during emersion could be also difficult, especially in port areas, where infrastructure facilities and other vessels can block and reflect navigation signals [1,2]. Due to these difficulties, it becomes necessary to use dedicated navigation systems (NS) in such cases [3,4,5,6,7,8,9,10]. These dedicated navigation systems generally:

- use devices to carry out navigational parameter measurements in water, such as log, hydrostatic pressure sensor, or echo-sounder,

In the case of BAUV, a selection of devices and methods for determining the position can be made dependent on the parameters of its operation, shape, and size of the hull (construction), and type of propulsion used.

A BAUV, imitating fish in terms of the design and movement, is currently being prepared at the Polish Naval Academy (PNA) (Figure 1).

Starting from the bow to the tail fin, the BAUV consists of the following modules [9]:

- module with camera and “looking” forward echo sounder (“wet” compartment),
- module of the sensors (in the upper part: USBL, hydro modem and “looking” up echo sounder; in the bottom part: sonar and “looking” down echo sounder—“wet” compartment),
- module of the lateral fins (“dry” compartment),
- module of electronics and batteries (the batteries located in the bottom part, which can move along the longitudinal axis of the vehicle giving the possibility of trimming; two computers PC-104 and power management system mounted in the upper part—the whole compartment is “dry”),
- module of the caudal fin consisting of two segments (the tail fin driven by an electric motor with nominal power 250 W, rotary motion converted into an oscillating motion).

The range of swimming is designed for about 2 nautical miles and assumed optimum cruise speed is around 1 m/s (2 knots). The swimming range is an important operating parameter and it is a determining factor in the construction of the designed NS.

Several NS variants have been designed for the conceptual stage of the BAUV. They differ from each other, mainly in applied navigation devices and in algorithms of the position coordinates estimation. The conceptual NS described in this publication consists of components installed on the BAUV:

- GPS Aided Inertial Navigation System (GPS/INS) “VN-200” [21]—due to the operating environment, it is not possible to use GPS, therefore this subsystem is referred to as INS “VN-200” in subsequent parts of this article,
- “ALIZE” electro-magnetic log [22],
- ”WIKA S-20” hydrostatic pressure sensor [23],
- USBL “MicronNav System” transponder system [24],
- “MICRON DATA MODEM” slave hydromodem [25],

- USBL “MicronNav System” transceiver system,
- “MICRON DATA MODEM” master.

The USBL system will be dedicated to measure the approximate position coordinates (sending to the BAUV by hydromodem). The INS “VN-200” will measure the course, while the log will measure speed, and the pressure sensor will be used to submergence determination. These parameters will then be used to estimate submerged BAUV position by the extended Kalman filter. The BAUV moves using wave motion.

The NS constructed in this way will be subject to a comparative evaluation of the dead reckoning NS (that uses only the course and speed measurement), and the NS based only on the USBL system. The basic criterion for this assessment is the accuracy of the estimated position coordinates.

## 2. Evaluation of the Accuracy of Determining the Position Coordinates Using the USBL System

Determining the horizontal coordinates using USBL system is based on the measurement of two parameters: direction α (yaw) and the distance between the Transceiver and the Transponder $r$ (Figure 2).

The mean error ${M}_{xy}$ and the mean error ellipse (i.e., the length of the a and b axes) of determined horizontal coordinates $\left(x,y\right)$ using the USBL system can be calculated by applying the law of mean errors propagation [26,27,28]. Knowing the functions of the individual test result:
the equations of position lines mean error can be represented by:
that enables determination of:
where $\theta $—the intersection angle of the position lines. In the case of USBL it always equals 90°; ${\sigma}_{\alpha}$—α (yaw) mean measurement error; ${\sigma}_{r}$—$r$ (distance) mean measurement error; $r$—distance between the Transceiver and Transponder; $a$—the length of the major axis of the mean error ellipse; $b$—the length of the minor axis of the mean error ellipse.

$$\alpha =\text{arctg}\frac{x}{y}$$

$$r=\sqrt{{x}^{2}+{y}^{2}}$$

$${\sigma}_{l(\alpha )}={\sigma}_{\alpha}/\sqrt{{\left(\frac{\partial \alpha}{\partial x}\right)}^{2}+{\left(\frac{\partial \alpha}{\partial y}\right)}^{2}}$$

$${\sigma}_{l(r)}={\sigma}_{r}/\sqrt{{\left(\frac{\partial r}{\partial x}\right)}^{2}+{\left(\frac{\partial r}{\partial y}\right)}^{2}}$$

$${M}_{xy}=\frac{1}{\mathrm{sin}\theta}\sqrt{{{\sigma}_{l(\alpha )}}^{2}+{{\sigma}_{l(r)}}^{2}}=\sqrt{{\left({\sigma}_{\alpha}\xb7r\right)}^{2}+{{\sigma}_{r}}^{2}}$$

$$a={\sigma}_{\alpha}\xb7r$$

$$b={\sigma}_{r}$$

Based on the dependence Equation (5), information provided by the manufacturer, and the values of the mean errors of measurement, ${\sigma}_{\alpha}=3\xb0$ i·${\sigma}_{r}=0.2$ m, an area map has been compiled on the accuracy of position coordinates, determined by the Tritech USBL “MicronNav System” (Figure 3) [24].

Figure 3 shows that the mean errors of position coordinate increases with the measured distance. It reaches 26.2 m at a distance of 500 m. This is the distance limit at which the USBL “MicronNav System” can work [24].

Figure 4 shows the mean error ellipse of position coordinates determined in the same direction, in one hundred meter intervals.

It shows clearly that the coordinate error increases substantially in a perpendicular direction to the measurement direction with increasing distance. However, it is constant in direction of the measurement being carried out, and is only 0.2 m.

## 3. Evaluating the Accuracy of Determining the Position Coordinates Using Dead Reckoning

If the impact of current, drift, and waving of sea on the BAUV movement can be avoided, the only sources of errors are the devices used to dead reckoning. In the case of the BAUV, these are the Inertial Navigation System, INS “VN-200”, which determines course over ground (COG) [21] and the electromagnetic log “ALIZE”, which determines speed through water (STW). In this case, the STW is equal to the speed over ground (SOG) [22].

By measuring using these devices $\text{COG}\left(k\right)$, $\text{SOG}\left(k\right)$ at the time of $\left(k\right)$, vector ${x}_{h}$ of horizontal coordinates and their accuracy can be calculated for the time $\left(k+1\right)$. Functions $f\left({x}_{h}\left(k\right),{u}_{h}\left(k\right),{w}_{h}\left(k\right)\right)$ can be used for this purpose describing the non-linear BAUV movement model:
where $x\left(k\right),y\left(k\right)$—dead reckoned horizontal coordinates of BAUV position at the time of $k$; $\text{COG}\left(k\right)$—the BAUV course over ground at the time of $k$; $\text{SOG}\left(k\right)$—the BAUV speed over ground at the time of $k$; $\Delta t\left(k\right)$—duration between the time of k and k + 1; ${w}_{x}\left(k\right),{w}_{y}\left(k\right)$—the so-called intentional interference in determining the coordinates at the time of k (expressed as a zero mean normal distribution N[0, 1]); and calculated on the basis of Equation (8) the covariance matrix
here: ${F}_{x}\left(k+1\right)=\left[\begin{array}{cc}\frac{\partial {f}_{x}}{\partial x}& \frac{\partial {f}_{x}}{\partial y}\\ \frac{\partial {f}_{y}}{\partial x}& \frac{\partial {f}_{y}}{\partial y}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$—the so-called matrix system is calculated as Jacobian matrix from the function $f\left(x\left(k\right),u\left(k\right),0\right)$; $P\left(k\right)$—the covariance matrix, determined at the time of $k$.
${\sigma}_{COG}$—the course over ground mean measurement error; ${\sigma}_{SOG}$—the speed over ground mean measurement error.

$${x}_{h}\left(k+1\right)=\left[\begin{array}{c}\begin{array}{c}{f}_{x}\left(k+1\right)\\ {f}_{y}\left(k+1\right)\end{array}\end{array}\right]=\left[\begin{array}{c}\begin{array}{c}x\left(k\right)\\ y\left(k\right)\end{array}\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}\Delta t\left(k\right)\xb7\text{SOG}\left(k\right)\xb7\mathrm{sin}\text{COG}\left(k\right)\\ \Delta t\left(k\right)\xb7\text{SOG}\left(k\right)\xb7\mathrm{cos}\text{COG}\left(k\right)\end{array}\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}{w}_{\mathrm{x}}\left(k\right)\\ {w}_{\mathrm{y}}\left(k\right)\end{array}\end{array}\right]$$

$$P\left(k+1\right)={F}_{x}\left(k+1\right)\mathbf{P}\left(k\right){F}_{x}{\left(k+1\right)}^{T}+Q\left(k\right)$$

$$Q\left(k\right)=\left[\begin{array}{cc}\frac{\partial {f}_{x}}{\partial COG}& \frac{\partial {f}_{x}}{\partial \text{SOG}}\\ \frac{\partial {f}_{y}}{\partial \text{COG}}& \frac{\partial {f}_{y}}{\partial \text{SOG}}\end{array}\right]\left[\begin{array}{cc}{{\sigma}_{COG}}^{2}& 0\\ 0& {{\sigma}_{SOG}}^{2}\end{array}\right]{\left[\begin{array}{cc}\frac{\partial {f}_{x}}{\partial COG}& \frac{\partial {f}_{x}}{\partial \text{SOG}}\\ \frac{\partial {f}_{y}}{\partial \text{COG}}& \frac{\partial {f}_{y}}{\partial \text{SOG}}\end{array}\right]}^{T}$$

Knowing the covariance matrix of initial (the previous) position coordinates
where

**P**($k$) and the covariance matrix of the vector of growth coordinates**P**($k+1/1$), taking into account the errors resulting from the operation of dead reckoning navigation, the covariance matrix of the coordinates of the current BAUV position can be determined
$$P\left(k+1\right)=P\left(k\right)+P\left(k+1/1\right)$$

$$P\left(k+1\right)=\left[\begin{array}{cc}{{\sigma}_{x}}^{2}& {\sigma}_{xy}\\ {\sigma}_{yx}& {{\sigma}_{y}}^{2}\end{array}\right]$$

$${{\sigma}_{x}}^{2}=\Delta {t}^{2}\xb7{\left(SOG\left(k\right)\xb7{\sigma}_{COG}\xb7\mathrm{cos}\text{COG}\left(k\right)\right)}^{2}+\Delta {t}^{2}\xb7{\left({\sigma}_{SOG}\xb7\mathrm{sin}\text{COG}\left(k\right)\right)}^{2}$$

$${{\sigma}_{y}}^{2}=\Delta {t}^{2}\xb7{\left(SOG\left(k\right)\xb7{\sigma}_{COG}\xb7\mathrm{sin}\text{COG}\left(k\right)\right)}^{2}+\Delta {t}^{2}\xb7{\left({\sigma}_{SOG}\xb7\mathrm{cos}\text{COG}\left(k\right)\right)}^{2}$$

$${\sigma}_{xy}={\sigma}_{yx}=\Delta {t}^{2}\xb7\mathrm{sin}\left(2\xb7\text{COG}\left(k\right)\right)\xb7\left({{\sigma}_{SOG}}^{2}-{\left(SOG\xb7{\sigma}_{COG}\right)}^{2}\right)/2$$

Using this matrix, in turn, the mean error of coordinates can easily be calculated
and the parameters of the mean error ellipse:
where
$\tau $—direction angle of ellipse.

$${M}_{xy}=\sqrt{{{\sigma}_{x}}^{2}+{{\sigma}_{y}}^{2}}$$

$$a=\sqrt{\frac{1}{2}\xb7\left({{\sigma}_{x}}^{2}+{{\sigma}_{y}}^{2}+\sqrt{p}\right)}$$

$$b=\sqrt{\frac{1}{2}\xb7\left({{\sigma}_{x}}^{2}+{{\sigma}_{y}}^{2}-\sqrt{p}\right)}$$

$$\tau =\frac{1}{2}\text{arctg}\frac{2{\sigma}_{xy}}{{{\sigma}_{x}}^{2}-{{\sigma}_{y}}^{2}}$$

$$p={\left({{\sigma}_{x}}^{2}-{{\sigma}_{y}}^{2}\right)}^{2}+4\xb7{{\sigma}_{xy}}^{2}$$

Relying on the information provided by the manufacturer of log “ALIZE” that ${\sigma}_{SOG}=0.5$ kn and determined arbitrarily ${\sigma}_{COG}=3\xb0$ for INS “VN-200”, using Equations (10) and (11), a mean error graph of position coordinates determined using dead reckoning was prepared (Figure 5).

Figure 5 shows that the mean error of position coordinates increases linearly as a function of distance. Every hundred meters it increases constantly at 25.54 m, reaching a value of 127.7 m after traveling 500 m.

Figure 6 shows the mean error ellipses of position coordinates determined using the dead reckoning method after traveling 200 and 500 m (their dependencies No. 12–13 have been calculated).

Figure 6 shows that the largest coordinate error occurs in the direction according to COG. The length of the axis b is equal to the length of the mean error ellipse of coordinates determined using USBL system (Figure 4). This is due to the fact that in both methods of determining the position coordinates the positioning line is a result of the direction measurement that is burdened with the same error value ${\sigma}_{COG}={\sigma}_{\alpha}$.

## 4. The Genesis of the Problem

The choice between the USBL system and the method of dead reckoning based on the accuracy criterion is difficult. In some cases, the USBL system can be identified as being better (e.g., when measurements are carried out in small distances from the transceiver), while in others, the method of dead reckoning (e.g., immediately after updating coordinates by GNSS fixes). If a USBL system is selected, you give up additional COG and SOG. If you choose the DR method measurements $r$, $\alpha $, $\beta $ (range, yaw, pitch) are not taken into account. It is certain that skilful use of a greater number of measurements can improve their accuracy [29].

In the case of the NS BAUV, the different availability of measurements is assumed, i.e.,

- at regular intervals of time and with high frequency—the course and speed determined by navigation devices installed on the BAUV (not less than once per second),
- at irregular intervals and with low frequency—the direction and distance to the Transceiver of the appointed USBL system and transmitted by hydro-modem (less than once per second—because of the difficulties in carrying out USBL system measurements and sending the results of these measurements by hydro-modem caused interference in the propagation of acoustic waves in the water).

These parameters will then be used to determine the position coordinates using the extended Kalman filter (EKF).

In its action, the filter may combine the various measurements (e.g., the course with the distance to a navigation mark), carried out at different time intervals, taking into account their errors—adjusted dynamically by the matrix of weights. Thanks to this mechanism, measurements which are obviously erroneous (e.g., carried out in the USBL system, on the border of its operation) can be firmly suppressed, and accurate measurements amplified (e.g., SOG and COG determined in a short period of time from the moment of updating the coordinates by GNSS fixes).

However, will the coordinates obtained by the EKF method, a combination of parameter measurements of BAUV motion and the direction and distance of the USBL system, be more accurate than either the coordinate USBL system or the DR method in all cases?

The rest of this article presents a study on the positioning accuracy of BAUV by the EKF method compared to the USBL and DR positioning system, aimed at finding answers to the above question.

## 5. Description of Data Fusion Using the Extended Kalman Filter

Let us carry out a fusion of data using the extended Kalman filter combining $\text{COG}\left(k\right)$ and $\text{SOG}\left(k\right)$ measurements, changes in submergence $\Delta z\left(k\right)$ carried out every one second with 3D coordinate system measurements of the USBL system, carried out at different time intervals (in some moments $k+1$).

Let us assume that the results of the USBL measurement system will create the so-called vector of observation, $z\left(k+1\right)={\left[\begin{array}{c}r\left(k+1\right)\alpha \left(k+1\right)\end{array}\beta \left(k+1\right)\right]}^{\mathrm{T}}$, described by the function [30]:
where
${\nu}_{r}\left(k+1\right)$, ${\nu}_{\alpha}\left(k+1\right)$, ${\nu}_{\beta}\left(k+1\right)$—values of measurement errors (with zero mean normal distribution); $x\left(k+1\right)$, $y\left(k+1\right)$, $z\left(k+1\right)$—fixed coordinates of transceiver position; ${x}_{1}\left(k+1\right)$, ${y}_{1}\left(k+1\right)$, ${z}_{1}\left(k+1\right)$—variable coordinates of the transponder position (BAUV). And on the basis of $\text{COG}\left(k\right)$, $\text{SOG}\left(k\right)$ measurements and the submergence changes $\Delta z\left(k\right),$ vector $x$ of BAUV’s 3D position coordinates, using the function $f\left(x\left(k\right),u\left(k\right),w\left(k\right)\right)$:

$$h\left(x\left(k+1\right),\mathsf{\nu}\left(k+1\right)\right)=\left[\begin{array}{c}{f}_{r}\left(k+1\right)\\ {f}_{\alpha}\left(k+1\right)\\ {f}_{\beta}\left(k+1\right)\end{array}\right]=\left[\begin{array}{c}r\left(k+1\right)\\ \alpha \left(k+1\right)\\ \beta \left(k+1\right)\end{array}\right]+\left[\begin{array}{c}{\nu}_{r}\left(k+1\right)\\ {\nu}_{\alpha}\left(k+1\right)\\ {\nu}_{\beta}\left(k+1\right)\end{array}\right]$$

$$r\left(k+1\right)=\sqrt{{\left({x}_{1}\left(k+1\right)-x\left(k+1\right)\right)}^{2}+{\left({y}_{1}\left(k+1\right)-y\left(k+1\right)\right)}^{2}+{\left({z}_{1}\left(k+1\right)-z\left(k+1\right)\right)}^{2}}$$

$$\alpha \left(k+1\right)=\text{arctg}\frac{{x}_{1}\left(k+1\right)-x\left(k+1\right)}{{y}_{1}\left(k+1\right)-y\left(k+1\right)}$$

$$\beta \left(k+1\right)=-\text{arctg}\frac{{z}_{1}\left(k+1\right)-z\left(k+1\right)}{\sqrt{{\left({x}_{1}\left(k+1\right)-x\left(k+1\right)\right)}^{2}+{\left({y}_{1}\left(k+1\right)-y\left(k+1\right)\right)}^{2}}}$$

$$x\left(k+1\right)=\left[\begin{array}{c}\begin{array}{c}{f}_{x}\left(k+1\right)\\ {f}_{y}\left(k+1\right)\\ {f}_{z}\left(k+1\right)\end{array}\end{array}\right]=\left[\begin{array}{c}\begin{array}{c}x\left(k\right)\\ y\left(k\right)\\ z\left(k\right)\end{array}\end{array}\right]+\left[\begin{array}{c}\begin{array}{c}\Delta t\left(k\right)\xb7\text{SOG}\left(k\right)\xb7\mathrm{sin}\text{COG}\left(k\right)\\ \Delta t\left(k\right)\xb7\text{SOG}\left(k\right)\xb7\mathrm{cos}\text{COG}\left(k\right)\\ \Delta \mathrm{z}\left(k\right)\end{array}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{x}}\left(k\right)\\ {w}_{\mathrm{x}}\left(k\right)\\ {w}_{\mathrm{z}}\left(k\right)\end{array}\right]$$

In pursuing data fusion, based on functions $h\left(x\left(k+1\right),0\right)$ i $f\left(x\left(k\right),u\left(k\right),0\right)$ we calculate the estimated vector of coordinates
$\hat{\mathbf{x}}\left(k+1\right)$
and the covariance matrix $P\left(k+1\right)$, using the following dependencies:
where ${\hat{\mathbf{x}}\left(k+1\right)}^{-}\text{}i\text{}\mathbf{P}{\left(k+1\right)}^{-}$—estimated vector of coordinates of the BAUV position and its covariance matrix, determined a priori for the time of k + 1; $\hat{\mathbf{x}}\left(k+1\right)\text{}i\text{}\mathbf{P}\left(k+1\right)$—estimated vector of coordinates of the BAUV position and its covariance matrix, determined a posteriori for the time of k + 1, $F\left(k+1\right)=\left[\begin{array}{ccc}\frac{\partial {f}_{x}}{\partial x}& \frac{\partial {f}_{x}}{\partial y}& \frac{\partial {f}_{x}}{\partial z}\\ \frac{\partial {f}_{y}}{\partial x}& \frac{\partial {f}_{y}}{\partial y}& \frac{\partial {f}_{y}}{\partial z}\\ \frac{\partial {f}_{z}}{\partial x}& \frac{\partial {f}_{z}}{\partial y}& \frac{\partial {f}_{z}}{\partial z}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$—the so-called matrix system is calculated as a function of the Jacobian matrix $f\left(x\left(k\right),u\left(k\right),0\right)$, $Q\left(k\right)=\left[\begin{array}{ccc}{{\sigma}_{x}}^{2}& {\sigma}_{xy}& {\sigma}_{xz}\\ {\sigma}_{yx}& {{\sigma}_{y}}^{2}& {\sigma}_{yz}\\ {\sigma}_{zx}& {\sigma}_{zy}& {{\sigma}_{z}}^{2}\end{array}\right]=\left[\begin{array}{ccc}\frac{\partial {f}_{x}}{\partial COG}& \frac{\partial {f}_{x}}{\partial \text{SOG}}& \frac{\partial {f}_{x}}{\partial \Delta z}\\ \frac{\partial {f}_{y}}{\partial \text{COG}}& \frac{\partial {f}_{y}}{\partial \text{SOG}}& \frac{\partial {f}_{y}}{\partial \Delta z}\\ \frac{\partial {f}_{z}}{\partial \text{COG}}& \frac{\partial {f}_{z}}{\partial \text{SOG}}& \frac{\partial {f}_{z}}{\partial \Delta z}\end{array}\right]\left[\begin{array}{ccc}{{\sigma}_{COG}}^{2}& 0& 0\\ 0& {{\sigma}_{SOG}}^{2}& 0\\ 0& 0& {{\sigma}_{\Delta z}}^{2}\end{array}\right]{\left[\begin{array}{ccc}\frac{\partial {f}_{x}}{\partial COG}& \frac{\partial {f}_{x}}{\partial \text{SOG}}& \frac{\partial {f}_{x}}{\partial \Delta z}\\ \frac{\partial {f}_{y}}{\partial \text{COG}}& \frac{\partial {f}_{y}}{\partial \text{SOG}}& \frac{\partial {f}_{y}}{\partial \Delta z}\\ \frac{\partial {f}_{z}}{\partial \text{COG}}& \frac{\partial {f}_{z}}{\partial \text{SOG}}& \frac{\partial {f}_{z}}{\partial \Delta z}\end{array}\right]}^{\mathrm{T}}$—matrix of noises of the state vector at the time of k (with adopted mean errors ${\sigma}_{COG}$, ${\sigma}_{SOG}$, ${\sigma}_{\Delta z}$ of COG and SOG measurements and changes in submergence),
$P\left(k\right)$—updated covariance matrix of the state vector used in subsequent time k + 1, $H\left(k+1\right)=\left[\begin{array}{ccc}\frac{\partial {f}_{\mathrm{r}}}{\partial x}& \frac{\partial {f}_{r}}{\partial y}& \frac{\partial {f}_{r}}{\partial z}\\ \frac{\partial {f}_{\alpha}}{\partial x}& \frac{\partial {f}_{\alpha}}{\partial y}& \frac{\partial {f}_{\alpha}}{\partial z}\\ \frac{\partial {f}_{\beta}}{\partial x}& \frac{\partial {f}_{\beta}}{\partial y}& \frac{\partial {f}_{\beta}}{\partial z}\end{array}\right]=\left[\begin{array}{ccc}\frac{{x}_{1}\left(k+1\right)-x\left(k+1\right)}{\sqrt{{l}_{3}\left(k+1\right)}}& \frac{{y}_{1}\left(k+1\right)-y\left(k+1\right)}{\sqrt{{l}_{3}\left(k+1\right)}}& \frac{{z}_{1}\left(k+1\right)-z\left(k+1\right)}{\sqrt{{l}_{3}\left(k+1\right)}}\\ \frac{{y}_{1}\left(k+1\right)-y\left(k+1\right)}{{l}_{2}\left(k+1\right)}& -\frac{{x}_{1}\left(k+1\right)-x\left(k+1\right)}{{l}_{2}\left(k+1\right)}& 0\\ \frac{\left({x}_{1}\left(k+1\right)-x\left(k+1\right)\right)\cdot \left({z}_{1}\left(k+1\right)-z\left(k+1\right)\right)}{\sqrt{{l}_{2}\left(k+1\right)}\cdot {l}_{3}\left(k+1\right)}& \frac{\left({y}_{1}\left(k+1\right)-y\left(k+1\right)\right)\cdot \left({z}_{1}\left(k+1\right)-z\left(k+1\right)\right)}{\sqrt{{l}_{2}\left(k+1\right)}\cdot {l}_{3}\left(k+1\right)}& -\frac{\sqrt{{l}_{2}\left(k+1\right)}}{{l}_{3}\left(k+1\right)}\end{array}\right]$—Jacobian matrix with function $h\left(x\left(k\right),0\right)$,
$R\left(k+1\right)=\left[\begin{array}{ccc}{{\sigma}_{r}}^{2}& 0& 0\\ 0& {{\sigma}_{\alpha}}^{2}& 0\\ 0& 0& {{\sigma}_{\beta}}^{2}\end{array}\right]$—matrix of noises of the observation vector in time of k + 1 (adopted mean measurement errors: ${\sigma}_{r}$—range, ${\sigma}_{\alpha}$—yaw, ${\sigma}_{\beta}$—pitch); $I$—identity matrix.

$$\hat{\mathbf{x}}{\left(k+1\right)}^{-}=f(x\left(k\right),u\left(k\right),0)$$

$$\mathbf{P}{\left(k+1\right)}^{-}=\mathbf{F}\left(k+1\right)\mathbf{P}\left(k\right)F{\left(k+1\right)}^{\mathrm{T}}+\mathbf{Q}\left(k\right)$$

$$\stackrel{\sim}{\mathbf{y}}(k+1)=z(k+1)-h(\hat{\mathbf{x}}{\left(k+1\right)}^{-},0)$$

$$S\left(k+1\right)(\mathbf{H}\left(k+1\right)\mathbf{P}{\left(k+1\right)}^{-}H{\left(k+1\right)}^{\mathrm{T}}+R\left(k+1\right)$$

$${\mathbf{K}\left(k+1\right)=\mathbf{P}\left(k+1\right)}^{-}H{(k+1)}^{\mathrm{T}}\text{}S{\left(k+1\right)}^{-1}$$

$$\hat{\mathbf{x}}\left(k+1\right)=\hat{\mathbf{x}}{\left(k+1\right)}^{-}+K\left(k+1\right)\stackrel{\sim}{\mathbf{y}}\left(k+1\right)$$

$$\mathbf{P}\left(k+1\right)+\left(\mathbf{I}-\mathbf{K}\left(k+1\right)\mathbf{H}\left(k+1\right)\right){\mathbf{P}\left(k+1\right)}^{-}$$

$${\sigma}_{xz}={\sigma}_{yz}={\sigma}_{zx}={\sigma}_{zy}=0$$

$${{\sigma}_{z}}^{2}={{\sigma}_{\Delta z}}^{2}$$

$${l}_{3}\left(k+1\right)={\left({x}_{1}\left(k+1\right)-x\left(k+1\right)\right)}^{2}+{\left({y}_{1}\left(k+1\right)-y\left(k+1\right)\right)}^{2}+{\left({z}_{1}\left(k+1\right)-z\left(k+1\right)\right)}^{2}$$

$${l}_{2}\left(k+1\right)={\left({x}_{1}\left(k+1\right)-x\left(k+1\right)\right)}^{2}+{\left({y}_{1}\left(k+1\right)-y\left(k+1\right)\right)}^{2}$$

## 6. Research and Analysis of the Obtained Results

Let us now do an experiment to software simulation of the BAUV passage along the straight section with simultaneous:

- generating $\text{COG}$—measurements of INS carried out by “VN-200”, $\text{SOG}$—carried out with log “ALIZE” and $\Delta \mathrm{z}$—carried out with a hydrostatic pressure sensor “S-20 WIKA”;
- generating by $r$, $\alpha $, $\beta $ measurements—carried out by the USBL “MicronNav System”;
- calculating the BAUV estimated coordinates of positions, parallel to the USBL system, with the method of dead reckoning and using the extended Kalman filter.

As a measuring testing ground, we assume a sea area around the USBL transponder system (Figure 7).

Let the BAUV swims near the USBL transceiver system (located at a depth of 6 m) overcoming the test section with $\text{COG}=\text{}220\xb0$, $\text{SOG}=\text{}1\text{m}/\mathrm{s}$, submerged at $z\text{}=\text{}3\text{m}$. Meanwhile, $\tau =\langle 0\mathrm{s},600\mathrm{s}\rangle $ simulated measurements will be carried out by the on-board BAUV and USBL system, i.e.,

- at fixed moments $k+1$ (every one second) $\text{COG}\left(k+1\right)={\text{COG}}_{w}\left(k+1\right)+{\Delta}_{COG}$, $\text{SOG}\left(k+1\right)={\text{SOG}}_{w}\left(k+1\right)+{\Delta}_{SOG}$ and $\Delta \mathrm{z}\left(k+1\right)=\Delta {\mathrm{z}}_{w}\left(k+1\right)+{\Delta}_{\Delta z}$;
- at some moments$\text{}k+1$(every 5, 20 and 100 s) $\begin{array}{c}r\left(k+1\right)=\end{array}{r}_{w}\left(k+1\right)+{\Delta}_{r}$, $\begin{array}{c}\alpha \left(k+1\right)={\alpha}_{w}\left(k+1\right)+{\Delta}_{\alpha}\end{array}$, $\beta \left(k+1\right)={\beta}_{w}\left(k+1\right)+{\Delta}_{\beta}$.

Each simulated value of the measurement is obtained by adding the reference measurement and the error (e.g., simulated $\text{COG}\left(k+1\right)={\text{COG}}_{w}\left(k+1\right)+{\Delta}_{COG}$). Measurement error $\Delta $ will be treated as a random variable with uniform distribution, the value of which will be contained within three mean errors (3σ) of measurements made in the device (i.e., the probability of its occurrence will be at 99%).

Reference measurements of the USBL system ${r}_{w}\left(k+1\right)$, $\begin{array}{c}{\alpha}_{w}\left(k+1\right)\end{array}$, ${\beta}_{w}\left(k+1\right)$ will be determined relative to the reference positions consecutively occupied by the BAUV, which moves with undisturbed movement. On the other hand, the BAUV reference position will be dead reckoned (DR) on the basis of reference ${\text{COG}}_{w}\left(k+1\right)$, ${\text{SOG}}_{w}\left(k+1\right)$ and $\Delta {\mathrm{z}}_{w}\left(k+1\right)$, which will be located exactly at the measuring section (see Figure 7).

For realigning the experiment for calculating the accepted mean error values close to the actual values (provided by the manufacturer and determined on the basis of their own—selected arbitrarily):

- ${\sigma}_{COG}=8\xb0$—for INS “VN-200”,
- ${\sigma}_{SOG}=0.5$ kn—for the electromagnetic log “ALIZE”,
- ${\sigma}_{\Delta z}=0.25\text{m}$—for the hydrostatic pressure sensor “WIKA S-20”,
- ${\sigma}_{r}=0.2\text{m}$, ${\sigma}_{\alpha}={\sigma}_{\beta}=3\xb0$—for the USBL “MicronNav System” system.

The experiment will commence at the moment of $k=0$ and will be completed when the BAUV swims about 600 m. The extended Kalman filter will be “fine-tuned” at the start of the experiment and the coordinates of the initial BAUV position will be determined with a mean error of about 3 m (it is assumed that the coordinates were determined before submergence using GNSS) [31]. In the calculations, changes of $\text{COG}$, $\text{SOG}$, and $\Delta \mathrm{z}$ caused by the BAUV wave motion (swinging, on a sine curve) will be omitted.

#### 6.1. Test No. 1

The test consisted of making three and then one hundred passages along the test section.

$\text{COG}$, $\text{SOG}$ and $\Delta \mathrm{z}$ were measured every one second.

$r$, $\alpha $, $\beta $ were measured every 5 s.

#### First Passage

In Figure 8 determined routs using different methods of positioning are presented. Figure 9, Figure 10, Figure 11 and Figure 12 present graphs depicting simulated measurement errors (${\Delta}_{COG}$,${\Delta}_{SOG}$,${\Delta}_{r}$,$\text{}{\Delta}_{\alpha}$, ${\Delta}_{\beta}$) during the first passage of the test section (these graphs will not be displayed during the presentation of the results of further testing). Figure 9, Figure 10, Figure 11 and Figure 12 clearly show that the dispersion of the measurement errors are random and limited. The error value does not exceed $3\sigma $ fluctuating:

- ${\Delta}_{COG}$ in the range of $\langle -15\xb0,15\xb0\rangle $,
- ${\Delta}_{SOG}$ in the range of $\langle -0.8\text{m}/\mathrm{s},0.8\text{m}/\mathrm{s}\rangle $,
- ${\Delta}_{\alpha}$, ${\Delta}_{\beta}$ in the range of $\langle -9\xb0,9\xb0\rangle $,
- ${\Delta}_{r}$ in the range of $\langle -0.6{m},0.6\text{m}\rangle $.

In Figure 13, for comparison, three graphs of the reference position distance from the position estimated by the extended Kalman filter and dead reckoning methods and determined by the USBL system are presented.

They show that the accuracy of the estimated position coordinates of EKF method is the same during the entire test duration. The accuracy of the estimated position coordinates using the DR method decreases as a function of time. On the other hand, the accuracy of the USBL position system grows in the time interval <400 s, 500 s>—it is then that the distance between the transceiver and the transponder does not exceed 50 m.

Table 1 shows the statistical parameters characterizing the accuracy of coordinates of all positions set at the test section.

The results in Table 1 clearly show that the best values of the statistical parameters were obtained by the EKF method.

On the basis of simulation measurements during two successive passages along the test section, the following route graphs were prepared and the reference position distance from the estimated position (Figure 14, Figure 15, Figure 16 and Figure 17) as well as the statistical parameters (Table 2 and Table 3).

#### Second Passage

**Figure 15.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.1 | 6.4 | 2.3 |

DR | 0.1 | 22.6 | 11.8 |

USBL | 0.2 | 81.6 | 23.6 |

#### Third Passage

**Figure 17.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.1 | 5.3 | 1.8 |

DR | 0.2 | 14.0 | 4.7 |

USBL | 0.7 | 86.1 | 21.6 |

On the basis of graphs analysis of Figure 14, Figure 15, Figure 16 and Figure 17, as well as statistical parameters from Table 2 and Table 3, it can be stated that the accuracy of estimated position coordinates using the EKF method is higher than those estimated using the DR method and determined by the USBL system. The reference position distance from the estimated one by EKF method does not exceed 7 m. Unfortunately, on the graphs it can be noticed that time intervals have occurred in which the reference position is further from the position estimated by EKF method, in comparison to the estimated position by DR method or determined using the USBL system. One can ask oneself a question here: how often do such time intervals occur? Generalized results of statistical analysis can answer that question. It is based on two additional indicators.

The former describes the average $\xi $ distance to the reference position with estimated calculation on the basis of $n$ distance $\xi $ obtained for $n$ passages along the test section at the same moment of$k$, in accordance with the formula:

$$\xi \left(k\right)={\displaystyle \sum}_{i=0}^{n}{\zeta}_{i}\left(k\right)$$

#### One Hundred Passages

Generalizing the results of test No. 1, based on Figure 19, it can be stated that the average value of the reference position distance from the estimated position is constant in the case of EKF, it increases linearly for DR and decreases and then increases linearly in the case of the USBL system.

The second indicator to the generalized analysis is based on the histogram showing the frequency of the same distance to the reference position from the estimated position. The histogram is presented in Figure 20.

It results from the histogram that a two meter distance from the estimated position by EKF occurred most often. In addition, as a function of the distance to 5 m, EKF histogram columns occur most often. DR and USBL histograms are flattened as a function of distance. This proves that the incidence of very scattered distance values (up to 15 m) is highly probable. The statistical parameters of the positioning errors using EKF, DR methods and the USBL system are presented in Table 4.

The results in Table 4 clearly show that the best values of the statistical parameters were obtained by the EKF method.

#### 6.2. Test No. 2

The test depended on carrying out three, followed by a hundred passages in the test section. $\text{COG}$, $\text{SOG}$ and $\Delta \mathrm{z}$ were measured every one second. $r$, $\alpha $, $\beta $ were measured every 20 s.

#### First Passage

#### Second Passage

**Table 6.**Statistical parameters of positioning errors using EKF and DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.1 | 7.5 | 2.5 |

DR | 0.1 | 26.7 | 9.7 |

USBL | 1.4 | 64.3 | 21.0 |

#### Third Passage

The analysis of Figure 21, Figure 22 and Figure 23 shows that the accuracy of the estimated position coordinates using EKF method improves almost every 20 s, when measurements from the USBL system are included in the calculations. Even if the position determined by the USBL system is significantly distant from the reference position, the accuracy of the estimated position using EKF method increases. This may be due to the fact that EKF suppresses the significant and obviously erroneous measurement direction of $\begin{array}{c}\alpha \left(k+1\right)\end{array}$, $\beta \left(k+1\right)$ and reinforces the accurate measurement of the distance of $r\left(k+1\right)$.

On the basis of analysis of the graphs in Figure 21, Figure 22 and Figure 23, and the statistical parameters from Table 6 and Table 7, it can be concluded that the accuracy of the estimated position coordinates using EKF method is greater than the estimated ones using DR method and determined by USBL system. The maximum reference position distance from the estimated ones using EKF method does not exceed 8 m. However, as in test No. 1, it can be seen on the graphs that there were intervals in which the reference position is more distant from the estimated position by the EKF method, compared to the estimated position by the DR method or that determined by the USBL system.

**Table 7.**Statistical parameters of positioning errors using EKF and DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.3 | 6.1 | 2.2 |

DR | 0.3 | 15.3 | 6.2 |

USBL | 1.3 | 72.6 | 21.1 |

#### One Hundred Passages

#### 6.3. Test No. 3

The test consisted of carrying out three, and then a hundred passages along the test section. $\text{COG}$, $\text{SOG}$ and $\Delta \mathrm{z}$ were measured every one second. $r$, $\alpha $, $\beta $ were measured every 100 s. The statistical parameters of the positioning errors using EKF, DR methods and the USBL system for single passage are presented in Table 9 (for first passage) in Table 10 (for second passage) and in Table 11 (for third passage).

#### First Passage

**Table 9.**Statistical parameters of positioning errors using EKF, DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance from the Reference Position (m) | Maximum Distance from the Reference Position (m) | Average Distance from the Reference Position (m) |
---|---|---|---|

EKF | 0.1 | 11.6 | 4.0 |

DR | 0.3 | 33.8 | 22.0 |

USBL | 3.4 | 37.5 | 18.7 |

#### Second Passage

**Table 10.**Statistical parameters of positioning errors using EKF, DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance from the Reference Position (m) | Maximum Distance from the Reference Position (m) | Average Distance from the Reference Position (m) |
---|---|---|---|

EKF | 0.5 | 11.4 | 4.9 |

DR | 0.5 | 28.7 | 12.0 |

USBL | 7.6 | 20.0 | 14.0 |

#### Third Passage

**Table 11.**Statistical parameters of positioning errors using EKF, DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance from the Reference Position (m) | Maximum Distance from the Reference Position (m) | Average Distance from the Reference Position (m) |
---|---|---|---|

EKF | 0.2 | 9.1 | 4.0 |

DR | 0.2 | 20.3 | 8.9 |

USBL | 3.5 | 22.0 | 11.7 |

Graphs of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system for single passage are presented in Figure 27, Figure 28 and Figure 29. The analysis of these Figures shows that the accuracy of the estimated position coordinates using EKF method improves almost every 100 s, when measurements from the USBL system are included in the calculations. The accuracy of the estimated position coordinates using the EKF method is greater than the estimated ones using the DR method and determined by the USBL system. The reference position distance from the estimated ones using EKF method is slightly larger than in previous tests and reach about 11 m. This is due to the long period of estimating the position using EKF without taking measurements of the USBL system.

#### One Hundred Passages

Figure 30 presents determined routs during one hundred passages using different methods of positioning. Statistical parameters of positioning errors using EKF, DR methods and the USBL system for these passages are presented in Table 12.

The generalized results of the Test No. 3, clearly show that the estimation of position coordinates using the EKF method gives the best results. In Figure 31, a marked improvement in the accuracy of the position can be seen when calculating additional measurements from the USBL system (at 100 s). The histogram in Figure 32 confirms the exact estimated position coordinates by the EKF method can be most frequently expected.

## 7. Conclusions

NS of the BAUV, which uses only the dead reckoning method based on the course and speed measurements, determines the position coordinates which error has quickly built up over time. In a study, after 500 s (corresponding to travelling 500 m) the average error of the position coordinates reached a value of 127.7 m (Figure 5).

In turn, the USBL system ensures high accuracy of determining the position coordinates, though only at short distances from the transceiver. In studies, at a distance of 500 m, the average error of determining the position coordinates will reach a value of 26.2 m (Figure 3).

Combining the EKF measurement method in the calculations carried out by devices installed on the BAUV (log, INS, hydrostatic pressure sensor) with distance and direction measurements carried out in the USBL system allows for obtaining the position coordinates significantly more accurate. This happens mainly because the position coordinates obtained are based on low accurate COG and SOG measurements and may be adjusted every now and then by more precise distance measurements of r carried out by the USBL system. This is confirmed by the full results of the three tests presented in the article.

The presented research concerned only method of BAUV positioning during undisturbed motion along the straight line. The obtained results are based only on simulated measurements. Further research will focus on tests in real environment. Sea currents and waving of sea will be taken into consideration. Furthermore, positioning accuracy of BAUV will be assessed during movement along composed trajectories including straight sections as well as curves lines.

## Author Contributions

The work presented in the paper was carried out in equal contribution of all authors. Krzysztof Naus conceived the concept and idea. Authors together implemented software and carried out the experiments. The manuscript was also written as a common work.

## Conflicts of Interest

The authors declare no conflict of interest.

## References

- Nowak, A. The Proposal to “Snapshot” RAIM method for GNSS vessel receivers working in poor space segment geometry. Pol. Marit. Res.
**2015**, 22, 3–8. [Google Scholar] [CrossRef] - Janowski, A.; Nowak, A.; Przyborski, M.; Szulwic, J. Mobile Indicators in GIS and GPS positioning accuracy in cities. In Rough Sets and Intelligent Systems Paradigms, Lecture Nots in Computer Science; Springer International Publishing: Berlin, Germany, 2014; pp. 309–318. [Google Scholar]
- Naus, K. Electronic navigational chart as an equivalent to image produced by hypercatadioptric camera system. Pol. Marit. Res.
**2015**, 22, 3–9. [Google Scholar] [CrossRef] - Naus, K.; Waz, M. Precision in determining ship position using the omnidirectional map to visual shoreline image comparative method. J. Navig.
**2016**, 69, 391–413. [Google Scholar] [CrossRef] - Naus, K.; Waz, M. A simplified navigational chart pyramid dedicated to an autonomous navigational system. Pol. Hyperb. Res.
**2012**, 3, 139–163. [Google Scholar] - Naus, K.; Szymak, P. Accuracy of positioning autonomous biomimetic underwater vehicle using additional measurement of distances. Annu. Navig.
**2015**, 22, 65–79. [Google Scholar] - Ribas, D.; Ridao, P.J.; Neira, J.; Tardos, J.D. SLAM using an imaging sonar for partially structured underwater environments. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Beijing, China, 9–15 October 2006; pp. 5040–5045.
- Syder, F.D.; Morris, D.D.; Haley, P.H.; Collins, R.T.; Okerholm, A.M. Northrop Grumman. Autonomous River Navigation. Proc. SPIE Mob. Robots XVII
**2004**, II, 221–232. [Google Scholar] - Szymak, P.; Praczyk, T.; Naus, K.; Szturomski, B.; Malec, M.; Morawski, M. Autonomous Underwater Vehicle with Silent Undulating Propulsion for Underwater ISR; Technical Report on Stage No. 2 from Development Project No. DOBR-BIO4/033/13015/2013 Entitled; Polish Naval Academy: Gdynia, Poland, 2015. [Google Scholar]
- Wang, C. Simultaneous Localization, Mapping and Moving Object Tracking. Ph.D. Thesis, Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA, 2004. [Google Scholar]
- Bovio, E.; Cecchi, D.; Baralli, F. Autonomous underwater vehicles for scientific and naval operations. Annu. Rev. Contr.
**2006**, 30, 117–130. [Google Scholar] [CrossRef] - Daniel, M. SSS-SLAM: An Object Oriented Matlab Framework for Underwater SLAM Using Side Scan Sonar. In Proceedings of the Days Automatic, Automatic Spanish Committee of the IFAC (CEA-IFAC), Valencia, Spain, 3–5 September 2014; pp. 1–8.
- Gomez-Gil, J.; Ruiz-Gonzalez, R.; Alonso-Garcia, S.; Gomez-Gil, F.J. A Kalman Filter implementation for precision improvement in low-cost GPS positioning of tractors. Sensors
**2013**, 13, 15307–15323. [Google Scholar] [CrossRef] [PubMed] - Hasan, A.H.; Grachev, A.N. Target tracking by adaptive EKF using fast genetic algorithm. Int. J. Inf. Eng.
**2014**, 4, 50–58. [Google Scholar] - Hegrenæs, Ø.; Hallingstad, O.; Gade, K. Towards model-aided navigation of underwater vehicles. Model. Identif. Control
**2007**, 28, 113–123. [Google Scholar] [CrossRef] - McColgan, J.; McGookin, W.E. Coordination of multiple biomimetic autonomous underwater vehicles using strategies based on the schooling behaviour of fish. Robotics
**2016**, 5, 1–23. [Google Scholar] [CrossRef] - Muhaiyuddin, M.D.; Hassan, Z.; Jia, C.; Zhou, H.; Pan, R.; Anvar, A.M. Modelling and simulation of the Autonomous Underwater Vehicle (AUV) robot. In Proceedings of the 21st International Congress on Modelling and Simulation, Gold Coast, Australia, 29 November–4 December 2015; pp. 739–745.
- Tian, S.; He, B.; Chen, S.; Liu, G. Application of Forward-Looking Scanning Sonar on Simultaneous Localization and Mapping for an Autonomous Underwater Vehicle; IACSIT Press: Singapore, 2012. [Google Scholar]
- Wang, S.; Chen, L.; Hu, H.; Gu, D. Single beacon based localization of AUVs using moving horizon. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, 3–7 November 2013; pp. 885–900.
- Wang, S.; Chen, L.; Hu, H.; Gu, D. An optimization based moving horizon estimation with application to localization of autonomous underwater vehicles. Robot. Auton. Syst.
**2014**, 62, 1581–1596. [Google Scholar] [CrossRef] - INS/GPS VectorNav VN-200 User Manual. Available online: http://www.vectornav.com (accessed on 1 April 2016).
- Electromagnetic Speed Log Alize Specification. Available online: http://www.benmarine.fr/files/ALIZE_en.pdf (accessed on 1 April 2016).
- Pressure Transmitter WIKA S-20. Available online: http://www.przetworniki24.pl/pl/katalog/przetworniki_ciNSienia/S_20 (accessed on 1 April 2016).
- USBL MicronNav System. Available online: http://www.tritech.co.uk/media/products/usbl-tracking-system-micronnav.pdf (accessed on 1 April 2016).
- Hydromodem Micron Data Modem. Available online: http://www.tritech.co.uk/product/micron-data-modem (accessed on 1 April 2016).
- Naus, K. Evaluation of accuracy the position of the vessel designated stereoscopic cameras system. Rep. Geod.
**2010**, 88, 79–88. [Google Scholar] - Naus, K. Accuracy in fixing ship’s positions by CCD camera survey of horizontal angles. Geomat. Environ. Eng.
**2011**, 5, 47–61. [Google Scholar] - Naus, K.; Waz, M. Accuracy in fixing ship’s positions by camera survey of bearings. Geod. Cartogr.
**2011**, 60, 61–73. [Google Scholar] [CrossRef] - Waz, M.; Naus, K. Ripping Radar Image with the Sea Map Using the Alignment Method; Logistyka, Institute of Logistics and Warehousing: Radom, Poland, 2010; pp. 2009–2019. [Google Scholar]
- Kiriy, E.; Buehler, M. Three-State Extended Kalman Filter for Mobile Robot Localization; Technical Report TR-CIM 05.07; McGill University: Montreal, QC, Canada, 2002; pp. 1–17. [Google Scholar]
- What is SBAS? Available online: http://egnos-portal.gsa.europa.eu/discover-egnos/about-egnos/what-sbas (accessed on 1 April 2016).

**Figure 1.**Side view of 3D model of the BAUV built in Polish Naval Academy (3D design by Bogdan Szturomski) [9].

**Figure 2.**The principle of determining the horizontal coordinates $\left(x,y\right)$ using USBL system.

**Figure 3.**Mapping the accuracy of the coordinate position of determined USBL system (describes mean error ${M}_{xy}$ in meters).

**Figure 5.**Mean error of coordinates as a function of distance covered (adopted $P\left(k=0\right)=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]$, SOG = 1 m/s, COG = 90°).

**Figure 13.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

**Figure 18.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (one hundred passages).

**Figure 20.**Histogram of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (one hundred passages).

**Figure 21.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

**Figure 22.**Graph distancing the reference position from the position determined by EKF and DR methods of the USBL system (single passage).

**Figure 23.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

**Figure 25.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (hundred passages).

**Figure 26.**Histogram of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (one hundred passages).

**Figure 27.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

**Figure 28.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

**Figure 29.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (single passage).

**Figure 31.**Graph of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (one hundred passages).

**Figure 32.**Histogram of the reference position distance from the position estimated by EKF and DR methods and determined by the USBL system (one hundred passages).

**Table 1.**Statistical parameters of positioning errors by EKF and DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.1 | 6.6 | 2.2 |

DR | 0.7 | 25.8 | 15.9 |

USBL | 0.3 | 75.2 | 22.6 |

**Table 4.**The statistical parameters of the positioning errors using EKF, DR methods and the USBL system (one hundred passages).

Positioning Method | Minimum Distance from the Reference Position (m) | Maximum Distance from the Reference Position (m) | Average Distance from the Reference Position (m) |
---|---|---|---|

EKF | 0.6 | 3.3 | 2.2 |

DR | 0.6 | 20.1 | 10.6 |

USBL | 1.3 | 57.0 | 23.4 |

**Table 5.**Statistical parameters of positioning errors using EKF and DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.2 | 7.3 | 3.4 |

DR | 0.7 | 34.9 | 10.9 |

USBL | 0.4 | 68.0 | 24.3 |

**Table 8.**Statistical parameters of positioning errors using EKF, DR methods and the USBL system (single passage).

Positioning Method | Minimum Distance to the Reference Position (m) | Maximum Distance to the Reference Position (m) | Average Distance to the Reference Position (m) |
---|---|---|---|

EKF | 0.7 | 4.4 | 2.9 |

DR | 0.7 | 19.1 | 10.8 |

USBL | 1.3 | 55.9 | 22.7 |

**Table 12.**Statistical parameters of positioning errors using EKF, DR methods and the USBL system (one hundred passages).

Positioning Method | Minimum Distance from the Reference Position (m) | Maximum Distance from the Reference Position (m) | Average Distance from the Reference Position (m) |
---|---|---|---|

EKF | 0.6 | 6.4 | 4.4 |

DR | 0.6 | 18.9 | 10.7 |

USBL | 4.8 | 43.0 | 20.5 |

© 2016 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC-BY) license (http://creativecommons.org/licenses/by/4.0/).