Robust self-adaptive Kalman filter with application in target tracking

Kalman filter has been applied extensively to the target tracking. The estimation performance of Kalman filter is closely resulted by the quality of prior information about the process noise covariance (Q) and the measurement noise covariance (R). Therefore, the development of adaptive Kalman filter is mainly to reduce the estimation errors produced by the uncertainty of Q and R. In this paper, the proposed self-adaptive Kalman filter algorithm has solved the problems of covariance-matching method about the determination of the width of the window and the addition of storage burden and that can update Q and R simultaneously. Simulation results confirm that the proposed method outperforms the traditional Kalman filter and has the better estimation performance than the other two adaptive Kalman filters in the target tracking. The developed filtering algorithm has the following characteristics: high robustness, low computing load, easy operation and tuning Q, R simultaneously.


Introduction
The effective estimation of target tracking is to reduce the noise disturbance to obtain the useful information of the target state from the observations. 1Target tracking algorithm should not only meet the requirement of real-time processing system but also ensure a high accuracy tracking.Kalman filter algorithm has been widely used for target tracking but the extreme maneuvers of target may degrade its estimation performance seriously. 2alman proposed a recursive optimal solution to the linear filtering problem based on the state-space dynamic system combined with the recursive leastsquare method in 1960. 3][6][7][8][9][10] It is essentially one of the most important filter theories and many improved filters are developed based on Kalman filter such as Extended Kalman filter and unscented Kalman filter. 11The operation of Kalman filter depends on the precise system dynamic model and the statistical property of noise fundamentally.Besides, the covariance matrices of process noise (Q) and measurement noise (R) must be known.In practice, Q and R are generally fixed to some value during the whole operation process and obtained by the statistical characteristics of noise from the comprehensive analysis of huge experiments in advance.However, it may be doubtful to apply this method to the dynamic system and easy to produce huge estimation errors or diverge.The noise level may be influenced due to the huge variety of system dynamics and the uncertainty of the parameters Q and R may degrade the estimation performance seriously. 12he above shortcomings of Kalman filter seriously influence the estimation performance for the dynamic systems such as the target tracking.Therefore, several kinds of adaptive Kalman filter algorithms are investigated to reduce the estimation errors produced by the uncertainty of Q and R and mainly divided into four types: covariance-matching method, [13][14][15][16] covariancescaling method, 12,[17][18][19][20][21][22][23] multiple-model adaptive estimation, [24][25][26] and others. 5,27,28The adaptive Kalman filter algorithms have been widely applied to GPS/INS, navigation, geodetic positioning and UAV.Covariancematching method contains innovation-based adaptive estimation (IAE) and residual-based adaptive estimation (RAE).This algorithm utilizes the innovation sequence or the residual sequence by choosing the width of the moving window after the recursive steps to estimate Q and R and that would increase the storage burden of computer.Besides, the estimation precision of filter would be also influenced by the determination of the width of the moving window.Furthermore, IAE and RAE estimators require that the number, type and distribution of the measurement data for all the recursive steps within a window should be consistent. 23ovariance-scaling method chiefly applies a multiplication factor to Q to improve the filter performance and the adaptive R is a fixed value generally.The computation of the scaling factor can either be totally empirical or based on certain criteria derived from filter innovations and it usually needs to design several parameters in advance during the operation process. 12In the multiple-model adaptive estimation, a bank of Kalman filters operates in parallel combined with different models and the overall estimate is computed by a weighted sum of the estimates from the sub-filters of different models.However, the scheme of multiplemodel adaptive estimation increases the computing load and reduces the operation speed. 5In summary, the concepts of Q and R adaptive estimation utilize one of the following methods 23,29 : (1) Q must be the completely known and fixed value.
Trial-and-error method is used to adjust R to compute the realistic values such that the filter would produce the stable state estimates; or (2) On the contrary, R is the completely known and fixed value, and it needs to adjust Q to get the stable state estimates of filter, otherwise; (3) Q and R are unknown.Adjust both of Q and R simultaneously and the initial values should be chosen carefully for obtaining the optimal stable state estimates of filter.
Covariance-matching method is one of the most popular techniques.However, adaptive estimation of R is linked to Q because the derivation is based on the operation of Kalman filter.It is questionable to compute the estimation of Q and R simultaneously.Besides, increasing the window size for obtaining the smoothing covariance estimation would degrade the dynamic response of the adaptive mechanism and the small window size could influence the estimation precision of Q and R or the filter may diverge. 14,23n this paper, a robust self-adaptive Kalman filter based on the covariance-matching method has been developed to obtain the precise estimation of system dynamics.The proposed method has solved the problems of covariance-matching method about the determination of the width of the window.It can not only solve the shortcomings of Kalman filer about the uncertainty of Q and R but also estimate the adaptive Q and R simultaneously according to the latest measurement data without the comprehensive analysis of huge experiments in advance.Besides, the proposed filter algorithm is excellent to the response of dynamic system with high estimation precision and the estimation behavior would quickly converge to the stable process.The scheme of this paper is designed as follows: Adaptive Kalman filter based on covariance-matching method and the proposed algorithm are introduced carefully in Section 2. In Section 3, simulation experiments are carried out to prove the effectiveness of the proposed algorithm applied to maneuvering target tracking.Section 4 is the conclusion.

