Centralized Multi-Sensor Square Root Cubature Joint Probabilistic Data Association.

This paper focuses on the tracking problem of multiple targets with multiple sensors in a nonlinear cluttered environment. To avoid Jacobian matrix computation and scaling parameter adjustment, improve numerical stability, and acquire more accurate estimated results for centralized nonlinear tracking, a novel centralized multi-sensor square root cubature joint probabilistic data association algorithm (CMSCJPDA) is proposed. Firstly, the multi-sensor tracking problem is decomposed into several single-sensor multi-target tracking problems, which are sequentially processed during the estimation. Then, in each sensor, the assignment of its measurements to target tracks is accomplished on the basis of joint probabilistic data association (JPDA), and a weighted probability fusion method with square root version of a cubature Kalman filter (SRCKF) is utilized to estimate the targets' state. With the measurements in all sensors processed CMSCJPDA is derived and the global estimated state is achieved. Experimental results show that CMSCJPDA is superior to the state-of-the-art algorithms in the aspects of tracking accuracy, numerical stability, and computational cost, which provides a new idea to solve multi-sensor tracking problems.

1. Introduction

The centralized state estimation method for multiple-target tracking with multiple sensors can integrate detecting information from different sensors according to certain rules, which will make the tracking results more accurate than that of a single sensor [1,2,3]. Multi-sensor fusion is an important data processing method in the field of target tracking, especially under nonlinear conditions, and the fused result with multiple sensors is often better for multiple-target tracking, which has been widely concerned by scholars both at home and abroad [3,4,5,6,7,8,9,10,11,12].

The state-of-the-art techniques for multiple-target tracking mainly contains two categories. The first category is the tracking method based on a random finite set; the other is the method based on measurement to track association. The former method can directly track multiple targets without associating the detected measurements with the tracks in the surveillance area. However, this kind of method faces complex integral operations, which are often difficult to find an exact solution. Although approximation methods have been a good choice for this problem, the computational cost is too great, and improper approximation will lead to degradation of the tracking accuracy, thus making it a long way from being extensively utilized in practical applications [4]. The latter method has been widely applied in various tracking systems, and is still the most common method for centralized tracking [5,6,7,8,9,10,11,12,13,14].

In the second category, the sequential multi-sensor joint probabilistic data association algorithm (MSJPDA) is a classical and effective method for multi-sensor tracking, which achieves better tracking performance compared with its parallel counterparts [10,11]. Unfortunately, current MSJPDA algorithms are often proposed to solve association problems under linear circumstances, and the nonlinear environment is seldom involved in the literature. As is known, the nonlinear equation can be linearized according to Taylor expansion, then a MSJPDA algorithm based on the extended Kalman filter (EKF), the so called MSJPDA-EKF is derived [12,13,14]. However, it is essential for MSJPDA-EKF to calculate the Jacobian matrix in the linearization step in each iteration, and the ignorance of higher orders may introduce large linearization errors, which may result in degraded performance, and even divergence [1,2,14]. In addition, the estimated state will affect the calculation of association probability in the next iteration, thus causing changes in the computation of weights of joint events, which may eventually makes false correlations, and even lead to divergence [15]. To conquer the shortcomings of MSJPDA-EKF, the MSJPDA algorithm based on the unscented Kalman filter (MSJPDA-UKF) is proposed. MSJPDA-UKF can acquire higher estimation accuracy than MSJPDA-EKF, and is relatively more stable [16]. However, reasonable adjustments are needed in MSJPDA-UKF to achieve the ideal performance, and a numerical instability phenomenon is easy to occur in the situation of high dimensional estimation [16,17,18].

