Adaptive Kalman Filter-Based Single-Beacon Underwater Tracking with Unknown Effective Sound Velocity

Abstract

In the single-beacon underwater tracking system, vehicles rely on slant range measurements from an acoustic beacon to bound errors accumulated by dead reckoning. Ranges are usually obtained based on a presumed known effective sound velocity (ESV). Since the ESV is difficult to determine accurately, traditional methods suffer from large positioning error. By treating the unknown ESV as a state variable, a novel single-beacon tracking model (the so called “5-sv” model) and an extended Kalman filter (EKF)-based solution method have been discussed to solve the problem of ESV estimation. However, due to the uncertainty of underwater acoustic propagation, the probabilistic characteristics of the ESV uncertainty and acoustic measurement noise are unknown and varying both with time and location. EKF, which runs with presupposed noise parameters, cannot describe the practical noise specifications. To overcome the divergence issue of EKF-based single-beacon tracking methods, this paper proposes an adaptive Kalman filter-based single-beacon tracking algorithm which employs the “5-sv” model as the baseline model. Through numerical examples using simulated and field data, both the filter and smoother results show that while implementing the proposed algorithm, the tracking accuracy can be significantly improved, and the estimated noise parameter agrees well with its true value.

1. Introduction

Autonomous underwater vehicles (AUVs) have been used for a variety of offshore commercial and scientific applications [1,2,3,4,5,6]. For these applications, accurate tracking of AUVs is essential to ensure the accuracy of the gathered data [7]. Many underwater tracking approaches have been implemented and tested; however, traditional underwater tracking techniques, such as dead reckoning, suffer from unbounded tracking errors. The widely used acoustic approaches, such as long-base line (LBL) system, are restricted by the costly setting up since the location of each deployed beacon in the array must be precisely surveyed before conducting navigation operations. Therefore, tracking AUVs using a single beacon would provide dramatic time and cost savings for underwater vehicle operations, which has been proposed and studied in [7,8,9,10,11,12,13,14,15].

In the single-beacon underwater tracking system, AUVs rely on range measurements from an acoustic beacon with known position to bound errors accumulated by dead reckoning. Early researches on single-beacon tracking can be traced back to Larsen, in which the positioning and velocity error of AUV were treated as state variables, and the extended Kalman filter (EKF) was applied to estimate the vehicle position over time [16]. For positioning an AUV based on time of arrival (TOA) measurements from a single beacon, Casey et al. established a single-beacon tracking model with unknown ocean current using EKF as the state estimator [17]. Later, Fallon developed an AUV- USV (unmanned Surface Vehicle) cooperative tracking model using EKF to update the AUV position [18]. Considering the packet loss problem in underwater communication, Walls proposed a robust origin state method for range-based cooperative tracking based upon a single-beacon tracking scheme, and used the extended information filter (EIF) as the state estimator [19].

In the single-beacon underwater tracking methods mentioned above, the slant range was commonly adopted as the primary measurement, which was obtained from the measured acoustic transit time in conjunction with a presumed effective sound velocity (ESV) for the acoustic propagation between the beacon and the receiver. A known constant ESV was routinely assumed in [8,9,10,11,16,17,18,19]. Since the ESV, in practice, is location dependent, and is very difficult to determine accurately, these methods suffer from large ESV measurement error and consequently has large positioning error. By treating the unknown ESV as a state variable and the transit time as a measurement, Zhu et al. proposed a novel single-beacon localization model (namely the “5-sv” model), and used the EKF as the state estimator [20,21,22]. It was shown that by properly tuning the process and measurement noise parameters, the EKF based on the “5-sv” model could estimate the unknown ESV well, and the tracking performance of the “5-sv” model significantly outperform the traditional model.

By augmenting the unknown ESV as a state variable, the “5-sv” model not only introduced an additional parameter related to the ESV uncertainty needed to be tuned, but also increases the nonlinearity of the measurement model. It had been shown that the EKF performance depended largely on the initial setting of the ESV uncertainty parameter which would stay invariant during the whole estimation process [20]. Furthermore, while implementing the “5-sv” model, the parameters of the process noise covariance matrix and the measurement noise covariance matrix were difficult to tune, and these were usually predetermined based on experience or data post-processing. However, the huge variation of underwater environment leads to a rapid change in the probabilistic properties of the acoustic measurement noise. The problem of EKF estimates degradation occurs frequently while implementing the “5-sv” model due to the violation of known noise statistics assumptions.

To overcome the divergence issue of EKF-based single-beacon tracking methods, this paper will propose an adaptive Kalman filter (AKF)-based single-beacon underwater tracking algorithm. Although the AKF has been intensively investigated to reduce the influence of process noise covariance matrix and measurement noise covariance matrix errors in fields like integrated navigation, economic projection, and chemistry, to the best knowledge of the authors, little literature has been published implementing AKF for single-beacon underwater tracking, especially based on the model with unknown ESV [23,24,25]. Through simulation and field data, the estimation results between using the proposed adaptive algorithm and the traditional EKF will be compared. In addition to the real-time filter tracking, this paper also implements the Rauch-Tung-Striebel (RTS) smoother for post-processing [26,27]. Both simulation and field data will be used to study the possible improvement of the proposed adaptive single-beacon underwater tracking algorithm.