Kalman filter
Consider a linear discrete-time dynamic system: where X k is the system state vector, A k is the state transition matrix, Y k is the measurement vector, C k is the measurement matrix, w k is the process noise vector, v k is the measurement noise vector.Besides, w k and v k are regarded as zero-mean and uncorrelated white Gaussian noises.
where E Á f g denotes the expectation function.The operation process of Kalman filter algorithm is organized mainly by five equations: where XÀ k is the system state prediction, Xk is the system state estimation, P À k is the prediction error covariance, P k is the estimation error covariance and K k is the Kalman gain.Q k denotes the covariance of process noise; R k denotes the covariance of measurement noise.
Q k and R k play the important roles in the recursive optimal process.The unsuitable Q k and R k would seriously degrade the filter performance.In most cases, Q k and R k are actually unknown and basically obtained by the statistical characteristics of noise from extensive practical experiments.However, the uncertainty of noise makes it hard to guarantee the precision and stability of state estimation. 24

Covariance-matching method
Covariance-matching method contains the innovationbased adaptive estimation (IAE) and the residual-based adaptive estimation (RAE). 14The innovation sequence of IAE and the residual sequence of RAE are defined respectively as follows: The covariance-matching method algorithm can be derived by substituting the equation ( 2) into the equation ( 9): The variable v k is the white Gaussian noise with zero mean.Taking variances on both sides of equation (11), and considering the equation ( 3) and the orthogonality between the measurement error and the system state prediction error 12 : The measurement noise covariance R k can be estimated directly when the covariance of innovation sequence F k is available.F k is normally computed by using the width of the moving window m after k recursive steps: IAE utilizes f k to compute the adaptive R k at the k step: The same principle can be applied to RAE which utilizes m k to compute the adaptive R k at the k step: M k is the covariance of residual sequence computed by using the width of the moving window m after k recursive steps: Furthermore, the following equations are utilized to compute the approximative adaptive Q k : or The computation of R k and Q k at the same time is a critical problem during the operation Kalman filter.From the equation ( 14), it needs to obtain P À k before computing the adaptive R k by using the innovation sequence, and the equation ( 5) explains Q k is essential before computing P À k .The same problem would produce in RAE.Therefore, Q k is a known and fixed value and the adaptive R k is computed by using try-and-error method (Determine the width of the moving window) during the operation process so that the filter would get the stable system state estimation; on the other hand, the computation of adaptive Q k by using the equation ( 17) or ( 18) also utilizes the known and fixed R k .The detailed derivation and analysis about the covariance-matching method are introduced carefully in Mohamed and Schwarz 14 , and Yang and Xu. 21In summary, the covariance-matching method has the following problems: (1) The width of the window would increase the storage burden of computer.(2) The determination of the window influences the estimation effectiveness.Increasing the window size for obtaining the smoothing covariance estimation would degrade the dynamic response of the adaptive mechanism and the small window size may influence the estimation precision of R k and Q k or the filter may diverge.(3) The number, type and distribution of the measurements for all steps within a window should be consistent.(4) R k and Q k may not be computed simultaneously.

