Strong Tracking Filtering Algorithm of Randomly Delayed Measurements for Nonlinear Systems

This paper focuses on the filtering problems of nonlinear discrete-time stochastic dynamic systems, such as themodel simplification, noise characteristics uncertainty, initial conditions uncertainty, or system parametric variation. Under these circumstances, the measurements of system have one sampling time random delay. A new method, that is, strong tracking filtering algorithm of randomly delayed measurements (STF/RDM) for nonlinear systems based on recursive operating by analytical computation and first-order linear approximations, is proposed; a principle of extended orthogonality is presented as a criterion of designing the STF/RDM, and through the residuals between available and predicted measurements, the formula of fading factor is obtained. Under the premise of using the extended orthogonality principle, STF/RDM proposed in this paper can adjust the fading factor online via calculating the covariance of residuals, and then the gain matrices of the STF/RDM adjust in real time to enhance the performance of the proposed method. Lastly, in order to prove that the performance of STF/RDM precedes existing EKF method, the experiment of tracking maneuvering aircraft is carried out.


Introduction
Filtering task is obtaining state variables from a series of noisy measurements available online.The main aim is to minimize the estimation error, which is referred to as the residual or innovation vector.
Currently, most suboptimal methods for obtaining the posterior density in nonlinear discrete-time stochastic dynamic systems are using global and local approximation methods.Taking the point-mass filter based on adaptive algorithm [1] and particle filters with Gaussian mixtures based on Gaussian mixture approximation [2], for example, it is the advantage of the global approximate approach that any clear assumption pertaining to the form of posterior density is not needed.Although the global methods have strong adaptability, they suffer from enormous computational complexity.In contrast, the local methods have simple design of the filter and fast implementing speed, and the performance of this method is always with acceptable accuracy in actual applications.In local methods, the form of posterior density usually is assumed to be Gaussian.Sometimes several local filters can be derived without an assumption on the density (just the first two moments are required) such as extended Kalman filter or unscented Kalman filter.Up till now, there are plenty of variations about local methods of approximation, such as the extended Kalman filter (EKF) based on the method of linearization [3], the central-difference Kalman filter (CDKF) based on the interpolation method [4], the unscented Kalman filter (UKF) based on the method of unscented transform [5], the quadrature Kalman filter (QKF) based on the rule of Gauss-Hermite [6], and the cubature Kalman filter (CKF) based on the cubature rule of spherical-radial cubature [7].However, since the aforementioned state estimation methods are all formulated under the assumption of statistics of the noises and system parameters accurate modeling, in real applications they have some disadvantages; for example, the uncertainties in the model, initial conditions, or noise characteristics may lead to bias in the estimation process.In order to overcome the above disadvantages, one solution is to introduce fading factors in the state error covariance matrix based on the residual sequence.This method is named as the strong tracking filter (STF), which was proposed by Zhou and Frank [8][9][10][11].
In general, all the above filtering estimations often consider the fact that in real time the measurements generated by system are available, but the measurements directly obtained are affected by random delay in many actual applications.Therefore, the problem of filtering having randomly delayed measurements has been attracting wide attention [12][13][14][15], in nonlinear state estimation.In [12], two modified filtering algorithms, EKF and UKF, with one sampling time randomly delayed measurements have been proposed; an improved unscented filtering algorithm in [13] was proposed based on two-step randomly delayed measurements; the literature [14] considered one-stage prediction, filtering, and fixed-point smoothing problems in nonlinear discrete-time stochastic systems having one-step randomly delayed measurements; in this situation, the recursive estimation algorithm, that is, the signal produced by state-space model is uncertain and only the covariance information can be utilized, has been proposed; recently, considering observations of one-step randomly delayed measurements, a generic framework of Gaussian approximation (GA) filter has been given in [15].
To overcome the common disadvantages of filtering method having one-step randomly delayed measurements and normal filtering method, here, a novel STF/RDM is proposed; an extended orthogonality principle is presented as a criterion of designing the STF/RDM and through the residuals between available and predicted measurements, the formula of fading factor is obtained.Since the STF/RDM can implement the online tuning of the fading factor by monitoring the covariance of residuals, the gain of the STF/RDM will be adjusted in real time to enhance performance of ESFT.
The structure of this paper is as follows.The basis of theory and elementary knowledge about the existing EKF having one-step randomly delayed measurements is reviewed in Section 2.Then, the extended orthogonality principle which is the basis of STF/RDM is proposed in Section 3. Thereafter, in Section 4, the STF/RDM having one-step randomly delayed measurements is derived.In Section 5, simulation experiment on tracking a maneuvering aircraft is implemented to compare the performance of the STF/RDM with existing EKF.Finally, Section 6 gives some conclusions.
Throughout this paper, [⋅] stands for mathematical expectation;  stands for the unit matrix; diag{⋅ ⋅ ⋅ } denotes a block-diagonal matrix; the superscripts −1, , ∧ , and ∼, respectively, denote the inverse matrix, the matrix transposition, the estimate, and the estimation error.For example, x stands for the estimate of variable  and x =  − x stands for the estimate error of variable .

