Improved adaptive unscented Kalman filter algorithm for target tracking

An adaptive unscented Kalman filter (AUKF) algorithm is proposed to solve the problem that the statistical characteristics of the process noise are unknown in the target tracking, which leads to filter divergence or low filtering precision. The improved Sage-Husa estimator is used to estimate the statistical characteristics of the unknown process noise in the filtering process, and to judge and suppress the filtering divergence, which effectively improves the numerical stability of the filtering and reduces the error of the state estimation. The simulation results show that the improved AUKF algorithm not only keeps convergence but also improves the accuracy and stability of the target tracking under the condition of unknown time-varying process noise statistic, compared with the standard UKF algorithm.


Introduction
Target tracking is a basic problem in the military and civil fields.Its task is to use the measured information obtained by the sensor to estimate the current state of the target and the future state of the target.In the tracking of high maneuvering targets, such as fighter, ballistic missile and reentry vehicle, the state estimation is confronted with three problems: one is the nonlinearity of the system, including the nonlinearity of the motion model and the measurement model, the second is that the process noise can't be accurately described, the third is that the measurement noise is usually not accurate.
The most widely used method to deal with nonlinear estimation is the extended Kalman filter (EKF).This method has the obvious drawback that the linearization error is large and the Jacobian matrix is difficult to be calculated.In the case of high nonlinearity, the filter is unstable, and the estimation error will be large or even divergent.Unscented Kalman filter (UKF) is a nonlinear filtering method, and it does not need to calculate the Jacobian matrix of the nonlinear state equation and the nonlinear measurement equation, and the unscented transformation (UT) can theoretically be approximated to the posterior mean and covariance of the state at least the third-order Taylor precision, regardless of the degree of the nonlinearity system [1] [2].UKF has the advantages of simple realization, high filtering precision and good convergence.Therefore, UKF has received extensive attention from domestic and foreign scholars.
UKF filtering algorithm applied to target tracking need to know the mathematical model of the object to be studied and the priori knowledge of noise statistical characteristics.If the filter is designed based on inaccurate mathematical model and inaccurate noise statistical characteristics, it may lead to large state estimation error and even filter divergence.In general, the statistical properties of the measurement noise can be obtained in advance by the physical characteristics of the sensor.However, for tracking systems, it is difficult to represent process noise with an accurate statistical characteristics due to external disturbances, physical characteristics of acceleration, and maneuvering [3].Therefore, its statistical properties are often unknown and time-varying.
To solve that problem, adaptive filtering algorithm [4][5] [6] came into being.The adaptive filtering algorithm can be divided into three categories: scaling state covariance matrix (P-matrix), multi-model adaptive estimation and adaptive stochastic model modeling.[7] proposed a filtering algorithm of scaling the process noise covariance matrix Q.It is believed that the tuning parameters are less and the robustness is better than the algorithm of scaling the state covariance matrix.But admitting is different from the general adaptive random Model modeling algorithm, can not adjust the variance of a single noise.In recent years, the UKF has been widely used in estimation of state and parameters, control, target tracking, navigation and fault diagnosis [8][9].Many scholars have tried to improve the UKF in terms of its numerical stability and computational speed, e.g., a decrease sigma points UKF and square root decomposition of the UKF [10].
The Sage-Husa algorithm is a suboptimal unbiased maximum posteriori noise estimator for the uncertain process noise and measurement noise.
In this paper, an AUKF algorithm is proposed.The algorithm uses the improved Sage-Husa algorithm to estimate the unknown noise, and estimates and corrects the process noise statistical characteristics in real time to reduce the state estimation error.This method adopts a method based on covariance matching criterion to judge the filter divergence trend, introduces the adaptive attenuation factor to correct the prediction covariance, and adjusts the filter gain matrix to eliminate the divergence phenomenon in the adaptive filtering.Both theoretical analysis and experimental results show that the proposed AUKF algorithm is superior to the standard UKF algorithm in estimating performance and stability, which greatly improves the ability of real-time target tracking.

Mathematical description of state estimation
Consider a nonlinear discrete system as follows: where Q is non-negative definite symmetric matrix, R is positive definite symmetric Hypothesis2: the initial state 0 x is not related with k v and k w , and obeys the Gaussian normal distribution.The mean and covariance matrix of the initial state are The problem is to design an adaptive UKF filtering algorithm with noise statistic estimator based on the measurement , , , when the statistical characteristics of the process noise are unknown or time varying.

