Adaptive Weight Update Algorithm for Target Tracking of UUV Based on Improved Gaussian Mixture Cubature Kalman Filter

,


Introduction
Nonlinear filtering is widely applied in the fields of target tracking, localization, navigation, signal processing, communication, and automatic control. Based on the stochastic state space mode, the core task of nonlinear filtering involves the computation of the probability density function (PDF). Bayesian estimation is usually used to handle nonlinear filtering problems, which can provide an optimal solution for the dynamic state estimation by iteratively estimating the PDF of the states [1]. However, since a multidimensional nonlinear integral has no analytic solution for nonlinear stochastic dynamic systems, most nonlinear filters cannot provide an optimal estimation. To solve the problems of state estimation for nonlinear stochastic dynamic systems, Gaussian approximation filtering is widely applied because of its high estimation accuracy and computational efficiency.
A classic nonlinear Gaussian mixture filter is the EKF [2], which approximates the nonlinear system model using a first-order Taylor expansion. e Gaussian hypothesis is poor when the system model is highly nonlinear, and the estimation results are divergent. e core task of the Gaussian mixture filter is to compute the Gaussian weight integral, and various algorithms have been proposed to enable nonlinear filtering. Unscented Kalman filters (UKFs) [3,4] are nonlinear filters based on the unscented transformation. In principle, a UKF is simple and easy to implement as it does not require the calculation of Jacobian matrices at each time step. An improved UKF algorithm [5] was proposed that the modeling of target state and measurement vectors is carried out, and the UKF is integrated into the model to result in evolution of simulator. A new kind of the nonlinear state estimation algorithm [6] was proposed, which is robust to non-Gaussian noise. Furthermore, a cubature Kalman filter (CKF) [7], whose sampling approximation method is similar to that of the UKF, was proposed; in this approach, the state of the CKF approximation system and the probability density obtained using the spherical-radial cubature rule can be used to determine the propagation system state without linearizing the nonlinear model [8].
e CKF has several advantages such as excellent stability, low calculation complexity, and high estimation accuracy. To improve the performance of the CKF, Jia et al. [9] proposed a new class of CKFs having arbitrary degrees of accuracy to compute the spherical and radial integrals. Sparse-grid quadrature nonlinear filtering [10] was proposed to approximate the multidimensional integrals using weighted sparse-grid quadrature points, and such filters could achieve higher accuracy than conventional filters could. To address the problem of low accuracy of the CKF in several practical applications, a square root embedded CKF [11] was proposed, which employed an embedded cubature rule. e spherical simplex radial CKF [12] was proposed to improve the accuracy and efficiency of the traditional CKF; the simplex radial CKF calculated the spherical and radial integrals using a regular simplex, whereas the conventional CKF employed the moment matching method for the same purpose. e stochastic integration filter [13] was proposed to realize the estimation of the asymptotically exact integral by using the stochastic integration method. A robust PF-based multitarget tracking solution for passive sonar systems [14] was proposed to track an unknown time-varying number of multiple targets while keeping continuous track of such targets. e PF algorithm is unable to avoid the disadvantages of particle degradation and sample dilution; hence, the research on PFs is primarily focused on the resolution of particle degradation and sample dilution [15,16]. To overcome the problem of particle degradation, several studies focused on the design of the importance density function [17,18], and the authors proposed that the importance density function be designed using the EKF, UKF, and CDKF. In the phase of measurement updation, more particles are moved to the region with a higher likelihood function, which alleviates the particle degradation problem, and the precision of this approach is better than that attained using the traditional particle filter algorithm.
Most suboptimal nonlinear filtering algorithms implicate an important hypothesis condition that the system state has a single mode. However, in practical situations, several problems are more complex. A typical case is one in which the noise of the system processes or observations do not have ideal Gaussian densities. In addition, the various Gaussian approximation filtering algorithms based on Gaussian noise do not demonstrate ideal performance because of the mismatched model. One approach to resolve this problem is the multimode approximation method [19], which involves adopting several Gaussian components to represent the Gaussian mixture density to approximate the multimode problems, yielding the so-called Gaussian sum filter. Recently, a modified interacting multiple model filter was proposed [20], which was derived for jump Markov systems with unknown process and measurement noise covariances. Using the inverse Wishart distribution as the conjugate prior of noise covariances, the system state together with the noise parameters for each mode is inferred by the variational Bayesian method.
In the literature [21], the Gaussian-sum CKF has been proposed for bearings-only tracking problems, in which the CKF demonstrates better performance than the particle filter does. Furthermore, for highly nonlinear passive tracking systems, an improved Gaussian mixture filter algorithm has been proposed in the literature [22], and the limited Gaussian mixture model has been used to approximate the posterior density of the state and process noises and measurement noises. However, in all these methods, while propagating the uncertainty through a nonlinear system, the weights of the Gaussian components are not regular and are updated only in the measurement update stage.
is assumption is valid if the system is marginally nonlinear or the measurements are precise and frequently available. Terejanu et al. [23,24] proposed a new Gaussian sum filter by adapting the weights of the Gaussian components for the time and measurement update stages; the proposed approach could achieve a better approximation of the posterior probability density function.
In summary, when the conventional Gaussian mixture filter transfers the Gaussian component by using the state transfer model in the time update phase, the weight of the Gaussian component remains unchanged, and the weight of the Gaussian component is only updated with the arrival of the measurement value in the measurement update phase. When the equivalent measurement value is not enough to satisfy the real-time update, the state estimation accuracy will decrease if the weight remains unchanged.
is paper proposes an adaptive weight update scheme of the Gaussian components for the Gaussian mixture filter in the time update stage.
is method contributes to obtaining a better approximation of the posterior probability density function, which is constrained by large uncertainty in the measurements or ambiguity in the model. e Gaussian mixture filter is improved through combination with the CKF. e Gaussian components are predicted and updated using a CKF with the results merged and weighted. e estimation accuracy of the proposed algorithm is analyzed using a Taylor expansion. Section 2 describes the principles of the Gaussian mixture CKF algorithm. In Section 3, the method of the Gaussian component weight updating is introduced, and the IGM-CKF algorithm is described in Section 4. e results of a simulation experiment conducted to compare the various filtering algorithms are presented in Section 5. Finally, Section 6 presents the results of the trials performed to verify the feasibility of the proposed UUV IGM-CKFbased navigation algorithm.

