An Adaptive Target Tracking Algorithm Based on EKF for AUV with Unknown Non-Gaussian Process Noise

: An adaptive target tracking method based on extended Kalman ﬁlter (TT-EKF) is proposed to simultaneously estimate the state of an Autonomous Underwater Vehicle (AUV) and an mobile recovery system (MRS) with unknown non-Gaussian process noise in homing process. In the application scenario of this article, the process noise includes the measurement noise of AUV heading and forward speed and the estimation error of MRS heading and forward speed. The accuracy of process noise covariance matrix (PNCM) can affect the state estimation performance of the TT-EKF. The variational Bayesian based algorithm is applied to estimate the process noise statistics. We use a Gaussian mixture distribution to model the non-Gaussian noisy forward speed of AUV and MRS. We use a von-Mises distribution to model the noisy heading of AUV and MRS. The variational Bayesian algorithm is applied to estimate the parameters of these distributions, and then the PNCM can be calculated. The prediction error of TT-EKF is online compensated by using a multilayer neural network, and the neural network is online trained during the target tracking process. Matlab simulation and experimental data analysis results verify the effectiveness of the proposed method.


Introduction
For target tracking in the ocean environment, two approaches are generally used. Linear Kalman filter (KF) is a first and foremost approach, and the extended Kalman filter(EKF) is the second approach [1][2][3][4][5]. KF/EKF is highly sensitive to the accuracy of the noise statistics. In practical applications, as is often the case, the covariance matrixes of the process noise and the measurement noise may be unknown or inaccurate. In the case of unknown covariance matrixes, the state estimation performance of KF/EKF will be decreased and it may even diverge [6]. More and more adaptive filters with model uncertainty are developed to solve this problem. In this manuscript, we focus on the model uncertainty of the process noise, specifically, the process noise is unknown and non-Gaussian.
A variety of filters were proposed to address the state estimation problem with unknown noise covariance matrixes, such as robust KF [7,8], adaptive KF [9][10][11][12][13][14][15], and others [16]. Generally Speaking, the adaptive KF methods can be divided into covariance matching, maximum likelihood (ML), Bayesian methods and so on. More recently, the variational Bayesian (VB) methods wasn applied into the filtering framework [17][18][19]. In [14,20], a VB adaptive KF are proposed to jointly estimate the state and the measurement noise covariance matrix (MNCM) where the prior distribution of the covariance is governed by an inverse-Gamma distribution and inverse-Wishart distribution, respectively.
To the best of our knowledge, the VB-based adaptive filters are only used for online state estimation with unknown MNCM, and rarely used for the online state estimation with unknown process noise covariance matrix (PNCM). In [13], the online state estimation with inaccurate PNCM is treated, in which the prediction error covariance matrix rather than PNCM is approximated by the VB method. It cannot give the estimated value of the unknown PNCM. Besides, the authors of [21] experimentally show that the state estimation performance of the proposed adaptive filter in [13] may be worse than that of the traditional KF in some cases. In [22], a new adaptive KF algorithm is proposed to cope with the unknown PNCM for the linear discrete-time systems. The PNCM is estimated by the proposed algorithm based on the measurement sequence. In [23], an adaptive KF algorithm in presence of PNCM based on black box variational inference is proposed. The black box variational inference method is recently introduced to conduct the approximate Bayesian inference for the non-conjugate probabilistic model. They structured a posterior model of the system state and PNCM firstly, and then, the black box variational inference online inference for the posterior distribution of the PNCM is derived.
In this paper, we address the online state estimation problem in the presence of the unknown and non-Gaussian PNCM. For the target tracking in the ocean environment, the process noise of AUV is unknown or inaccurate, and the process noise of MRS is unknown totally. Due to the complexity of an underwater environment, the actual noise of AUV speed sensor is non-Gaussian. The main contributions of this paper are summarized as follows: 1.
We propose a target tracking method based on EKF (TT-EKF) to simultaneously estimate the states of an AUV and an MRS.

2.
We estimate the noise statistics of the heading and forward speed of an AUV and an MRS by using the variational Bayesian algorithm. The noisy forward speed is modeled by the Gaussian mixture distribution (GMD). The noisy heading angle is modeled by a von Mises (vM) distribution.

3.
We build a neural network compensator to make up for the prediction error caused by the process noise.