Unscented transformation
UKF uses a deterministic sampling strategy to approximate the nonlinear distribution.Its core idea is to use the unscented transformation to estimate the distribution of random variables through a nonlinear system, and still use the standard Kalman filtering framework. Let (5) The main idea of UT is to design a series of sigma points , 0,1, , to approximate the distribution of random variables under the premise of the sampling mean x and covariance x P , then calculate the value γ 0,1, , of the sigma point after passing through the nonlinear system, and calculate the distribution of   ˆ, z z z P .In the UT algorithm, the most important thing is to determine the sigma sampling strategy, which is to determine the number, the location and the corresponding weights of sigma points.There are several typical sigma sampling strategy, symmetric sampling, single sampling, third-order moment skew sampling and fourth-order moment symmetric sampling of Gaussian distribution.
The UT implementation is described as follows: StepA1: According to the selected sampling strategy, calculate the sigma sampling points and their weight coefficients on the basis of the statistical characteristics of   ˆ, x x N x P .Let the corresponding weight of sigma points W , which are the weight coefficients of the first and second order statistical properties respectively.The sampling strategy given in is given below: where StepA3: calculate the mean ẑ and covariance z P of the random vector x after passing through the

Standard UKF
On the basis of the standard KF, the mean and covariance of the state vector through the nonlinear system are calculated by means of the UT, hence the UKF.UKF algorithm for the process: StepB1: initialize the state 0 x , state covariance 0 P , process noise covariance Q and measurement noise covariance R .
StepB2: select the sigma point sampling strategy.
Select the sigma points 1 ξ i,k  according to the estimated mean and covariance matrix of the current state, and evaluate their corresponding The mean of the transformed set of sigma points | 1 And the predicted covariance is The predicted observation is It should be noted that , | 1 ξ i k k  can also be obtained by resampling the state prediction value where k K is the filter gain matrix.Then update the mean and covariance of state vector x .
The standard UKF has a good performance in the estimation of general nonlinear systems, but it has a poor performance in tracking mutation parameters and states.

Time-varying noise estimator
Incorrect mathematical models and inaccurate noise statistic properties often lead the filter to divergence.In order to solve this problem, adaptive filtering technology has been widely studied.The Sage-Husa algorithm is a noise estimator based on the suboptimal unbiased maximum a posteriori (MAP), which is simple in calculation and clear in principle, and can estimate the first and second moments of noise at the same time.
For the unknown process noise in Eq.( 1), we use the Sage-Husa algorithm to estimate k q and k Q .
x q (24) The suboptimal Sage-Husa estimator can be obtained by replacing the smoothing estimate | ˆj k x with the estimation value ˆj x approximately.For time varying noise, the role of recent data should be emphasized, and the role of old data should be gradually forgotten and disappeared.Therefore, we use fade-out memory weighting method, that each term of Eq.( 23) and Eq.( 24) is multiplied by a different weighting factor [12].The suboptimal Sage-Husa estimator is combined with the KF to obtain an adaptive filter for linear systems with time-varying noise characteristics [11] [12], and can be combined with EKF to generalize to nonlinear systems.Then k q and k Q can be recursively obtained by the time-varying noise estimator: where b is the forgetting factor, the value range is 0.95 0.99 b   . The forgetting factor b can be used to limit the memory length of the filter.The larger b , the greater the weight of the new measurement data.When the noise statistical characteristics change quickly, b should take a large value; when the change is slow, it should be smaller.
The sub-optimal Sage-Husa estimator is introduced into the UKF algorithm to estimate and correct the statistical properties of the process noise in real time, which reduces the model error, suppresses the filtering divergence and improves the filtering accuracy.The estimated mean, estimation variance, and filter gain in Eq.( 25) and Eq.( 26) can be obtained during UKF filtering.

Suppression of filtering divergence
The suboptimal Sage-Husa algorithm will be divergent sometimes.In this paper, a method based on covariance matching criterion is proposed to judge the filter divergence trend.

 
tr 1 where S is adjustable coefficient, k z is the residual sequence, i.e. .If Eq.( 28) does not hold, correct one-step prediction covariance matrix as follows: (29) When the convergence condition is satisfied, the Sage-Husa algorithm described above is used.When the filter is divergent, the adaptive weighting coefficient is computed by Eq.(30).And then use k  to amend | 1 k k P  , thereby increasing the role of the current observation to suppress the filter divergence.The adaptive weighting factor k  is calculated as follows: where  is the attenuation coefficient.The attenuation coefficient can further improve the fast tracking ability of the filter.Usually, the value of  is about 0.95.The larger the value, the smaller the proportion of information before current time, and the more effect of current residual vector.This filter algorithm has strong ability to track the mutation state, and keeps the tracking ability for the gradual change and the mutation state when the filtering reaches the steady state.