The Gaussian Mixture Cubature Kalman Filter (GM-CKF)
Consider a nonlinear system model with non-Gaussian noise, as follows: e prior and posterior densities can be defined using a Gaussian sum. e GM-CKF was designed by combining the Gaussian mixture density distribution model with the CKF. e algorithm for the GM-CKF can be described as follows. e probability density function for the initial system state (1) can be defined as e state and covariance can be propagated using the CKF time update stage as follows: where m is the number of cubature points; r � (i − 1)I + j; I denotes the number of Gaussian components in the system state decomposition; j denotes the number of Gaussian components decomposed by process noise of the system; ω t k|k− 1 (r) denotes the weight of the Gaussian components after the time update; ω p k (j) is the weight of the j − th Gaussian noise of the process noise; ω s k− 1|k− 1 (i) is the weight of the i − th state Gaussian component at time k − 1; ω s k|k− 1 denotes the weight after the propagation; and w k denotes the covariance error for the covariance error matrix Q k . e state and covariance are updated in the CKF measurement update stage. e final outputs of the filter and the weights of the components are updated as follows: where ω k (n) is the weighted value of the n − th Gaussian component, which can be updated as follows: where ω m k (l) is the weight of the l − th Gaussian component of the observation noise. p(z k | x k , n) is the observation likelihood distribution of the n − th Gaussian component and can be calculated as follows: Note that the weights ω s k|k− 1 (i) do not change during the propagation from (7), which is acceptable if the system has a precise model. e reason that the weight remains constant is that the covariance is assumed to be sufficiently small. is aspect is a problem particularly when the system model has a large uncertainty and the measurements are not frequently available.

