On AUV Control with the Aid of Position Estimation Algorithms Based on Acoustic Seabed Sensing and DOA Measurements

Abstract

This article discusses various approaches to the control of autonomous underwater vehicles (AUVs) with the aid of different velocity-position estimation algorithms. Traditionally this field is considered as the area of the extended Kalman filter (EKF) application: It became a universal tool for nonlinear observation models and its use is ubiquitous. Meanwhile, the specific characteristics of underwater navigation, such as an incomplete sets of measurements, constraints on the range metering or even impossibility of range measurements, observations provided by rather specific acoustic beacons, sonar observations, and other features seriously narrow the applicability of common instruments due to a high level of uncertainty and nonlinearity. The AUV navigation system, not being able to rely on a single source of position estimation, has to take into account all available information. This leads to the necessity of various complex estimation and data fusion algorithms, which are the matter of the present article. Here we discuss some approaches to the AUV position estimation such as conditionally minimax nonlinear filtering (CMNF) and unbiased pseudo-measurement filters (UPMFs) in conjunction with velocity estimation based on the seabed profile acoustic sensing. The presented estimation algorithms serve as a basis for a locally optimal AUV motion control algorithm, which is also presented.

1. Introduction

Navigation of AUVs (autonomous underwater vehicles), being essentially different from the navigation of UAVs (unmanned autonomous vehicles) due to the principal difference between the sources of navigation information, nevertheless shares several characteristic features with the latter. Indeed, in both cases, the INS (inertial navigation system) plays the main role in determining the position-velocity state of the vehicle. Navigation with the aid of INS is usually based on the dynamic model of the vehicle motion with inputs from various sensors of linear and angular velocities and uses filters (usually of Kalman type) for calculation of the position and attitude. However, all types of velocity sensors used in INS are subject to drift, which must be systematically compensated to keep the position estimation accuracy at an acceptable level. In UAV navigation such compensation may be accomplished with the aid of satellite positioning systems (GPS or GLONASS), or in the case of a so-called GPS denied environment, with the aid of specific video survey tools [1]. In an underwater environment, all electromagnetic (and therefore video tools) have a very restricted range of operation, and only acoustic waves permit sensing and communication for rather long distances. The problem is that acoustic measurements are notably barren in comparison with the electromagnetic ones: They do not have such features as color or contrast and just measure the direction of arrival (DOA) and/or the distance from the vehicle to the reflecting surface, such as obstacle, seabed, or ship. Therefore, even using multiple irradiating sensors like acoustic sonars, one can only obtain an image of distances to the seabed relief. An introduction of acoustical beacons with known positions can help to determine the own AUV position by using an angular observation filter, and this is the area of the (short and/or long) baseline methods application [2]. These methods are especially effective in the limited area of operation; moreover, they may be used in the case of a moving baseline established on a ship [3] and even in docking maneuvering [4]. Acoustic methods used in active mode give access to the distance measurements, which make the position estimation problem much easier, though in some areas of application such measurements are not acceptable or must be restricted due to the energetic or masking constraints [5].

The possibility of the sonar employment in AUV navigation has already been mentioned by some authors either in application to the situation, when an accurate seabed map is available, or in the case of presence of significant seabed features [6]. Moreover, the active pinging can be used to determine one’s speed over the seabed by tracking its features or by measuring the Doppler shift of returns [7], the same idea is used in the well-known DVL (Doppler velocity log) [8]. Moreover, acoustic sonars admit the usage of phased array technology, which permits to achieve the angular resolution up to 0.1° [9]. Angular measurements and particularly the bearing-only observations which are inherent to underwater acoustics lead to the necessity of special type algorithms for the position and velocity estimation in AUV navigation and tracking underwater targets [10]. All such algorithms are inevitably the modifications of the Kalman filter, especially in the problems dealing with the fusion of different sources of navigation information such as INS and DVL [11], INS and various geo fields, and INS and sonars [12,13]. In the present work, the approach to sonar terrain matching [14] will be investigated by analogy to the UAV optical flow. The principal difference from the standard Lucas-Kanade [15] algorithm is the presence of the AUV velocity components in observations only in implicit form, fortunately in a linear one, but the coefficients depend on the surface slope and must be estimated also via observation of the range profile. This means the problem is nonlinear and therefore the evaluation of the quality of the velocity estimation is possible via simulation only. Some preliminary results have already been obtained in [16,17]; below we discuss the extension of this approach.

The great majority of external navigation sensors provide nonlinear measurements of position- velocity parameters. That is the reason why in most cases the linear filtering theory does not apply. A common way to overcome this issue is the usage of various modifications of the Kalman filter, such as the extended Kalman filter (EKF) [18], particle filter (PF) [19], and unscented Kalman filter (UKF) [20]. However, careful analysis shows that all these filters, even if different from the viewpoint of complexity, give almost the same level of the AUV position estimation accuracy, especially in the case of bearing-only observations [21]. An alternative to traditional Kalman filtering is the conditionally optimal nonlinear filter proposed by V. S. Pugachev and developed by his successors into the conditionally minimax nonlinear filter (CMNF) [22]. This theory permits to develop the estimation procedure with given properties, for example, either with minimum dispersion or without bias, which is extremely important, for example, in target tracking based on bearing-only observations [23,24].

In acoustic position estimation and target tracking, the bearing-only observation plays the principal role. As it was observed many years ago, the bearing angles may be transformed into linear observation, but with bias [25]. This approach is called pseudo-measurements. In recent work, an unbiased modification of this method was proposed for UAV navigation based on radio beacons [26] and on a video recording of terrain objects with known coordinates [27].

In the present article, we consider new approaches to AUV navigation and control and compare them with traditional ones. Both the problems of control and position estimation are considered in the locally optimal formulation since the stochastic problems with nonlinear observations do not have explicit globally optimal solutions. The model is described in Section 2. In Section 3 the following problem of nominal trajectory is formulated as a control problem and its locally optimal solution is presented. Section 4 contains a position estimation algorithm based on dead reckoning with speed evaluated from the evolution of the “acoustic images”—the seabed distances registered with the aid of acoustic sonar. Section 5 is devoted to the filtering algorithms based on the dynamic model of the vehicle motion and DOA measurements. In Section 6 the performance of the locally optimal control based on the discussed filtering algorithms is evaluated through simulation.

2. The AUV Navigation System Model

Let the coordinates of the AUV at time instant $t_k$ be ${\mathbf{X}}_k=\mathbf{X}\left(t_k\right)={(X\left(t_k\right),Y\left(t_k\right),Z\left(t_k\right))}^T={(X_k,Y_k,Z_k)}^T$, where ${(·)}^T$ is the transposition operator. The sequence $t_k$, $k=0,\dots ,N$, $t_0=0$, $t_N=T$ is known, and the interval between the consequent time instants is assumed to be the same for the sake of simplicity: $t_{k+1}-t_k=\Delta t$, $k=0,\dots ,N-1$. The initial condition ${\mathbf{X}}_0$ is a random vector with known expectation ${\mathbf{m}}_0$ and covariance matrix ${\mathbf{S}}_0$, the distribution ${\mathbf{X}}_0\sim P^{{\mathbf{X}}_0}$ is assumed unknown. Denoting by $\mathcal{P}(\mathbf{m},\mathbf{S})$ the set of all distributions with expectation $\mathbf{m}$ and covariance $\mathbf{S}$, we can write $P^{{\mathbf{X}}_0}\in \mathcal{P}({\mathbf{m}}_0,{\mathbf{S}}_0)$.