To address the high dimensional nonlinear estimation problem, the cubature Kalman filter (CKF) based on the third-degree spherical-radial cubature rule has been recently proposed [19,20,21,22,23,24,25]. In the procedure of CKF, a certain number of typical cubature points are selected to approximate the posterior probability, then the mean and covariance are captured through a nonlinear transfer function to achieve accurate estimation of the target state [19]. CKF and UKF are both moment-matching filters which deterministically select a set of weighted sample points to approximate the posterior probability density. Compared with EKF, CKF and UKF do not have to calculate the Jacobian matrix in each iteration, thereby reducing the computational cost. At the same time, they both avoid the negative influence of the truncation error in the process of nonlinear state estimation, which is particularly effective in the case of state estimation with strong nonlinearity [19,21,22,23]. In UKF, to achieve a better estimation performance, the adjustment of the scaling parameter is an essential step, as different scaling parameters may yield completely distinct estimation results. The selection of the scaling parameter is of great significance to the performance of UKF in some sense [16,19,21,25]. However, there is no parameter to be adjusted for CKF, and the selected sampling points and weights are only related to the dimension of the target state, which can be calculated in advance to reduce the computational complexity. Furthermore, CKF is more accurate with respect to high dimensional estimation, and easy for design and implementation [19,21,22,23,24]. Especially in the case of high dimension, EKF and UKF suffer from the curse of dimensionality or divergence, or both, but CKF still performs well [19,21,25]. Unfortunately, in practice, arithmetic operations performed on finite word-length digital computers may introduce large errors; then the symmetry and positive definiteness of the covariance are often not satisfied. Moreover, perhaps the loss of positive definiteness is more perilous as it terminates the running of the CKF. Then, the square root version of CKF, the so called SRCKF is proposed to address this problem [19,21,23,24]. In SRCKF, only the square root factors are propagated during the iterations. Therefore, the square rooting operations of the matrix are avoided, thus reducing the computation cost. The symmetry and positive definiteness of the covariance are preserved, and the numerical accuracy is also improved [19,21,25].

In this paper, to deal with the target tracking problem in a nonlinear system under a cluttered environment, a novel centralized multi-sensor square root cubature joint probabilistic data association algorithm (CMSCJPDA) is proposed. In CMSCJPDA, measurements of each single sensor are sequentially processed to compute the association probabilities based on the similar rules in JPDA, then SRCKF and the validated measurements are chosen to estimate the state of targets according to weighed state fusion. Eventually, the accurate estimation of multiple targets’ states with multiple sensors is accomplished.

The rest of this paper is organized as follows: Section 2 describes the problem of tracking multiple targets with multiple sensors. Then, in Section 3, a simple overview of the numerical integral approximation method based on spherical-radial principle is introduced. The main idea of CMSCJPDA is detailed in Section 4, and an algorithm flow is given. The numerical experiments are designed in Section 5, and the comparison and analysis of CMSCJPDA against several existing methods is also presented in this section. Concluding remarks of this paper and future work are given in Section 6.

2. Problem Formulation

Consider sensors are applied to track targets in a cluttered environment. For arbitrary target t , denotes the state of target t at discrete time instant k. For brevity, without the control input term, the discrete-time state equation of a nonlinear system is: where k is the discrete time instant, is a known nonlinear function, and is independent zero-mean Gaussian process noise with covariance: where is the Kronecker delta function.

The measurement equation of sensor i for target t is: where represents the label of sensors. is the measurement vector of sensor i for target t at time instant k. represents a known nonlinear function. is the zero-mean, independent of process noise in Equation (3) and independent from sensor to sensor, with covariance:

3. Numerical Approximation of the Multi-Dimensional Weighted Integral

3.1. Third-Degree Spherical-Radial Rule

Before introducing the CMSCJPDA method, a brief description of the numerical integral approximation based on spherical-radial principle is given. Consider the following multi-dimensional Gaussian weighted integral: where is an arbitrary function, denotes the integral region. As is known, the key to addressing the problem of nonlinear filtering based on Bayesian theory is to compute the first-order and second-order moments. In other words, the core of the Bayesian filter is how to compute Gaussian weighted integral which is of the form nonlinear function × Gaussian density that is illustrated in Equation (5).

Let , where , So that for . Then the integral Equation (5) can be rewritten as: where is the surface of the sphere and is the spherical surface measure or the area element on . Therefore, we can split Equation (6) into two integrals: where is the radial weighted integral that is shown in Equation (7), is the spherical integral with the unit weighting function that is shown in Equation (8).