2. The “5-sv” Single-Beacon Tracking Model

A review of the “5-sv” single-beacon underwater tracking model, which is served as the baseline model employed in this paper, is offered here as a refresher. This review is also for acquainting the reader with the notation to be used throughout the article.

2.1. Kinematic Model

By treating the unknown ESV as a state variable, a single-beacon underwater tracking model—referred to as the “5-sv” (5 state variables) model—was proposed in [20,21,22]:

$$\left\{\begin{array}{c}\dot{x}(t)\\ \dot{y}(t)\\ {\dot{v}}_{cx}(t)\\ {\dot{v}}_{cy}(t)\\ {\dot{v}}_e(t)\end{array}\right\}=\left\{\begin{array}{c}{v}_w(t)cos\phi (t)+v_{cx}(t)\\ v_w(t)sin\phi (t)+v_{cy}(t)\\ 0\\ 0\\ 0\end{array}\right\}$$

(1)

where the state variables $x(t)$ and $y(t)$ represent the horizontal position of the vehicle; $v_{cx}(t)$ and $v_{cy}(t)$ are the two unknown ocean current components in the x and y directions, respectively; and $v_e(t)$ is the ESV between the beacon and the vehicle. When the ESV is multiplied by the acoustic signal transit time between two underwater point yields the geometric or slant range between them. In Equation (1), $v_w(t)$ and $\phi (t)$ are the vehicle speed and heading through water, which are measured from a speed sensor and an electronic compass, respectively.

While treating $v_w(t)$ and $\phi (t)$ as control parameters, and including the process noise $\mathbf{w}(t)$, Equation (1) can be rewritten as

$$\dot{\mathbf{x}}(t)=\mathbf{F}(t)\mathbf{x}(t)+\mathbf{L}(t)\mathbf{u}(t)+\mathbf{w}(t)$$

(2)

where

$$\mathbf{F}(t)=\left[\begin{array}{ccccc}0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right],\mathbf{L}(t)=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]$$

(3)

and

$$\mathbf{x}(t)=\left\{\begin{array}{c}x(t)\\ y(t)\\ v_{cx}(t)\\ v_{cy}(t)\\ v_e(t)\end{array}\right\},\mathbf{u}(t)=\left\{\begin{array}{c}{v}_{wx}(t)\\ v_{wy}(t)\end{array}\right\}$$

(4)

in which $v_{wx}=v_{w}cos\phi$ and $v_{wy}=v_{w}sin\phi$ are the in-water speed components of the vehicle in the directions of x and y, respectively. The process noise vector $\mathbf{w}(t)\in {\mathbb{R}}^{5\times 1}$ accounts for the uncertainty of the kinematic model.

Assuming that the control term $\mathbf{u}(t)$ is constant within the discrete sampling interval $\Delta t$, we obtain the corresponding discrete kinematic equation

$${\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}+{\mathbf{w}}_{k-1}$$

(5)

where

$${\mathbf{A}}_k=\left[\begin{array}{ccccc}1& 0& \Delta t& 0& 0\\ 0& 1& 0& \Delta t& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right],{\mathbf{B}}_k=\left[\begin{array}{cc}\Delta t& 0\\ 0& \Delta t\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]$$

(6)

and

$${\mathbf{x}}_k=\left\{\begin{array}{c}{x}_k\\ y_k\\ v_{cx,k}\\ v_{cy,k}\\ v_{e,k}\end{array}\right\},{\mathbf{u}}_k=\left\{\begin{array}{c}{v}_{wx,k}\\ v_{wy,k}\end{array}\right\},{\mathbf{w}}_k=\left\{\begin{array}{c}{w}_{x,k}\\ w_{y,k}\\ w_{cx,k}\\ w_{cy,k}\\ w_{e,k}\end{array}\right\}$$

(7)

in which the subscript k denotes the kth time step. The uncertainty ${\mathbf{w}}_k$ associated with the discrete kinematic model is modeled as Gaussian white noise processes, consisting of five components in three groups: (1) $w_{x,k}$ and $w_{y,k}$, position uncertainty in the x and y directions; (2) $w_{cx,k}$ and $w_{cy,k}$, ocean current uncertainty in the x and y directions; and (3) $w_{e,k}$, uncertainty associated with the ESV. The corresponding process noise covariance matrix is:

$${\mathbf{Q}}_k=\left[\begin{array}{ccccc}\Delta t^2({\sigma }_w^2{cos}^2{\phi }_k+{\sigma }_c^2)& \Delta t^2{\sigma }_w^{2}cos{\phi }_{k}sin{\phi }_k& 0& 0& 0\\ \Delta t^2{\sigma }_w^{2}cos{\phi }_{k}sin{\phi }_k& \Delta t^2({\sigma }_w^2{sin}^2{\phi }_k+{\sigma }_c^2)& 0& 0& 0\\ 0& 0& {\sigma }_c^2& 0& 0\\ 0& 0& 0& {\sigma }_c^2& 0\\ 0& 0& 0& 0& {\sigma }_{e,k}^2\end{array}\right]$$

(8)

where ${\sigma }_w$ is the standard deviation of $v_{w,k}$ uncertainty, ${\sigma }_c$ is the standard deviation of $w_{cx,k}$ and $w_{cy,k}$, and ${\sigma }_{e,k}$ is that of $w_{e,k}$.

