# Strong Tracking Filtering Algorithm of Randomly Delayed Measurements for Nonlinear Systems

Authors: Hongtao Yang; Huibin Gao; Xin Liu
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

This paper focuses on the filtering problems of nonlinear discrete-time stochastic dynamic systems, such as the model 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.


---

## Body

## 1. 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–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–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 Section2. 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,E[·] stands for mathematical expectation; I stands for the unit matrix; diag{⋯} denotes a block-diagonal matrix; the superscripts -1, T, ∧, and ~, respectively, denote the inverse matrix, the matrix transposition, the estimate, and the estimation error. For example, x^ stands for the estimate of variable x and x~=x-x^ stands for the estimate error of variable x.


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


### 2.1. Nonlinear System Model

Consider a nonlinear, discrete-time, stochastic system as state model shown by(1)xk+1=fkxk+wk,k≥0and the model of one sampling randomly delayed observation(2)zk=hkxk+vk,k≥1,(3)yk=1-γkzk+γkzk-1,k>1;y1=z1,where xk;k≥0 is the n×1 state vector, zk;k≥1 is the m×1 real measurement, yk;k>1 is the m×1 available measurements, wk;k≥0 and vk;k≥1 are sequences of uncorrelated Gaussian white noises that have zero means and the covariance matrices which are E[wkwlT]=Qkδkl and E[vkvlT]=Rkδkl, respectively, where δkl is the Kronecker delta function, the initial state x0 is a random Gaussian vector having mean E[x0]=x^0∣0 and covariance E[x0x0T]=P0∣0, for all k, the nonlinear functions fk and hk are infinitely continuously differentiable, and γk;k>1 denotes a sequence of uncorrelated Bernoulli random variables that can take the values 0 or 1 with(4)pγk=1=Eγk=pk,pγk=0=1-Eγk=1-pk,Eγk-pk2=1-pkpk,Eγk-pk=0,where pk represents the probability of a delay in measurement at time k.Substituting (2) into (3) gives(5)yk+1=1-γkhk+1xk+1+vk+1+γk+1hkxk+vk.In (1)–(3), assume that x0, wk;k≥0, vk;k≥1, and γk;k>1 are mutually independent.Obviously, the Bernoulli variable in (3) imitates the random delay in the following sense: at each time k>1, if γk=1, then yk=zk-1 which means that the measurement is one sampling time randomly delayed; otherwise, if γk=0, then yk=zk which means that the measurement is updated.


