Stochastic Feedback Based Continuous-Discrete Cubature Kalman Filtering for Bearings-Only Tracking.

Bearings-only tracking only adopts measurements from angle sensors to realize target tracking, thus, the accuracy of the state prediction has a significant influence on the final results of filtering. There exist unpredictable approximation errors in the process of filtering due to state propagation, discretization, linearization or other adverse effects. The idea of online covariance adaption is proposed in this work, where the post covariance information is proved to be effective for the covariance adaption. With theoretical deduction, the relationship between the posterior covariance and the priori covariance is investigated; the priori covariance is modified online based on the feedback rule of covariance updating. The general framework integrates the continuous-discrete cubature Kalman filtering and the feedback rule of covariance updating. Numerical results illustrated that the proposed method has advantages over decreasing unpredictable errors and improving the computational accuracy and efficiency.

1. Introduction

Bearings-only tracking (BOT) [1] has universal applications in the field of navigation, especially in passive target tracking. In the BOT process, only angle parameters measured by the angle sensors (such as passive angle sensor, sonar, antenna array, sensor network, infrared sensor) are adopted to achieve the localization and tracking of the target object. The accuracy of state prediction and measurement information are two main factors that influence results of the target tracking. In recent years increasing accuracy of bearings with the development of the passive angle sensors has been witnessed. Generally, filtering methods describe the state process in a discretized form, such as extended Kalman filtering (EKF) [2], unscented Kalman filtering (UKF) [3], particle filter (PF) [4], cubature Kalman filtering (CKF) [5] and so on. The form is propitious for calculation and is convenient to implement mathematical deductions. However, the state process of a BOT system is continuous, yet the measurement process is discrete, and discretization of state process would inevitably lead to the increase of estimation errors. Thus, the continuous-discrete (CD) filtering methods are more suitable to solve a BOT problem than those traditional filtering methods. The CD filtering methods can also be applied for target tracking [6], finance [7], stochastic control [8], etc.

While state models for most target tracking algorithms are described as stochastic differential equations (SDEs), CD filtering methods are quite different from traditional methods. Thus, mathematical models for CD filtering methods are more complicated than traditional ones, while the CD methods are potentially more accurate [9]. The common form of the stochastic system can be expressed in the form of stochastic differential equation (SDE): where is the n-dimensional target state vector, is known as the drift function, is the diffusion matrix, and is a Brownian motion, which is known as a Wiener process. The alternative form is shown below:

This form known as Langevin form in Physics is widely used, while the derivative of the Wiener term actually does not exist formally. It should be written as Equation (1). Moreover, Equation (1) cannot be solved simply as the following integral function if the diffusion matrix is a random process related to noises. where the covariance (suppose it is 1-dimension) of the Wiener process is related to t, the value of the covariance will trend to be infinitely large; therefore, it makes no sense for the second item to be calculated based on the rules of Riemann--Stieltjes calculus. The SDEs problem mentioned above can be solved by Itô calculus and Stratonovich calculus [10], the difference between them can be referred to in Reference [11]. These characteristics distinguish the CD filtering methods from the discrete time domain filtering methods.