Adaptive Weight Update
Consider the nonlinear system (1) with the probability density function of the initial conditions being p(x 0 ). e Gaussian mixture approximation of the probability density function is defined as e true probability density function of the system state can be defined using the Chapman-Kolmogorov formula [25]: e mean-square optimal new weights can be obtained by minimizing the following integral square difference between the true probability p(x k ) and the approximated one p(x k ) by using the least squares algorithm: where w k|k− 1 � ω k|k− 1 (1) ω k|k− 1 (2) . . . ω k|k− 1 (I) T denotes the vector of the weights of every Gaussian component at time k. To resolve formula (13), the cost function can be defined as follows [26]: where the first term denotes the self-likeness of the true probability density function of the system state and J pp (w k|k− 1 ) � R n p(x k )p(x k )dx k . e second term denotes the cross likeness of the true probability and the approximation, and J pp (w k|k− 1 ) � R n p(x k )p(x k )dx k . e last term represents the self-likeness of the approximation probability of the system state, and It is assumed that the Gaussian mixture approximation is equal to the true probability density function at time k − 1; specifically, p(x k− 1 ) ≈ p(x k− 1 ), and the proof procedure is as follows.
Substitute formula (11) into formula (12) and the following relationship can be given: where Assuming that all covariance p k− 1 (i) of the Gaussian components is small enough so that the linearization around a mean is representative for the dynamics in the vicinity of the respective mean and that there are sufficient number of Gaussian components, and then can be rewritten as e derivation of the terms of the cost function can be expressed as formula (16): Similarly, e elements of the matrices are given as follows: where (ω c , ξ c ) denotes are the cubature points.
e first term in the cost function (13) is not required in the optimization process, and it is only used to represent an overall magnitude of the uncertainty propagation error [25]. According to the aforementioned relations, the final formulation of formula (13) can be expressed as follows:

Complexity
Based on the above derivation, the summary of the IGM-CKF with the adaptive weight update can be presented as in Table 1.

Analysis of the Estimation Accuracy of IGM-CKF
e estimation accuracy of the UKF [27] and CKF [28] has been discussed using the Taylor expansion. Using a similar approach, the performance and estimation accuracy of the proposed algorithm were further examined and evaluated mathematically using the Taylor expansion, as described in this section.

Time Update.
To evaluate the proposed estimation accuracy, the IGM-CKF is expanded into the Taylor terms.
can be expanded into a Taylor series as follows: Where e estimated value of the cubature points is e predicted value of the Gaussian component is All of the odd moments in the formula sum to 0 because of the symmetrisation of x k|k− 1 (r). erefore, the formula can be rewritten as To simplify, we retain the second-order term of the formula by neglecting the higher-order terms: According to the definition of the covariance matrix, the second-order term of the formula is Combining formulas (25) and (28), the covariance matrix can be determined as Similar to the case of the EKF, we define T x f � Φ k x, where Φ k is the Jacobian matrix of f(x), and the formula can be rewritten as Complexity 5 e aforementioned formulas indicate that the proposed algorithm can propagate the second-order and higher-order terms of the nonlinear system. e higher-order terms of each Gaussian component are regarded as 0 when the system is linear, which is the same as in the EKF.

Measurement
Update. Similar to the time update stage described in Section 4.1, we define ( ). e measurement function h(x k | k− 1 (r)) can be expanded into a Taylor series as follows: (32) e estimated value of the cubature points is Analogous to the calculation in the time update stage, the self-correlation covariance matrix is calculated as Time update Complexity Considering that H k is the Jacobian matrix of h(·), the formula can be rewritten as Similarly, the cross correlation covariance matrix can be calculated as e Kalman gain can be calculated using the following expression: e posterior state and covariance matrix can be calculated as e above deduction indicates that the IGM-CKF can approximate the state that is related not only to the firstorder terms but also to the higher-order terms. is finding demonstrates that the IGM-CKF can capture the higherorder terms of the nonlinear function.