### 2.2. 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 p(xk+1∣Yk) and p(yk+1∣Yk) are all assumed to be Gaussian, where Yk={yi}i=1k is the set of the available measurements in (3). In (5), it is clear that the Gaussian approximation of p(xk+1∣Yk+1) and p(vk+1∣Yk+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:(6)xk+1a=xk+1vk+1,whose mean and covariance are conditioned by Yk+1, approximated by(7)x^k+1∣k+1a=x^k+1∣k+1v^k+1∣k+1,Pk+1∣k+1a=Pk+1∣k+1Pk+1∣k+1xvPk+1∣k+1xvTPk+1∣k+1vv.Note that vk+1 is independent of Yk and x~k+1∣k. Hence, the augmented state prediction x^k+1∣ka and the covariance Pk+1∣ka are(8)x^k+1∣ka=x^k+1∣k0m×1,Pk+1∣ka=Pk+1∣k0n×m0m×nRk+1.In [15], the equations describing the Gaussian approximation (GA) filter applied in the system shown by (1)–(3) are as follows: (9)x^k+1∣k+1=x^k+1∣k+Kk+1xy~k+1∣k,(10)v^k+1∣k+1=Kk+1vy~k+1∣k,(11)Pk+1∣k+1=Pk+1∣k-Kk+1xPk+1∣kyyKk+1xT,(12)Pk+1∣k+1xv=-Kk+1xPk+1∣kyyKk+1vT,(13)Pk+1∣k+1vv=Rk+1-Kk+1vPk+1∣kyyKk+1vT,(14)Kk+1x=Pk+1∣kxyPk+1∣kyy-1,(15)y~k+1∣k=1-γk+1zk+1-z^k+1∣k+γk+1zk-z^k∣k+γk+1-pk+1z^k∣k-z^k+1∣k,(16)Kk+1v=Pk+1∣kvyPk+1∣kyy-1,(17)Pk+1∣kyy=1-pk+1Pk+1∣kzz+pk+1Pk∣kzz+1-pk+1pk+1z^k+1∣k-z^k∣kz^k+1∣k-z^k∣kT,(18)Pk+1∣kxy=1-pk+1Pk+1∣kxz+pk+1Pk+1,k∣kxz,(19)Pk+1∣kvy=1-pk+1Rk+1,where Pk+1∣k+1xv=E[x~k+1∣k+1v~k+1∣k+1T∣Yk+1], Pk+1∣kxy=E[x~k+1∣ky~k+1∣kT∣Yk], Pk+1∣kxz=E[x~k+1∣kz~k+1∣kT∣Yk], Pk+1,k∣kxz=E[x~k+1∣kz~k∣kT∣Yk], Pk+1∣kvy=E[vk+1y~k+1∣kT∣Yk], Pk+1∣kzz=E[z~k+1∣kz~k+1∣kT∣Yk], Pk∣kzz=E[z~k∣kz~k∣kT∣Yk], z^k+1∣k=E[zk+1∣Yk], z^k∣k=E[zk∣Yk], and Kk+1x and Kk+1v, 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.Assuming that thex^k∣ka and Pk∣ka at time k have been computed, by linearization of fk(xk) and hk(xk) with the first term of the Taylor series expansion about xk∣k=x^k∣k, we get (20)xk+1≈fkx^k∣k+Fkxk-x^k∣k+wk,(21)zk≈hkx^k∣k+Hkxk-x^k∣k+vk,where Fk=∂fk(xk)/∂xkxk=x^k∣k and Hk=∂hkxk/∂xkxk=x^k∣k. Then, x^k+1∣k and Pk+1∣k are approximated by the linear Kalman filter: (22)x^k+1∣k=fkx^k∣k,(23)Pk+1∣k=FkPk∣kFkT+Qk.Moreover, given x^k+1∣k and Pk+1∣k by (22) and (23), by linearization of hk+1(xk+1) with the first term of the Taylor series expansion about xk+1∣k=x^k+1∣k, we get(24)zk+1≈hk+1x^k+1∣k+Hk+1xk+1-x^k+1∣k+vk+1,where Hk+1=∂hk+1xk+1/∂xk+1xk+1=x^k+1∣k. Then, z^k+1∣k, z^k∣k, Pk+1∣kzz, Pk∣kzz, Pk+1∣kxz, and Pk+1,k∣kxz are computed by the linear Kalman filter as follows:(25)z^k+1∣k=hk+1x^k+1∣k,(26)z^k∣k=hkx^k∣k+v^k∣k,(27)Pk+1∣kzz=Hk+1Pk+1∣kHk+1T+Rk+1,(28)Pk∣kzz=HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv,(29)Pk+1∣kxz=Pk+1∣kHk+1T,(30)Pk+1,k∣kxz=FkPk∣kHkT+FkPk∣kxv.At time k+1, combining (22)-(23) and (25)–(30) with (9)–(19) computes x^k+1∣k+1a and Pk+1∣k+1a in (7). For the derivation process of (20), (21), (24), and (26), see the literature [12].


## 2.1. Nonlinear System Model

Consider a nonlinear, discrete-time, stochastic system as state model shown by(1)xk+1=fkxk+wk,k≥0and the model of one sampling randomly delayed observation(2)zk=hkxk+vk,k≥1,(3)yk=1-γkzk+γkzk-1,k>1;y1=z1,where xk;k≥0 is the n×1 state vector, zk;k≥1 is the m×1 real measurement, yk;k>1 is the m×1 available measurements, wk;k≥0 and vk;k≥1 are sequences of uncorrelated Gaussian white noises that have zero means and the covariance matrices which are E[wkwlT]=Qkδkl and E[vkvlT]=Rkδkl, respectively, where δkl is the Kronecker delta function, the initial state x0 is a random Gaussian vector having mean E[x0]=x^0∣0 and covariance E[x0x0T]=P0∣0, for all k, the nonlinear functions fk and hk are infinitely continuously differentiable, and γk;k>1 denotes a sequence of uncorrelated Bernoulli random variables that can take the values 0 or 1 with(4)pγk=1=Eγk=pk,pγk=0=1-Eγk=1-pk,Eγk-pk2=1-pkpk,Eγk-pk=0,where pk represents the probability of a delay in measurement at time k.Substituting (2) into (3) gives(5)yk+1=1-γkhk+1xk+1+vk+1+γk+1hkxk+vk.In (1)–(3), assume that x0, wk;k≥0, vk;k≥1, and γk;k>1 are mutually independent.Obviously, the Bernoulli variable in (3) imitates the random delay in the following sense: at each time k>1, if γk=1, then yk=zk-1 which means that the measurement is one sampling time randomly delayed; otherwise, if γk=0, then yk=zk which means that the measurement is updated.