Robust self-adaptive Kalman filter
Covariance-matching method is a simple and effective estimation algorithm applied to the adaptive Kalman filter but it needs to solve the existing problems about the determination of the width of the window and the addition of storage burden.In this paper, the authors propose a robust self-adaptive Kalman filter to improve the covariance-matching method and explain this algorithm as follows: The excess complexity for the filter algorithm would restrain the application.The proposed method can estimate the R k and Q k simultaneously to compute the filtering output by on-line operation.In order to reduce the complex operation steps of filter and storage burden, the proposed filter algorithm would be simplified reasonably for the engineering application.The computation result must be positive definitely in the equation (14) or equation (15) when IAE or RAE is used to compute the R k .However, the outcome is not guaranteed in the equation ( 14) since the subtraction operation of two positive matrices.Besides, RAE has the better estimation performance than IAE. 16,29Therefore, the residual sequence is utilized to compute the R k at k recursive step: It needs to know the P k before computing the R k in the equation ( 15), and that is impossible according to the operation process of Kalman filter.Therefore, the residual sequence at the last time interval of recursive computation is used to compute the current R k : In this study, an adaptive factor is developed for adjusting the R k opportunely.The innovation sequence of the equation ( 9) is used to be the judging criterion for evaluating the change of system dynamics.The variation of system state would directly influence the output of innovation sequence.The modulus of innovation sequence would be larger when the system state prompts variation.Therefore, the modulus of innovation sequence e k and the square root of the current covariance of measurement noise s k are utilized to design the adaptive factor O k defined in the equation (21), and it will be used when the modulus of innovation sequence is larger than the square root of the current covariance of measurement noise.On the contrary, the modulus of innovation sequence would be less when the system state has smooth variation and the adaptive factor would become bigger.It is equal to one when the modulus of innovation sequence is less than or equal to the square root of the current covariance of measurement noise.
The R k should get more attention when the system state produces huge amount of variance, and the R kÀ1 would be the major consideration when the system state has less variation.The adaptive estimation equation for R k combined with the adaptive factor O k is defined as follows: The same principle would be applied to compute the adaptive Q k at k recursive step: In summary, the operation process of the proposed robust self-adaptive Kalman filter is described in the following equations and the flow chart is shown in Figure 1.

Results and discussion
The proposed robust filtering algorithm would be applied to the maneuvering target tracking in this paper.As an experimental platform the observation of maneuvering target is combined with the noise disturbance and the performance of Kalman filter are verified by the maneuvers of target in the sky.The constant acceleration model is the common system model for the target tracking technique and the target state vector is defined as T .The linear discrete equations of the system model and the measurement model are described in the following equations, and the sampling time is T: Several simulation scenarios are used to prove the estimation performance of the proposed algorithm and that are also compared with three different filters.
Besides, the authors apply Monte Carlo method by 100 times to the simulation experiments and the root mean square error (RMSE) of position estimation errors is used to evaluate the estimation precision of proposed algorithm and other compared filters.The characteristics of Q and R for each filter are explained as follows: 1. Traditional Kalman filter (KF): Q, R are known and fixed.

Residual-based adaptive estimation (RAE):
Q is known and fixed; R is unknown.
3. Covariance-scaling method (CS) 12 : R is known and fixed; Q is unknown and can be scaled by computing the ratio between the calculated innovation covariance and the predicted one.A scaling factor is defined as follows: Substitute the equation ( 5) into the equation ( 35) and the scaling factor can be expressed as follows: An intuitive adaptation rule is defined as the equation (37) and the square root is utilized to contribute a smoothing application.