IGM-CKF Simulation
Next, we compared the performance of the GM-EKF, GM-UKF, GM-CKF, and IGM-CKF with the naïve model: where w k− 1 is the zero-mean Gaussian process noise with variance Q. v k is the glint noise, which can be modeled as the mixing form of the Gaussian and Laplace distributions or a combination of several Gaussian distributions with different covariances; in the simulation performed in this study, the glint noise was modeled as the additive sum of two Gaussian distributions with different covariances.
where ε is the glint frequency factor and ε � 0.25.
To implement a fair and reasonable performance comparison of different filter algorithms, an important requirement was that all the filters undergo the same initialization with an initial state of x 0 � 1, Q � 10, R 1 � 0.1, and R 2 � 1, along with an initial state covariance of P 0 � 10. In addition, the parameters α, β, and κ of the GM-UKF were selected as 1, 2, and 0, respectively, and five sigma points were present in the unscented transformation.
e number of cubature points in the GM-CKF was selected to be 2, resulting in a dimension of state of 1. e number of simulation steps was set as 100.
e system state was split into 4 Gaussian components in the IGM-CKF algorithm, and each component was generated around the state value along with a random P 0 . e initial weight value of each component was determined using e process noise was selected as a single component, with a value given of Q � 10, and the observation noise was split into 2 Gaussian components. Under the given conditions, the number of state Gaussian components increased from 4 to 8 when one recursion was completed. e redundant components were merged, and thus the total number of components remained 4. Figures 1 and 2 show the estimation results and a comparison of the errors associated with the various filtering algorithms. e accuracy achieved using the filtering methods was analyzed in terms of the following metrics. e root-mean-square error (RMSE) is defined as where t step is the simulation step time and x k and x k denote the state and estimate values at time k, respectively. e average noncredibility index (NCI) [29,30] is defined as where P k|k is the covariance of the estimation provided by the filter and RMSE denotes the mean-square error of the estimation, as calculated using (42). e RMSE and NCI values for various filtering algorithm calculations are presented in Table 2.
From Figures 1 and 2 and Table 2, it can be observed that the estimation performance of the GM-EKF is the worst and prone to fluctuations. e simulation results indicate that the GM-CKF and IGM-CKF exhibit the same estimation precision. e RMSE values indicate that the precision of the IGM-CKF is better, exhibiting a decrease of approximately 22.5% in the RMSE. e average NCI provides an evaluation of the relative estimation error and also of the self-assessment provided by each filter in the form of the covariance matrix of the estimated error. e average NCI values of the GM-UKF, GM-CKF, and IGM-CKF are listed in Table 1, and they indicate that the filters are credible.
If it is assumed that the dimension of state is N, the dimension of observation is M, and the number of Gaussian components is IJL, the overall naïve computational complexity of the filter due to the time and measurement updates is O(N 3 + M 3 + N 2 M + NM 2 ). e complexity to obtain the matrix J pp , as defined in (23)

Experimental Verification of the Navigation
Algorithm Based on IGM-CKF 6.1. System Model. e method uses simple 3-degree-offreedom (DOF) constant velocity kinematics to predict the evolution of the state from time: x where [x, y, ψ] denotes the position and heading of the UUV in global coordinates; [u, v, r] represents the linear and angular velocities of the UUV in vessel coordinates; u and v, respectively, denote the velocities of the UUV in the directions of surge and sway; and r is the yaw angle velocity. e input includes the location (x, y), the heading ψ, and the velocities u and v. w k− 1 is the process noise with covariance Q.

Observation Model.
e UUV uses a Doppler velocimeter (DVL) and OCTANS [31] to record direct measurements of the velocity and heading of the vehicle, respectively. us, the observation model can be defined as follows: where z is the observation vector with 3 components, including the heading ψ and the line velocities u and v in the   8 Complexity direction of surge, sway, and heave, respectively; H varies with changes in the measurement; and v k is the observation noise. e observation data of the UUV have a non-Gaussian density because of the underwater environment and the characteristic of the sensors. e observation noise could be approximated as glint noise, which is composed of the Gaussian noise and Laplace noise. For simplicity, in this study, we modeled the observation noise as 2 Gaussian components with different covariances.