## 2.2. 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 p(xk+1∣Yk) and p(yk+1∣Yk) are all assumed to be Gaussian, where Yk={yi}i=1k is the set of the available measurements in (3). In (5), it is clear that the Gaussian approximation of p(xk+1∣Yk+1) and p(vk+1∣Yk+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:(6)xk+1a=xk+1vk+1,whose mean and covariance are conditioned by Yk+1, approximated by(7)x^k+1∣k+1a=x^k+1∣k+1v^k+1∣k+1,Pk+1∣k+1a=Pk+1∣k+1Pk+1∣k+1xvPk+1∣k+1xvTPk+1∣k+1vv.Note that vk+1 is independent of Yk and x~k+1∣k. Hence, the augmented state prediction x^k+1∣ka and the covariance Pk+1∣ka are(8)x^k+1∣ka=x^k+1∣k0m×1,Pk+1∣ka=Pk+1∣k0n×m0m×nRk+1.In [15], the equations describing the Gaussian approximation (GA) filter applied in the system shown by (1)–(3) are as follows: (9)x^k+1∣k+1=x^k+1∣k+Kk+1xy~k+1∣k,(10)v^k+1∣k+1=Kk+1vy~k+1∣k,(11)Pk+1∣k+1=Pk+1∣k-Kk+1xPk+1∣kyyKk+1xT,(12)Pk+1∣k+1xv=-Kk+1xPk+1∣kyyKk+1vT,(13)Pk+1∣k+1vv=Rk+1-Kk+1vPk+1∣kyyKk+1vT,(14)Kk+1x=Pk+1∣kxyPk+1∣kyy-1,(15)y~k+1∣k=1-γk+1zk+1-z^k+1∣k+γk+1zk-z^k∣k+γk+1-pk+1z^k∣k-z^k+1∣k,(16)Kk+1v=Pk+1∣kvyPk+1∣kyy-1,(17)Pk+1∣kyy=1-pk+1Pk+1∣kzz+pk+1Pk∣kzz+1-pk+1pk+1z^k+1∣k-z^k∣kz^k+1∣k-z^k∣kT,(18)Pk+1∣kxy=1-pk+1Pk+1∣kxz+pk+1Pk+1,k∣kxz,(19)Pk+1∣kvy=1-pk+1Rk+1,where Pk+1∣k+1xv=E[x~k+1∣k+1v~k+1∣k+1T∣Yk+1], Pk+1∣kxy=E[x~k+1∣ky~k+1∣kT∣Yk], Pk+1∣kxz=E[x~k+1∣kz~k+1∣kT∣Yk], Pk+1,k∣kxz=E[x~k+1∣kz~k∣kT∣Yk], Pk+1∣kvy=E[vk+1y~k+1∣kT∣Yk], Pk+1∣kzz=E[z~k+1∣kz~k+1∣kT∣Yk], Pk∣kzz=E[z~k∣kz~k∣kT∣Yk], z^k+1∣k=E[zk+1∣Yk], z^k∣k=E[zk∣Yk], and Kk+1x and Kk+1v, 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.Assuming that thex^k∣ka and Pk∣ka at time k have been computed, by linearization of fk(xk) and hk(xk) with the first term of the Taylor series expansion about xk∣k=x^k∣k, we get (20)xk+1≈fkx^k∣k+Fkxk-x^k∣k+wk,(21)zk≈hkx^k∣k+Hkxk-x^k∣k+vk,where Fk=∂fk(xk)/∂xkxk=x^k∣k and Hk=∂hkxk/∂xkxk=x^k∣k. Then, x^k+1∣k and Pk+1∣k are approximated by the linear Kalman filter: (22)x^k+1∣k=fkx^k∣k,(23)Pk+1∣k=FkPk∣kFkT+Qk.Moreover, given x^k+1∣k and Pk+1∣k by (22) and (23), by linearization of hk+1(xk+1) with the first term of the Taylor series expansion about xk+1∣k=x^k+1∣k, we get(24)zk+1≈hk+1x^k+1∣k+Hk+1xk+1-x^k+1∣k+vk+1,where Hk+1=∂hk+1xk+1/∂xk+1xk+1=x^k+1∣k. Then, z^k+1∣k, z^k∣k, Pk+1∣kzz, Pk∣kzz, Pk+1∣kxz, and Pk+1,k∣kxz are computed by the linear Kalman filter as follows:(25)z^k+1∣k=hk+1x^k+1∣k,(26)z^k∣k=hkx^k∣k+v^k∣k,(27)Pk+1∣kzz=Hk+1Pk+1∣kHk+1T+Rk+1,(28)Pk∣kzz=HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv,(29)Pk+1∣kxz=Pk+1∣kHk+1T,(30)Pk+1,k∣kxz=FkPk∣kHkT+FkPk∣kxv.At time k+1, combining (22)-(23) and (25)–(30) with (9)–(19) computes x^k+1∣k+1a and Pk+1∣k+1a in (7). For the derivation process of (20), (21), (24), and (26), see the literature [12].


