Abstract
The Gaussian mixture filter can solve the non-Gaussian problem of target tracking in complex environment by the multimode approximation method, but the weights of the Gaussian component of the conventional Gaussian mixture filter are only updated with the arrival of the measurement value in the measurement update stage. When the nonlinear degree of the system is high or the measurement value is missing, the weight of the Gauss component remains unchanged, and the probability density function of the system state cannot be accurately approximated. To solve this problem, this paper proposes an algorithm to update adaptive weights for the Gaussian components of a Gaussian mixture cubature Kalman filter (CKF) in the time update stage. The proposed method approximates the non-Gaussian noise by splitting the system state, process noise, and observation noise into several Gaussian components and updates the weight of the Gaussian components in the time update stage. The method contributes to obtaining a better approximation of the posterior probability density function, which is constrained by the substantial uncertainty associated with the measurements or ambiguity in the model. The estimation accuracy of the proposed algorithm was analyzed using a Taylor expansion. A series of extensive trials was performed to assess the estimation precision corresponding to various algorithms. The results based on the data pertaining to the lake trial of an unmanned underwater vehicle (UUV) demonstrated the superiority of the proposed algorithm in terms of its better accuracy and stability compared to those of conventional tracking algorithms, along with the associated reasonable computational time that could satisfy real-time tracking requirements.
1. 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. The Gaussian hypothesis is poor when the system model is highly nonlinear, and the estimation results are divergent. The 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]. The 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. The 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. The 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. The 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. This 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. This paper proposes an adaptive weight update scheme of the Gaussian components for the Gaussian mixture filter in the time update stage. This 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. The Gaussian mixture filter is improved through combination with the CKF. The Gaussian components are predicted and updated using a CKF with the results merged and weighted. The 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. The 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-CKF-based navigation algorithm.
2. The Gaussian Mixture Cubature Kalman Filter (GM-CKF)
Consider a nonlinear system model with non-Gaussian noise, as follows:
The prior and posterior densities can be defined using a Gaussian sum. The GM-CKF was designed by combining the Gaussian mixture density distribution model with the CKF. The algorithm for the GM-CKF can be described as follows.
The probability density function for the initial system state (1) can be defined as
The state and covariance can be propagated using the CKF time update stage as follows:where is the number of cubature points; ; denotes the number of Gaussian components in the system state decomposition; denotes the number of Gaussian components decomposed by process noise of the system; denotes the weight of the Gaussian components after the time update; is the weight of the Gaussian noise of the process noise; is the weight of the state Gaussian component at time ; denotes the weight after the propagation; and denotes the covariance error for the covariance error matrix .
The state and covariance are updated in the CKF measurement update stage. The final outputs of the filter and the weights of the components are updated as follows:where is the weighted value of the Gaussian component, which can be updated as follows:where is the weight of the Gaussian component of the observation noise. is the observation likelihood distribution of the Gaussian component and can be calculated as follows:
Note that the weights do not change during the propagation from (7), which is acceptable if the system has a precise model. The reason that the weight remains constant is that the covariance is assumed to be sufficiently small. This aspect is a problem particularly when the system model has a large uncertainty and the measurements are not frequently available.
3. Adaptive Weight Update
Consider the nonlinear system (1) with the probability density function of the initial conditions being . The Gaussian mixture approximation of the probability density function is defined as
The true probability density function of the system state can be defined using the Chapman–Kolmogorov formula [25]:
The mean-square optimal new weights can be obtained by minimizing the following integral square difference between the true probability and the approximated one by using the least squares algorithm:where denotes the vector of the weights of every Gaussian component at time . 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 . The second term denotes the cross likeness of the true probability and the approximation, and . The 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 ; specifically, , 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 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 implies . can be rewritten as
The derivation of the terms of the cost function can be expressed as formula (16):
Similarly,
The elements of the matrices are given as follows:where denotes are the cubature points.
The 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:where .
Based on the above derivation, the summary of the IGM-CKF with the adaptive weight update can be presented as in Table 1.
4. Analysis of the Estimation Accuracy of IGM-CKF
The 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.
4.1. Time Update
To evaluate the proposed estimation accuracy, the IGM-CKF is expanded into the Taylor terms. Considering the Gaussian component and defining , the state transition function can be expanded into a Taylor series as follows:Where and denotes the differential of .
The estimated value of the cubature points is
The predicted value of the Gaussian component is
All of the odd moments in the formula sum to 0 because of the symmetrisation of . Therefore, 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 , where is the Jacobian matrix of , and the formula can be rewritten as
The aforementioned formulas indicate that the proposed algorithm can propagate the second-order and higher-order terms of the nonlinear system. The higher-order terms of each Gaussian component are regarded as 0 when the system is linear, which is the same as in the EKF.
4.2. Measurement Update
Similar to the time update stage described in Section 4.1, we define and . The measurement function can be expanded into a Taylor series as follows:
The estimated value of the cubature points is
Analogous to the calculation in the time update stage, the self-correlation covariance matrix is calculated as
Considering that is the Jacobian matrix of , the formula can be rewritten as
Similarly, the cross correlation covariance matrix can be calculated as
The Kalman gain can be calculated using the following expression:
The posterior state and covariance matrix can be calculated as
The above deduction indicates that the IGM-CKF can approximate the state that is related not only to the first-order terms but also to the higher-order terms. This finding demonstrates that the IGM-CKF can capture the higher-order terms of the nonlinear function.
5. 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 is the zero-mean Gaussian process noise with variance . 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 .
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 , , , and , along with an initial state covariance of . 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. The number of cubature points in the GM-CKF was selected to be 2, resulting in a dimension of state of 1. The number of simulation steps was set as 100.
The 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 . The initial weight value of each component was determined using
The process noise was selected as a single component, with a value given of , 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. The 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.
The accuracy achieved using the filtering methods was analyzed in terms of the following metrics.
The root-mean-square error (RMSE) is defined aswhere is the simulation step time and and denote the state and estimate values at time , respectively.
The average noncredibility index (NCI) [29, 30] is defined aswhere is the covariance of the estimation provided by the filter and RMSE denotes the mean-square error of the estimation, as calculated using (42).
The 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. The simulation results indicate that the GM-CKF and IGM-CKF exhibit the same estimation precision. The RMSE values indicate that the precision of the IGM-CKF is better, exhibiting a decrease of approximately 22.5% in the RMSE. The 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. The 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 . The complexity to obtain the matrix , as defined in (23), is , and the complexity of can be obtained using the , where is the number of cubature points. The additional computational cost for the optimizing the weights can be solved in polynomial time using , where is the length of the weight matrix.
6. Experimental Verification of the Navigation Algorithm Based on IGM-CKF
6.1. System Model
The method uses simple 3-degree-of-freedom (DOF) constant velocity kinematics to predict the evolution of the state from time:where denotes the position and heading of the UUV in global coordinates; represents the linear and angular velocities of the UUV in vessel coordinates; , respectively, denote the velocities of the UUV in the directions of surge and sway; and is the yaw angle velocity.
The input includes the location , the heading , and the velocities . is the process noise with covariance .
6.2. Observation Model
The UUV uses a Doppler velocimeter (DVL) and OCTANS [31] to record direct measurements of the velocity and heading of the vehicle, respectively. Thus, the observation model can be defined as follows:where is the observation vector with 3 components, including the heading and the line velocities in the direction of surge, sway, and heave, respectively; varies with changes in the measurement; and is the observation noise. The observation data of the UUV have a non-Gaussian density because of the underwater environment and the characteristic of the sensors. The 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.
6.3. Simulation and Analysis
The data used in the test conducted in this work were obtained from a UUV lake trial completed in March 2016 at in a region with a size of region and a trial performed in September 2018 at in a region with a size of approximately . The position, velocity, and heading data of the UUV were measured using a GPS, DVL, and OCTANS, respectively. The longitude and latitude data were transformed into geodetic coordinates. A total of 1,450 and 39,000 data points were used. The experimental results pertaining to the GM-EKF, GM-UKF, GM-CKF, cubature Kalman particle filter (CKPF), and IGM-CKF were compared.
The system and observation models, respectively, defined in formulas (43) and (44) were employed. The observation noise was modeled using formula (39), where , , , and . The number of particles of the CKPF was 100. The system process noise covariance and observation noise covariance values are listed in Table 3.
The system state was split into 12 Gaussian components in the IGM-CKF algorithm, and the initial weight values of all the components were equal. The 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 , and the end point is .
Figures 4 and 5 show a comparison of the position errors of different algorithms, in the eastern and northern directions, respectively, under 1,450 data points.
Figures 4 and 5 indicate that the GM-EKF exhibits the worst stability, and an error of up to 10 m exists in the east and north directions. The improvement in performance of the GM-UKF compared to that of the GM-EKF can be clearly observed, and the error is limited to 2 m. The position error of the IGM-CKF is smaller than that of the GM-CKF; the error in the east direction is limited to 1.5 m, and the maximum error in the north direction is 1.5 m. The position error of the CKPF is similar to that of the IGM-CKF; the errors in the east and north directions are approximately 1.4 m and 1.3 m, respectively.
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. The various filters had the same initialization conditions as described previously for the test. The results of the trajectory estimations pertaining to different algorithms vs. the GPS trajectory are shown in Figure 6.
Figures 7 and 8 show a comparison of the position errors in the eastern and northern directions, pertaining to the different algorithms under 39,000 data points.
From Figure 8, it can be observed that the stability of the GM-EKF is the worst in the east direction; the GM-UKF and IGM-CKF exhibit similar accuracies when 30,000 data points are used, whereas the accuracy and stability of the GM-CKF are worse. The GM-CKF and IGM-CKF exhibit similar accuracies under 30,000–39,000 data points, whereas the GM-UKF exhibits poor navigational accuracy. The IGM-CKF exhibits the best navigation accuracy and stability in the east direction. From Figure 8, it can be observed that the GM-UKF exhibits an error accumulation trend with the worst accuracy in the north direction; the largest navigation error is 60 m. The maximum errors of the GM-EKF and GM-CKF are 20 m, and the stability of the GM-EKF is worse. The IGM-CKF exhibits the best navigation accuracy and stability in the north direction, with the error limited to 10 m.
Similar to formulas (42) and (43), we define the position RMSE and NCI of different navigation algorithms using the following expressions:where is the run time steps; and are, respectively, the true and estimated positions of the UUV at time ; and and , respectively, denote the diagonal elements in the covariance matrix of the position information and .
Table 4 lists the values of the RMSE, NCI, and computation cost for one iteration of the different tested algorithms. The values indicate that the RMSE and run times of the various algorithms increase with increase in the amount of test data. The run time cost for the GM-EKF is the lowest, although it exhibits the worst navigation accuracy. The 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. The time costs for the GM-CKF and IGM-CKF are considerably greater because they both need to estimate 12 cubature points. The navigation accuracy in these cases with 39,000 data points processed is considerably better, with an increase of approximately 36.8%. The 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. The NCI values of the IGM-CKF and GM-CKF are less than 1, which suggests that the filters have a high credibility. The 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.
7. Conclusions
This paper proposes an alternative efficient nonlinear filtering algorithm, which is a Gaussian mixture cubature Kalman filter involving weight updation of the Gaussian components. The 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. The feasibility of the algorithm was proved using a set of simulation trials. The navigation algorithm was compared with conventional navigation algorithms and combined with a motion model and observation model based on lake trial data. The 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
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This study was supported in part by the National Natural Science Foundation of China (nos. 61633008 and 51609046) and the Research Fund from Science and Technology on Underwater Vehicle Technology (grant no. 614221502061701).