2.2. Measurement Model

In the application of the Kalman filter for single-beacon underwater tracking, the measurement equation is a nonlinear function of the state variables, with the general form

$${\mathbf{m}}_k={\mathbf{h}}_k({\mathbf{x}}_k)+{\mathbf{v}}_k$$

(9)

in which ${\mathbf{h}}_k(·)$ is a nonlinear function. It is assumed that the process and measurement noise ${\mathbf{w}}_k$ and ${\mathbf{v}}_k$ are mutually independent, zero-mean, Gaussian random processes, with covariance matrices ${\mathbf{Q}}_k$ and ${\mathbf{R}}_k$, respectively.

In the present application, if the time of emission (TOE) $T_k^e$ is known and the TOA $T_k^a$ is measured, then the transit time $T_k^t$ is given by

$$T_k^t=T_k^a-T_k^e$$

(10)

By treating the transit time $T_k^t$ as the measured quantity for $m_k$, we have the nonlinear measurement equation

$$m_k=h_k({\mathbf{x}}_k)+{\upsilon }_{t,k}$$

(11)

where $h_k$ can be expressed in terms of the state variables $x_k$, $y_k$ and $v_{e,k}$ to be

$$h_k=\frac{\sqrt{{(x_k-x_b)}^2+{(y_k-y_b)}^2+{(z_k-z_b)}^2}}{v_{e,k}}$$

(12)

in which $(x_k,y_k,z_k)$ and $(x_b,y_b,z_b)$ are the positions of the vehicle and the beacon, respectively. Throughout this paper, the vertical position of an underwater vehicle $z_k$ assumed to be a known value obtained from a depth sensor. The transit time measurement noise ${\upsilon }_{t,k}$ is modeled as a white Gaussian noise with variance $R_{t,k}={\sigma }_{t,k}^2$.

It is also assumed that the vehicle velocity relative to the ground ${\mathbf{v}}_g$ is measured from a device, such as a Doppler velocity log (DVL). From the measured ${\mathbf{v}}_g$ and the vehicle velocity through water ${\mathbf{v}}_w$, using ${\mathbf{v}}_c={\mathbf{v}}_g-{\mathbf{v}}_w$, one has an indirect measurement for the ocean current:

$${\mathbf{v}}_{c,k}=\left\{\begin{array}{c}{v}_{cx,k}\\ v_{cy,k}\end{array}\right\}$$

(13)

The corresponding measurement matrix ${\mathbf{H}}_k$ associated with the ocean current measurements is simply

$${\mathbf{H}}_k=\left[\begin{array}{ccccc}0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\end{array}\right]$$

(14)

2.3. Comparison with Traditional Model

Traditional single-beacon tracking methods always treated the ESV as a known quantity. In reference to Equations (7) and (8), the corresponding discrete state vector and process uncertainty model are:

$${\mathbf{x}}_k=\left\{\begin{array}{c}{x}_k\\ y_k\\ v_{cx,k}\\ v_{cy,k}\end{array}\right\}$$

(15)

and

$${\mathbf{Q}}_k=\left[\begin{array}{cccc}\Delta t^2({\sigma }_w^2{cos}^2{\phi }_k+{\sigma }_c^2)& \Delta t^2{\sigma }_w^{2}cos{\phi }_{k}sin{\phi }_k& 0& 0\\ \Delta t^2{\sigma }_w^{2}cos{\phi }_{k}sin{\phi }_k& \Delta t^2({\sigma }_w^2{sin}^2{\phi }_k+{\sigma }_c^2)& 0& 0\\ 0& 0& {\sigma }_c^2& 0\\ 0& 0& 0& {\sigma }_c^2\end{array}\right]$$

(16)

From Equations (4), (8), (15) and (16), it is clear that an additional parameter ${\sigma }_e$ is introduced and needed to be properly tuned while implementing EKF based on the “5-sv” model.

Furthermore, when the slant range $r_k$ computed from a transit time measurement is used in traditional models, the counterpart of Equation (12) is

$$h_k=\sqrt{{(x_k-x_b)}^2+{(y_k-y_b)}^2+{(z_k-z_b)}^2}$$

(17)

Interested readers are referred to [20,21,22] for detailed comparison between the “5-sv” model and traditional models.

Compared from Equations (12) and (17), it can be seen that the degree of nonlinearity of the measurement equation in the “5-sv” model is much severer than that of traditional models. In essence, the “5-sv” model has converted the problem of estimating two unknowns ($x_k$ and $y_k$) from one measurement ($r_k$) into the problem of estimating three unknowns ($x_k$, $y_k$ and $v_{e,k}$) from one measurement ($T_k^t$). Difficulties of the Kalman tuning process while using the “5-sv” model also appears in that the setting of ESV uncertainty ${\sigma }_e$ interacts with the initial setting of position uncertainties. EKF based on the “5-sv” model is much harder to tune, and more likely to diverge than traditional models if the initial settings are improper. Furthermore, the physics of acoustic wave propagation and the underwater acoustic environment creates some unique differences, including longer transit times and severe refraction through the stratified ocean. Thus, uncertainty associated with the acoustic propagation velocity are very difficult to deal with because they cannot be measured and are generally varying with both space and time. Even if an approximate ${\sigma }_e$ is obtained from experience or data post-processing, divergence issue of EKF based on the “5-sv” model would still occur frequently due to the violation of known noise statistics assumptions. In Section 4.2, a sensitivity study of ${\sigma }_e$ based on filed data will be done to indicate the effect of ${\sigma }_e$ on the estimator performance.

