Abstract
We propose a robust method for tracking nonlinear target with the fusion unscented Kalman filter (FUKF). We noticed that when some outliers exist in the measurements of the sensors, they cannot track the target accurately by using the standard Kalman filters. The robust statistics theory is used in this paper to solve this problem. The measurement noise variance which is at the time of the outlier is restructured through minimizing the designed cost function. Then, the standard fusion unscented Kalman filter is used to track the target in order to avoid the bias brought by the linear approximation. Compared to the traditional tracking method and Huber robust method (HFUKF), this method has a more accurate performance and can track the target efficiently while the outliers exist. Last, simulation examples in three different conditions are given and the simulation results show the advantages of the proposed method over the fusion unscented Kalman filter (FUKF) and the Huber robust method (HFUKF).
1. Introduction
Multisensor networks have received increasing attentions in recent years, due to the huge potential in applications, such as communication, signal process, routing and tracking, and many other areas. It is well known that the standard Kalman filter (KF) is an available method to estimate the state parameters of the linear system given by the equation set composed of the dynamic model and the measurement model. And many methods have been presented to extend the KF to the nonlinear dynamic and measurement models by forming a Gaussian approximation to the posterior state distribution such as the extended Kalman filter (EKF) [1] and the unscented Kalman filter (UKF) [2]. However, the Taylor expansion is used in the EKF method to approximate the nonlinear system by ignoring the nonlinear part of the Taylor expansion. These crude approximations in the EKF will lead to a poor accuracy in estimating the states. As a promising alternative for EKF, UKF does not need to ignore any information of the system, in which the probability distribution of the model state is approximated by a set of deterministically chosen sample points known as sigma points and propagated through the nonlinear process and measurement equations. Not needing to calculate the Jacobian/Hessian matrices, the UKF is much easier to implement and performs better than the EKF. The UKF has been successfully applied to many practical problems [3].
Unfortunately, in practice, the distribution of the noise is not strict Gaussian distribution. The noise is characterized by too many elements which would generate high-intensity noise realizations, named outliers. The contaminated Gaussian noise and observation outliers appear naturally in many areas of engineering, such as hardware discontinuities in digital control systems, tracking in aerospace applications, and power system state estimation especially tracking in underwater environment. Because the Kalman filters do not take these outliers into account, the Kalman filters perform very poorly when the outliers appear. In other words, the standard Kalman filters may not be robust enough if the covariances of the process and measurement noise are changed [4]. In order to solve this problem, many methods have been proposed. For example, the Gaussian sum approach assumes the noise probability distribution function to be known a priori and it is computationally intensive with increasing order of state variables [5]. The proposed particle filter in [6] can also achieve robust estimation. But they still may not be feasible for real-time implementation in many applications. An efficient way to solve the non-Gaussian distribution problem is to apply robust statistical theory which relies on Huber’s generalized maximum likelihood methodology. The Huber technique has long been used in dynamic filtering problem, including underwater vehicle tracking. It is a combined minimum and -norm estimation technique and it exhibits robustness with respect to deviations from the assumed Gaussian distribution. The Huber technique has been directly applied into the linear Kalman filter and the extended Kalman filter. The document [7] has proposed an algorithm called Huber-based unscented filtering (HUF), which applies Huber technique into unscented Kalman filter. However, this algorithm also approximates the nonlinear measurement equation as linear equation, which will result in a loss of accuracy.
In this paper, we have analyzed the linearized Huber-based Kalman filter and find out how the Huber cost function affects the Kalman gain and the estimation states. The effect can be viewed to modify the measurement noise covariance. Under the normal circumstance, the measurement noise covariance remains unchanged, while at the time that outliers appear, the measurement noise covariance is changed to restrain the outliers. Based on the above principles, a robust unscented Kalman filter is proposed. The measurement noise covariance is reconstructed using Huber technique at each step. Then, the standard process of unscented Kalman filter without linear approximation is used for estimating. The proposed method can perform better than both standard unscented Kalman filter and the linearized Huber-based filter.
This paper is organized as follows: Section 2 presents the mathematical model of the multisensor tracking system using centralized fusion algorithm. Section 3 analyzes the linear Huber-based Kalman filter and shows the effect that Huber cost function brings to the Kalman gain and the estimation states. In Section 4, the new Huber-based nonlinear Kalman filter is proposed and the algorithm is concluded to a brief realization. Simulations in three different conditions are given to demonstrate the superiority of the proposed algorithm in Section 5. Then, Section 6 concludes the performance of the proposed algorithm.
2. Mathematical Model of the Centralized Fusion System
In this section, the structure of the centralized fusion system is modeled. In the multisensor network tracking system, each sensor of the system detects the target and gains the measurement information of the target and then translates the information to the fusion center. The fusion center then collects information from all of the sensors in the system to form a whole measurement of the target. So each senor’s measurement is sufficiently used and there is not any loss of the measurements. The performance of this method is optimal. Based on the above, the frame diagram is shown in Figure 1.
In the target tracking problem, the motion of the target is often considered as a dynamic function:where is the -dimensional state of the motion target at time step , is the processing noise distribution matrix, and is the noise sequence with the covariance . It is assumed that the noises at each time are independent of each other; that is,where is Kronecker delta function; that is,
There are sensors that observe the target; the measurement functions are as follows:where is the measurement value of sensor at time step and is the corresponding measurement noise with its covariance being and mean being zero. All of the noises are supposed to be independence. Thus, in the centralized algorithm, all the measurements are sent to the fusion center and fused as a whole measurement. Generally, we define the following:
So, the equivalent generalized measurement function is
And we can get
Then, we can apply the filtering algorithm to (1) and (6). If there is not any outlier in the measurement of the sensors, the traditional centralized Kalman filtering will be optimal to track the target. But in many situations, the existence of outliers is unavoidable. In these cases, the traditional Kalman filter will become invalid.
3. Linear Huber-Based Kalman Filter
In this section, the linear Huber-based Kalman filter has been analyzed. And the effects of the Huber cost function on the filter have been found out. In the linearized space, the tracking systems (1) and (6) can be written aswhere and are the linear processing function and observation function. Let denote the estimation of the state , and let denote the corresponding error covariance. Then, the standard linear Kalman filter can be derived as [8]
From the above, we can see that can also be seen as a particular weighted least squares solution. Combine the prediction with the observations as follows:where is the error between the true state and its prediction and is the -dimensional identity matrix.
Define
Then,
Let .
Define
Multiplying (10) by , we can obtain
In order to achieve the robust tracking, we use -estimate to minimize a so-called cost function that increases slowly with the error , whereand is the th component of .
In the Huber method, we often define the cost function as
Then, the problem is to find that let the cost function get the minimum. That is,
In (18), can usually be defined as
To minimize the cost function, let the differential of be equal to zero, and then we can getwhere named the influence function. We have
Then, we define ; we get
Thus, by defining the matrix , (21) can be written as
The solution is [8] as follows:
By applying the above method, the linear motion system can be efficiently tracked in the condition that the outliers exist. But obviously, this algorithm cannot be used in the nonlinear tracking system directly. The most direct process to apply this algorithm is to linearize or statistically linearize the nonlinear function. But that may lead to a low accuracy, cumbersome derivation. And to evaluate the Jacobian/Hessian matrixes may be very complex. Our research is to find an algorithm that does not need linearized approximation.
Review the algorithm above; we decompose into two parts and corresponding to the state prediction and measurement prediction residuals. That is,
Substitute (13), (15), and (27) to (25); we can obtainwhere
In fact, the true state is unknown; so the error is set to zero; so ; thus, (29) and (30) can be
Compared to the standard Kalman filter, we can see that the change brought by Huber methodology only reflects on the covariance of the measurement noise. Due to this, we can extend this algorithm to the nonlinear tracking system by reformulating the measurement noise for UKF algorithm.
4. Nonlinear Robust Kalman Filter
Due to the analysis in the previous section, we know that we can achieve robust tracking only by reformulating the measurement noise. Now, consider the nonlinear dynamic tracking system as follows:
is also the covariance of and .
Define quantities as follows:
Let ; then, the cost function is
In this paper, in order to get a more robust performance, we choose
Then, we can obtain
Also define ; thus, the reformulated covariance of the measurement noise is
Then, the standard unscented Kalman filter is applied with the new measurement noise covariance. The algorithm is as follows.
(1) Time Update. Calculate sigma points:
Compute the propagated sigma points:
Compute the predicted state and covariance:
(2) Measurement Update. Compute the reformulated measurement noise covariance .
Calculate sigma points:
Compute the propagated sigma points:
Compute the predicted mean and the predicted covariance of the measurement and the cross-covariance of the state and the measurement:
Compute the filter gain and the estimated state and its covariance:whereand ; often we choose , , and .
Obviously, this algorithm does not linearize the process function and the measurement function. So, the accuracy of UKF remains, and because of the reformulated measurement noise covariance, this algorithm also has the robust performance.
In the proposed algorithm, the selection of the cost function is different from the traditional Huber method. From the above, the function can be seen as a reweighting function and its varying trend is as in Figure 2.
From Figure 2, we can obtain that when the error , the reweighting function remains unchanged. For these time steps, the proposed algorithm is just as the standard unscented Kalman filter. When the error , the reweighting function is a decreasing function, the reformulated measurement noise covariance is less than the real measurement noise, and thus the robust performance is achieved. And we can easily notice that the proposed method is decreasing faster than Huber method; so the proposed method has a better performance than the Huber method. Moreover, when the error , that means the error is too large to be considered. Then, the reweighting function of the proposed method is almost equal to zero. That means to throw these outliers. However, the Huber method also allocates a weight to the error, and that will lead to a low accuracy.
5. Simulations and Analysis
This section compares the performance of the proposed algorithm against the standard UKF and the Huber-based UKF. The target moves with a constant angular velocity ; the state vector ; we can get that the system function iswhereand represents the process noise sequence.
We can get its discrete form as follows:and the covariance of the noise is as follows:
The measurement functions of the sensors are as in Table 1:where and is the measurement noise, andwhere is the position of the target and is the position of the th sensor.
First, we use the mixed Gaussian distribution to simulate the outliers. The probability density function isLet the simulation time ; the initial position of the target is . The times of the Monte-Carlo experiment are . Define the root mean square error:
The values of the variables are as follows.
Case 1. When (i.e., the noise is Gaussian distribution), the result of the tracking process after one time is as in Figure 3.
And after 50 times’ Monte-Carlo experiment, the root mean square error of each variable is as in Figures 4, 5, 6, and 7.
We can see from Figures 4, 5, 6, and 7 that the RMSE of the proposed algorithm is almost the same as the centralized UKF and is less than the centralized HUKF. Because the centralized HUKF has linearized the measurement function and lost its accuracy, the proposed algorithm does not need to be linearized.
Case 2. When , that means the outliers exist in the measurement value. The result of the tracking process after one time is as in Figure 8.
And after 50 times’ Monte-Carlo experiment, the root mean square error of each variable is as in Figures 9, 10, 11, and 12.
From Figures 9, 10, 11, and 12, we can see that, in the processing, there are three outliers which are pointed out in Figure 8. At these points, the outliers turned up, the error of the centralized UKF raised rapidly. So the estimation was unbelievable. And the centralized HUKF and the proposed algorithm can also remain in the period that is believable. The error is much less than the centralized UKF. And we can also observe that the proposed algorithm is more accurate than the centralized HUKF.
Another condition is that when the measurement environment changes, the measurement noise is changed. But this is unknown for the filter. Similarly, we should reformulate the measurement noise covariance to adjust the changes. In the simulation, we set that the measurement environment changes in the period from step 50 to step 80. The simulation results are as in Figure 13.
And after 50 times’ Monte-Carlo experiment, the root mean square error of each variable is as in Figures 14, 15, 16, and 17.
We can see that in the period steps 50 to 80, the centralized UKF cannot track the target very well and the error is very large. And the other two algorithms can track the target partly. And obviously, the performance of the proposed algorithm is better than the centralized HUKF.
6. Conclusion
In this paper, we have proposed a robust tracking method for the nonlinear tracking system without linear approximation by analyzing the Huber-based Kalman filter for linear system. The proposed algorithm uses the standard unscented Kalman filter but replaces the measurement noise covariance by a new one which is reformulated through minimizing the so-called cost function. And in this paper, we design a new equation for the cost function and indicate its advantages over the previous function. Simulation results illustrate that the proposed algorithm has a more accurate and more robust performance than the standard unscented Kalman filter and the traditional Huber-based Kalman filter.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.