System Overview
In this section, we introduce an AUV and a mobile recovery system, which are illustrated in Figure 1. The MRS includes an AUV (is called AUV-I) and a docking station. The docking station was rigidly fixed to the underbelly of AUV-I. The entrance of the docking station was a funnel-like, 2 m in diameter. The AUV to be recovered is called AUV-I I. AUV-I I was a torpedo-shaped vehicle that was 384 mm in diameter and 5486 mm in length. It was equipped with a GPS, an inertial measurement unit (IMU), a Doppler velocity log (DVL) and an acoustic sensor. The acoustic sensor, Evologics 32C R, was used for long-range positioning. The AUV-I carries a transponder, and AUV-I I carries transceiver. The application scenario is: The MRS is moved with constant velocity and travels in a straight path, the AUV chases MRS to complete the homing process. The line-of-sight guidance algorithm is used to guide AUV to MRS. The AUV estimates the states of AUV and MRS based on TT-EKF with its own heading and forward speed. To estimate the PNCM, we model the heading and forward speed of AUV and MRS, and calculate the parameters of these distributions. The neural network is applied to compensate the prediction error. The framework of our proposed algorithm is shown in Figure 2.

Target Tracking Method Based on EKF
In this scenario, the AUV tracks MRS with the acoustic sensor, which measures the position of MRS in the AUV coordinate frame. The state of the target motion model is described by a nearly constant velocity model and at time step (k) includes the position in two dimensional (2D) Cartesian coordinates x and y and the velocity towards those coordinates axes v x and v y . All coordinate frames are shown in Figure 3.
In Figure 3, (x v , y v ) and ψ v are the position and heading of AUV, respectively. x f , y f and ψ f are the position and heading of MRS, respectively. O 1 is the global North, East coordinate frame, O 2 is the local AUV coordinate frame. The measurement at time index k can be expressed as: z k = z x z y T . z x and z y are the horizontal and vertical direction of the measurement components in O 2 local coordinate system. d x and d y are the horizontal and vertical direction of relative distance components between the AUV and MRS in global coordinate frame. The system state Cartesian coordinates is defined as follow: where x k is the system state at time index k, x v,k and y v,k are the Cartesian coordinates of AUV, x f ,k and y f ,k are the Cartesian coordinates of MRS,ẋ f ,k andẏ f ,k are the horizontal and vertical direction of the forward speed components of MRS in O 1 global coordinate frame. The state transition model is: is the forward speed of AUV at time index k, ψ v,k is the heading of AUV at time index k. u f ,k is the forward speed of MRS at time index k, ψ f ,k is the heading of MRS at time index k. w k is process noise which is assumed to be zero mean non-Gaussian noise, and its covariance is a fourth-order square matrix. The covariance of process noise is Q = is the noise variance of AUV speed, Q (ψ v ) is the noise variance of AUV heading, Q u f is the noise variance of MRS speed, Q ψ f is the noise variance of MRS heading. The MRS is tracked by an acoustic sensor in the underwater and provides measurements of the position of MRS in the AUV coordinate frame. The measurement model is given as where v k is uncorrelated, zero-mean white Gaussian noise with known second order covariance matrix. The measurement noise covariance maxtrix is denoted as R. The target tracking algorithm based on EKF is described as follows: where F, B k , Γ k , w k , Q, R have mentioned above. x k−1 and P k−1 are the state estimate and state covariance matrix at time index k − 1, respectively. x − k and P − k are the prediction state estimate and prediction state covariance matrix at time index k, respectively. x k and P k are the updated state estimate and updated state covariance matrix at time index k, respectively.ẑ k and z k are the predicted measurement and actual measurement at time index k, respectively. K k is the filter gain at time index k. S k is an auxiliary variable. h (·) is measurement model and H is measurement matrix, which is given by: According to the state vector x k , we can calculate the forward speed and heading of MRS at time index k.

Adaptive Target Tracking Algorithm Based on EKF
For the application scenarios in this paper, the forward speed and heading of MRS are unknown. The noise statistics of forward speed and heading of AUV and MRS are unknown. It is assumed that the measurement noise statistics are Gaussian and known. The adaptive TT-EKF is applied to simultaneously estimate the position of AUV and MRS and the horizontal and vertical direction of the velocity components of MRS with unknown PNCM.
The structure of proposed adaptive target tracking algorithm is shown in Figure 4. In the algorithm, Variational Bayesian algorithm is used to estimate the PNCM. A multilayer neural network which acts as a feedforward error compensator is embedded in target tracking system to deal with the prediction errors caused by the noisy input vector.

Variational Bayesian
The forward speed of AUV is non-Gaussian due to the non-Gaussian process noise. The heading angle is a periodic variable. The probability distribution of forward speed and heading is modeled in Section 5.1. The parameter estimation of probability distribution of forward speed and heading is calculated in Section 5.2. The PNCM is estimated in Section 5.3. The framework of the algorithm proposed in this section is shown in Figure 5:

Modeling of Forward Speed and Heading
In this paper, we use the Gaussian mixture distribution to model the forward speed of AUV and MRS. u is denoted as the forward speed of an AUV or an MRS.
where µ and Σ are the mean and variance of distribution. ∑ N n=1 ω n = 1, N is the number of Gaussian components of the GMD, and ω n , µ n , Λ n denote the weight, mean, and precision matrix of the n th Gaussian component, respectively. We introduce an N n -dimensional discrete latent variable z, where the element z n is a binary random variable that satisfies ∑ n z n = 1 and p (z n = 1) = π n . π is a probability. The conditional distribution p (z|π) is formulated as: Since the heading angle is a periodic variable, we use von Mises (vM) distribution to model the heading. ψ is the heading of AUV or MRS.
where I 0 in the normalization factor is the modified zero-order Bessel function of the first kind. Parameter m ≥ 0 denotes the concentration of the distribution around the mean ψ 0 and for m = 0 the distribution collapses in a uniform distribution. The probability density function (PDF) of a two-dimensional unit random vector l = [cos (ψ) , sin (ψ)] T is given by: where the PDF of l is characterized by the mean direction µ and the concentration parameter λ.

Parameters Estimation Based on VB
Variational Bayesian (VB) has been widely used to approximate the non-analytical posterior distribution by minimizing the Kullback-Leibler divergence (KLD) between the true posterior and a simple predefines distribution. The parameters estimation of GMD and vM distribution are described below, respectively.

Parameter Estimation of GMD
We can get the forward speed of AUV by the speed sensor. The observable data set of AUV speed concludes the forward speed of AUV at all time, i.e., U = [u v,k ] T 1:K . The observable data set of MRS speed can be obtained by: The observable data set U consists of the all forward speed at previous K time, and the kth component is denoted as u k . The conditional probability distribution of U with the latent variable set Z = {z 1 , · · · , z K } can be described by: (20) where µ = {µ n }, Λ = {Λ n }, which are the mean and precision matrix of Gaussian distribution. The application of conjugate prior distribution will greatly simplify the analysis process. The coefficient π is modeled by a Dirichlet distribution.
where α 0 is a parameter of the Direchlet distribution, α 0 is a set of α 0 . C (α 0 ) is a normalization constant of the Direchlet distribution. The joint distribution of µ and Λ is modeled by a Gaussian-Wishart distribution.
where m 0 and β 0 are the parameters of the Gaussian distribution. W 0 and ν 0 are the parameters of Wishart distribution. The joint distribution of U and distribution parameters is given by: The variational distribution is considered: Assuming that q (Z) and p (Z | π) have the same functional form, q (π) and p (π) have the same functional form, q (µ, Λ) and p (µ, Λ) have the same functional form. q (π) and p (µ, Λ) are given by: where α is parameter set of the Direchlet variational distribution. m, β, W and ν are the parameters of the Gaussian-Wishart variational distribution. The detailed derivation is given in [17]. The relevant formulas of VB algorithm for calculating the parameters of GMD are shown in Appendix A.

Parameter Estimation of vM Distribution
We can get the noisy heading of AUV by the angle sensor. we build a vector which constitutes the cosine set and sine set of AUV heading, i.e., L = [cos (ψ v,k ) , sin (ψ v,k )] T 1:K . The cosine set and sine set of MRS heading is obtained by: The goal is to infer the posterior distribution for the mean direction µ and concentration parameter λ given the set L. The generative model is given by the joint distribution of all random variables as: We assume that a vM-Gamma distribution as the prior over µ and λ as The conjugate conditional distribution of µ given λ is a vM distribution. The choice of the Gamma distribution for p (λ) is reasonable, and more specific introduction are provided by [24]. G (λ|a 0 , b 0 ) is a Gamma density function with the shape parameter a 0 and the inverse scale parameter b 0 . The specific expression is: where Γ (·) denotes the Gamma function, a 0 and b 0 are regarded as prior parameters. The derivation process and relevant formulas of VB algorithm for calculating the parameters of VM distribution are shown in Appendix B.

Estimation of PNCM
The forward speed and heading of AUV and MRS have been modeled by GMD and vM distribution. The parameters of GMD and vM distribution have been estimated by VB algorithm.
In this paper, Q (u v ) and Q u f are calculated in the same way, Q (ψ v ) and Q ψ f are calculated in the same way. The nth weight of Gaussian distribution is the expectation of Direchlet distribution, the nth mean of Gaussian distribution is the expectation of Gaussian distribution, the nth precision matrix of Gaussian distribution is the expectation of Wishart distribution, which are: The GMD in this paper contains two single Gaussian distributions. Q (u v ) and Q u f are calculated in the same way. The variance of this GMD is given by: We use a Gaussian distribution to approximate the vM distribution to estimate the variance of heading.
where K is the number of the cosine/sine of angle. k is index. s is the parameter of vM distribution.