Then the above integrals described in Equations (7) and (8) can be solved according to the third-degree spherical cubature rule and radical rule, respectively: where represents the i-th element of generator , and is a generator if , where . For simplicity, the zero coordinates are ignored and notation is used to represent a complete fully-symmetric set of points that can be obtained by permutating and changing the sign of generator in all possible ways. For example, denotes the following set of points: where is a known weighting function which is non-negative on the interval .

As is shown in Equation (9), the spherical cubature rule indicates that the spherical integral can be approximated by a weighted sum of function values at the sample points, which are located at the intersection of the unit sphere and its axes. The radical rule described in Equation (10) indicates that an m-point Gaussian quadrature is equal to the sum of (2m − 1) polynomials [21].

Especially, a standard Gaussian weighted integral can be computed based on third-degree spherical-radial rule as follows [25]: where: where is the cubature point, is the corresponding weight. denotes the i-th row or column of the generator .

Actually, the weighting function term of the integrand is often not subject to the standard Gaussian distribution. In other words, the Gaussian weighted integral in nonlinear filtering cannot be solved directly by Equation (12). Fortunately, the following equation makes it possible to work out the nonstandard Gaussian weighted integrals which occur in nonlinear condition:

Through Equation (14), which is proved in Appendix A, the nonstandard Gaussian weighted integrals can be transformed into the standard ones. Then, the approximation of posterior mean and error covariance can be addressed by Equation (12). Thus, an iteration of the time and the measurement updates in the Bayesian filter is completed.

3.2. Accuracy Analysis

Assume an n-dimensional vector , where is the mean of , and is the corresponding covariance. We extend the nonlinear function at based on Taylor extension: where , , is the partial derivative of .

We use the third-degree spherical-radial rule to approximate the multi-dimensional integral , then: where is the sampled cubature point, . Through proper simplification, the final result can be written as: where , , is the partial derivative of at the ith component of . The interested reader can be referred to paper [26] for details in the extension.

Like the derivation process of , we use UKF to operate the approximation, and the final result is: where is the scaling parameter, and the remaining notations are the same as Equation (17). To capture the kurtosis of the prior density as correctly as possible, is suggested to be [16].

This paper is to tackle the high-dimensional estimation problem, so here we just discuss the case in which the state dimension . As is shown in Equations (17) and (18), the first two terms in both equations are the same, the only difference is in the last term. If , , and 2k is an even number, then , which indicates that CKF is more accurate than UKF in high-dimensional estimation.

We choose the stability factor defined as the measure of the numerical stability of the multi-dimensional integral, where is the sampling weight. It is proven that the sampling rule implemented in a finite-precision arithmetic machine introduces a large amount of round-off errors when the stability factor I is larger than unity [19].

In UKF, if , and , so , then . The stability factor is , and it scales linearly with state dimension , which indicates that UKF introduces significant perturbations in numerical estimates for the moment integral. However, in CKF, the stability factor always meets , which shows that CKF is more accurate than UKF for high-dimensional state estimation.

4. Centralized Multi-Sensor Square Root Cubature Joint Probabilistic Data Association

In CMSCJPDA, measurements of each sensor are processed in sequence, then the multi-sensor multi-target tracking problem can be reduced to several single-sensor multi-target tracking problems, which are easier to solve.

Assume there are validated measurements for target t obtained by sensor i at time instant k. is the label of validated measurements from sensor , represents the jth measurements in the validated region. Especially, indicates that there is no measurement in the validated region. represents the th validated measurement, represents the predicted measurement, which will be discussed in detail later. represents the association probability that the measurement originated from target t, is the filtering gain. The state estimate and the corresponding error covariance after processing the measurements of the ith sensor are denoted by and , respectively. and represent the initial estimation, and and represent the final estimation at time instant k, respectively. Then, the state update is as follows: where:

Then, the update of the error covariance is:

