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.


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].
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.

Problem Formulation
Consider sensors are applied to track N t targets in a cluttered environment. For arbitrary target t (1 ≤ t ≤ N t ), X t (k) 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, f t (·) is a known nonlinear function, and V t (k) is independent zero-mean Gaussian process noise with covariance: where δ(k, j) is the Kronecker delta function. The measurement equation of sensor i for target t is: where i = 1, 2, · · · , N s represents the label of sensors. Z i,t (k) is the measurement vector of sensor i for target t at time instant k. h i (·) represents a known nonlinear function. W i (k) is the zero-mean, independent of process noise in Equation (3) and independent from sensor to sensor, with covariance:

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 f (·) is an arbitrary function, R n 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 x = ry, where y T y = 1, So that x T x = r 2 for r ∈ [0, ∞). Then the integral Equation (5) can be rewritten as: where U n = y ∈ R n y T y = 1 is the surface of the sphere and σ(·) is the spherical surface measure or the area element on U n . Therefore, we can split Equation (6) into two integrals: where I( f ) is the radial weighted integral that is shown in Equation (7), S(r) is the spherical integral with the unit weighting function ω(y) = 1 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 [u] i represents the i-th element of generator u, and u is a generator if u = (u 1 , u 2 , . . . , u r , 0, . . For simplicity, the (n − r) zero coordinates are ignored and notation [u 1 , u 2 , . . . , u r ] is used to represent a complete fully-symmetric set of points that can be obtained by permutating and changing the sign of generator u in all possible ways. For example, [1] ∈ R 2 denotes the following set of points: where ω(x) is a known weighting function which is non-negative on the interval [a, b].
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 ξ i is the cubature point, ω i is the corresponding weight. [1] i denotes the i-th row or column of the generator [1]. 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.

Accuracy Analysis
Assume an n-dimensional vector x ∼ N(x, P), where x is the mean of x, and P is the corresponding covariance. We extend the nonlinear function f (x) at x based on Taylor extension: (1) We use the third-degree spherical-radial rule to approximate the multi-dimensional integral I( f ), then: where x i is the sampled cubature point, ∆x = x i − x. Through proper simplification, the final result can be written as: (17) where k = 1, 2, 3, · · · , a ij = √ P ij , i, j = 1, 2, · · · , n, ∂ ∂x i f is the partial derivative of f (x) at the ith component of x. The interested reader can be referred to paper [26] for details in the extension. (2) Like the derivation process of I CKF ( f ), we use UKF to operate the approximation, and the final result is: (18) 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 κ = 3 − n x [16].
This paper is to tackle the high-dimensional estimation problem, so here we just discuss the case in which the state dimension n x > 3. 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 n x > 3, n k−1 > (n + κ) k−1 , and 2k is an even number, then I CKF ( f ) > I UKF ( f ), which indicates that CKF is more accurate than UKF in high-dimensional estimation.
We choose the stability factor I = ∑ i |ω i |/∑ i ω i defined as the measure of the numerical stability of the multi-dimensional integral, where ω i 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 n x > 3, and κ + n x = 3, so κ = 3 − n x < 0, then ω 0 = 1 − n x /3 < 0. The stability factor is I UKF = 2n x 3 − 1 > 1, and it scales linearly with state dimension n x , which indicates that UKF introduces significant perturbations in numerical estimates for the moment integral. However, in CKF, the stability factor I CKF always meets I CKF = 1, which shows that CKF is more accurate than UKF for high-dimensional state estimation.

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 m t i, k validated measurements for target t obtained by sensor i at time instant k.
i, k is the label of validated measurements from sensor i, l i = j (j = 0) represents the jth measurements in the validated region. Especially, l i = 0 indicates that there is no measurement in the validated region. Z i,t l i (k) represents the l i th validated measurement,Ẑ t i|i−1 (k|k) represents the predicted measurement, which will be discussed in detail later. β t l i ,i (k) represents the association probability that the measurement l i originated from target t, K t i (k) is the filtering gain. The state estimate and the corresponding error covariance after processing the measurements of the ith sensor are denoted bŷ X t i (k|k) and P t i (k|k), respectively.X t 0 (k|k) and P t 0 (k|k) represent the initial estimation, andX t (k|k) and P t (k|k) 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 β t i, l i (k) and nonlinear state estimationX t i (k|k). JPDA is thought to be an effective method for multiple targets tracking with single sensor, so we choose JPDA to compute The remaining problem is to obtainX t i (k|k). 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.

Computation of Association Probabilities
The probability β t i, l i (k) that measurement l i originated from target t can be calculated by summing over all feasible events for which it is true [1]: Pr θ m (k) Z t i (k) represents the posterior probability of event m, which can be computed by Equation (24): where C is a constant, φ[θ m (k)] is the total number of false measurements in event m. V is the volume of the entire surveillance region, and P t D is the detection probability of target t. The target detection 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 l i is correlated with a target in event m. N i, t is a conditional probability density function under Gaussian assumption, i.e.:

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 i > 1 are as follows: Step 1: Calculate the cubature points (j = 1, 2, . . . , 2n x ): where n x is the dimension of the state. S t i|i−1 (k|k) is the square root factor of P t i−1 (k|k), i.e.: Equations (28) and (29) indicate that the estimated stateX t i−1|i−1 (k|k) and error covariance P t i−1 (k|k) of the previous sensor are regarded as the predicted stateX t i|i−1 (k|k) and error covariance P t i|i−1 (k|k) of the next sensor, which is the greatest difference of the sensors with label i > 1 compared with the first sensor with label i = 1 in the process of state estimation. That is to say, the time update is not needed in sensors with label i > 1, 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: , and the weighed, centred matrix: Note that S = Tria(A) denotes the QR decomposition, where S is a lower triangular matrix. The relationship of matrices S and A is as follows: Let B be an upper triangular matrix obtained from the QR decomposition on A T . Then, the lower triangular matrix S is obtained as S = R T .
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 i > 1. Now the estimation problem in the first sensor with label i = 1 is considered, the process of the measurement update is the same as that in sensors with label i > 1, but the time update is quite different. In the sensor with label i = 1, 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:X and the predicted error covariance is: Equations (39)-(42) describe the time update in the first sensor with label i = 1. With the measurement update process composed of Equations (28)-(38) applied, the estimated stateX t 1 (k|k) and error covariance P t 1 (k|k) 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.

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. T = 1 s 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 m = 2. The detection probabilities of all targets are assumed to be P D = 0.9. The probability mass is assumed to be P G = 0.9997. 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 i is: where i = 1, 2, 3 is the sensor label. (x(k), y(k)) is the true state of the targets, x pi , y pi is the position of sensor i. Other parameters of the sensors are set as shown in Table 2. 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 M is the total number of Monte Carlo simulations. x t i (k) andx t i (k|k) 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.

Simulation Scenario
Consider a two-crossing target tracking case. The state model is: where the component of process noise q 1 = q 2 = 0.01. The state transition matrix F(k) is defined as: and the process noise distribution matrix is: The initial states of two targets are X 1

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.
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 Figures 2 and 3, while the root mean square velocity error in the x and y directions is shown in Figures 4 and 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.

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.
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 Figures 2 and 3, while the root mean square velocity error in the x and y directions is shown in Figures 4 and 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.

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.
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 Figures 2 and 3, while the root mean square velocity error in the x and y directions is shown in Figures 4 and 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.      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    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     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 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 (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.

Results and Analysis
The root mean square (RMS) position error of two targets is compared in Figures 7 and 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.

Results and Analysis
The root mean square (RMS) position error of two targets is compared in Figures 7 and 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.   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.

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

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

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.