Let the controlled motion model of the AUV be defined by the following vector equation:

$${\mathbf{X}}_{k+1}={\mathbf{X}}_k+{\mathbf{V}}_k\Delta t+{\mathbf{W}}_k,$$

(1)

where ${\mathbf{V}}_k$ is the vector of the AUV speed at the moment $t_k$ and ${\mathbf{W}}_k={(W_k^X,W_k^Y,W_k^Z)}^T$ is a sequence of independent and identically distributed (i.i.d.) random vectors with zero mean and covariance matrix ${\mathbf{S}}^W$ independent from the initial condition: ${\mathbf{W}}_k\sim P^{{\mathbf{W}}_k}\in \mathcal{P}(\mathbf{0},{\mathbf{S}}^W)$. The exact distribution of these vectors is assumed unknown.

The AUV speed vector is defined by its absolute value $V_k=\parallel {\mathbf{V}}_k\parallel$ and two angles ${\gamma }_k,{\theta }_k$ shown in Figure 1. The angle ${\gamma }_k$, is the angle between ${\mathbf{V}}_k$ and the horizontal plane $x0y$, and ${\theta }_k$ is the angle between the projection of ${\mathbf{V}}_k$ on the horizontal plane $x0y$ and the axis $0x$. Thus, the components of the speed vector are defined by ${\mathbf{V}}_k=V_k{(\cos {\gamma }_k\cos {\theta }_k,\cos {\gamma }_k\sin {\theta }_k,\sin {\gamma }_k)}^T$ and vector model given by Equation (1) can be rewritten in the form of a system of scalar equations:

$$\left\{\begin{array}{c}{X}_{k+1}=X_k+V_k\cos {\gamma }_k\cos {\theta }_k\Delta t+W_k^X,\hfill \\ Y_{k+1}=Y_k+V_k\cos {\gamma }_k\sin {\theta }_k\Delta t+W_k^Y,\hfill \\ Z_{k+1}=Z_k+V_k\sin {\gamma }_k\Delta t+W_k^Z.\hfill \end{array}\right.$$

(2)

The vector of three parameters ${\mathbf{u}}_k={({\gamma }_k,{\theta }_k,V_k)}^T$ that define the direction and the absolute value of the speed vector ${\mathbf{V}}_k={\mathbf{V}}_k\left({\mathbf{u}}_k\right)$ (and hence the whole dynamics of the AUV), is further called the control vector.

3. Locally Optimal AUV Path Control

We define the control problem for the AUV as a problem of following some predefined path ${\mathring{\mathbf{X}}}_k$, $k=0,\dots ,N$, which is given by the control sequence ${\mathring{\mathbf{u}}}_k={({\mathring{\gamma }}_k,{\mathring{\theta }}_k,{\mathring{V}}_k)}^T$ and calculated in the absence of any noise:

$$\left\{\begin{array}{c}{\mathring{X}}_{k+1}={\mathring{X}}_k+{\mathring{V}}_k\cos {\mathring{\gamma }}_k\cos {\mathring{\theta }}_k\Delta t,\hfill \\ {\mathring{Y}}_{k+1}={\mathring{Y}}_k+{\mathring{V}}_k\cos {\mathring{\gamma }}_k\sin {\mathring{\theta }}_k\Delta t,\hfill \\ {\mathring{Z}}_{k+1}={\mathring{Z}}_k+{\mathring{V}}_k\sin {\mathring{\gamma }}_k\Delta t.\hfill \end{array}\right.$$

(3)

We first consider the optimal control problem with full information, where the real coordinates of the AUV ${\mathbf{X}}_k$ and its speed ${\mathbf{V}}_k$ are known at all times $t_k$, $k=0,\dots ,N$.

The quality of the control at time instant $t_k$ is defined by the following criterion:

$$J\left({\mathbf{u}}_k\right)=\mathbf{E}\left\{{∥{\mathring{\mathbf{X}}}_{k+1}-{\mathbf{X}}_{k+1}\left({\mathbf{u}}_k\right)∥}^2\right\}=\mathbf{E}\left\{{\left({\mathring{X}}_{k+1}-X_{k+1}\right)}^2+{\left({\mathring{Y}}_{k+1}-Y_{k+1}\right)}^2+{\left({\mathring{Z}}_{k+1}-Z_{k+1}\right)}^2\right\}$$

(4)

The control ${\mathbf{u}}_k^{*}={({\gamma }_k^{*},{\theta }_k^{*},V_k^{*})}^T$ that minimizes Criterion (4) is called locally optimal since at any time instant $t_k$ it provides the least deviation from the nominal path on the next step $t_{k+1}$.

Lemma 1.

Let $\Delta {\mathring{\mathbf{X}}}_k=(\Delta {\mathring{X}}_k,\Delta {\mathring{Y}}_k,\Delta {\mathring{Z}}_k)={({\mathring{X}}_k-X_k,{\mathring{Y}}_k-Y_k,{\mathring{Z}}_k-Z_k)}^T$ denote the deviation of the real path of the AUV from the nominal one at the time instant $t_k$. Let also ${\mathring{\mathbf{V}}}_k={({\mathring{V}}_k^X,{\mathring{V}}_k^Y,{\mathring{V}}_k^Z)}^T={\mathring{V}}_k{(\cos {\mathring{\gamma }}_k\cos {\mathring{\theta }}_k,\cos {\mathring{\gamma }}_k\sin {\mathring{\theta }}_k,\sin {\mathring{\gamma }}_k)}^T$ denote the nominal speed vector and its components. Then the solution to the optimization problem:

$${\mathbf{u}}_k^{*}=\underset{{\mathbf{u}}_k}{\mathrm{argmin}}J\left({\mathbf{u}}_k\right)$$

(5)

is given by the following relations:

$$\begin{array}{c}{\theta }_k^{*}=\mathrm{atan}2(\Delta {\mathring{Y}}_k+\Delta t{\mathring{V}}_k^Y,\Delta {\mathring{X}}_k+\Delta t{\mathring{V}}_k^X),\hfill \\ {\gamma }_k^{*}=\mathrm{atan}2(\Delta {\mathring{Z}}_k+\Delta t{\mathring{V}}_k^Z,(\Delta {\mathring{X}}_k+\Delta t{\mathring{V}}_k^X)/\cos {\theta }_k^{*}),\hfill \\ V_k^{*}=∥\Delta {\mathring{\mathbf{X}}}_k+\Delta t{\mathring{\mathbf{V}}}_k∥/\Delta t,\hfill \end{array}$$

(6)

where $2(y,x)$ is the angle between the positive direction of axis $0x$ and the vector ${(x,y)}^T$.

The proof of Lemma 1 is given in Appendix B.

The relations for the optimal angles and the absolute value have straightforward geometrical interpretation. From Figure 2, it follows that the locally optimal control at the time instant $t_k$ determines the AUV movement speed ${\mathbf{V}}_k^{*}=V_k^{*}{(\cos {\gamma }_k^{*}\cos {\theta }_k^{*},\cos {\gamma }_k^{*}\sin {\theta }_k^{*},\sin {\gamma }_k^{*})}^T$ aiming towards the point of the nominal path ${\mathring{\mathbf{X}}}_{k+1}$ and possessing the magnitude $V_k^{*}$, which is necessary to reach that point at the next time instant $t_{k+1}$.

The last observation provides the justification for a control policy, which is derived from Relations (6) by substituting the real AUV coordinates ${\mathbf{X}}_k$ by an unbiased estimate ${\hat{\mathbf{X}}}_k$:

$$\begin{array}{c}{\hat{\theta }}_k^{*}=2({\mathring{Y}}_k-{\hat{Y}}_k+\Delta t{\mathring{V}}_k^Y,{\mathring{X}}_k-{\hat{X}}_k+\Delta t{\mathring{V}}_k^X),\hfill \\ {\hat{\gamma }}_k^{*}=2({\mathring{Z}}_k-{\hat{Z}}_k+\Delta t{\mathring{V}}_k^Z,({\mathring{X}}_k-{\hat{X}}_k+\Delta t{\mathring{V}}_k^X)/\cos {\theta }_k^{*}),\hfill \\ {\hat{V}}_k^{*}=∥{\mathring{\mathbf{X}}}_k-{\hat{\mathbf{X}}}_k+\Delta t{\mathring{\mathbf{V}}}_k∥/\Delta t\hfill \end{array}$$

(7)

The original optimal control problem for the nonlinear stochastic System (2) with incomplete information is rather hard for analysis. Moreover, it does not have an explicit solution even in the case of the quadratic criterion. In these circumstances, suboptimal control policies like Relations (7) seems reasonable substitution for the unattainable optimum.

Another feature of the optimal control in Relations (6) is that in general, it is unbounded, whereas for the real AUV navigation system the limits on the instantaneous course and speed change (or acceleration constraints) would be natural. Nevertheless, this does not affect the applicability of the estimation methods, which are the primary focus of this study. In presence of such constraints, optimization Problem (5) no longer has a solution in an explicit form, but since it is a well-known quadratic optimization problem with linear (box) constraints, the solution can be calculated using numerous effective numerical methods [28].

4. Position Estimation with Seabed Sensing

In this section, we present an approach to the position estimation based on dead reckoning with speed evaluated from the evolution of the “acoustic images”. Each image consists of the seabed distance measurements $L_k^{ij}=L({\mathbf{X}}_k,{\mathbf{u}}_k,{\gamma }^i,{\theta }^j)$ made at the same time instant $t_k$ by acoustic sensors aimed at different angles $({\gamma }^i,{\theta }^j)$ with respect to the AUV attitude, given by its movement direction ${\mathbf{u}}_k$. The information about the AUV position shift $\Delta {\mathbf{X}}_{k+1}={\mathbf{X}}_{k+1}-{\mathbf{X}}_k$, or the speed ${\mathbf{V}}_k$ is then derived from the difference between the values of correspondent distance measurements ${\mathbf{L}}_k^{ij}$ and ${\mathbf{L}}_{k+1}^{ij}$ made at the consecutive time instants $t_{k+1}$ and $t_k$. The way this is done is close to the Lucas-Kanade method for optical flow estimation [15] with the set of acoustic measurements treated as an image and a single distance to the seabed treated as a pixel intensity.

In detail, the model of acoustic measurements is presented in Figure 3. Let $({\gamma }^i,{\theta }^j)$, $i,j=1,\dots ,M$ be the set of aiming angles at which the sensors of the AUV emit the acoustic beams and acquire the distance to the seabed. These angles are defined with respect to the AUV attitude, which means that the absolute beam $(i,j)$ direction is given by the angles $({\gamma }_k+{\gamma }^i,{\theta }_k+{\theta }^j)$. Thus the coordinates of the point ${\mathbf{x}}_k^{ij}={(x_k^{ij},y_k^{ij},z_k^{ij})}^T$, where the beam $(i,j)$ reaches the seabed at the time instant $t_k$ are expressed as:

$${\mathbf{x}}_k^{ij}={\mathbf{X}}_k+L_k^{ij}{\mathbf{e}}_k,$$

(8)

where ${\mathbf{e}}_k={(e_k^X,e_k^Y,e_k^Z)}^T={(\cos ({\gamma }_k+{\gamma }^i)\cos ({\theta }_k+{\theta }^j),\cos ({\gamma }_k+{\gamma }^i)\sin ({\theta }_k+{\theta }^j),\sin ({\gamma }_k+{\gamma }^i))}^T$ defines the direction of the beam. It should be noted that this model differs from the one considered in [16,17] since it takes into account the controlled AUV attitude, whether in the mentioned works the beam directions were assumed constant. Note also, that the number of aiming angles ${\gamma }^i$ and ${\theta }^j$ could be assumed different with no effect on the further derivations.

Let the seabed profile be defined by the equation $\psi \left(\mathbf{x}\right)=0$, where $\psi (·)$ is some smooth function. Then considering $\mathbf{x}$ as a function of variables $\mathbf{X}$, L, and $\mathbf{e}$, each of which in turn depends on the time t:

$$\mathbf{x}\left(t\right)=\mathbf{X}\left(t\right)+L\left(t\right)\mathbf{e}\left(t\right),$$

and calculating the total derivative $\frac{d\psi \left(\mathbf{x}\right)}{dt}=\frac{d\psi \left(\mathbf{x}\right(\mathbf{X}\left(t\right),L\left(t\right),\mathbf{e}\left(t\right)\left)\right)}{dt}$, we get:

$$\frac{\delta \psi }{\delta x}\left(\frac{dX}{dt}+\frac{d{e}^X}{dt}L+e^X\frac{dL}{dt}\right)+\frac{\delta \psi }{\delta y}\left(\frac{dY}{dt}+\frac{d{e}^Y}{dt}L+e^Y\frac{dL}{dt}\right)+\frac{\delta \psi }{\delta z}\left(\frac{dZ}{dt}+\frac{d{e}^Z}{dt}L+e^Z\frac{dL}{dt}\right)=0,$$

(9)

where the actual functions’ arguments were omitted for the sake of simplicity. Assume that the partial derivatives of the function $\psi (·)$ are known at any point of the seabed which could be reached with an acoustic beam. Rewriting Equation (9) in discrete time with substitution of the differentials with corresponding increments:

$$\begin{array}{ccc}\hfill \Delta {\mathbf{X}}_{k+1}& =& {\mathbf{X}}_{k+1}-{\mathbf{X}}_k={(\Delta X_{k+1},\Delta Y_{k+1},\Delta Z_{k+1})}^T,\hfill \\ \hfill \Delta L_{k+1}^{ij}& =& L({\mathbf{X}}_{k+1},{\mathbf{u}}_{k+1},{\gamma }^i,{\theta }^j)-L({\mathbf{X}}_k,{\mathbf{u}}_k,{\gamma }^i,{\theta }^j),\hfill \\ \hfill \Delta {\mathbf{e}}_{k+1}& =& {(\Delta e_{k+1}^X,\Delta e_{k+1}^Y,\Delta e_{k+1}^Z)}^T\hfill \\ \hfill \Delta e_{k+1}^X& =& -\Delta {\gamma }_{k+1}\left(\sin ({\gamma }_k+{\gamma }^i)\cos ({\theta }_k+{\theta }^j)\right)-\Delta {\theta }_{k+1}\left(\cos ({\gamma }_k+{\gamma }^i)\sin ({\theta }_k+{\theta }^j)\right),\hfill \\ \hfill \Delta e_{k+1}^Y& =& -\Delta {\gamma }_{k+1}\left(\sin ({\gamma }_k+{\gamma }^i)\sin ({\theta }_k+{\theta }^j)\right)-\Delta {\theta }_{k+1}\left(\cos ({\gamma }_k+{\gamma }^i)\cos ({\theta }_k+{\theta }^j)\right),\hfill \\ \hfill \Delta e_{k+1}^Z& =& \Delta {\gamma }_{k+1}\left(\cos ({\gamma }_k+{\gamma }^i)\right),\hfill \\ \hfill \Delta {\gamma }_{k+1}& =& {\gamma }_{k+1}-{\gamma }_k,\hfill \\ \hfill \Delta {\theta }_{k+1}& =& {\theta }_{k+1}-{\theta }_k,\hfill \end{array}$$

(10)

we have the following equation:

$$\begin{array}{c}\hfill \frac{\delta \psi }{\delta x}({\mathbf{x}}_k^{ij})\left(\Delta X_{k+1}+\Delta e_{k+1}^X{L}_k^{ij}+e_k^X\Delta L_{k+1}^{ij}\right)\\ \hfill +\frac{\delta \psi }{\delta y}({\mathbf{x}}_k^{ij})\left(\Delta Y_{k+1}+\Delta e_{k+1}^Y{L}_k^{ij}+e_k^Y\Delta L_{k+1}^{ij}\right)\\ \hfill +\frac{\delta \psi }{\delta z}({\mathbf{x}}_k^{ij})\left(\Delta Z_{k+1}+\Delta e_{k+1}^Z{L}_k^{ij}+e_k^Z\Delta L_{k+1}^{ij}\right)& =& 0,\hfill \end{array}$$

which in turn can be expressed with unknowns $\Delta X_{k+1}$, $\Delta Y_{k+1}$, $\Delta Z_{k+1}$ collected on the left-hand side:

$$\frac{\delta \psi }{\delta x}({\mathbf{x}}_k^{ij})\Delta X_{k+1}+\frac{\delta \psi }{\delta y}({\mathbf{x}}_k^{ij})\Delta Y_{k+1}+\frac{\delta \psi }{\delta z}({\mathbf{x}}_k^{ij})\Delta Z_{k+1}=B_k^{ij},$$

(11)

with

$$\begin{array}{ccc}\hfill B_k^{ij}& =& -\frac{\delta \psi }{\delta x}({\mathbf{x}}_k^{ij})\left(\Delta e_{k+1}^X{L}_k^{ij}+e_k^X\Delta L_{k+1}^{ij}\right)\hfill \\ & & -\frac{\delta \psi }{\delta y}({\mathbf{x}}_k^{ij})\left(\Delta e_{k+1}^Y{L}_k^{ij}+e_k^Y\Delta L_{k+1}^{ij}\right)\hfill \\ & & -\frac{\delta \psi }{\delta z}({\mathbf{x}}_k^{ij})\left(\Delta e_{k+1}^Z{L}_k^{ij}+e_k^Z\Delta L_{k+1}^{ij}\right).\hfill \end{array}$$

(12)

The unknowns $\Delta X_{k+1}$, $\Delta Y_{k+1}$, $\Delta Z_{k+1}$ can not be derived from a single equation, but assuming that these shifts are the same for all the acoustic sensors, we have a set of Equation (11) for different aiming angle values $({\gamma }^i,{\theta }^j)$, $i,j=1,\dots ,M$. Finally it is possible to estimate the unknowns with the least-squares method:

$$\Delta {\hat{\mathbf{X}}}_{k+1}=\underset{\Delta {\mathbf{X}}_{k+1}}{\mathrm{argmin}}\sum _{i,j=1}^M{\left(\frac{\delta \psi }{\delta x}({\mathbf{x}}_k^{ij})\Delta X_{k+1}+\frac{\delta \psi }{\delta y}({\mathbf{x}}_k^{ij})\Delta Y_{k+1}+\frac{\delta \psi }{\delta z}({\mathbf{x}}_k^{ij})\Delta Z_{k+1}-B_k^{ij}\right)}^2$$

(13)

Vectorization of Equation (11) with respect to the set of aiming angles $({\gamma }^i,{\theta }^j)$, $i,j=1,\dots ,M$ gives:

$${\mathbf{A}}_k\Delta {\mathbf{X}}_{k+1}={\mathbf{B}}_k,$$

where ${\mathbf{A}}_k$ and ${\mathbf{B}}_k$ are made of vertically stacked row-vectors and values corresponding to the individual observations:

$${\mathbf{A}}_k=\left(\begin{array}{ccc}\frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{11}\right)& \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{11}\right)& \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{11}\right)\\ ⋮& ⋮& ⋮\\ \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{1M}\right)& \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{1M}\right)& \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{1M}\right)\\ ⋮& ⋮& ⋮\\ \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{MM}\right)& \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{MM}\right)& \frac{\delta \psi }{\delta x}\left({\mathbf{x}}_k^{MM}\right)\end{array}\right),{\mathbf{B}}_k=\left(\begin{array}{c}{B}_k^{11}\\ ⋮\\ B_k^{1M}\\ ⋮\\ B_k^{MM}\end{array}\right).$$