3. Windows-Based Adaptive Single-Beacon Tracking Algorithm

Unlike the EKF, the AKF can adjust the noise parameters online based upon the immediate measurements and had proven to be a good solution to improve the robustness of Kalman filters. In this section, a Windows-based adaptive single-beacon tracking algorithm based on the “5-sv” model will be proposed to solve the divergence issue of traditional EKF-based methods.

As presented in many textbooks [26,28], the discrete Kalman filter equations include the prediction and correction equations. Compared with EKFs, the Windows-based AKF introduced an additional step for estimating the process and measurement noise covariance matrices based upon the innovation sequence.

3.1. State Prediction

Since the kinematic equation of the “5-sv” model is linear, the prediction equation of the adaptive single-beacon tracking algorithm is the same as the standard Kalman filter, so the estimate of the state vector ${\widehat{\mathbf{x}}}_k^{-}$ and the corresponding covariance matrix ${\mathbf{P}}_k^{-}$ are

$${\widehat{\mathbf{x}}}_k^{-}={\mathbf{A}}_{k-1}{\widehat{\mathbf{x}}}_{k-1}^{+}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}$$

(18)

and

$${\mathbf{P}}_k^{-}={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}^{+}{\mathbf{A}}_{k-1}^T+{\widehat{\mathbf{Q}}}_{k-1}$$

(19)

where ${\widehat{\mathbf{Q}}}_{k-1}$ is the estimated process noise covariance matrix at the $(k-1)$th epoch.

Throughout this paper, a variable with a hat, such as $\widehat{\mathbf{x}}$, represents the estimate of the variable, superscripts “−” and “+” denote quantities associated with a priori(prediction) and a posteriori(correction) estimates, respectively.

3.2. State Correction

For the correction equations of adaptive single-beacon tracking algorithm, the estimate of the state vector ${\widehat{\mathbf{x}}}_k^{+}$ and the corresponding covariance matrix ${\mathbf{P}}_k^{+}$ are

$${\widehat{\mathbf{x}}}_k^{+}={\widehat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{m}}_k-\mathbf{h}({\widehat{\mathbf{x}}}_k^{-}))$$

(20)

and

$${\mathbf{P}}_k^{+}=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}$$

(21)

in which

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\widehat{\mathbf{R}}}_k)}^{-1}$$

(22)

is the Kalman gain matrix and ${\widehat{\mathbf{R}}}_k$ is the estimated measurement noise covariance matrix. For the single-beacon underwater tracking algorithm discussed herein, ${\mathbf{H}}_k$ is the Jaccbian of nonlinear measurement equation $\mathbf{h}(•)$ at the a priori estimate ${\widehat{\mathbf{x}}}_k^{-}$

$${\mathbf{H}}_k({\widehat{\mathbf{x}}}_k^{-})=\left[\begin{array}{ccccc}\frac{{\widehat{x}}_k^{-}-x_b}{{\widehat{v}}_{e,k}^{-}{\widehat{r}}_k}& \frac{{\widehat{y}}_k^{-}-y_b}{{\widehat{v}}_{e,k}^{-}{\widehat{r}}_k}& 0& 0& -\frac{{\widehat{r}}_k}{{({\widehat{v}}_{e,k}^{-})}^2}\end{array}\right]$$

(23)

in which ${\widehat{r}}_k$ is computed by

$${\widehat{r}}_k=\sqrt{{({\widehat{x}}_k^{-}-x_b)}^2+{({\widehat{y}}_k^{-}-y_b)}^2+{({\widehat{z}}_k^{-}-z_b)}^2}$$

(24)

Noting that ${\widehat{z}}_k^{-}$ is available from a depth sensor. The measurement innovation $({\mathbf{m}}_k-\mathbf{h}({\widehat{\mathbf{x}}}_k^{-}))$ in Equation (20) is denoted by ${\overline{\mathbf{e}}}_k$ in this paper.

3.3. Estimation of Unknown ${\mathbf{R}}_k$

From literature [23], the main purpose of the Windows-based adaptive estimation is to estimate the ${\mathbf{R}}_k$ through the innovation sequence, in which the innovation is considered to be an intermediate variable. The fundamental principle is making the elements in the actual innovation covariance matrix consistent with their theoretical value. Their actual values are intractable; however, they can be estimated based on maximum likelihood (ML) criterion. The likelihood function is chosen to be the joint conditional probability of measurement given estimated variable within the fixed windows W:

$$\begin{array}{cc}\hfill J_k& ={\displaystyle \prod _{j=k-W+1}^k}p({\mathbf{z}}_j|{\overline{\mathbf{C}}}_{vj})\hfill \end{array}$$

(25)