Many numerical methods are applied to solve approximately the SDEs problem in continuous time domain filtering methods. Taylor approximation and Runge-Kutta approximation are two main numerical methods which are widely used to design continuous time domain filtering methods. In Reference [12], the order 0.5 Euler-Maruyama method was proposed, the Euler scheme of SDEs was applied in the method to achieve the 0.5 rate of convergence. Similarly, the order 1.5 strong Taylor approximations and CKF are integrated in Reference [13] to propose continuous-discrete CKF (CD-CKF); however, the computation efficiency is compromised by too many derivative operations in the process of state propagating. The deterministic Runge-Kutta methods and a moment matching technique are used for continuous-time cubature Kalman filters in Reference [14], where the non-additive process noise is considered. The accurate and effective implementation of the filtering method is a key to most researches. In Reference [15], an efficient embedded Runge-Kutta pair and automatic global error control are proposed to improve performance of the complex computational procedure. Similarly, the global error control is proposed to enhance the accuracy and the robustness of the continuous-discrete extended Kalman filter (CD-EKF) in Reference [6]. The Explicit Singly Diagonally Implicit Runge-Kutta (ESDIRK) integrator with sensitivity analysis capabilities in Reference [16] was proved to be efficient to solve nonlinear continuous-discrete filtering problems for stochastic systems, in which the internal integration step is chosen by the step-size controller for Runge-Kutta method. The step-size control technology was also adopted in Reference [17], and the variable-step-size Gauss- and Lobatto-type nested implicit Runge-Kutta formulas of orders four and six are built within the EKF framework. Automatic local and global error regulation mechanisms were implemented in Reference [16,17], which are similar to the notion of accurate continuous-discrete extended Kalman filtering (ACD-EKF) proposed by Kulikov and Kulikova [6,15]. There exist the unpredictable errors due to discretization, linearization, several state predictions in measurement intervals or other causes, which are not considered in the above methods. In Reference [18], Kalman fitler (KF) and CKF are integrated to reduce the problem of the amount of calculations, low accuracy and poor convergence in discrete-time domain, and the adaptive fading factor to adjust the error covariance. In Reference [19], the covariance adaption scheme within the EKF framework was presented to solve the problem. However, the details of the state propagation in continuous-discrete framework are not fully considered, and undoubtedly, larger errors occur when EKF is applied to deal with nonlinear problems.

To alleviate the effects of unpredictable errors, the stochastic feedback of continuous discrete cubature Kalman filter (SFCD-CKF) is proposed in this paper. Compared with the CD-CKF and other CD filtering methods, SFCD-CKF adopts posterior information in the stochastic feedback framework to adapt the priori error covariance online, and online adaption decreases the costly computation during the approximation process and improves the accuracy of state estimation. The paper is structured as follows: Section 2 presents the BOT model, Section 3 briefly introduces general types of continuous-discrete filters, and Section 4 deduces covariance updating and presents stochastic feedback framework of CD-CKF. Simulations results are shown in Section 5, and Section 6 concludes the main work.

2. The Model of BOT in Continuous-Discrete Form

Suppose that the positions are set in a 2-dimensional Cartesian coordinate, the measurement information is provided by the observer with the passive angel sensor, the state of target is: , where the position and velocity are represented by vector: and respectively. The state of the observer is: , and the related state is: .

The state equation of the bearings-only tracking system is given by a stochastic differential equation: where is the diffusion matrix, the other parameters are the same as Equation (1). The measurements can be described in a discrete time form: where the measurement noise is assumed to follow independent Gaussian distribution with zero mean and known covariance matrix .

3. General Types of Continuous-Discrete Filters

3.1. The Taylor Approximation

The Taylor approximation and Runge-Kutta approximation are two main approaches applied to CD filters. The forms of the approximation equations used in these approaches are quite different, and details of Taylor approximation are illustrated below.

The CD-EKF [20] shows the general form of the Taylor approximation. For the time interval , the state is: where is the Gaussian random variable, which is independent of the state. The expectation can be described as:

Let and , the covariance matrix is: where is the measurement sampling interval. is extended around the known estimate , which is: where is the Jacobian of , and are the high-order terms.

The predicted state estimate is:

Then the predicted covariance is:

The 1.5 order approximation is more accurate and the equation is more complex than the method shown above, but the basic form and the perception for deduction are similar. The state is: where are the pair of correlated Gaussian random variables. The noise-free process function is defined as: where

3.2. CD-CKF

The cubature rule used in CKF has the advantage in solving nonlinear filtering problems, in which the linearization is not needed in filtering. The 1.5 order Itô-type approximation CD-CKF is easily understood based on the details shown in the last section. For the interval , the predicted state estimate is:

For interval , the error covariance matrix will also be propagated after the state propagating.

Based on the cubature rule, the predicted state estimate can be expressed as:

Then, the predicted state error covariance can be expressed as: where

The details of CD-CKF algorithm can be referred to Reference [13].

4. Stochastic Feedback Framework of CD-CKF