Problem Formulation and Preliminaries
In this section, the nonlinear model having one-step randomly delayed observations and the filtering algorithm derived from this model are reviewed.

Nonlinear System
Model.Consider a nonlinear, discretetime, stochastic system as state model shown by and the model of one sampling randomly delayed observation where {  ;  ≥ 0} is the  × where   represents the probability of a delay in measurement at time .Substituting (2) into (3) gives In ( 1)-(3), assume that  0 , {  ;  ≥ 0}, {V  ;  ≥ 1}, and {  ;  > 1} are mutually independent.Obviously, the Bernoulli variable in (3) imitates the random delay in the following sense: at each time  > 1, if   = 1, then   =  −1 which means that the measurement is one sampling time randomly delayed; otherwise, if   = 0, then   =   which means that the measurement is updated.

Extended Kalman Filter with One-Step Randomly Delayed
Observations.In [15], a general and common framework of Gaussian approximation (GA) applied in the system shown by ( 1)-(3) has been presented; under these circumstances, the measurements with one sampling time random delay often occur.Here, the functions of one-step posterior predictive probability density ( +1 |   ) and ( +1 |   ) are all assumed to be Gaussian, where   = {  }  =1 is the set of the available measurements in (3).In (5), it is clear that the Gaussian approximation of ( +1 |  +1 ) and (V +1 |  +1 ) needs to be known when deriving a GA filter for the system in (1)- (3).Therefore, the augmented state vector is defined as follows: whose mean and covariance are conditioned by  +1 , approximated by Note that V +1 is independent of   and x+1| .Hence, the augmented state prediction x +1| and the covariance   +1| are In [15], the equations describing the Gaussian approximation (GA) filter applied in the system shown by ( 1)-( 3) are as follows: where and   +1 and  V +1 , respectively, express the gain matrices of the filtering estimated state and measurement noise.Based on ( 9)-( 19), the extended Kalman filter in [12] can be described by the following equations.

Extended Orthogonality Principle
As is known to all, model mismatch due to model simplification, noise characteristics uncertainty, initial conditions uncertainty, or system parametric variation causes the robustness of EKF to be bad and even diverging [16,17].
Under the orthogonality principle, the literature [18] first presented the strong tracking filter (STF) applied in nonlinear systems with white noise.The excellent characteristics of STF are described as follows: (1) It has strong robustness when the model is uncertain.
(2) For the state changing suddenly or slowly and even the system reaching a steady state or not, it has excellent ability of tracking to the states.
Further, we hold opinion that STF fit coping with the problem of model uncertainties and other unpredictable disturbances in nonlinear state estimation that have one-step randomly delayed observations.The standard STF cannot be directly applied to the state estimation with one-step randomly delayed observations, because of the arbitrarily selected pairs of residuals, in orthogonality principle of standard STF, which is calculated according to all observations having been updated.Therefore, in the following section, STF/RDM is proposed according to the principle of extended orthogonality applied in fusion onestep randomly delayed observations efficiently.
Definition 1 (extended orthogonality principle).For the discrete-time nonlinear process having one-step randomly delayed observations in ( 1)-( 3), ( 9), (10), and ( 22), the sufficient condition of the augmented state estimator is called a strong tracking filter that the criteria must satisfy (through choosing time varying gain matrices   +1 and  V +1 online): where the criteria of minimum mean square error (MMSE) is shown as (31), the condition of orthogonality is shown as (32), in which the condition is that all of the residuals should be mutually orthogonal at any time.
Remark 2. Equation ( 31) is just the criterion of the existing EKF, the derivation of which for the problem is presented in the Appendix.Equation (32) is the core formulation of extended orthogonality principle; using other criteria to replace (31), the deformation of extended orthogonality principle can be obtained.Therefore, once (32) was introduced into the original filter, it has the characteristics of STF.