## 3. 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.(3)
It has moderate computational complexity.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 one-step 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 Kk+1x and Kk+1v online): (31)Exk+1a-x^k+1∣k+1axk+1a-x^k+1∣k+1aT=min,(32)Ey~k+1+j∣k+jy~k+1∣kT=0,k=0,1,2,…;j=1,2,…,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.


## 4. 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 k through introducing the fading factor of suboptimal as follows:(33)Pk∣k=λk+1Pk∣k.Then, the covariance of predicted state error is also modified through substituting (33) into (23) as follows:(34)Pk+1∣k=λk+1FkPk∣kFkT+Qk,where λk+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λk+1 according to the principle of extended orthogonality.Substituting (21) and (24)–(26) into (15) yields(35)y~k+1∣k=1-γk+1Hk+1xk+1-x^k+1∣k+vk+1+γk+1Hkxk-x^k∣k+vk-v^k∣k+γk+1-pk+1hkx^k∣k-hk+1x^k+1∣k+v^k∣k.Using (20) minus (22) yields(36)xk+1-x^k+1∣k=Fkxk-x^k∣k+wk.Substituting (22) and (36) into (35) yields(37)y~k+1∣k=1-γk+1Hk+1Fk+γk+1Hkxk-x^k∣k+1-γk+1Hk+1wk+1-γk+1vk+1+γk+1vk-v^k∣k+γk+1-pk+1hkx^k∣k-hk+1fkx^k∣k+v^k∣k.Using a similar derivation method yields(38)y~k+1+j∣k+j=1-γk+1+jHk+1+jFk+j+γk+1+jHk+jxk+j-x^k+j∣k+j+1-γk+1+jHk+1+jwk+j1-γk+1+jvk+1+j+γk+1+jvk+j-v^k+j∣k+j+γk+1+j-pk+1+jhk+jx^k+j∣k+j-hk+1+jfk+jx^k+j∣k+j+v^k+j∣k+j.Substituting (38) into (32) yields(39)Ey~k+1+j∣k+jy~k+1∣kT=E1-γk+1+jHk+1+jFk+j+γk+1+jHk+jxk+j-x^k+j∣k+j+1-γk+1+jHk+1+jwk+j+1-γk+1+jvk+1+j+γk+1+jvk+j-v^k+j∣k+j+γk+1+j-pk+1+jhk+jx^k+j∣k+j-hk+1+jfk+jx^k+j∣k+j+v^k+j∣k+jy~k+1∣kT.Since the initial state x0, wk;k≥0, vk;k≥1, and γk;k>1 that can generate the state and observations are mutually independent and taking (4) into account, then the (39) can be simplified to(40)Ey~k+1+j∣k+jy~k+1∣kT=1-pk+1+jHk+1+jFk+j+pk+1+jHk+jExk+j-x^k+j∣k+jy~k+1∣kT.Based on (9), (20), and (22) and by a similar derivation method applied in (38), this yields(41)xk+j-x^k+j∣k+j=xk+j-x^k+j∣k+j-1-Kk+jxy~k+j∣k+j-1=I-1-γk+jKk+jxHk+jFk+j-1-Kk+jxγk+jHk+j-1xk+j-1-x^k+j-1∣k+j-1+wk+j-1-Kk+jx1-γk+jHk+jwk+j-1+1-γk+jvk+j+γk+jvk+j-1-v^k+j-1∣k+j-1+γk+j-pk+jhk+j-1x^k+j-1∣k+j-1-hk+jfk+j-1x^k+j-1∣k+j-1+v^k+j-1∣k+j-1.Substituting (41) into (40) yields(42)Ey~k+1+j∣k+jy~k+1∣kT=1-pk+1+jHk+1+jFk+j+pk+1+jHk+jEI-1-γk+jKk+jxHk+jFk+j-1-Kk+jxγk+jHk+j-1xk+j-1-x^k+j-1∣k+j-1+wk+j-1-Kk+jx1-γk+jHk+jwk+j-1+1-γk+jvk+j+γk+jvk+j-1-v^k+j-1∣k+j-1+γk+j-pk+jhk+j-1x^k+j-1∣k+j-1-hk+jfk+j-1x^k+j-1∣k+j-1+v^k+j-1∣k+j-1y~k+1∣kT.Again, since the initial state x0, wk;k≥0, vk;k≥1, and γk;k>1 that can generate the state and observations are mutually independent and taking (4) into account, then (42) can be simplified to(43)Ey~k+1+j∣k+jy~k+1∣kT=1-pk+1+jHk+1+jFk+j+pk+1+jHk+jI-1-pk+jKk+jxHk+jFk+j-1-Kk+jxpk+jHk+j-1Exk+j-1-x^k+j-1∣k+j-1y~k+1∣kT.From (40) and (43), the following form can be obtained by using an iterative operation(44)Ey~k+1+j∣k+jy~k+1∣kT=1-pk+1+jHk+1+jFk+j+pk+1+jHk+j∏i=2jI-1-pk+iKk+ixHk+iFk+i-1-Kk+ixpk+iHk+i-1Exk+1-x^k+1∣k+1y~k+1∣kT.Equation (9) yields(45)Exk+1-x^k+1∣k+1y~k+1∣kT=Exk+1-x^k+1∣k-Kk+1xy~k+1∣ky~k+1∣kT=Exk+1-x^k+1∣ky~k+1∣kT-Kk+1xEy~k+1∣ky~k+1∣kT=Ex~k+1∣ky~k+1∣kT-Kk+1xEy~k+1∣ky~k+1∣kT=Pk+1∣kxy-Kk+1xEy~k+1∣ky~k+1∣kT.Substituting (29) and (30) into (18) yields(46)Pk+1∣kxy=1-pk+1Pk+1∣kHk+1T+pk+1FkPk∣kHkT+FkPk∣kxv.Substituting (46) into (45) yields(47)Exk+1-x^k+1∣k+1y~k+1∣kT=1-pk+1Pk+1∣kHk+1T+pk+1FkPk∣kHkT+FkPk∣kxv-Kk+1xVk+10,where Vk+10≜E{y~k+1∣ky~k+1∣kT} is the covariance of the residual.Substituting (17), (22), (25)–(28), and (46) into (14) yields(48)Kk+1x=1-pk+1Pk+1∣kHk+1T+pk+1FkPk∣kHkT+FkPk∣kxv1-pk+1Hk+1Pk+1∣kHk+1T+Rk+1+pk+1HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv+1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-1.Substituting (48) into (47) yields (49)Exk+1-x^k+1∣k+1y~k+1∣kT=1-pk+1Pk+1∣kHk+1T+pk+1FkPk∣kHkT+FkPk∣kxvI-1-pk+1Hk+1Pk+1∣kHk+1T+Rk+1+pk+1HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv+1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-1Vk+10.Substituting (49) into (44) yields(50)Ey~k+1+j∣k+jy~k+1∣kT=1-pk+1+jHk+1+jFk+j+pk+1+jHk+j∏i=2jI-1-pk+iKk+ixHk+iFk+i-1-Kk+ixpk+iHk+i-11-pk+1Pk+1∣kHk+1T+pk+1FkPk∣kHkT+FkPk∣kxvI-1-pk+1Hk+1Pk+1∣kHk+1T+Rk+1+pk+1HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv+1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-1Vk+10.In order to satisfy the principle of extended orthogonality in (32), an appropriate fading factor λk+1 needs to be chosen, according to (50), to ensure that (51) is workable:(51)I-1-pk+1Hk+1Pk+1∣kHk+1T+Rk+1+pk+1HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv+1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-1Vk+10=0.Equation (51) is equivalent to(52)1-pk+1Hk+1Pk+1∣kHk+1T+pk+1HkPk∣kHkT=Vk+10-1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-pk+1HkPk∣kxv+HkPk∣kxvT+Pk∣kvv-1-pk+1Rk+1.Substituting (33) and (34) into (52) yields(53)λk+11-pk+1Hk+1FkPk∣kFkTHk+1T+pk+1HkPk∣kHkT=Vk+10-1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-pk+1HkPk∣kxv+HkPk∣kxvT+Pk∣kvv-1-pk+1Rk+1+Hk+1QkHk+1T.In both sides of (53), the traces are directly calculated similarly to the idea of the literature [8] as follows:(54)trλk+11-pk+1Hk+1FkPk∣kFkTHk+1T+pk+1HkPk∣kHkT=trVk+10-1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-pk+1HkPk∣kxv+HkPk∣kxvT+Pk∣kvv-1-pk+1Rk+1+Hk+1QkHk+1T.Define(55)Mk+1≜1-pk+1Hk+1FkPk∣kFkTHk+1T+pk+1HkPk∣kHkT,(56)Nk+1≜Vk+10-1-pk+1pk+1hk+1fkx^k∣k-hkx^k∣k-v^k∣khk+1fkx^k∣k-hkx^k∣k-v^k∣kT-pk+1HkPk∣kxv+HkPk∣kxvT+Pk∣kvv-1-pk+1Rk+1-1-pk+1Hk+1QkHk+1T.Hence, (54) is equivalent to(57)trλk+1Mk+1=trNk+1.So, the fading factor λk+1 can be calculated by(58)λk+1=trNk+1trMk+1.In (56), the actual value of the covariance of residual Vk+10 is unknown, which can be calculated roughly by(59)Vk+10=y~1∣0y~1∣0T,k=0ρVk0+y~k+1∣ky~k+1∣kT1+ρ,k≥1,where 0<ρ≤1 is a forgetting factor which can be heuristically selected like that in the literature [18]; for details, see the simulation results in situation I of Section 5. Because the fading factor λk+1 takes effect only when λk+1≥1, it can be finally determined as follows:(60)λk+1=max1,trNk+1trMk+1.The formulae of the STF/RDM algorithm are similar to those of the EKF with one-step randomly delayed observations shown by (22)-(23) and (25)–(30). The differences are that (23), (28), and (30) should be rewritten as follows:(61)Pk+1∣k=λk+1FkPk∣kFkT+Qk,(62)Pk∣kzz=λk+1HkPk∣kHkT+HkPk∣kxv+HkPk∣kxvT+Pk∣kvv,(63)Pk+1,k∣kxz=λk+1FkPk∣kHkT+FkPk∣kxv.Then, the calculating process of the STF/RDM algorithm is inserting (22), (61), (25)–(27), (62), (29), and (63) into (9)–(19) and computes x^k+1∣k+1a and Pk+1∣k+1a in (7).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 λk+1, such as calculating the traces directly in both sides of (53) and roughly determining Vk+10 through (59) to ensure that the filtering algorithm can be calculated in real time.