Note that the key point for nonlinear single sensor multi-target tracking problem is to compute the association probability and nonlinear state estimation . JPDA is thought to be an effective method for multiple targets tracking with single sensor, so we choose JPDA to compute here. The remaining problem is to obtain . Although CKF is a good method to deal with the nonlinear state estimation problem, it is easily influenced by limited computer word-length and numerical errors, which may lead to loss of positive definiteness and symmetry of the error covariance matrix [19,21,22,23,24]. The effective way to preserve both properties and improve the numerical stability is to design a square root version of the CKF. Despite the fact that the square root cubature Kalman filter (SRCKF) is reformulated to propagate the square roots of the covariance matrices, both CKF and SRCKF are algebraically equivalent to each other, the interested reader is referred to [19]. In the proposed method, we choose SRCKF to update the state of the targets after the correlation step.

4.1. Computation of Association Probabilities

The probability that measurement l originated from target t can be calculated by summing over all feasible events for which it is true [1]: where is the number of joint events, denotes the event m, and is the following binary element.

represents the posterior probability of event m, which can be computed by Equation (24): where C is a constant, is the total number of false measurements in event m. V is the volume of the entire surveillance region, and is the detection probability of target t. The target detection indicator is defined as: which indicates whether any measurement is correlated with target t in event m. In the same way, it is also easy to define measurement association indicator: which indicates whether measurement is correlated with a target in event m. is a conditional probability density function under Gaussian assumption, i.e.:

4.2. State Update

During the update of state, the data processing in first sensor is a little different from that in other sensors. Firstly, the specific procedures for the state update in sensors with label are as follows:

Step 1: Calculate the cubature points (j = 1, 2, …, 2n): where n is the dimension of the state. is the square root factor of , i.e.:

Equations (28) and (29) indicate that the estimated state and error covariance of the previous sensor are regarded as the predicted state and error covariance of the next sensor, which is the greatest difference of the sensors with label compared with the first sensor with label in the process of state estimation. That is to say, the time update is not needed in sensors with label , and the estimation of the previous sensor is used as the prediction of the next sensor.

Step 2: Evaluate the prediction of cubature points:

Step 3: Estimate the prediction of the corresponding measurement:

Step 4: Estimate the square-root of the innovation covariance: where is a lower triangular matrix. denotes a square root factor of that , and the weighed, centred matrix:

Note that denotes the QR decomposition, where is a lower triangular matrix. The relationship of matrices and is as follows: Let be an upper triangular matrix obtained from the QR decomposition on . Then, the lower triangular matrix is obtained as .

Step 5: Estimate the cross-covariance matrix: where:

Step 6: Estimate the filter gain:

Step 7: Update the nonlinear state:

Equations (28)–(38) give a solution to the estimation of the target state and error covariance at time instant k with sensor label .

Now the estimation problem in the first sensor with label is considered, the process of the measurement update is the same as that in sensors with label , but the time update is quite different. In the sensor with label , the estimation of prediction of the state and error covariance is essential. The current cubature points are:

The propagated cubature points are:

Then the predicted state is: and the predicted error covariance is:

Equations (39)–(42) describe the time update in the first sensor with label . With the measurement update process composed of Equations (28)–(38) applied, the estimated state and error covariance can be achieved respectively after the data of the first sensor is processed.

To state the proposed method more clearly, the algorithm flow of CMSCJPDA is illustrated in Table 1.

Table 1

Algorithm flow of CMSCJPDA.

Input: The estimated state X^^t(k−1|k−1) and error covariance P^t(k−1|k−1) at time instant k−1; Output: The estimated state X^^t(k|k) and error covariance P^t(k|k) at time instant k;

Step 1. Time update (i=1)  Compute the predicted state X^^t(k|k−1) and error covariance P^t(k|k−1) according to Equations (39)–(42); Step 2. Initialization  X^0t(k|k)=X^^t(k|k−1), P0t(k|k)=P^t(k|k−1); Step 3. Measurement update  For t=1:N_t   For i=1:N_s    Compute the estimation of state X^it(k|k) and error covariance Pit(k|k) with the validated measurements of sensor i according to Equations (19)–(38);   End  End Step 4. Establish the final estimation  X^^t(k|k)=X^N_st(k|k), P^t(k|k)=PN_st(k|k−1)

