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

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.


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 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 ϕ(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 ϕ(t) as control parameters, and including the process noise w(t), Equation (1) can be rewritten asẋ (t) = F(t)x(t) + L(t)u(t) + w(t) (2) where and in which v wx = v w cos ϕ and v wy = v w sin ϕ are the in-water speed components of the vehicle in the directions of x and y, respectively. The process noise vector w(t) ∈ R 5×1 accounts for the uncertainty of the kinematic model. Assuming that the control term u(t) is constant within the discrete sampling interval ∆t, we obtain the corresponding discrete kinematic equation where and in which the subscript k denotes the kth time step. The uncertainty 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: where σ w is the standard deviation of v w,k uncertainty, σ c is the standard deviation of w cx,k and w cy,k , and σ e,k is that of w e,k .

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 in which h k (·) is a nonlinear function. It is assumed that the process and measurement noise w k and v k are mutually independent, zero-mean, Gaussian random processes, with covariance matrices Q k and R k , respectively.
In the present application, if the time of emission (TOE) T e k is known and the TOA T a k is measured, then the transit time T t k is given by By treating the transit time T t k as the measured quantity for m k , we have the nonlinear measurement equation where h k can be expressed in terms of the state variables x k , y k and v e,k to be 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 υ t,k is modeled as a white Gaussian noise with variance R t,k = σ 2 t,k . It is also assumed that the vehicle velocity relative to the ground v g is measured from a device, such as a Doppler velocity log (DVL). From the measured v g and the vehicle velocity through water v w , using v c = v g − v w , one has an indirect measurement for the ocean current: The corresponding measurement matrix H k associated with the ocean current measurements is simply

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: and From Equations (4), (8), (15) and (16), it is clear that an additional parameter σ 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 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 t k ). Difficulties of the Kalman tuning process while using the "5-sv" model also appears in that the setting of ESV uncertainty σ 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 σ 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 σ e based on filed data will be done to indicate the effect of σ e on the estimator performance.

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.

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 vectorx − k and the corresponding covariance matrix P − k arê and whereQ k−1 is the estimated process noise covariance matrix at the (k − 1)th epoch. Throughout this paper, a variable with a hat, such asx, represents the estimate of the variable, superscripts "−" and "+" denote quantities associated with a priori (prediction) and a posteriori (correction) estimates, respectively.

State Correction
For the correction equations of adaptive single-beacon tracking algorithm, the estimate of the state vectorx + k and the corresponding covariance matrix P + k arê and is the Kalman gain matrix andR k is the estimated measurement noise covariance matrix. For the single-beacon underwater tracking algorithm discussed herein, H k is the Jaccbian of nonlinear in whichr k is computed byr Noting thatẑ − k is available from a depth sensor. The measurement innovation (m k − h(x − k )) in Equation (20) is denoted byē k in this paper.

Estimation of Unknown R k
From literature [23], the main purpose of the Windows-based adaptive estimation is to estimate the 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: whereC vj = E(ē jē T j ) is the covariance matrix of innovation vector at time t j . Assumingē j satisfies zero-mean Gauss distribution, Equation (25) can be written as where N (x : µ, Q) means the vector x satisfies Gaussian distribution with mean µ and covariance matrix Q. The probability density function is The logarithmic form of Equation (26) is taken as To simplify the expression, the following hypothesis is made: For a Windows-based AKF, the innovation covariance matrix is constant within the windows width W, that isC 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 The minimum value is calculated by the partial of J 1,k with respect toC vk To obtain the above formula, the following two relations from matrix differential calculus have been used ∂ ln |A| ∂x Based on the relations from vector inner product x T x = tr(xx T ) and the trace property of matrix, Equation (31) can be written as Let then the estimated innovation covariance matrix is obtained aŝ On the other hand, the theoretical innovation covariance matrix can be calculated through the definition of innovation, and haveC Let the estimated value of the innovation covariance matrix equals to the theoretical value, the estimated R k can then be obtained This estimated R k will be used for calculating the Kalman gain at epoch k.

Estimation of Unknown Q k
To estimate the unknown Q k , rewritten Equation (22) as whereĈ vk is assumed to predetermined from Equation (36). SinceĈ vk and P − k are symmetric matrices, Equation (39) can be written asĈ Plugging the left side of Equation (40) into Equation (21) yields From Equations (41) and (19), the estimatedQ k−1 can be get To calculateQ k , the following hypothesis is made: The varying period of Q k is much larger than the sampling period ∆t, thus we have Based on the Hypothesis 2, Equations (36) and (42), the estimated Q k at time t k can be obtained Algorithm 1 summarizes the detailed procedure of the Windows-based adaptive single-beacon underwater tracking algorithm.

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].

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 σ e is chosen as 0.1 m/s. σ c and σ w are both set to 0.01 m/s. For clarity, the estimation performance of the unknown R k and Q k are studied in two distinct scenarios, respectively. The Q k is assumed to be precisely available during the estimation of unknown 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. 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; in which (x s k , y s k ) and (x s k ,ŷ s k ) represent respectively the target and the estimated positions at the sth Monte Carlo run, and v s e,k ,v s e,k represent respectively the target and the estimated ESV at the sth Monte Carlo run. M = 50 represents the total number of Monte Carlo runs.

Estimate the Unknown R k
For the first simulation, the measurement noise parameter is unknown to both the ASB and ESB algorithms, while the parameters of Q k are chosen exactly their target value. Specifically, the initial σ e is chosen the same as its target value as 0.1 m/s, while the initial σ t is set as 0.05 s with the same initial offset for both the ASB and ESB algorithms.
The comparison of the RMS ∆ 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ē t for the ESB and the ASB algorithm is compared in Figure 3. Sinceē t is a signed variable, it is compared from one randomly selected Monte Carlo simulation. From Figure 3, it is obviously thatē 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 σ t . From the comparison of RMS ∆ ve 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 σ t while implementing the ASB algorithm from running 50 Monte Carlo simulations is shown in Figure 5. The estimated σ t agrees well with its target value, while that in the ESB algorithm remains invariant as respected.

Estimate the Unknown 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 σ 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 σ t is chosen as zero for consistency.
For comparing the performance of the ASB algorithm with those of the ESB algorithm, the RMS ∆ 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 R k scenario can be observed. The estimated RMS ∆ ve 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 σ e from 50 Monte Carlo simulations is shown in Figure 9. Although the estimated σ e exist large variation at the beginning, the ASB algorithm can estimate its target value after a certain period of time. Figure 9. Estimated process noise comparison between the ESB and the ASB algorithms using simulation data with unknown Q k .

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, σ c = 0.01 m/s, σ w = 0.01 m/s and σ t = 0.0001 s. First, to demonstrate the Kalman tuning issue of traditional ESB algorithms mentioned in Section 2.3, a sensitivity study of σ e is carried out based on the field data. Shown in Figure 10a is the averaged RMS of the horizontal distance error varying from 0.01 to 1 m/s. Shown in Figure 10b is the averaged RMS of the estimated ESV error (v e,i −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 σ e has a great influence on the tracking performance. Improper σ 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 ∆ H for the filter and the RTS smoother, respectively. The component errors ∆x and ∆y for the ASB and ESB are shown in Figure 13. From Figures 11-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.

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.