Neural Network
Define an auxiliary input vector containing the forward speed and heading of AUV and MRS, which is given by: Denote the prediction error e k as the error between the motion incremental model with true input vector f (ū k ) and the motion incremental model with noisy input vector f (ū k + δū k ), given by: e k = f (ū k ) − f (ū k + δū k ). We use a neural network with a single hidden layer to estimate the e k , namely where net (ū k , Q, W k−1 ) denotes the neural network error compensator, and W k−1 indicates the linkweights and threshold value of the neural network, the inputs of neural network are the auxiliary input vectorū, the process noise covariance matrix Q, the desired output of neural network is prediction error e k . e k contains the horizontal and vertical direction of prediction error of AUV and MRS, given by: e k = e x,v e y,v e x, f e y, f T . where the subscript x means horizontal direction, subscript y means vertical direction, subscript v means AUV, subscript f means MRS. The motion incremental model is given by: f (a) = a · dt. where a denotes the input of the motion incremental model, and dt is the time interval.
The neural network has been widely used in a lot of fields since it is a good tool for modeling complicated nonlinear systems [25]. The neural network structure used in this paper is a multi-inputs and multi-outputs nonlinear system, which is shown in Figure 6. Each node that is connected by the links with all nodes in the adjacent layer computes a weighted sum of inputs. The node output is computed by passing the sum through a nonlinear function, e.g., sigmoid function. The jth output of hidden layer and kth output of output layer can be given by: where I is the number of inputs, which is equal to 8, J is the number of hidden layer nodes, which is equal to 5, K is the number of outputs, which is equal to 4; The output of hidden layer is O. w 2 kj and θ 2 kj are the linkweight and threshold value between the jth hidden layer node and the kth output; w 1 ji and θ 1 ji indicates the linkweight and threshold value between the ith input and the jth hidden layer node; The sigmoid function is chosen as the nonlinear transfer function. Backpropagation (BP) has been widely used in neural network training algorithm. We use BP to train the neural network proposed in this paper. The principle of BP is introduced in the Appendix C.

Algorithm Process
Overall, the process of proposed adaptive algorithm is summarized as following.

Initialization
-Initilize the PNCM Q, the measurement noise covariance matrix R, the initial state x 0 .
-Initilize the parameters used in VB algorithm.
-Initilize the neural network parameters state W 0 , which contains the linkweight and threshold value. -Initilize the prediction error e 0 , whose all compents are equals to 0.

MATLAB Simulation and Experimental Data Analysis
We verify the effectiveness of the proposed method through MATLAB simulation and Experimental data analysis.

MATLAB Simulation
Firstly, the MATLAB simulation results are presented to validate the proposed adaptive target tracking method based on EKF. The initial speed of MRS is u f ,0 = 0 m/s, the initial heading of MRS is ψ f ,0 = 20 • in O 1 coordinate frame. The initial position of MRS is x f ,0 , y f ,0 = (400, 300). The task of MRS is to move with u f = 0.3 m/s and travels in a straight path with ψ f = 20 • . The initial speed of AUV is u v,0 = 0 m/s, the initial heading of MRS is ψ v,0 = 0 • in O 1 coordinate frame. The initial position of AUV is (x v,0 , y v,0 ) = (300, 200). The task of AUV is to home to the MRS with u v = 0.5 m/s. The noise variance of the AUV heading is 0.0053. The noise variance of the forward speed of AUV is GMD, which contains two single Gaussian distributions. The parameters of GMD are the weights(w), mean(µ) and variance(Σ), which are shown in Table 1: Table 1. The parameters of GMD.