5. Numerical Experiments

In this section, the numerical results and the effectiveness of the proposed CMSCJPDA algorithm are discussed in two simulation scenarios: crossing target tracking and maneuvering target tracking. The performance of CMSCJPDA is compared with MSJPDA-EKF and MSJPDA-UKF in two scenarios.

For a fair comparison, we make 50 independent Monte Carlo runs. In each run, the measurement noises are generated independently, and the corresponding noisy position measurements are used to initialize the estimate, which guarantees consistency of the initialization of the filter and makes the final estimate unbiased. Except for the initialization, the process of each Monte Carlo run is the same, and the mean value of 50 runs is considered as the final result. The total number of steps per run is 100. is the sampling interval. The nonparametric model is used for the probability mass function (PMF) of the number of false measurements. The expected number of false measurements in the validation gate is . The detection probabilities of all targets are assumed to be . The probability mass is assumed to be . Due to the large initial error, we display the numerical results after the tenth step for clarity.

Targets in the surveillance area are observed by three two-dimensional sensors. The measurement equation of sensor is: where is the sensor label. is the true state of the targets, is the position of sensor i. Other parameters of the sensors are set as shown in Table 2.

Table 2

Sensor position and parameter setting.

Sensor Labe	Sensor Position (m)	Ranging Error (m)	Angle Error (rad)
1	(0, 0)	100	0.01
2	(−500, −500)	200	0.02
3	(−500, 500)	300	0.03

Performance metrics: To compare the tracking performance of different algorithms, the root mean square error (RMSE) is chosen as the metric. The root mean square position error of target t at time instant k is defined as: where is the total number of Monte Carlo simulations. and are true and estimated positions of target t in the x direction in the n-th Monte Carlo run. Similarly, we may also define formulas of the root mean square velocity error.

5.1.Crossing Targets Tracking

5.1.1. Simulation Scenario

Consider a two-crossing target tracking case. The state model is: where the component of process noise . The state transition matrix is defined as: and the process noise distribution matrix is:

The initial states of two targets are 1(0) = [−29,500 m, 400 m/s, 34,500 m, 2,212,400 m/s]T, 2(0) = [−26,500 m, 296 m/s, 34,500 m, −400 m/s]T. After about 31 s, the two targets cross at the point (−17,000 m, 22,000 m).

5.1.2. Results and Analysis

The true and filtered tracks of two crossing targets are illustrated in Figure 1, which shows the tracking performance of three algorithms in a single run.

Figure 1

True tracks and filtered tracks of two crossing targets.

To give a more explicit picture of the estimated results, the root mean square position error of three data association algorithms in the x and y directions is shown in Figure 2 and Figure 3, while the root mean square velocity error in the x and y directions is shown in Figure 4 and Figure 5. As is shown, the three algorithms can effectively estimate the state of both targets. However, CMSCJPDA and MSJPDA-UKF, which are based on deterministic sampling, are of higher tracking accuracy than MSJPDA-EKF, and CMSCJPDA is slightly more accurate than MSJPDA-UKF, which just validates the argument in [19]. With the square root version of CKF applied in the proposed method, the square rooting operations of covariance matrix is avoided and the tracking performance is effectively enhanced.

Figure 2

Root mean square position error of target 1.

Figure 3

Root mean square position error of target 2.

Figure 4

Root mean square velocity error of target 1.

Figure 5

Root mean square velocity error of target 2.

To further validate the effectiveness of the proposed algorithm, numerical stability and computational complexity of the three algorithms are compared in Table 3, which gives the average divergence times (ADT) and average time consumption (ATC) in 50 Monte Carlo runs. As is shown, the ADT of MSJPDA-EKF is twice more than that of MSJPDA-UKF, and nearly four times more than that of CMSCJPDA, to some extent CMSCJPDA is relatively more stable in terms of convergence. As for the computational cost, the ATC of CMSCJPDA is almost half that of MSJPDA-EKF, and less than that of MSJPDA-UKF. In a word, compared with MSJPDA-EKF and MSJPDA-UKF, CMSCJPDA can achieve relatively higher tracking accuracy, and is more stable and less computationally complex under a nonlinear cluttered environment.