(14)

Now the solution to the least-squares optimization Problem (13) can be expressed in the standard form:

$$\Delta {\hat{\mathbf{X}}}_{k+1}={\left[{\mathbf{A}}_k^T{\mathbf{A}}_k\right]}^{-1}{\mathbf{A}}_k^T{\mathbf{B}}_k.$$

(15)

Finally the AUV position dead reckoning estimate is:

$${\hat{\mathbf{X}}}_{k+1}={\hat{\mathbf{X}}}_k+\Delta {\hat{\mathbf{X}}}_{k+1}.$$

(16)

It should be noted that Equation (16) requires the values of the partial derivatives of the function $\psi (·)$ at the points ${\mathbf{x}}_k^{ij}$, $k=0,\dots ,N$, $i,j=1,\dots ,M$, where the acoustic beams reach the seabed. Precise calculation of these values even in the case of known seabed profile is not possible since it requires the precise values of the points ${\mathbf{x}}_k^{ij}$, which depend on the unavailable precise AUV position ${\mathbf{X}}_k$ due to Equation (8). The way to overcome this issue is to use the acoustic image data to estimate the seabed slope, i.e., the required partial derivatives of $\psi (·)$. In Appendix A, we present a method of estimation, based on the approximation of the slope function $\psi (·)$, which requires only the values of seabed distances $L_k^{ij}$. Another method is proposed in [16].

The following algorithm summarizes the proposed acoustic seabed sensing AUV position estimation method:

• at time instant $t_{k+1}$ collect the seabed distance measurements $L_{k+1}^{ij}$ from the acoustic sensors $i,j=1,\dots ,M$ and calculate the increments $\Delta L_{k+1}^{ij}=L_{k+1}^{ij}-L_k^{ij}$;
• using the control values on the current $({\gamma }_{k+1},{\theta }_{k+1})$ and the previous $({\gamma }_k,{\theta }_k)$ steps calculate the increments ${(\Delta e_{k+1}^X,\Delta e_{k+1}^Y,\Delta e_{k+1}^Z)}^T$ according to Equation (10);
• evaluate the slope estimates $\widehat{\frac{\delta \psi }{\delta x}}({\mathbf{x}}_k^{ij})$, $\widehat{\frac{\delta \psi }{\delta y}}({\mathbf{x}}_k^{ij})$, $\widehat{\frac{\delta \psi }{\delta z}}({\mathbf{x}}_k^{ij})$ using Equation (A1) or another method;
• using Equations (12) and (14) calculate the matrix ${\mathbf{A}}_k$ and the vector ${\mathbf{B}}_k$;
• calculate the estimate of the AUV position shift $\Delta {\hat{\mathbf{X}}}_{k+1}$ with Equation (15) and the position estimate ${\hat{\mathbf{X}}}_{k+1}$ with Equation (16).

5. Position Estimation with DOA Measurements

In this section we discuss various position estimation algorithms based on the dynamic model of the vehicle motion, Equation (1), and the external bearing-only measurements provided by a passive acoustic DOA estimation device (pressure hydrophone or acoustic vector sensor array) [6].

The position of the acoustic source ${\mathbf{X}}_B={(X_B,Y_B,Z_B)}^T$ is assumed to be known and constant which is the case of a pre-deployed stationary acoustic beacon. It is assumed that at any time instant $t_k$ the bearing vector (Figure 4) is available for observation in the following form:

$${\mathbf{Y}}_k=\left(\begin{array}{c}\tan {\phi }_k\\ \tan {\lambda }_k\end{array}\right),\begin{array}{c}{\displaystyle \tan {\phi }_k=\frac{Y_B-Y_k}{X_B-X_k}+{\epsilon }_k^{\phi },}\hfill \\ {\displaystyle \tan {\lambda }_k=\frac{(Z_B-Z_k)\cos {\phi }_k}{X_B-X_k}+{\epsilon }_k^{\lambda },}\hfill \end{array}$$

(17)

where ${\mathbf{E}}_k={({\epsilon }_k^{\phi },{\epsilon }_k^{\lambda })}^T\sim P^{{\mathbf{E}}_k}\in \mathcal{P}(\mathbf{0},{\mathbf{S}}^{\epsilon })$ is a sequence of i.i.d. random vectors independent from ${\mathbf{W}}_k$ and ${\mathbf{X}}_0$. The exact distribution of these vectors, as in the case of the noise in the Model (2), is assumed unknown.

Systems (2) and (17) can be expressed in the following general vector form:

$$\begin{array}{c}{\mathbf{X}}_{k+1}={\mathsf{\Phi }}_k({\mathbf{X}}_k,{\mathbf{u}}_k)+{\mathbf{W}}_k,\hfill \\ {\mathbf{Y}}_k={\mathsf{\Psi }}_k\left({\mathbf{X}}_k\right)+{\mathbf{E}}_k.\hfill \end{array}$$

(18)

The common way to deal with the nonlinear System (18) is the extended Kalman filter. In this section we present alternative approaches, which in some cases allow to achieve better estimation quality then by direct system linearization. In Section 5.1, we present an observation Equation (17) transformation, allowing to reduce the original problem to a form, where the optimal filtering solution is also available in the form of the Kalman filter. In Section 5.2, we provide the conditionally minimax filtering problem statement and solution for the original nonlinear model of Equation (18) and show how this particular filtering approach allows data fusion from the DOA measurements and the dead reckoning navigation system based on the acoustic seabed sensing from Section 4.

5.1. Pseudo Measurements Filter

Rewriting the observations of Equation (17) in the following form:

$$\begin{array}{c}{\displaystyle (X_B-X_k)\sin {\phi }_k=(Y_B-Y_k)\cos {\phi }_k+{\epsilon }_k^{\phi }(X_B-X_k)\cos {\phi }_k,}\hfill \\ {\displaystyle (X_B-X_k)\sin {\lambda }_k=(Z_B-Z_k)\cos {\phi }_k\cos {\lambda }_k+{\epsilon }_k^{\lambda }(X_B-X_k)\cos {\lambda }_k,}\hfill \end{array}$$

and gathering all the known or measured values at the left-hand side, we get:

$$\begin{array}{c}{\displaystyle X_B\sin {\phi }_k-Y_B\cos {\phi }_k=X_k\sin {\phi }_k-Y_k\cos {\phi }_k+{\epsilon }_k^{\phi }(X_B-X_k)\cos {\phi }_k,}\hfill \\ {\displaystyle X_B\sin {\lambda }_k-Z_B\cos {\phi }_k\cos {\lambda }_k=X_k\sin {\lambda }_k-Z_k\cos {\phi }_k\cos {\lambda }_k+{\epsilon }_k^{\lambda }(X_B-X_k)\cos {\lambda }_k.}\hfill \end{array}$$

(19)

The right-hand side in the previous expression is linear with respect to the system state ${\mathbf{X}}_k={(X_k,Y_k,Z_k)}^T$, but at the same time, it involves the additive noise, whose covariance is now state-dependent. Denote:

$${\mathbf{Y}}_k^{\prime }=\left(\begin{array}{c}{\displaystyle X_B\sin {\phi }_k-Y_B\cos {\phi }_k}\hfill \\ {\displaystyle X_B\sin {\lambda }_k-Z_B\cos {\phi }_k\cos {\lambda }_k}\hfill \end{array}\right)$$

and rewrite Equation (19) in the vector form:

$${\mathbf{Y}}_k^{\prime }={\mathsf{\Psi }}_k^1{\mathbf{X}}_k+{\mathsf{\Psi }}_k^2{\mathbf{E}}_k,$$

(20)

where

$$\begin{array}{c}{\mathsf{\Psi }}_k^1={\mathsf{\Psi }}_k^1\left({\mathbf{Y}}_k\right)=\left(\begin{array}{ccc}{\displaystyle \sin {\phi }_k}& -\cos {\phi }_k& 0\\ {\displaystyle \sin {\lambda }_k}& 0& -\cos {\phi }_k\cos {\lambda }_k\end{array}\right)\\ {\mathsf{\Psi }}_k^2={\mathsf{\Psi }}_k^2({\mathbf{X}}_k,{\mathbf{Y}}_k)=\left(\begin{array}{cc}{\displaystyle (X_B-X_k)\cos {\phi }_k}& 0\\ {\displaystyle 0}& (X_B-X_k)\cos {\lambda }_k\end{array}\right)\end{array}$$

The introduction of ${\mathbf{Y}}_k^{\prime }$, which is called pseudo-measurements, is a common method of nonlinear systems analysis [21,25]. The idea is based on the fact that linear Kalman filtering estimate is the linear-optimal one in the case of linear observations and system dynamics and on the assumption that the noise ${\mathsf{\Psi }}_k^2{\mathbf{E}}_k$ covariance, even being state-dependent, may be evaluated or replaced by an upper bound. In our previous work [26] the filtering algorithm based on the pseudo-measurements has been suggested and its recurrence relations has been derived from the assumption of the unbiasedness of the estimate on the previous step. As applied to the problem at hand these relations are as follows:

$$\begin{array}{c}{\tilde{\mathbf{X}}}_k={\mathsf{\Phi }}_{k-1}({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1}),\hfill \\ {\tilde{\mathbf{K}}}_k={\hat{\mathbf{K}}}_{k-1}+{\mathbf{S}}^W,\hfill \\ {\overline{\mathbf{K}}}_k={\tilde{\mathbf{K}}}_k{{\mathsf{\Psi }}_k^1}^T(Y_k){\left({\mathsf{\Psi }}_k^1(Y_k){\tilde{\mathbf{K}}}_k{{\mathsf{\Psi }}_k^1}^T(Y_k)+{\mathsf{\Psi }}_k^2({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k){\mathbf{S}}^{\epsilon }{{\mathsf{\Psi }}_k^2}^T({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)\right)}^{+}\hfill \\ {\hat{\mathbf{X}}}_k={\tilde{\mathbf{X}}}_k+{\overline{\mathbf{K}}}_k\left({\mathbf{Y}}_k^{\prime }-{\mathsf{\Psi }}_k^1{\mathbf{X}}_k\right)\hfill \\ {\hat{\mathbf{K}}}_k=(\mathbf{I}-{\overline{\mathbf{K}}}_k{\mathsf{\Psi }}_k^1\left(Y_k\right)){\tilde{\mathbf{K}}}_k\hfill \end{array}$$

(21)

This filter has the common Kalman structure with items recurrently calculated upon the bearing measurements update. The difference with the classic case is that here the Riccati equation is not solvable a priori, because it involves estimate-dependent terms and current measurement values.

5.2. Conditionnaly Minimax Nonlinear Filter

Let the functions ${\mathsf{\Phi }}_k(·,·)$, ${\mathsf{\Psi }}_k(·)$ and the feedback control ${\mathbf{u}}_k={\mathbf{u}}_k({\mathbf{Y}}_0,\dots ,{\mathbf{Y}}_k)$ be such that the first and second moments of the state ${\mathbf{X}}_k$ and observation ${\mathbf{Y}}_k$ are finite. Introduce two sets of functions: Basic prediction ${\alpha }_k(x,u)$ and basic correction functions ${\beta }_k(x,y)$. Then the CMNF estimate is defined by the following recurrent relations:

$$\begin{array}{cc}{\tilde{\mathbf{X}}}_k=F_k{\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1})+f_k,\hfill & F_k=\mathrm{cov}({\mathbf{X}}_k,{\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1})){\mathrm{cov}}^{+}({\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1}),{\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1})),\hfill \\ & f_k=\mathbf{E}\left\{{\mathbf{X}}_k\right\}-F_k\mathbf{E}\left\{{\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1})\right\}\hfill \\ {\hat{\mathbf{X}}}_k={\tilde{\mathbf{X}}}_k+H_k{\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)+h_k,\hfill & H_k=\mathrm{cov}({\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k,{\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)){\mathrm{cov}}^{+}({\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k),{\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)),\hfill \\ & h_k=-H_k\mathbf{E}\left\{{\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)\right\},\hfill \end{array}$$

(22)

where $\mathrm{cov}(\mathbf{x},\mathbf{y})$ is the covariance matrix of two random vectors $\mathbf{x}$, $\mathbf{y}$, ${\mathbf{A}}^{+}$ denotes matrix pseudo-inversion, and ${\hat{\mathbf{X}}}_0={\mathbf{m}}_0$.

In the case when the functions ${\alpha }_k(·,·)$ and ${\beta }_k(·,·)$ have first and second order moments for all the random arguments in Equation (22), then the CMNF estimate exists and has the following minimax property. Let ${\hat{\mathbf{X}}}_{k-1}$ be the CMNF estimate at the time instant $t_{k-1}$. Then the linear functions ${\mathcal{F}}_k^{*}\left(\xi \right)=F_k\xi +f_k$, ${\mathcal{H}}_k^{*}\left(\zeta \right)=H_k\zeta +h_k$ defined by Equation (22) provide the solution for the following minimax optimization problem:

$$\begin{array}{c}{\displaystyle {\mathcal{F}}_k^{*}(·)=\underset{{\mathcal{F}}_k(·)}{\mathrm{argmin}}\underset{P_k^{\prime }}{\max }\mathbf{E}\left\{\parallel {\mathcal{F}}_k({\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1}))-{\mathbf{X}}_k{\parallel }^2\right\},}\hfill \\ {\displaystyle {\mathcal{H}}_k^{*}(·)=\underset{{\mathcal{H}}_k(·)}{\mathrm{argmin}}\underset{P_k^{″}}{\max }\mathbf{E}\left\{\parallel {\mathcal{H}}_k({\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k))-({\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k){\parallel }^2\right\},}\hfill \end{array}$$

(23)

where $P_k^{\prime }\in \mathcal{P}(\mathbf{E}\left\{{\mathcal{Z}}_k^{\prime }\right\},\mathrm{cov}({\mathcal{Z}}_k^{\prime },{\mathcal{Z}}_k^{\prime }))$—the set of all possible distributions of the compound vector ${\mathcal{Z}}_k^{\prime }={({\mathbf{X}}_k,{\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1}))}^T$ and $P_k^{″}\in \mathcal{P}(\mathbf{E}\left\{{\mathcal{Z}}_k^{″}\right\},\mathrm{cov}({\mathcal{Z}}_k^{″},{\mathcal{Z}}_k^{″}))$—the set of all possible distributions of the compound vector ${\mathcal{Z}}_k^{″}={({\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k,{\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k))}^T$.

The details on the CMNF approach to the nonlinear stochastic systems state estimation, including the thorough justification of Equation (22) being the solution to Equation (23) and the conditions of the solution in Equation (23) existence, could be found in [22]. Further application of the concept along with the comparative numerical study is the matter of the works [23,24].

The fact that the CMNF estimate, Equation (22), is the solution to the minimax problem, Equation (23), means that at each time instant $t_k$ it delivers the minimum for the worst case (with respect to the a priori uncertainty in the distributions $P_k^{\prime }$ and $P_k^{″}$) of the mean-square error of the prediction ${\tilde{\mathbf{X}}}_k$ and correction ${\hat{\mathbf{X}}}_k$. It should be noted that both ${\tilde{\mathbf{X}}}_k$ and ${\hat{\mathbf{X}}}_k$ are unbiased estimates of ${\mathbf{X}}_k$ and the quality of these estimates is a priori known:

$$\begin{array}{c}{\displaystyle \mathrm{cov}({\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k,{\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k)=\mathrm{cov}({\mathbf{X}}_k,{\mathbf{X}}_k)-F_k\mathrm{cov}({\alpha }_k({\hat{\mathbf{X}}}_{k-1},{\mathbf{u}}_{k-1}),{\mathbf{X}}_k),}\hfill \\ {\displaystyle \mathrm{cov}({\mathbf{X}}_k-{\hat{\mathbf{X}}}_k,{\mathbf{X}}_k-{\hat{\mathbf{X}}}_k)=\mathrm{cov}({\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k,{\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k)-H_k\mathrm{cov}({\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k),{\mathbf{X}}_k-{\tilde{\mathbf{X}}}_k).}\hfill \end{array}$$

Though the CMNF filter for a nonlinear stochastic system in a rather general form, Equation (18), which indeed covers the AUV navigation problem at hand, is fully defined by Equation (6), there are still two questions which need to be discussed in order to clarify all the aspects of the CMNF application on practice: The functions $\alpha (x,u)$, $\beta (x,y)$, and the calculation of the covariances in Equation (22).

The basic prediction and correction functions $\alpha (x,u)$, $\beta (x,y)$ define the structure of the CMNF filter. Their choice is model-specific and it can reflect particular features of the nonlinear functions ${\mathsf{\Phi }}_k(·,·)$, ${\mathsf{\Psi }}_k(·)$. The common option nevertheless is the prediction “by virtue of the system” and the correction in the form of the residual, which in the case of Systems (2) and (17) with $\mathbf{E}\left\{{\mathbf{W}}_k\right\}=0$ and $\mathbf{E}\left\{{\mathbf{E}}_k\right\}=0$ are as follows:

$$\begin{array}{c}{\alpha }_{k+1}({\hat{\mathbf{X}}}_k,{\mathbf{u}}_k)={\mathsf{\Phi }}_k({\hat{\mathbf{X}}}_k,{\mathbf{u}}_k)={\hat{\mathbf{X}}}_k+{\mathbf{V}}_k({\mathbf{u}}_k)\Delta t,\hfill \\ {\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)={\mathbf{Y}}_k-{\mathsf{\Psi }}_k({\tilde{\mathbf{X}}}_k).\hfill \end{array}$$

Unlike the EKF and pseudo-measurements filtering, the CMNF approach provides a natural way of data fusion from the INS and external measurements: The basic prediction function ${\alpha }_{k+1}({\hat{\mathbf{X}}}_k,{\mathbf{u}}_k)$ can be chosen in the form of the estimate from the internal navigation System (16). Finally, the CMNF estimate, which is based on the data fusion from the dead reckoning seabed sensing and external bearing-only measurements is defined by Equation (22) with structure functions

$$\begin{array}{c}{\alpha }_{k+1}({\hat{\mathbf{X}}}_k,{\mathbf{u}}_k)={\hat{\mathbf{X}}}_k+\Delta {\hat{\mathbf{X}}}_{k+1},\hfill \\ {\beta }_k({\tilde{\mathbf{X}}}_k,{\mathbf{Y}}_k)={\mathbf{Y}}_k-\left(\begin{array}{c}{\displaystyle \frac{Y_B-{\tilde{Y}}_k}{X_B-{\tilde{X}}_k}}\\ {\displaystyle \frac{Z_B-{\tilde{Z}}_k}{\sqrt{{(X_B-{\tilde{X}}_k)}^2+{(Y_B-{\tilde{Y}}_k)}^2}}}\end{array}\right),\hfill \end{array}$$

where the shift $\Delta {\hat{\mathbf{X}}}_{k+1}$ is defined by Equation (15) and the corresponding algorithm from the Section 4.

The only question left is the covariance matrices, which are necessary for the calculation of the linear estimator coefficients in Equation (22). The general CMNF approach implies that instead of the real covariances one uses their estimates, obtained by means of the Monte-Carlo sampling.

6. AUV Control Simulation

In this section, we provide the results of the numerical simulation of the described filtering and control algorithms. We compare the results of the simulation in a case when the nominal AUV speed ${\mathbf{V}}_k$ in the motion dynamic Equation (2) is known exactly and when it is evaluated by an external estimator—seabed sensing algorithm from Section 4.

The nominal trajectory, i.e., the path, which AUV has to follow, is given by the Equation (3) with ${\mathring{\gamma }}_k=\frac{\pi }{100}\cos (\frac{\pi }{100}{t}_k)$, ${\mathring{\theta }}_k=\frac{\pi }{3}\cos (\frac{\pi }{25}{t}_k)$, where $k=0,\dots ,N$, $t_0=0$, $t_N=T=300$ and $\Delta t=t_{k+1}-t_k=1$ second. The absolute value of the AUV speed is constant: ${\mathring{\mathbf{V}}}_k=2 \mathrm{m}/\mathrm{s}$ for all k and the initial condition is ${\mathring{\mathbf{X}}}_0={(0,0,-10)}^T$ (hereinafter all distances are measured in meters). The nominal path of the AUV is shown in Figure 5 and Figure 6 in 3D and component-wise respectively.

It should be noted that this particular path is chosen for modeling purposes only: There was no aim to reflect any specific real-world AUV mission, but simply to get the variation of the components in different scale: ${\mathring{X}}_k\sim 100$ m, ${\mathring{Y}}_k\sim 10$ m, ${\mathring{Z}}_k\sim 1$ m.

The sample paths of the system are simulated using the Equation (2), where the disturbances ${\mathbf{W}}_k$ are i.i.d. Gaussian vectors with zero mean and covariance ${\mathbf{S}}^W$ equal to the identity matrix ${\mathbf{I}}^{3\times 3}$: ${\mathbf{W}}_k\sim \mathcal{N}(\mathbf{0},{\mathbf{I}}^{3\times 3})$. The initial condition is supposed to have the same distribution: ${\mathbf{X}}_0\sim \mathcal{N}(\mathbf{0},{\mathbf{I}}^{3\times 3})$.

In this simulation, it is assumed that on the sea surface there is a set of four sources of acoustic signals (beacons) with known coordinates $X_B^i$, $i=1,\dots ,4$: $X_B^1={(500,100,0)}^T$, $X_B^2={(500,-100,0)}^T$, $X_B^3={(-100,100,0)}^T$, $X_B^4={(-100,-100,0)}^T$. These coordinates are chosen so that the situation of the bearing angles ${\varphi }_k$, ${\lambda }_k$ close to $\pm \frac{\pi }{2}$ would not be possible and there would be no need in the regularization of the observations in Equation (17). The beacons’ positions with respect to the AUV nominal path are shown in Figure 5. Note that despite the fact that in all the previous considerations a single beacon was assumed, the transition to the multiple observations case is rather obvious and requires only vertical stacking of correspondent values in the original Equations (17) and (18) and pseudo-measurements of Equations (19) and (20).

The precision of the bearing angles measurements is assumed to be equal to 0.5°, which in this simulation means that the noise in Equation (17) is Gaussian with the distribution ${\mathbf{E}}_k={({\epsilon }_k^{\phi },{\epsilon }_k^{\lambda })}^T\sim \mathcal{N}(\mathbf{0},{\tan }^2\left({0.5}^{\circ }\right){\mathbf{I}}^{2\times 2})$.

The control sequence is defined by System (7) with position estimates ${\hat{\mathbf{X}}}_k^{CMNF}$, ${\hat{\mathbf{X}}}_k^{UPMF}$ and ${\hat{\mathbf{X}}}_k^{EKF}$ given by:

• $CMNF$: Conditionally minimax nonlinear Filter (22) with basic structure Functions (24) and covariances calculated by the Monte-Carlo sampling on a separate test set of ${10}^5$ AUV paths;
• $UPMF$: Unbiased pseudo-measurements Filter (21);
• $EKF$: Extended Kalman filter, which provides a common approach to deal with the non-linearity in the system dynamics or observations [18].

The estimates and the correspondent random processes of state evolution (and their samples) are further referenced as ${\hat{\mathbf{X}}}_k^F$ and ${\mathbf{X}}_k^F={\mathbf{X}}_k({\mathbf{u}}_k^{*}({\hat{\mathbf{X}}}_k^F))$ respectively, where $F\in \{CMNF,UPMF,EKF\}$.

The simulation is done for two cases of the prediction ${\tilde{\mathbf{X}}}_{k+1}$ used by all three filtering algorithms:

• prediction by virtue of System (18): ${\tilde{\mathbf{X}}}_{k+1}={\mathsf{\Phi }}_k({\hat{\mathbf{X}}}_k,{\mathbf{u}}_k^{*}({\hat{\mathbf{X}}}_k))={\hat{\mathbf{X}}}_k+{\mathbf{V}}_k({\mathbf{u}}_k^{*}({\hat{\mathbf{X}}}_k))\Delta t$
• prediction based on the acoustic measurements: ${\tilde{\mathbf{X}}}_{k+1}={\hat{\mathbf{X}}}_k+\Delta {\hat{\mathbf{X}}}_{k+1}$, with $\Delta {\hat{\mathbf{X}}}_{k+1}$ defined in Equation (15).

The former takes advantage of the exact values of the AUV speed, while the latter uses an estimate of the AUV position shift.

For the position estimation with seabed sensing simulation, the following setting is used. The direction of the $i,k$-th measurement beam from Equation (8) uniformly varies in ranges ${\gamma }_i\in ({42}^{\circ },{82}^{\circ })$, ${\theta }_k\in (-{10}^{\circ },{30}^{\circ })$, $i,k=1,\dots ,8$. The accuracy of the range measurement is assumed to be 0.1 m, which corresponds to the sonar’s accuracy for the depths of ∼10 m. The unknown seabed profile is defined by the equation $\psi (\mathbf{x})=0$, where $\psi (\mathbf{x})$ is the following smooth harmonic function:

$$\psi (\mathbf{x})=a_0+z+\sum _{l=1}^K\left(a_l\sin \left(\frac{2\pi l}{P_x}x\right)+b_l\cos \left(\frac{2\pi l}{P_x}x\right)+c_l\sin \left(\frac{2\pi l}{P_y}y\right)+d_l\cos \left(\frac{2\pi l}{P_y}y\right)\right),$$

(24)

where $K=2$, the parameters $a_0=30.0$ m, $P_x=P_y=20.0$ are fixed, and the coefficients $a_l$, $b_l$, $c_l$ and $d_l$ are randomly generated for each trajectory using the standard uniform distribution.

In Figure 7, we show sample paths of the system for three mentioned state estimators and the nominal trajectory which they intended to follow. In Figure 8, we show the estimate error mean and standard deviation calculated on the set of ${10}^5$ sample paths.

The simulation shows almost equal performance of all three filters. This can be explained by the fact that the chosen acoustic sources positions provide the observations close to linear and in this case all the filters at hand act like the Kalman filter, which is optimal for linear systems.

This, nevertheless, can not serve as a justification for the EKF supreme applicability. In Figure 9, we present the results for the same setting, but with one beacon moved closer to the AUV starting position $X_0$, namely $X_B^4={(-5,-5,0)}^T$ and for a shorter time period equal to 10 s. In this situation the linearization errors play more significant role and hence the EKF demonstrates divergence, while the other two filters remain stable.

In the final simulation we aim to demonstrate how inaccurate description of the system dynamic can dramatically affect the quality of the estimation and hence the control of the UAV. To that end we apply the same three filters used in the previous simulations but now with inaccurate prediction provided by the acoustic measurements. In Figure 10 we show sample paths for the acoustic seabed sensing predictor. The control based on the extended Kalman filter and the pseudo-measurements filter fail to follow the nominal path. In Figure 11, we show the results for the conditionally minimax nonlinear filter based on the data fusion from the dead reckoning seabed sensing and external bearing-only measurements defined by Equation (22) with structure Functions (24). The increase of the sampled standard deviation shown in Figure 11 in comparison with the prediction by virtue of the system case shown in Figure 8 is not very high. It should be noted, that according to the simulation CMNF exhibits a regular bias. This bias is inherited from the acoustic seabed shift estimate which is also known to have a bias conforming to the nominal path [16].

As a final assessment of the estimate/control performance we provide the sample values of the uniform version of Criterion (4):

$${\overline{J}}^F=\underset{k=0,\dots ,N}{max}J({\mathbf{u}}_k^{*}({\hat{\mathbf{X}}}_k^F))=\underset{k=0,\dots ,N}{max}\mathbf{E}\left\{{∥{\mathring{\mathbf{X}}}_k-{\mathbf{X}}_k^F∥}^2\right\}$$

The sampling is performed on the same sets of paths as for the calculation of the sample mean and standard deviation presented in Figure 8 and Figure 11. The uniform performance criterion sample values are presented in

Table 1. Uniform performance criterion sample values ${\overline{J}}^F$, for $F\in \{CMNF,UPMF,EKF\}$.

Prediction	CMNF	UPMF	EKF
${\tilde{\mathbf{X}}}_{k+1}={\widehat{\mathbf{X}}}_k+{\mathbf{V}}_k({\mathbf{u}}_k^{*}({\hat{\mathbf{X}}}_k))\Delta t$	$14.89$	$14.80$	$14.93$
${\tilde{\mathbf{X}}}_{k+1}={\hat{\mathbf{X}}}_k+\Delta {\hat{\mathbf{X}}}_{k+1}$	$27.25$	—	—

. The first row corresponds to the position prediction by virtue of System (18) and the second row shows the results with prediction obtained by velocity estimation via seabed sensing, Equation (15). The columns correspond to the filtering algorithms applied: The conditionally minimax nonlinear Filter (22) with basic structure Functions (24), the unbiased pseudo-measurements filter, Equation (21), and the extended Kalman filter. Note that for the seabed sensing prediction only the CMNF filter allowed to acheive a reasonable estimate/control performance.

The simulation shows that in the ideal situation with good initial accuracy and observation conditions close to that of a linear system, the gain in the estimation quality and hence in the control performance is insignificant in comparison with the standard extended Kalman filter. Nevertheless, the proposed filtering algorithms can demonstrate better qualities in less favorable settings, e.g., when one of the beacons is close to the initial point of the path, the EKF diverges, while the proposed filters remain stable. Another result is that the linear filters are highly sensitive to the dynamic model description, and inaccurate parameters’ determination or evaluation can even lead to divergence.

7. Conclusions

In the present paper a mathematical model for an AUV navigation system based on the locally optimal (predefined path following) control and position estimation provided by seabed acoustic sensing and external DOA measurements. The performance of the proposed algorithms, namely the conditionally minimax nonlinear filter and pseudo-measurements filter, was evaluated by numerical experiments. It was demonstrated, that the CMNF approach provides a natural way for data fusion from different sources of observation by allowing a certain level of liberty in the basic prediction function choice and, at the same time, it allows the errors of the prediction step to be taken into account. The nonlinear character of measurement equations does not permit to evaluate the control quality in advance and needs a detailed analysis of the sensor fusion, which requires detailed modeling in the settings, which take into account the environment features and noise characteristics. We suppose that the methods considered in the present paper permit one to obtain sufficiently reliable approaches to underwater navigation without costly field experiments.