Simulation and Analysis.
e data used in the test conducted in this work were obtained from a UUV lake trial completed in March 2016 at 119°E, 29°N in a region with a size of 350 m × 350 m region and a trial performed in September 2018 at 127°E, 45°N in a region with a size of approximately 600 m × 600 m . e position, velocity, and heading data of the UUV were measured using a GPS, DVL, and OCTANS, respectively. e longitude and latitude data were transformed into geodetic coordinates. A total of 1,450 and 39,000 data points were used. e experimental results pertaining to the GM-EKF, GM-UKF, GM-CKF, cubature Kalman particle filter (CKPF), and IGM-CKF were compared.
e system and observation models, respectively, defined in formulas (43) and (44) were employed. e observation noise was modeled using formula (39), where ε � 0.25, u 1 � u 2 � 0, R 1 � R, and R 2 � 100R. e number of particles of the CKPF was 100. e system process noise covariance and observation noise covariance values are listed in Table 3.
e system state was split into 12 Gaussian components in the IGM-CKF algorithm, and the initial weight values of all the components were equal. e process noise was selected to have a single component, and the observation noise was split into 2 Gaussian components. Under the given conditions, the number of state Gaussian components increased from 12 to 24 while finishing one recursion and the redundant components were merged so that the total number of components remained constant. Figure 3 shows the results of the trajectory estimations from the different algorithms versus the GPS trajectory.
In Figure 3, the starting point is (0, 0), and the end point is To verify the performance of the IGM-CKF-based navigation algorithm under long-endurance conditions, 39,000 data points from the lake trial were selected. e various filters had the same initialization conditions as described previously for the test. e results of the trajectory estimations pertaining to different algorithms vs. the GPS trajectory are shown in Figure 6.     Complexity 10 ln where t step is the run time steps; (x vx,k , x vy,k ) and (x vx,k , x vy,k ) are, respectively, the true and estimated positions of the UUV at time k; and P 2 vx,k and P 2 vy,k , respectively, denote the diagonal elements in the covariance matrix P of the position information x and y. Table 4 lists the values of the RMSE, NCI, and computation cost for one iteration of the different tested algorithms. e values indicate that the RMSE and run times of the various algorithms increase with increase in the amount of test data. e run time cost for the GM-EKF is the lowest, although it exhibits the worst navigation accuracy. e run time of the GM-UKF is higher because of the calculation of the sigma points, but the navigation accuracy is better than that of GM-EKF. e time costs for the GM-CKF and IGM-CKF are considerably greater because they both need to estimate 12 cubature points. e navigation accuracy in these cases with 39,000 data points processed is considerably better, with an increase of approximately 36.8%. e navigation accuracy of the CKPF is similar to that of the IGM-CKF; however, the run time of the CKPF is the largest among those of all the algorithms. e NCI values of the IGM-CKF and GM-CKF are less than 1, which suggests that the filters have a high credibility. e NCI of the GM-EKF is significantly larger than 1, which suggests that the credibility of the filter is poor, likely due to the high nonlinearity and non-Gaussian distribution. In addition, it was determined that the IGM-CKF algorithm can ensure a high navigation accuracy in long-endurance navigation of the UUV. Although the run time of IGM-CKF is the longest, it can satisfy the requirements for the real-time navigation of UUVs.

Conclusions
is paper proposes an alternative efficient nonlinear filtering algorithm, which is a Gaussian mixture cubature Kalman filter involving weight updation of the Gaussian components.
e proposed algorithm approximates the non-Gaussian noise by splitting the system state, process noise, and observation noise into several Gaussian components, and it updates the weight of the Gaussian components in the time update stage.
In this paper, an IGM-CKF algorithm was proposed to adaptively update the Gaussian component weights in the time update phase by using the Chapman-Kolmogorov equation, which is used to solve the problem that the Gaussian component weights in the conventional Gaussian mixture filter remain unchanged in the time update phase when the system is highly nonlinear. e feasibility of the algorithm was proved using a set of simulation trials. e navigation algorithm was compared with conventional navigation algorithms and combined with a motion model and observation model based on lake trial data. e comparison indicated that the IGM-CKF algorithm represents a novel approach for UUV navigation and location. Future work on this topic will include examining the filtering performance of the IGM-CKF with highly nonlinear noise and the calculation of the optimal number of split components.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that they have no conflicts of interest.