where ${\overline{\mathbf{C}}}_{vj}=\mathbf{E}({\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T)$ is the covariance matrix of innovation vector at time $t_j$.

Assuming ${\overline{\mathbf{e}}}_j$ satisfies zero-mean Gauss distribution, Equation (25) can be written as

$$\begin{array}{cc}\hfill J_k& ={\displaystyle \prod _{j=k-W+1}^k}\mathcal{N}({\mathbf{z}}_j:\mathbf{h}({\widehat{\mathbf{x}}}_k^{-}),{\overline{\mathbf{C}}}_{vj})\hfill \end{array}$$

(26)

where $\mathcal{N}(\mathbf{x}:\mathbf{\mu },\mathbf{Q})$ means the vector $\mathbf{x}$ satisfies Gaussian distribution with mean $\mathbf{\mu }$ and covariance matrix $\mathbf{Q}$. The probability density function is

$$\mathcal{N}(\mathbf{x}:\mathbf{\mu },\mathbf{Q})=\frac{1}{\sqrt{\left|2\pi \mathbf{Q}\right|}}exp(-\frac{1}{2}{(\mathbf{x}-\mathbf{\mu })}^T{\mathbf{Q}}^{-1}(\mathbf{x}-\mathbf{\mu }))$$

(27)

The logarithmic form of Equation (26) is taken as

$$ln{J}_k={\displaystyle \sum _{j=k-W+1}^k}-\frac{1}{2}(ln|2\pi \times {\overline{\mathbf{C}}}_{vj}|+{\overline{\mathbf{e}}}_j^T{\overline{\mathbf{C}}}_{vj}^{-1}{\overline{\mathbf{e}}}_j)$$

(28)

To simplify the expression, the following hypothesis is made:

Hypothesis 1.

For a Windows-based AKF, the innovation covariance matrix is constant within the windows width W, that is

$${\overline{\mathbf{C}}}_{vj}={\overline{\mathbf{C}}}_{vk},(k-W+1)\le j\le k$$

(29)

After multiplying Equation (28) by $-2$ and neglecting the constant term, the ML criterion of maximizing J within the windows width W becomes the minimization problem which is described as

$$\left\{\begin{array}{c}{\widehat{\overline{\mathbf{C}}}}_{vk}=argmin{J}_{1,k}\hfill \\ J_{1,k}={\sum }_{j=k-W+1}^{k}ln\left|{\overline{\mathbf{C}}}_{vk}\right|+{\displaystyle \sum _{j=k-W+1}^k}{\overline{\mathbf{e}}}_j^T{\overline{\mathbf{C}}}_{vk}^{-1}{\overline{\mathbf{e}}}_j\hfill \end{array}\right.$$

(30)

The minimum value is calculated by the partial of $J_{1,k}$ with respect to ${\overline{\mathbf{C}}}_{vk}$

$$\begin{array}{cc}\hfill \frac{\partial J_{1,k}}{\partial {\overline{\mathbf{C}}}_{vk}}& =W\mathrm{tr}\left\{{\overline{\mathbf{C}}}_{vk}^{-1}\right\}-{\displaystyle \sum _{j=k-W+1}^k}{\overline{\mathbf{e}}}_j^T{\overline{\mathbf{C}}}_{vk}^{-1}{\overline{\mathbf{C}}}_{vk}^{-1}{\overline{\mathbf{e}}}_j\hfill \end{array}$$

(31)

To obtain the above formula, the following two relations from matrix differential calculus have been used

$$\frac{\partial ln\left|\mathbf{A}\right|}{\partial x}=\frac{1}{\left|\mathbf{A}\right|}\frac{\partial \left|\mathbf{A}\right|}{\partial x}=\mathrm{tr}\{{\mathbf{A}}^{-1}\frac{\partial \left|\mathbf{A}\right|}{\partial x}\}$$

(32)

$$\frac{\partial {\mathbf{A}}^{-1}}{\partial x}=-{\mathbf{A}}^{-1}\frac{\partial \left|\mathbf{A}\right|}{\partial x}{\mathbf{A}}^{-1}$$

(33)

Based on the relations from vector inner product ${\mathbf{x}}^T\mathbf{x}=\mathrm{tr}(\mathbf{x}{\mathbf{x}}^T)$ and the trace property of matrix, Equation (31) can be written as

$$\begin{array}{cc}\hfill \frac{\partial J_{1,k}}{\partial {\overline{\mathbf{C}}}_{vk}}& =\mathrm{tr}\{{\overline{\mathbf{C}}}_{vk}^{-1}(W{\overline{\mathbf{C}}}_{vk}-{\displaystyle \sum _{j=k-W+1}^k}{\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T){\overline{\mathbf{C}}}_{vk}^{-1}\}\hfill \end{array}$$

(34)

Let

$$\frac{\partial J_{1,k}}{\partial {\overline{\mathbf{C}}}_{vk}}=0$$

(35)

then the estimated innovation covariance matrix is obtained as

$${\widehat{\overline{\mathbf{C}}}}_{vk}=\frac{1}{W}{\displaystyle \sum _{j=k-W+1}^k}{\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T$$

(36)

On the other hand, the theoretical innovation covariance matrix can be calculated through the definition of innovation, and have

$${\overline{\mathbf{C}}}_{vk}={\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k$$

(37)

Let the estimated value of the innovation covariance matrix equals to the theoretical value, the estimated ${\mathbf{R}}_k$ can then be obtained

$${\widehat{\mathbf{R}}}_k=\frac{1}{W}{\displaystyle \sum _{j=k-W+1}^k}{\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T-{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T$$

(38)

This estimated ${\mathbf{R}}_k$ will be used for calculating the Kalman gain at epoch k.

3.4. Estimation of Unknown ${\mathbf{Q}}_k$

To estimate the unknown ${\mathbf{Q}}_k$, rewritten Equation (22) as

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\widehat{\overline{\mathbf{C}}}}_{vk}^{-1}$$

(39)

where ${\widehat{\overline{\mathbf{C}}}}_{vk}$ is assumed to predetermined from Equation (36). Since ${\widehat{\overline{\mathbf{C}}}}_{vk}$ and ${\mathbf{P}}_k^{-}$ are symmetric matrices, Equation (39) can be written as

$${\widehat{\overline{\mathbf{C}}}}_{vk}{\mathbf{K}}_k^T={\mathbf{H}}_k{\mathbf{P}}_k^{-}$$

(40)

Plugging the left side of Equation (40) into Equation (21) yields

$$\begin{array}{c}\hfill {\mathbf{P}}_k^{-}={\mathbf{K}}_k{\widehat{\overline{\mathbf{C}}}}_{vk}{\mathbf{K}}_k^T+{\mathbf{P}}_k^{+}\end{array}$$

(41)

From Equations (41) and (19), the estimated ${\widehat{\mathbf{Q}}}_{k-1}$ can be get

$${\widehat{\mathbf{Q}}}_{k-1}={\mathbf{K}}_k{\widehat{\overline{\mathbf{C}}}}_{vk}{\mathbf{K}}_k^T-{\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}^{+}{\mathbf{A}}_{k-1}^T+{\mathbf{P}}_k^{+}$$

(42)

To calculate ${\widehat{\mathbf{Q}}}_k$, the following hypothesis is made:

Hypothesis 2.

The varying period of ${\mathbf{Q}}_k$ is much larger than the sampling period $\Delta t$, thus we have

$${\mathbf{Q}}_k\approx {\mathbf{Q}}_{k-1}$$

(43)

Based on the Hypothesis 2, Equations (36) and (42), the estimated ${\mathbf{Q}}_k$ at time $t_k$ can be obtained

$$\begin{array}{cc}\hfill {\widehat{\mathbf{Q}}}_k& =\frac{1}{W}{\displaystyle \sum _{j=k-W+1}^k}{\mathbf{K}}_k{\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T{\mathbf{K}}_k^T-{\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}^{+}{\mathbf{A}}_{k-1}^T+{\mathbf{P}}_k^{+}\hfill \end{array}$$

(44)

Algorithm 1 summarizes the detailed procedure of the Windows-based adaptive single-beacon underwater tracking algorithm.

Algorithm 1 Windows-based adaptive single-beacon underwater tracking algorithm

Input:${\widehat{\mathbf{x}}}_{k-1}^{+},{\mathbf{P}}_{k-1}^{+},\{{\overline{\mathbf{e}}}_{k-W+1},{\overline{\mathbf{e}}}_{k-W+2},\cdots {\overline{\mathbf{e}}}_{k-1}\}$ Prediction: 1: ${\widehat{\mathbf{x}}}_k^{-}={\mathbf{A}}_{k-1}{\widehat{\mathbf{x}}}_{k-1}^{+}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}$ 2: ${\mathbf{P}}_k^{-}={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}^{+}{\mathbf{A}}_{k-1}^T+{\widehat{\mathbf{Q}}}_{k-1}$ Correction: 1: ${\widehat{r}}_k=\sqrt{{({\widehat{x}}_k^{-}-x_b)}^2+{({\widehat{y}}_k^{-}-y_b)}^2+{({\widehat{z}}_k^{-}-z_b)}^2}$ 2: ${\mathbf{H}}_k=[\begin{array}{ccccc}\frac{{\widehat{x}}_k^{-}-x_b}{{\widehat{v}}_{e,k}^{-}{\widehat{r}}_k}& \frac{{\widehat{y}}_k^{-}-y_b}{{\widehat{v}}_{e,k}^{-}{\widehat{r}}_k}& 0& 0& -\frac{{\widehat{r}}_k}{{({\widehat{v}}_{e,k}^{-})}^2}\end{array}]$ 3: ${\overline{\mathbf{e}}}_k={\mathbf{m}}_k-\mathbf{h}({\widehat{\mathbf{x}}}_k^{-})$ 4: ${\widehat{\mathbf{R}}}_k=\frac{1}{W}{\sum }_{j=k-W+1}^k{\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T-{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T$ 5: ${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\widehat{\mathbf{R}}}_k)}^{-1}$ 6: ${\widehat{\mathbf{x}}}_k^{+}={\widehat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k{\overline{\mathbf{e}}}_k$ 7: ${\mathbf{P}}_k^{+}=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}$ 8: ${\widehat{\mathbf{Q}}}_k=\frac{1}{W}{\sum }_{j=k-W+1}^k{\mathbf{K}}_k{\overline{\mathbf{e}}}_j{\overline{\mathbf{e}}}_j^T{\mathbf{K}}_k^T-{\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1}^{+}{\mathbf{A}}_{k-1}^T+{\mathbf{P}}_k^{+}$ Return:${\widehat{\mathbf{x}}}_k^{+},{\mathbf{P}}_k^{+},{\overline{\mathbf{e}}}_k$

4. Numerical Studies

Both simulation and field data are used to evaluate the performance of the Windows-based adaptive single-beacon underwater tracking algorithm (to be referred to as the “ASB” algorithm) against that of the EKF-based algorithm (to be referred to as the “ESB” algorithm). In addition to the real-time filter tracking, this paper also implements the RTS smoother for post-processing. The RTS smoother is the most commonly used fixed-interval smoother, which operates backward in time steps after the complete filtering solution has been obtained [26,27].

4.1. Simulation Data

The reader is referred to [20] regarding the simulation technique for generating TOA data. For consistency, the trajectory in simulation was chosen to be the same as the trajectory of field data (shown in Figure 1). For the simulation of TOAs, the ESV has been set equal to a constant 1530 m/s during the whole trajectory, with TOEs every 10 seconds. Both ocean current components in the x and y directions are $0.3$ m/s. The process noise parameter ${\sigma }_e$ is chosen as $0.1$ m/s. ${\sigma }_c$ and ${\sigma }_w$ are both set to $0.01$ m/s. For clarity, the estimation performance of the unknown ${\mathbf{R}}_k$ and ${\mathbf{Q}}_k$ are studied in two distinct scenarios, respectively. The ${\mathbf{Q}}_k$ is assumed to be precisely available during the estimation of unknown ${\mathbf{R}}_k$, and vice versa.

The sampling frequency of each simulated measurement and the corresponding noise level characterized by its standard deviation are summarized in

Table 1. Sampling frequency and noise level of simulated measurements.

Sensor	Measurement	Simulated Sampling Frequency (Hz)	Noise ($\mathbf{\sigma }$)
DVL	$v_{gx},v_{gy}$	1	$0.002$ m/s
electronic compass	$\phi$	1	0.2${}^{\circ }$
speed sensor	$v_w$	1	$0.001$ m/s
hydrophone	TOA	–	$0.001$ s

. Notice that the sampling interval of the hydrophone is irregular due to varying acoustic propagation times because of vehicle motion.

In the application of the ASB and the ESB algorithms, the following initial settings have been chosen: (1) a total of 10 meters in both the x and y directions for the initial position offset; (2) $v_{e,0}=1540$ m/s for the initial ESV; (3) $0.35$ m/s for the initial ocean current in both x and y directions; and (4) ${\sigma }_w=0.01$ m/s; (5) ${\sigma }_c=0.01$ m/s. Specifically, the innovation window width W is chosen as 10 while implementing the ASB algorithm.

Fifty Monte Carlo simulations have been done to compare the performance of the ESB and the ASB algorithm. The root mean square (RMS) of the horizontal distance error ${\Delta }_H$ and the estimation error of the ESV ${\Delta }_{v_e}$ are chosen as the evaluation indexes of the Monte Carlo simulations, which are defined respectively as:

$$\left\{\begin{array}{c}{\mathrm{RMS}}_{{\Delta }_H}\triangleq \sqrt{\frac{1}{M}{\displaystyle \sum _{s=1}^M}\left({(x_k^s-{\widehat{x}}_k^s)}^2+{(y_k^s-{\widehat{y}}_k^s)}^2\right)}\hfill \\ {\mathrm{RMS}}_{{\Delta }_{v_e}}\triangleq \sqrt{\frac{1}{M}{\displaystyle \sum _{s=1}^M}\left({(v_{e,k}^s-{\widehat{v}}_{e,k}^s)}^2\right)}\hfill \end{array}\right.$$

(45)

in which $(x_k^s,y_k^s)$ and $({\widehat{x}}_k^s,{\widehat{y}}_k^s)$ represent respectively the target and the estimated positions at the sth Monte Carlo run, and $v_{e,k}^s$, ${\widehat{v}}_{e,k}^s$ represent respectively the target and the estimated ESV at the sth Monte Carlo run. M = 50 represents the total number of Monte Carlo runs.

4.1.1. Estimate the Unknown ${\mathbf{R}}_k$

For the first simulation, the measurement noise parameter is unknown to both the ASB and ESB algorithms, while the parameters of ${\mathbf{Q}}_k$ are chosen exactly their target value. Specifically, the initial ${\sigma }_e$ is chosen the same as its target value as $0.1$ m/s, while the initial ${\sigma }_t$ is set as $0.05$ s with the same initial offset for both the ASB and ESB algorithms.

The comparison of the ${\mathrm{RMS}}_{{\Delta }_H}$ between the ASB and the ESB algorithm is shown in Figure 2a,b from running the filter and the RTS smoother, respectively. It can be concluded that the tracking error associated with the ASB algorithm is noticeably smaller than that of the ESB algorithm.

Furthermore, the innovation of the transit time measurement ${\overline{\mathbf{e}}}_t$ for the ESB and the ASB algorithm is compared in Figure 3. Since ${\overline{\mathbf{e}}}_t$ is a signed variable, it is compared from one randomly selected Monte Carlo simulation. From Figure 3, it is obviously that ${\overline{\mathbf{e}}}_t$ of the ASB algorithm converges rapidly then fluctuates around zero indicating a consistent filter, while that of the ESB algorithm has a large variation due to the improper setting of ${\sigma }_t$.

From the comparison of ${\mathrm{RMS}}_{{\Delta }_{v_e}}$ shown in Figure 4, it can be seen that considering both the converging rate and steady state estimation error, the estimation performance of the ESV while implementing the ASB algorithm outperforms that of the ESB algorithm significantly.

Finally, the mean value of the estimated ${\sigma }_t$ while implementing the ASB algorithm from running 50 Monte Carlo simulations is shown in Figure 5. The estimated ${\sigma }_t$ agrees well with its target value, while that in the ESB algorithm remains invariant as respected.

4.1.2. Estimate the Unknown ${\mathbf{Q}}_k$

In the second scenario, the process noise parameter is unknown to both the ASB and ESB algorithms, while the parameter of measurement noise is chosen exactly their target value. Specifically, the initial ${\sigma }_e$ is set as $0.5$ m/s for both the ASB and ESB algorithms with the same initial offset. To eliminate the possible impact of measurement noise, the hydrophone is kept clean without random noise contamination while generating the simulation data, and the initial ${\sigma }_t$ is chosen as zero for consistency.

For comparing the performance of the ASB algorithm with those of the ESB algorithm, the ${\mathrm{RMS}}_{{\Delta }_H}$ for both two algorithms are shown in Figure 6a,b from running the filter and the RTS smoother, respectively. Clearly, using the ASB algorithm can significantly enhance the tracking accuracy over the ESB algorithm.

The comparison of the measurement innovations is shown in Figure 7, in which similar features with the unknown ${\mathbf{R}}_k$ scenario can be observed.

The estimated ${\mathrm{RMS}}_{{\Delta }_{v_e}}$ is shown in Figure 8a,b from running the filter and the RTS smoother, respectively. Once again, the estimated ESV of ASB algorithm has a faster converging rate and smaller steady state error than that of ESB algorithm.

For completeness, the mean value of the estimated unknown ${\sigma }_e$ from 50 Monte Carlo simulations is shown in Figure 9. Although the estimated ${\sigma }_e$ exist large variation at the beginning, the ASB algorithm can estimate its target value after a certain period of time.

4.2. Field Data

The field data used to demonstrate the efficiency of the ASB algorithm was collected from a surface boat equipped with a hydrophone. The boat also had access to the GPS, so the ground truth trajectory of the vehicle was available. A beacon was mounted at the sea floor with a surveyed location. Since the slant range between the vehicle and beacon could be computed from their known locations, the ground truth of every instant ESV could be computed from dividing the slant range by the corresponding transit time, obtained from the TOA measurement minus a known TOE.

While implementing the filter and the RTS smoother on the field data, the following initial quantities were chosen: $v_{e,0}=1560$ m/s, ${\sigma }_c=0.01$ m/s, ${\sigma }_w=0.01$ m/s and ${\sigma }_t=0.0001$ s. First, to demonstrate the Kalman tuning issue of traditional ESB algorithms mentioned in Section 2.3, a sensitivity study of ${\sigma }_e$ is carried out based on the field data. Shown in Figure 10a is the averaged RMS of the horizontal distance error ${\mathrm{ARMS}}_{{\Delta }_H}=\sqrt{{\displaystyle \sum _{i=1}^N}({(x_i-{\widehat{x}}_i)}^2+{(y_i-{\widehat{y}}_i)}^2)}$ with ${\sigma }_e$ varying from $0.01$ to 1 m/s. Shown in Figure 10b is the averaged RMS of the estimated ESV error ${\mathrm{ARMS}}_{{\Delta }_{ve}}=\sqrt{{\displaystyle \sum _{i=1}^K}{(v_{e,i}-{\widehat{v}}_{e,i})}^2}$. N and K are the total number of fixed sampling interval and transit time measurements, respectively. From Figure 10a,b, it is obviously that ${\sigma }_e$ has a great influence on the tracking performance. Improper ${\sigma }_e$ in the ESB algorithm will degenerate the estimation accuracy drastically.

The comparison of the estimated trajectories between the ASB and ESB is shown in Figure 11a,b from running the filter and the RTS smoother, respectively. Similarly, Figure 12a,b are the corresponding comparison of the horizontal distance error ${\Delta }_H$ for the filter and the RTS smoother, respectively. The component errors $\Delta x$ and $\Delta y$ for the ASB and ESB are shown in Figure 13. From Figure 11, Figure 12 and Figure 13, it can be concluded that using the ASB could greatly improve the tracking accuracy.

Shown in Figure 14 is the estimated ESV from the filter and RTS smoother for these two algorithms, together with the target ESV. Similarly, the estimated ESV while implementing the ASB algorithm agrees better with its target value than the ESB algorithm.

5. Conclusions

To eliminate the range measurement error induced by imprecise knowledge of ESV, the so called “5-sv” single-beacon underwater tracking model was recently proposed. However, for applying the EKF based on the specific “5-sv” model, the tracking performance depended largely on the initial knowledge of process and measurement noise statistics, and filter divergence occurred frequently due to the huge variation of underwater acoustic propagation.

To overcome the limitation of EKF-based methods, this paper proposed an adaptive Kalman filter-based single-beacon underwater tracking algorithm basing upon the “5-sv” model. Through numerical examples of using simulated and field data, both the filter and RTS smoother results suggested that the tracking accuracy was significantly improved while implementing the proposed adaptive algorithm. The estimated process and measurement noise parameters also agreed well with their true value.