Although the accuracy of the CD-CKF algorithm is superior over CD-EKF, the unpredictable error is inevitable. In this work, the stochastic feedback framework of CD-CKF is proposed, and the key is that the posteriori information is utilized to adapt the priori error covariance online.

4.1. Covariance Adaption

First, the states, the gain, and the covariance at each measurement interval are considered.

Estimate the updated state is:

The continuous-discrete cubature gain is: where and are cross-covariance matrix and innovations covariance matrix respectively.

The error covariance matrix is:

The innovation is:

The predicted measurement is: where is the cubature points, and is the Jacobian matrix of partial derivatives of h. The innovations covariance matrix is:

Substituting Equations (24) and (25) into Equation (26), the innovations covariance matrix can be modified as: where is when .

The cross-covariance matrix is:

Substituting Equation (25) into Equation (29) yields:

Namely, . Then, the continuous-discrete cubature gain is:

Secondly, the covariance updated online is considered.

Here, an assumption is presented at first: The covariance is nearly a constant. Actually, the assumption is not stringently valid within the whole filtering process, but it is a perfect simplification method that ensures the explicit and real time online estimation of the covariance. The method was also applied in References [21,22].

Let’s consider a maximum likelihood estimation problem. where is the time window, can be regarded as a normally distributed random variable, so can be written as:

is a scalar and is a symmetric with size . For derivative , the s-th row and t-th column element of the derivative is: where is the s-th row and t-th column element of . Then can be yielded by setting to zero, i.e., .

Because and H are independent of , with Equations (28) and (36),

Then pre- and post-multiply the matrix inside by and its inverse (or the generalized inverse) can be expressed as: because is a constant matrix and its s-th row and t-th column element is one while other elements are zero. Based on the multiplication rule, the t-th column of the matrix cannot be zero while the other column must be zero. So, the value of is equal to the t-th diagonal element of :

Furthermore, because the s-th row and t-th column element of is one and other elements are zero to ensure elements of to be zero, we can obtain: where t, s can be any value within (0,n), so:

Let us multiply both sides of Equation (42) by , we get:

With Equations (31) and (35), we have:

Based on Equations (21) and (23), we can get:

Then:

Considering the assumption, can be approximated by: where is defined as an intermediate matrix. Hence:

4.2. Stochastic Feedback Framework

The functions, Equations (47–49), constitute a major part of the covariance adaption framework. As shown in Figure 1, the state prediction and measurement updating are completed within CD-CKF, the difference is the covariance prediction process. The post-covariance information in the memorizer is used to update the covariance online, and it is also used to update the cubature gain . Then the is calculated to generate the state estimation , and can be used for covariance generation from feedback channel.

Figure 1

Stochastic feedback framework.

Compared with CD-EKF and CD-CKF, the advantages of the stochastic feedback CD-CKF (SFCD-CKF) framework can be concluded as:

Post covariance information is used to decrease the influence of the unpredictable error within continuous-time domain state prediction process, and as a result, it enhances the accuracy of the filtering.

Online covariance updating can decrease the computational complexity in derivative or matrix operation compared with traditional methods.

The cubature rule improves the performance of the filtering when dealing with non-linear problems, and it also provides a more accurate innovation covariance matrix than CD-EKF and other methods.

5. Numerical Simulation

To illustrate the performance of the proposed method, the linear and non-linear continuous time domain models with discrete measurements are considered in this section. Specifically, “linear” and “non-linear” described here refer to the type of state models, because the measurement models in BOT are all non-linear in this study. Itô-1.5 order CD-CKF [13], CD-EKF [20] and continuous-discrete adaptive Kalman filter (CD-AKF, which is also called SFCD-CKF) [19] methods are compared in the simulations. The Monte Carlo simulation is set to be 200 times for each scenario.

5.1. Linear Model Tracking

The constant velocity (CV) model is used as the linear state model of BOT. The model can be rewritten as: where is a zero-mean Gaussian noise with covariance matrix , is the state transition matrix. The Gaussian noise distribution can approximately describe the state error in real physical space, and Gaussian noise is convenient to be expressed and deduced mathematically. The measurements function is Equation (5). is the intensity of process noise, and the measurement noise is . The time window N is 5. The observer’s initial state is . Target’s initial state is .