Scenario I: Evaluation of filter performance to the weak maneuvers
Figures 2 and 3 explain the target moves with the constant velocity or the weak maneuvers, and the maximum acceleration is under 10 (m/s 2 ). Figure 4 shows RMSE of position estimation errors for KF and RAE with the width of the window (m = 10, 30, 50).It is found that KF converges very fast during the overall estimation process.Compare RAE1-3, RAE1 with the width of the window (m = 10) has the fastest converging speed, the second is RAE2 (m = 30), RAE3 (m = 50) is the slowest.RAE1 owns the better adaptability to the change of target state and the position estimation errors are close to KF.The position estimation errors of RAE2 reduces gradually after the target maneuvering for 50 s (t .150 s) and the errors are larger slightly than KF and RAE1.RAE3 has the worst estimation performance and this explains it may degrade the dynamic response of the adaptive mechanism by increasing the window width.KF and RAE1-3 has almost the same position estimation errors when the target moves with the constant velocity (t .220 s).
Figure 5 shows RMSE of position estimation errors for CS and KF.CS converges faster than KF and the estimation process is stable, but the estimation errors are larger than KF. Figure 6 shows both the converging condition and the estimation errors of RSA-KF are close to KF but RSA-KF has the better estimation performance when the target changes motion state.In Figures 4 to 6, the adaptive RAE, CS and RSA-KF are compared with the traditional KF for the difference analysis of filter performance.Furthermore, three adaptive filters are also used to analyze the estimation performance in Scenario I. Figure 7 shows RMSE of position estimation errors for RAE1, CS and RSA-KF.CS is larger than RAE1 and RSA-KF during the overall process except the target changes the motion state.However, RAE1 produces the largest estimation errors in the time interval (180 s \ t \ 190 s) and the converging speed is the slowest.RSA-KF has the best estimation performance among three adaptive filters.In summary, RAE1 with the width of the window (m = 10) has the better adaptability and estimation precision in residual-based adaptive estimation method but the converging speed is the slowest among three different adaptive filters.CS owns the fast converging speed, stable estimation ability but the estimation precision is worse than the other two adaptive filters in Scenario I. RSA-KF performs well in converging speed and estimation effectiveness, and the process is quite steady.The estimation performance of RAS-KF is as well as KF but the better estimation ability to target maneuvers.

Scenario II: Evaluation of filter performance to the strong maneuvers
Figures 8 and 9 explain the target moves with the large acceleration during the flight process and the maximum acceleration would be up to 30 (m/s 2 ). Figure 10    with the width of the window (m = 10, 30, 50).KF converges quickly but the estimation errors increase obviously when the target produces the large acceleration.The estimation curve of RAE would trend to converge to the less estimation errors when the target moves with the constant velocity for a while (t .190 s) but the estimation performance is not satisfied due to the huge change of target state.
Figure 11 shows RMSE of position estimation errors for KF, CS and RSA-KF.The adaptive filters CS and RSA-KF have the faster converging speed and stable estimation process in which RSA-KF estimation errors are less than CS.The estimation precision of CS is still not satisfied during the estimation process.In summary, RAE performs badly to the strong maneuvering target.CS is a stable filter but the estimation precision must be improved.RSA-KF has the fast converging speed, stable estimation process and the better estimation precision.