Table 3

Performance comparison of the three algorithms.

Algorithms	Average Divergence Times	Average Time Consumption (s)
MSJPDA-EKF	0.87	0.423
MSJPDA-UKF	0.42	0.346
CMSCJPDA	0.27	0.257

5.2. Maneuvering Targets Tracking

5.2.1. Simulation Scenario

Assume there are two maneuvering targets in the surveillance area. The initial states of two targets are X1(0) = [20,000 m, −600 m/s, 1800 m, 500 m/s]T, X2(0) = [4000 m, 600 m/s,1800 m, 200 m/s]T. The two targets move at a constant speed in a straight line for 30 s. Then, from 30 s to 50 s, they both move with constant accelerations of a1 = −30 m/s2, a1 = 20 m/s2, a2 = 30 m/s2, a2 = 20 m/s2 in the x and y directions, respectively. From 50 s to 70 s, they still move with constant accelerations, and the accelerations for both targets in the x and y directions are a1 = −30 m/s2, a1 = −20 m/s2, a2 = 30 m/s2, a2 = −20 m/s2, respectively. After 70 s, they both move at a constant speed again until 100 s. The true trajectories of both targets are illustrated in Figure 6.

Figure 6

The trajectories of two cross-maneuvering targets.

5.2.2. Results and Analysis

The root mean square (RMS) position error of two targets is compared in Figure 7 and Figure 8. The results show that the estimation error of CMSCJPDA and MSJPDA-UKF is much smaller than that of MSJPDA-EKF in the whole simulation. When t is about 40 s, the estimation error of MSJPDA-EKF is about 200 m larger compared with CMSCJPDA and MSJPDA-UKF. Moreover, with the new association strategy for maneuvering target tracking, CMSCJPDA has relatively higher accuracy than MSJPDA-UKF. At some time instants, the RMS position error of CMSCJPDA is about 50 m smaller than that of MSJPDA-UKF, and it converges relatively faster.

Figure 7

Root mean square position error of target 1.

Figure 8

Root mean square position error of target 2.

Table 4 evaluates the tracking performance of the three algorithms in the aspects of mean RMS position error (MRMSE) and correct association rate (CAR). The results indicate that both CMSCJPDA and MSJPDA-UKF are more accurate than MSJPDA-UKF, and CMSCJPDA is slightly more accurate than MSJPDA-UKF. Meanwhile, the CAR of CMSCJPDA is the highest among the three algorithms, which is 11.1% higher than MSJPDA-UKF and 28.7% higher than MSJPDA-EKF, and CMSCJPDA is more time-saving.

Table 4

Performance comparison of three algorithms.

Algorithms	MRMSE (m)	CAR (%)	TC (s)
MSJPDA-EKF	206.2	47.6	0.735
MSJPDA-UKF	132.6	65.2	0.563
CMSCJPDA	104.3	76.3	0.432

6. Conclusions

In this paper, we propose a centralized multi-sensor square root cubature joint probabilistic data association algorithm, which is based on the spherical-radial principle and the sequential update scheme. Compared with the state-of-the-art method in two tracking scenarios, the proposed method is much more accurate than MSJPDA-EKF, and a slightly better than MSJPDA-UKF by using the square root version of cubature Kalman filter to estimate the target state. Furthermore, the experimental results also show that the proposed method is less prone to divergence than MSJPDA-EKF and MSJPDA-UKF, and is more stable with respect to its numerical characteristics. Although the proposed method is less time consuming among the compared methods, due to complicated calculation of the association probability, the joint probabilistic data association-based methods are of significant computational cost, which is still less practical in real applications. Currently, the sensors in our experiment are assumed to be synchronized sampling, and there is no time difference in measurements from different sensors. However, the actual applications can rarely meet this condition, and in the future we will pay more attention to reducing the computational cost and improving the real-time performance. At the same time, the tracking problem with asynchronous sampling and changing the number of targets also deserves consideration.