# Centralized Fusion of Unscented Kalman Filter Based on Huber Robust Method for Nonlinear Moving Target Tracking

Authors: Jue Huang; Bing Yan; Shouwei Hu
Journal: Mathematical Problems in Engineering
 (2015)

---

## 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).


---

## Body

## 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 l1 and l2-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: Section2 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 Figure1.Figure 1
Centralized fusion frame.In the target tracking problem, the motion of the target is often considered as a dynamic function:(1)xk+1=fxk+Γkwk,where xk∈Rn is the n-dimensional state of the motion target at time step k, Γk∈Rn×r is the processing noise distribution matrix, and wk∈Rr is the noise sequence with the covariance Sk. It is assumed that the noises at each time are independent of each other; that is,(2)cov⁡wk,wj=Qkδkj,Qk≥0,cov⁡x0,wk=0,where δkj is Kronecker delta function; that is, (3)δkj=0,k≠j,1,k=j.There areN sensors that observe the target; the measurement functions are as follows:(4)zk+1i=hk+1ixk+1+vk+1i,i=1,2,…N,where zk+1i∈Rm is the measurement value of sensor i at time step k+1 and vk+1i∈Rm is the corresponding measurement noise with its covariance being Rk+1 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:(5)zk+1=zk+11Tzk+12T⋯zk+1NTT,hk+1xk+1=hk+11xk+1Thk+12xk+1T⋯hk+1Nxk+1TT,vk+1=vk+11Tvk+12T⋯vk+1NTT.So, the equivalent generalized measurement function is(6)zk+1=hk+1xk+1+vk+1.And we can get(7)Evk+1=0,Rk+1=cov⁡vk+1,vk+1=diag⁡Rk+11,Rk+12,…,Rk+1N.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 as(8)xk+1=Fkxk+ωk,zk+1=Hk+1xk+1+vk+1,where Fk and Hk are the linear processing function and observation function. Let x^k∣k denote the estimation of the state xk, and let Pk∣k denote the corresponding error covariance. Then, the standard linear Kalman filter can be derived as [8](9)x^k+1∣k=Fkx^k∣k,Pk+1∣k=FkPk∣kFkT+Qk,Kk+1=Pk+1∣kHk+1THk+1Pk+1∣kHk+1T+Rk+1-1,x^k+1∣k+1=x^k+1∣k+Kk+1zk+1-Hk+1x^k+1∣k,Pk+1∣k+1=Pk+1∣k-Kk+1Hk+1Pk+1∣k.From the above, we can see thatx^k+1∣k+1 can also be seen as a particular weighted least squares solution. Combine the prediction with the observations as follows:(10)zk+1x^k+1∣k=Hk+1In×nxk+1+vk+1δxk+1∣k,where δxk+1∣k is the error between the true state and its prediction and In×n is the n-dimensional identity matrix.Define(11)ε=vk+1δxk+1∣k.Then,(12)Eε·εT=Rk+100Pk+1∣k.LetSk+1=E(ε·εT).Define(13)yk+1=Sk+1-1/2zk+1x^k+1∣k,(14)ξk+1=Sk+1-1/2ε,(15)Mk+1=Sk+1-1/2Hk+1In×n.Multiplying (10) by Sk+1-1, we can obtain(16)yk+1=Mk+1xk+1+ξk+1.In order to achieve the robust tracking, we useM-estimate to minimize a so-called cost function that increases slowly with the error ek+1,i, where(17)ek+1=yk+1-Mk+1xk+1and ek+1,i is the ith component of ek+1.In the Huber method, we often define the cost function as(18)Jxk+1=∑i=1m+nρek+1,i.Then, the problem is to findx^k+1∣k+1 that let the cost function J(xk+1) get the minimum. That is,(19)x^k+1∣k+1=argmin⁡Jxk+1.In (18), ρ can usually be defined as(20)ρek+1,i=12ek+1,i2,ek+1,i<u,uek+1,i-12u2,ek+1,i≥u.To minimize the cost function, let the differential ofJ(xk+1) be equal to zero, and then we can get(21)∑i=1nϕek+1,i∂ek+1,i∂xk+1=0,where ϕ=ρ′ named the influence function. We have(22)ϕek+1,i=ek+1,i,ek+1,i<u,usgn⁡ek+1,i,ek+1,i≥u.Then, we defineω(ek+1,i)=ϕ(ek+1,i)/ek+1,i; we get(23)ωek+1,i=1,ek+1,i<u,usgn⁡ek+1,iek+1,i,ek+1,i≥u.Thus, by defining the matrixΨ=diag⁡[ω(ek+1,i)], (21) can be written as(24)Mk+1TΨMk+1xk+1-yk+1=0.The solution is [8] as follows:(25)x^k+1∣k+1=Mk+1TΨMk+1-1Mk+1TΨyk+1,(26)Pk+1∣k+1=Mk+1TΨMk+1-1.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 Ψx and Ψy corresponding to the state prediction and measurement prediction residuals. That is,(27)Ψ=Ψy00Ψx.Substitute (13), (15), and (27) to (25); we can obtain(28)x^k+1∣k+1=x^k+1∣k+Kk+1′zk+1-Hk+1x^k+1∣k,(29)Pk+1∣k+1=I-Kk+1′Hk+1Pk+1∣k1/2Ψx-1Pk+1∣k1/2T,where(30)Kk+1′=Pk+1∣k1/2Ψx-1Pk+1∣k1/2THk+1THk+1Pk+1∣k1/2Ψx-1Pk+1∣k1/2THk+1T+Rk+11/2Ψy-1Rk+11/2T-1.In fact, the true state is unknown; so the errorδxk+1∣k is set to zero; so Ψx=In×n; thus, (29) and (30) can be(31)Pk+1∣k+1=I-Kk+1′Hk+1Pk+1∣k,Kk+1′=Pk+1∣kHk+1THk+1Pk+1∣kHk+1T+Rk+11/2Ψy-1Rk+11/2T-1.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:(32)xk+1=fkxk+wk,zk+1=hk+1xk+1+vk+1.Q
k
,
R
k is also the covariance of wk and vk.Define quantities as follows:(33)z˘k=Rk-1/2zk,gkxk=Rk-1/2hkxk.Letδk=z˘k-gk(xk); then, the cost function is(34)Jxk=∑i=1nρδk.In this paper, in order to get a more robust performance, we choose(35)ρδk,i=12δk,i2,δk,i<u,-u21+δk,iuexp⁡1-δk,iu,δk,i≥u.Then, we can obtain(36)ωδk,i=1,δk,i<u,exp⁡1-δk,iu,δk,i≥u.Also defineΨ=diag⁡[ω(δk,i)]; thus, the reformulated covariance of the measurement noise is(37)R~k=Rk1/2Ψk-1Rk1/2T.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:(38)ξk∣k=x^k∣kx^k∣k+n+λPk∣kx^k∣k-n+λPk∣k.Compute the propagated sigma points:(39)γk∣k=fξk∣k.Compute the predicted state and covariance:(40)x^k+1∣k=∑i=02nwimγk∣k,i,Pk+1∣k=∑i=02nwicγk∣k,i-x^k+1∣kγk∣k,i-x^k+1∣kT+Qk.(2) Measurement Update. Compute the reformulated measurement noise covariance R~k.Calculate sigma points:(41)ξk+1∣k=x^k+1∣kx^k+1∣k+n+λPk+1∣kx^k+1∣k-n+λPk+1∣k.Compute the propagated sigma points:(42)γk+1∣k=hξk+1∣k.Compute the predicted mean and the predicted covariance of the measurement and the cross-covariance of the state and the measurement:(43)z^k+1∣k=∑i=02nwimγk+1∣k,i,Pz~k=∑i=02nwicγk+1∣k,i-z^k+1∣kγk+1∣k,i-z^k+1∣kT+R~k,Px~kz~k=∑i=02nwicξk+1∣k,i-x^k+1∣kγk+1∣k,i-z^k+1∣kT.Compute the filter gain and the estimated state and its covariance:(44)Kk+1=Px~kz~kPz~k-1,x^k+1∣k+1=x^k+1∣k+Kk+1zk+1-z^k+1∣k,Pk+1∣k+1=Pk+1∣k-Kk+1Pz~kKk+1T,where(45)w0m=λn+λ,w0c=λn+λ+1-α2+β,wim=wic=0.5n+λ,i=1,2,…,n,and λ=α2(n+κ)-n; often we choose α=0.01, κ=0, and β=2.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.Figure 2
The reweighting functions vary withu.From Figure2, we can obtain that when the error u≤l1, the reweighting function remains unchanged. For these time steps, the proposed algorithm is just as the standard unscented Kalman filter. When the error u>l1, 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 u>l2, 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 X=[x,x˙,y,y˙]T; we can get that the system function is(46)X˙t=AωXt+Bωwt,where(47)Aω=0100000-ω00010ω00,Bω=01000001and w(t) represents the process noise sequence.We can get its discrete form as follows:(48)
X
k
+
1
=
1
sin
⁡
ω
T
ω
0
-
1
-
cos
⁡
ω
T
ω
0
cos
⁡
ω
T
0
-
sin
⁡
ω
T
0
1
-
cos
⁡
ω
T
ω
1
sin
⁡
ω
T
ω
0
sin
⁡
ω
T
0
cos
⁡
ω
T
X
k
+
T
2
2
0
T
0
0
T
2
2
0
T
w
kand the covariance of the noise is as follows:(49)Q=Sk2ωT-sin⁡ωTω31-cos⁡ωTω20ωT-sin⁡ωTω21-cos⁡ωTω2T-ωT-sin⁡ωTω200-ωT-sin⁡ωTω22ωT-sin⁡ωTω31-cos⁡ωTω2ωT-sin⁡ωTω201-cos⁡ωTω2T.The measurement functions of the sensors are as in Table1:(50)zi=hix+vi,where hi(x)=[hri,hθi]T and vi is the measurement noise, and(51)hri=xm-xi2+ym-yi2,hθi=arctan⁡ym-yixm-xi,where (xm,ym) is the position of the target and (xi,yi) is the position of the ith sensor.Table 1
ω
T
S
k
(
x
m
i
,
y
m
i
)
σ
2
σ
1
0.03
0.1
0.1
(10, 0)(0, 10)(0, 0)(10, 10)
100
σ
1
0.05First, we use the mixed Gaussian distribution to simulate the outliers. The probability density function is(52)pdfvki=1-εN0,σ1+εN0,σ2.Let the simulation time K=100; the initial position of the target is X0=[1,0.5,1,0.3]T. The times of the Monte-Carlo experiment are M=50. Define the root mean square error:(53)RMSEk=1M∑i=1Mxk∣ki-xki2.The values of the variables are as follows.Case 1.
Whenε=0 (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 Figures4, 5, 6, and 7.
We can see from Figures4, 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.Figure 3
The track after one time process.Figure 4
RMSE ofx-position.Figure 5
RMSE ofy-position.Figure 6
RMSE ofx-velocity.Figure 7
RMSE ofy-velocity.Case 2.
Whenε=0.01, 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 Figures9, 10, 11, and 12.
From Figures9, 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 Figure13.
And after 50 times’ Monte-Carlo experiment, the root mean square error of each variable is as in Figures14, 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.Figure 8
The track after one time process.Figure 9
RMSE ofx-position.Figure 10
RMSE ofy-position.Figure 11
RMSE ofx-velocity.Figure 12
RMSE ofy-velocity.Figure 13
The track after one time process.Figure 14
RMSE ofx-position.Figure 15
RMSE ofy-position.Figure 16
RMSE ofx-velocity.Figure 17
RMSE ofy-velocity.


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



---

*Source: 291913-2015-09-21.xml*