The root mean square error (RMSE) of position is applied to evaluate the accuracy of BOT. where is the number of Monte Carlo simulations, is the estimation at time k for Monte Carlo simulation i, is the real state of the target.

As is shown in Figure 2a, the RMSE of CD-CKF decreases faster than the other three methods in the first 10 min. Because of the superior performance of CD-CKF in the nonlinear problem (actually, it is a nonlinear measurement problem in this CV BOT model), CD-CKF is more accurate for filtering in the whole BOT process. The performance of SFCD-CKF is not as good as CD-CKF at the initial moment, while RMSE of SFCD-CKF remains in decline to the end of the process, and the accuracy is higher than the other methods. CD-AKF is a CD-EKF method modified by the idea of covariance online updating, and its accuracy is higher than CD-EKF; however, when compared with CD-CKF and SFCD-CKF, the framework of EKF used in CD filtering is suboptimal, and the accuracy of CD-AKF and CD-EKF are lower comparatively. Figure 3 shows the RMSE with different sampling intervals. In general, the accuracy can be improved with smaller sampling interval, while the computational efficiency would decrease. Figure 4 shows the state estimate with these different filtering methods, where the real state is the track (or path) of the target, and the path length is 48.53 km.

Figure 2

The performance comparison with the CV model.

Figure 3

The RMSE with different sampling intervals.

Figure 4

The state estimate with different filtering methods.

Figure 2b shows the relative ratio of the computational time, and the CD-EKF with sampling interval 0.01 is the datum point. In general, the computational time will decrease with the increase of sampling interval. Because of the complex matrix operation, CD-CKF is the most time-consuming method computationally. CD-AKF and SFCD-CKF exhibit better computational efficiency comparatively, which shows that the framework of covariance online updating has an advantage in improving the computational efficiency.

5.2. Nonlinear Model Tracking

In this section, the classical nonlinear model Van der Pol oscillator is used as the state model [15]. where in the Van der Pol oscillator model, and the initial state is . The state , where the initial position is . The parameters are λ = 0.3 and . The measurement is defined by Equation (5).

Figure 5a shows the different RMSEs of four methods when dealing with a nonlinear filtering problem. CD-CKF and SFCD-CKF show the advantage for nonlinear filtering. In the last stage of the target tracking process, the accuracy of SFCD-CKF is slightly higher than CD-CKF. As we can see from Figure 2a and Figure 5a, the performance of SFCD-CKF is superior over CD-CKF only when the error approaches zero. This indicates that the online covariance updating has an advantage in state estimation with micro variation of the covariance because the approach is sensitive to unpredictable errors in this moment than in the moments when the covariance changes rapidly. The computational efficiency of SFCD-CKF and CD-AKF are demonstrated to be better than the other two methods in Figure 5b. Figure 6 shows the RMSE with different sampling intervals. The accuracy of CD-EKF and CD-AKF are improved rapidly along with the decrease of the sampling intervals, because the error of EKF framework used by CD-EKF and CD-AKF is large when dealing with the nonlinear filtering problem, and the impact of sampling interval on estimation accuracy is significant. Figure 7 shows the state estimate with these different filtering methods, where the real state is the track (or path) of the target. The path length is 133.01 km. With superior computational efficiency, SFCD-CKF generally has an advantage in solving unpredictable error problems.

Figure 5

The performance comparison with the nonlinear model.

Figure 6

The RMSEs with different sampling intervals.

Figure 7

The state estimate with different filtering methods.

6. Conclusions

Considering the unpredictable error within the process of CD filtering, the framework of online covariance updating is proposed. The key is to use the post information to modify the state estimation. The post covariance information is proved to be valuable for covariance updating by theoretical deduction. The proposed framework integrates CD-CKF with the covariance updating approach, which enhance the performance for the method when implementing the nonlinear filtering, the efficient computation and the correction of unpredictable error. While the approach does not work well when the covariance changes significantly, the adaptive approaches which combine CD-CKF with SFCD-CKF will be the focus of our future research.