Derivation of the STF/RDM
In this section, an STF/RDM algorithm is derived according to the principle of extended orthogonality.It is easy to find that the idea of the EKF with one-step randomly delayed observations depends upon the past measurement data and the heavy reliance may lead to diverge state estimation.In order to restrain the divergence, the filter should be capable of eliminating the effect of past data from a current state estimate if these data are no longer meaningful.The literature [18] presented a method to modify the covariance of state error at time  through introducing the fading factor of suboptimal as follows: Then, the covariance of predicted state error is also modified through substituting (33) into (23) as follows: where  +1 ≥ 1.As a consequence, the influence of the latest measurement data in state estimation is dominant and divergence is restrained.The purpose of the STF/RDM is to impair the influence of the historical data when they are no longer significant, by using a time varying suboptimal fading factor, and modify the gain matrices online so that the filter has strong tracking ability.Therefore, a key problem in STF/RDM is how to calculate the suboptimal fading factor  +1 according to the principle of extended orthogonality.
Remark 3.For nonlinear systems having one-step randomly delayed observations, if directly applying the principle of extended orthogonality to them, ( 31) and (32) may be difficult to be strictly satisfied.Under these circumstances, the approximate method is usually applied to satisfy these two conditions and obtain the approximate solution of a fading factor  +1 , such as calculating the traces directly in both sides of (53) and roughly determining  0 +1 through (59) to ensure that the filtering algorithm can be calculated in real time.