Implementation of the improved AUKF algorithm
In order to solve the problem of target tracking described by Eq.( 1) and Eq.( 2), the proposed AUKF algorithm can realize the recursive estimation of the unknown process noise statistic and the UKF filtering of the nonlinear system.The concrete steps are as follows: StepC1: initialize the state , state covariance 0 P , process noise mean 0 q and covariance 0 Q , measurement noise covariance R .
StepC2: for a given 1 The predicted observation is the same as standard UKF.
StepC3: divergence judgment.According to Eq.( 28) to judge the divergence of the filter.If divergence, modify covariance .. according to Eq.( 29), otherwise go to the next step.
StepC4: measurement update.Calculate the covariance k z P , the cross correlation matrix k k x z P , the filter gain matrix k K , and then update the state mean and covariance according to Eq.(17) to Eq.( 22).
StepC5: Update the mean and covariance of the process noise according to Eq.(25) to Eq.(26).

Application in target tracking
In order to verify the filtering performance of the AUKF algorithm in this paper, the standard UKF algorithm and the AUKF algorithm are used to estimate the target state when the statistical characteristics of the process noise are unknown.In the case of the ballistic target tracking, the target state is in the ENU coordinate system.In the reentry stage, the motion model of the ballistic target can be expressed as: where 37)  is the gravitational constant of the earth,   R t is the distance from the target to the center of the earth at time , e R is the radius of the earth,   v t is the velocity of the target, ，  is the mass-to-resistance ratio, which is assumed to be known, 2 6 000 kg/m   .When filtering, it can be used to predict the state of the target at the next moment by means of discretization of differential equation or numerical integration, and will not be repeated here.The state data of the ballistic missile during the reentry process are obtained by numerical integration of the accurate ballistic motion equations.
k Z is the measurement vector, and its components are slanting distance R , azimuth A and pitch angle E .Radar distance measurement standard deviation is 100 , angle standard deviation is 3 10 rad , sampling period is 1s.The measurement noise covariance matrix is also a diagonal matrix.When the measurement noise is zero mean Gaussian distribution, the diagonal elements can be taken as 1/9 times the square of the angular accuracy.
Process noise and measurement noise are independent zero mean Gaussian white noise.The measurement noise is known, the statistical characteristics of the process noise is unknown, and is time varying.
In the initial stage of filtering, the error of the target state vector is large.By adjusting the process noise Q, the divergence of the filter can be effectively avoided.The standard UKF process noise covariance matrix is set to: The mean of the process noise for AUKF is , and the initial process noise covariance for AUKF is the same as standard UKF.The initial state and its covariance matrix are calculated according to the first two points of the measurement.Assuming that the first two points of measurement data in the ENU coordinate system are The initial state covariance is a diagonal matrix, and the square of the maximum error of the initial state filter value can be chosen as the initial value.The initial state covariance is set to 1|1 diag 10 10 10 10 10 10 P = (40) The two filtering methods are compared taking the root mean square error (RMSE) of the filtering method as an index.RMSE is defined as： where N is the Monte Carlo simulation times.
In an independent experiment, the true trajectory of the target is shown in Fig. 1, and the velocity versus time is shown as Fig. 2. Fig. 3 to Fig. 4 compared the RMSE of the two filtering algorithms under unknown time varying process noise.Tab.1 is the mean RMSE of the two filtering algorithms at different times.
The AUKF algorithm proposed in this paper uses the Sage-Husa estimator to estimate the statistical characteristics of the process noise in real-time.According to the innovation, the filtering divergence trend is judged and the attenuation factor is introduced to restrain the divergence, which can not only guarantee the positive definite and nonnegative definite of the process noise variance matrix, but also effectively solve the filter instability problem caused by unknown statistical characteristic of the process noise, and improve the convergence speed and the estimation precision of the filter.It can be seen from Fig. 3 and Fig. 4 that the algorithm has good filtering effect under the condition of unknown time varying noise, and the RMSE of position and velocity is obviously smaller than that of the standard UKF algorithm.Simulation results show that the AUKF algorithm is superior to the standard UKF algorithm in filtering precision and stability, and the state estimation is closer to the real state.

Conclusions
A new AUKF algorithm which adaptively adjusts the variance of noise based on innovation is proposed for non-linear target tracking problem with unknown process noise.In the filtering process, we adopt the Sage-Husa estimator to estimate and correct the process noise covariance, and take measures to suppress the possible divergence phenomenon.The new algorithm improves the real-time tracking ability and the numerical stability of the filter.The AUKF algorithm proposed in this paper is much better than the standard UKF algorithm, showing higher filtering precision and stronger anti-noise performance.

| 1
ˆk k x as stepA1.StepB4: measurement update.Calculate the covariance and the cross correlation matrix by

Figure 1 .Figure 2 .Figure 3 .Figure 4 .
Figure 1.Height of BM  is a scaling parameter determining the spread of the sigma points around x , n is the dimension of x .The parameter  is usually set to a small positive value, the matrix square root of x P , holds = T x x x P P P , i W is the weight associated with the i th point,

Table 1 .
RMSE of the two filtering algorithms