Weight
Mean Variance Initialize the parameters used in the adaptive target tracking algorithm based on EKF. The loss function is applied to test the performance of the proposed algorithm with a variety of conditions, and it is defined as: where  Table 2.  In this simulation, the real positions of AUV and MRS are calculated using the complicated motion model. The deviation between the real position and estimated position is positioning error.  Figure 8 based on TT-EKF with the PNCM is estimated by VB algorithm and the prediction error is compensated by BP neural network. In (a) and (b), the red lines are noisy forward speed and heading of AUV, respectively; The black lines are actual forward speed and heading of AUV, respectively; The blue lines are the estimated forward speed and heading of AUV based on VB algorithm, respectively. In (c) and (d), the red lines are noisy forward speed and heading of MRS, respectively; The black lines are actual forward speed and heading of MRS, respectively; The blue lines are the estimated forward speed and heading of MRS based on VB algorithm, respectively. The Figure  9 illustrates the real trajectory of AUV and MRS and estimated trajectory based on adaptive TT-EKF. To compare the positioning performance of the algorithm under various conditions, Tables 3 and 4     We use another set of simulation experiments to further illustrate the performance of the algorithm. The initial speed of MRS is u f ,0 = 0.3 m/s, the initial heading of MRS is ψ f ,0 = 20 • in O 1 coordinate frame. The initial position of MRS is x f ,0 , y f ,0 = (50, 50). The task of MRS is to move with u f = 0.3 m/s and travels in a straight path with ψ f = 45 • . The initial speed of AUV is u v,0 = 0 m/s, the initial heading of MRS is ψ v,0 = 0 • in O 1 coordinate frame. The initial position of AUV is (x v,0 , y v,0 ) = (0, 0). The task of AUV is to homing to the MRS with u v = 0.6 m/s. The noise variance of the AUV heading is 0.01. The noise variance of the forward speed of AUV is GMD, which contains two single Gaussian distributions. The parameters of GMD are the weights(w), mean (µ) and variance (Σ), which are shown in Table 5: Table 5. The parameters of GMD.

Weight
Mean Variance The simulation results are shown in Figures 10 and 11.

Experimental Data Analysis
The experimental data came from the tests on a lake at the xinanjiang experimental field of the institute of acoustics, Chinese Academy of Sciences in December 2018. AUV-I I sensed the position of MRS by using the ultra short baseline (USBL) system. The detailed descriptions of the docking test are given in reference [24]. The Experimental results are shown in Figure 12. The actual PNCM of AUV-I I is unknown, and the PNCM is given based on experience. We compare the performance of TT-EKF algorithm and adaptive TT-EKF algorithm with this PNCM. The original Experimental data analysis results are shown in Figures 13 and 14. Due to the sensor precision of AUV and the lake experimental environment, the measurement noise of AUV forward speed and heading is small. We add the same noise as the simulation to the input vector of AUV-II. The experimental data analysis results are shown in Figures 15 and 16. The representation of each color line in Figures 13-16 is the same as the simulation. From the MATLAB simulation results and Experimental data analysis results, we can know that:

1.
As the PNCM is unknown, we assumed the PNCM is Q 1 , which is inaccurate. The contrast between the red lines and the blue lines indicates that the inaccurate process noise statistics leads to the degradation of the state estimation performance of TT-EKF.

2.
The contrast between the red lines and the green lines indicates that the state estimation performance of TT-EKF has been improved with the PNCM is estimated by VB algorithm. 3.
The contrast between the green lines and the black lines indicates that the state estimation performance of TT-EKF has been further improved with the prediction error compensator based on neural network.

Conclusions
In this paper, we propose an adaptive target tracking method based on EKF with unknown non-Gaussian process noise. We use the Gaussian mixture distribution to model the noisy forward speed of AUV and MRS. We use a von Mises distribution to model the noisy heading of AUV and MRS. The parameters of these distribution are calculated based on variational Bayesian algorithm. The process noise covariance matrix is estimated according to the parameters of GMD and vM distribution. The prediction error caused by the process noise is compensated online based on BP neural network. According to the MATLAB simulation results and experimental data analysis results, we find that the state estimation performance of TT-EKF algorithm is affected by the accuracy of PNCM. If the PNCM is unknown or inaccurate, the state estimation performance of TT-EKF could be improved with the PNCM is estimated by VB algorithm. There are deviations between the real PNCM and the estimated PNCM, as well as between the real input vector and the estimated input vector. These factors cause prediction errors. The state estimation performance of TT-EKF could be further improved with the prediction error compensator based on neural network. However, the positioning error of MRS in the initial period is very large, because there is little understanding of MRS in this period. With the continuous measurement of MRS by AUV, the positioning performance of MRS is getting better and better. The future work plan includes: (1) reducing the positioning error of MRS in initial period by improving the TT-EKF algorithm; (2) the working effect of the prediction error compensator is not very satisfactory, and then further improving the performance of the neural network compensator; (3) the algorithm is tested in the ocean.  where r, ρ, N,ū, S are the intermediate variables of the formulas. E [·] represents the expectation. ψ (·) is Digamma function. The subscript k is the index of observable data set U, n is the index of the GMD.
The derivation process and related formula are complicated, we directly use the derivation results in [26] as follows: a = a 0 + k + βλ ∂ ∂βλ log I 0 βλ a 0 + k + βλ