Simulation Results and Analysis
In this section, to analyze and compare the performance of the proposed method in Section 4 and the existing EKF in Section 2, the simulation experiments of tracking a maneuvering aircraft are implemented.Assume that the initial position, velocity, and turn rate of the aircraft in the twodimensional plane are (1 km, 1 km), (300 m/s, 0 m/s), and 0 ∘ s −1 , respectively.The simulation aircraft trajectory is generated as follows: (1) It moves with constant velocity during 0-26 s.
Figures 1(a), 1(b), and 1(c) show the simulation trajectory of the aircraft position, velocity, and turn rate, respectively, during an interval of 0-100 s.
Considering the coordinated turn model with unknown turn rate Ω in [19], there is bias between real value of turn rate Mathematical Problems in Engineering and estimate value of it, and the bias leads to the model mismatch.The mismatch kinematics model of the maneuvering aircraft can be obtained, which is shown as follows: where  = [ ẋ  ẏ Ω]  is state vector; , , ẋ , and ẏ express the position and velocity in  direction and  direction, respectively; Ω denotes turn rate;  denotes sampling period;   denotes the process noise which has zero mean and covariance where the parameters  1 = 0.1 m 2 s −3 ,  2 = 0.1 m 2 s −3 , and  3 = 1.75 × 10 −4 s −3 , respectively, denote the coefficient of process noise in  direction,  direction, and turn rate.Using two-dimensional radar location the origin of plane measures the range and bearing of maneuvering aircraft.The measurement can be calculated by the following equation: where V  is radar measurement noise which has zero mean and its covariance where   = 10 m and   = √ 10 × 10 −3 rad.Assume that the measurements applied in the estimation have one sampling time random delay and the measurements can be calculated as follows: The  0 = [1000 m 300 ms −1 1000 m 0 ms −1 0 ∘ s −1 ]  is initial state.In each simulation, the initial state estimation x0 is selected randomly from ( 0 ,  0|0 ), where the initial covariance is The period of sampling is 1 second and the total time of each simulation is 100 seconds.In order to compare the filtering performance, the root mean square error (RMSE) is chosen, because it can yield a measure which combines the bias and variance of a filter estimate.At time , both RMSEs of position are defined by where  denotes the total number of Monte Carlo experiment, (   ,    ) and ( x  , ŷ  ), respectively, denote the simulated position, which can be replaced by true position and filtering estimate position, when th Monte Carlo experiment is run.Like the RMSE of position, the formulas of RMSE about velocity and turn rate can also be defined.
In situation I, assuming  = 1000,  = 0.5,  = 1, and  = 0.1, 0.2, . . ., 1, the average of RMSEs of position, velocity, and turn rate obtained by using the STF/RDM is shown in Figure 2. The values of fading factor determined by forgetting factor are shown in Figure 3.As shown in Figures 2 and 3, with the increase of the forgetting factor , the mean of RMSEs about the proposed STF/RDM is almost stable and  +1 is insensitive to the value of .Therefore, the forgetting factor is selected as  = 0.95 in the following situation.
In situation II, assuming  = 1,  = 0.5, and  = 1, the RMSEs of position, velocity, and turn rate obtained by using the proposed STF/RDM and the existing EKF are shown in Figures 4, 5  determined by proposed STF/RDM are shown in Figure 7.The estimated autocovariance of  and position calculated by STF/RDM and the existing EKF is, respectively, shown in Figures 8 and 9.According to Figures 4-9, the analysis is as follows: (  EKF.In this case, besides the RMSEs of position, velocity, and turn rate, the estimated autocovariance of  position and that of  position based on proposed STF/RDM and existing EKF are almost equal. (2) During 69-100 seconds, aircraft maneuvers with greater constant turn rate at first and then moves with constant velocity.Since the turn rate changes suddenly, both filters appear divergence.In the divergence period, the estimated autocovariance of  position and that of  position of STF/RDM are larger than the EKF and the RMSEs of existing EKF are larger than the STF/RDM.The proposed STF/RDM can timely detect the increase of residual covariance and through the fading factors adaptively increasing, the RMSEs reduce and the estimated autocovariance of  and  position increases.Comparing with the existing EKF, the increasing of estimated autocovariance of STF/RDM can reflect the sudden change of  and  position in time.The decrease of RMSEs ensures STF/RDM having better tracking performance.After the divergence period, the RMSEs and the estimated autocovariance of STF/RDM quickly decrease, while the RMSEs and the estimated autocovariance of EKF gradually increase; in other words, unlike the existing EKF, the STF/RDM can eliminate the influence of the cumulative estimation error by increasing the fading factor to avoid further divergence.The above results verify that proposed STF/ RDM have the ability to deal with the problem of system parametric variation.For a judicial comparison, the same condition is set to initialize all the filters in each simulation, and 1000 independent Monte Carlo experiments are carried out.
In situation III, assuming  = 1000,  = 0.5, and  = 1, the RMSEs of position, velocity, and turn rate obtained by using the proposed STF/RDM and the existing EKF are shown in Figures 10,11,and 12, respectively, and the mean of RMSEs in position, velocity, and turn rate is shown in Table 1.
As shown in    In situation IV, assuming  = 1000,  = 1, and  = 0.1, 0.2, . . ., 0.9, the mean of RMSE in position about two filters is shown in Figure 13.The mean of the proposed STF/ RDM is smaller than the existing EKF, which reflects that the filtering performance of proposed STF/RDM precedes the existing EKF when the delay probability  has a large range of change.In particular, as  increases, both mean of proposed STF/RDM and that of existing EKF are obviously decreased, but the STF/RDM has better filtering performance when  is equal to greater value.In situation V, assuming  = 1000,  = 0.5, and  = 1.1, 1.2, . . ., 1.9, the mean of RMSE in position about two filters is shown in Figure 14.As shown in Figure 14, with the increase of the noise level , the mean of RMSE about the existing EKF is increasing, while it is inspiring to find that the mean of RMSE about the proposed STF/RDM is almost stable.
Generally speaking, it can be found that the proposed STF/RDM has the definite robustness to the different change of the delay probability and the noise level on the basis of the simulation analysis from Figures 13 and 14.

Conclusion
In this paper, for the tracking problem of one sampling time randomly delayed measurements in nonlinear system, a new algorithm of STF/RDM is proposed.The recursive operation of this algorithm is carried out by first-order linearization approximation.When the model is inexact, for example, model simplification, noise characteristics, initial conditions uncertainty, or system parametric variation, based on extended orthogonality principle, the proposed STF/RDM can timely detect the change of residual covariance and keep well ability of tracking by changing the fading factor online.The simulation experiments are carried out to prove the proposed STF/RDM having good performance.Also the results show that STF/RDM is better than existing EKF in coping with the problem of tracking maneuvering target.Analyzing the outcome of simulation, the conclusion is that proposed STF/RDM has more high accuracy and robustness than existing EKF for the problem of model mismatch when tracking maneuvering target, besides advantages of existing EKF.Since the proposed STF/RDM is general filtering method, it can be applied to some relevant research areas such as fault diagnosis, signal processing, and state estimation of dynamic system.
, and 6, respectively.The values of fading factor
1 state vector, {  ;  ≥ 1} is the  × 1 real measurement, {  ;  > 1} is the  × 1 available measurements, {  ;  ≥ 0} and {V  ;  ≥ 1} are sequences of uncorrelated Gaussian white noises that have zero means and the covariance matrices which are [     ] =     and [V  V   ] =     , respectively, where   is the Kronecker delta function, the initial state  0 is a random Gaussian vector having mean [ 0 ] = x0|0 and covariance [ 0   0 ] =  0|0 , for all , the nonlinear functions   and ℎ  are infinitely continuously differentiable, and {  ;  > 1} denotes a sequence of uncorrelated Bernoulli random variables that can take the values 0 or 1 with

Table 1 and
Figures 10,11,and 12, the RMSEs of existing EKF gradually increasing lead the EKF to diverge due to randomly selecting the initial state estimation x0 from ( 0 ,  0|0 ) in each run.Contrarily, the RMSEs of STF/RDM are convergent.The performance of proposed STF/RDM is better than existing EKF, whatever in accuracy or convergence rate.It is inferred that the proposed STF/RDM can
solve the problem of initial conditions uncertainty to improve estimation accuracy, in spite of the existing EKF sensitive to this problem.Correspondingly, when  is equal to any other value during 0 to 1, similar result can be obtained.