Scenario III: Evaluation of robustness
In order to confirm the robust estimation effectiveness of RSA-KF, the scenario III has been assumed that the target would change the motion state suddenly in a short time and the target acceleration would be up to 60 (m/s 2 ) (Figure 12). Figure 13 shows KF increases the position estimation errors extremely due to the big acceleration input.The estimation process of CS is very stable even the target motion state extremely changes.However, the estimation precision of CS is not well compared with KF except the condition of big     acceleration input.RSA-KF performs well compared with KF and CS.This illustration explains the noise level would be influenced by the huge variety of system dynamics.Therefore, Q and R should be tuned opportunely to reduce the estimation errors of system dynamics.
Figures 14 and 15 show the magnitude of RSA-KF innovation in X, Y, Z axis of scenario II and scenario III.The two pictures illustrate the variation of system state would directly influence the output of innovation sequence.For an ideal filter, the magnitude of residual sequence has to be minimized after updating the measurement data.Figures 16 and 17 verify RSA-KF is a robust, steady, and precise filter.
In summarization, Kalman filter is at present the most widely used target tracking algorithm and the simulation experiments explain that Kalman filter have the satisfied estimation performance when the target moves with weak maneuvers.The tracking ability of Kalman filter would degrade seriously due to the high dynamic response of system such as the strong target maneuvering.Therefore, Kalman filter with fixed or improper Q and R may lead to the terrible estimation results or filter divergence if the dynamic system changes a lot.The adaptive Kalman filters (RAE and    CS) based on calculation of the covariance matrix of residual sequence with a moving sampling window actually can estimate the system state.However, the tracking ability of RAE degrades seriously to the strong target maneuvers.CS is very robust but the filer accuracy is not as well as RAE when the dynamic system is steady or changes slightly.The estimation performance of proposed method is excellent to the response of dynamic system and better than RAE and CS.It can not only solve the shortcomings of Kalman filer about the uncertainty of Q and R but owns the outstanding estimation ability.

Conclusion
The adaptive Kalman filter algorithms have been developed for a long time to reduce the estimation errors produced by the uncertainty of Q and R.However, most of these algorithms may not estimate Q and R simultaneously and the operation process may usually need to consider the additional parameter setting or increase the storage burden.In this paper, a high robust, low computing load and easy operation selfadaptive Kalman filter has been developed and that can solve the problems of covariance-matching method about the determination of the width of the window and the increment of the computing load.The maneuvering target tracking is used to verify the estimation performance of the robust self-adaptive Kalman filter.Simulation results show the proposed method definitely can tune Q and R simultaneously according to the change of system dynamics and own the properties of fast converging speed and steady estimation efficiency.The estimation performance is satisfied and has been verified by traditional Kalman filter and two adaptive Kalman filters.The proposed method utilizes the residual sequence at the last time interval of recursive computation to compute the current covariance of measurement noise.In order to promote the estimation precision as much as possible, the future work will be concentrated to the computation of estimation error covariance and measurement noise covariance at the same time.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Figure 1 .
Figure 1.Flow chart of robust self-adaptive Kalman filter.
Figures8 and 9explain the target moves with the large acceleration during the flight process and the maximum acceleration would be up to 30 (m/s 2 ).Figure10shows RMSE of position estimation errors for KF and RAE

Figure 4 .
Figure 4. (a) Scenario I: RMSE of position estimation errors for KF and RAE with the width of the window (m = 10, 30, 50).(b) Scenario I: RMSE of position estimation errors for KF and RAE with the width of the window (m = 10, 30, 50) in enlarged vertical scale.

Figure 2 . 3 .
Figure 2. Scenario I: Velocity variation of the target with weak maneuvers.

Figure 5 .
Figure 5. Scenario I: RMSE of position errors for CS and KF.

Figure 6 .
Figure 6.Scenario I: RMSE of position estimation errors for RSA-KF and KF.

Figure 8 .
Figure 8. Scenario II: Velocity variation of the target with strong maneuver.

Figure 9 .
Figure 9. Scenario II: Acceleration variation of the target with strong maneuver.

Figure 10 .
Figure 10.(a) Scenario II: RMSE of position estimation errors for KF and RAE with the width of the window (m = 10, 30, 50).(b) Scenario II: RMSE of position estimation errors for KF and RAE with the width of the window (m = 10, 30, 50) in enlarged vertical scale.

Figure 13 .
Figure 13.Scenario III: RMSE of position estimation errors for KF, CS, and RSA-KF.

Figure 14 .
Figure 14.RSA-KF innovation in X, Y, Z axis of Scenario II.

Figure 15 .
Figure 15.RSA-KF innovation in X, Y, Z axis of Scenario III.

Figure 16 .
Figure 16.RSA-KF residual in X, Y, Z axis of Scenario II.

Figure 17 .
Figure 17.RSA-KF residual in X, Y, Z axis of Scenario III.