## 5. Simulation Results and Analysis

In this section, to analyze and compare the performance of the proposed method in Section4 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 two-dimensional 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.(2)
It maneuvers and moves with constant turn rateΩ=5∘s-1 during 27–59 s.(3)
It moves with constant velocity during 60–68 s.(4)
It maneuvers and moves with constant turn rateΩ=-25∘s-1 during 69–73 s.(5)
It moves with constant velocity during 74–100 s.Figures1(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.Figure 1
(a) Position trajectory (●: initial point, ■: final point), ★: radar location. (b) Velocity trajectory. (c) Turn rate trajectory.
(a)
(b)
(c)Considering the coordinated turn model with unknown turn rateΩ in [19], there is bias between real value of turn rate 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:(64)Xk+1=1sin⁡ΩTΩ0cos⁡ΩT-1Ω00cos⁡ΩT0-sin⁡ΩT001-cos⁡ΩTΩ1sin⁡ΩTΩ00sin⁡ΩT0cos⁡ΩT000001Xk+wk,k≥0,where X=[xx˙yy˙Ω]T is state vector; x, y, x˙, and y˙ express the position and velocity in x direction and y direction, respectively; Ω denotes turn rate; T denotes sampling period; wk denotes the process noise which has zero mean and covariance(65)Q=μ·diag⁡q1Mq2Mq3T,where (66)M=T33T32T32T;the parameters q1=0.1m2s-3, q2=0.1m2s-3, and q3=1.75×10-4s-3, respectively, denote the coefficient of process noise in x direction, y 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:(67)zk=xk2+yk2tan-1⁡ykxk+vk,k≥1,where vk is radar measurement noise which has zero mean and its covariance R=diag⁡σr2σθ2, where σr=10m and σθ=10×10-3rad. Assume that the measurements applied in the estimation have one sampling time random delay and the measurements can be calculated as follows:(68)yk=1-γkzk+γkzk-1,k>1;y1=z1.The x0=[1000m300ms-11000m0ms-10∘s-1]T is initial state. In each simulation, the initial state estimation x^0 is selected randomly from N(x0,P0∣0), where the initial covariance is(69)P0∣0=diag⁡100m210m2s-2100m210m2s-20.1rad2s-2.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 timek, both RMSEs of position are defined by(70)RMSEk=1m∑n=1mxkn-x^kn2+ykn-y^kn21/2,1≤k≤100,where m denotes the total number of Monte Carlo experiment, xkn,ykn and x^kn,y^kn, respectively, denote the simulated position, which can be replaced by true position and filtering estimate position, when nth 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, assumingm=1000, p=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 λk+1 is insensitive to the value of ρ. Therefore, the forgetting factor is selected as ρ=0.95 in the following situation.Figure 2
Mean of RMSEs whenm=1000, p=0.5, μ=1, and ρ=0.1,0.2,…,1.Figure 3
Fading factors whenρ=0.1,0.5, and 0.9.In situation II, assumingm=1, p=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, and 6, respectively. The values of fading factor determined by proposed STF/RDM are shown in Figure 7. The estimated autocovariance of x andy 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:(1)
During 0–68 seconds, aircraft moves with constant velocity at first and then maneuvers with lesser turn rate. The values of fading factor are close to 1, and the proposed STF/RDM deteriorates into the existing EKF. In this case, besides the RMSEs of position, velocity, and turn rate, the estimated autocovariance ofx position and that of y 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 ofx position and that of y 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 x and y position increases. Comparing with the existing EKF, the increasing of estimated autocovariance of STF/RDM can reflect the sudden change of x and y 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.Figure 4
RMSE of position whenm=1, p=0.5, and μ=1.Figure 5
RMSE of velocity whenm=1, p=0.5, and μ=1.Figure 6
RMSE of turn rate whenm=1, p=0.5, and μ=1.Figure 7
Fading factors whenm=1, p=0.5, and μ=1.Figure 8
The estimated autocovariance ofx position when m=1, p=0.5, and μ=1.Figure 9
The estimated autocovariance ofy position when m=1, p=0.5, and μ=1.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, assumingm=1000, p=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.Table 1
Mean of RMSEs whenm=1000, p=0.5, and μ=1.
RMSE per state
EKF
STF/RDM
Position, km
1.727
0.144
Velocity, km/s
3.524
0.079
Turn rate, rad/s
0.74
0.06Figure 10
RMSE of position whenm=1000, p=0.5, and μ=1.Figure 11
RMSE of velocity whenm=1000, p=0.5, and μ=1.Figure 12
RMSE of turn rate whenm=1000, p=0.5, and μ=1.As shown in Table1 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 x^0 from N(x0,P0∣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 p is equal to any other value during 0 to 1, similar result can be obtained.In situation IV, assumingm=1000, μ=1, and p=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 p has a large range of change. In particular, as p increases, both mean of proposed STF/RDM and that of existing EKF are obviously decreased, but the STF/RDM has better filtering performance when p is equal to greater value.Figure 13
Mean of RMSE in position whenm=1000, μ=1, and p=0.1,0.2,…,0.9.In situation V, assumingm=1000, p=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.Figure 14
Mean of RMSE in position whenm=1000, p=0.5, and μ=1.1,1.2,.…,1.9.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 Figures13 and 14.


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



---

*Source: 869482-2015-10-13.xml*