Sensor Fusion with Square-Root Cubature Information Filtering

This paper derives a square-root information-type filtering algorithm for nonlinear multi-sensor fusion problems using the cubature Kalman filter theory. The resulting filter is called the square-root cubature Information filter (SCIF). The SCIF propagates the square-root information matrices derived from numerically stable matrix operations and is therefore numerically robust. The SCIF is applied to a highly maneuvering target tracking problem in a distributed sensor network with feedback. The SCIF’s performance is finally compared with the regular cubature information filter and the traditional extended information filter. The results, presented herein, indicate that the SCIF is the most reliable of all three filters and yields a more accurate estimate than the extended information filter.


Introduction
Sensor fusion is generally defined as the use of techniques that combine data from multiple sensors such that the resulting information is more accurate and more reliable than that from a single sensor.Sensor fusion techniques are widely used in many applications such as mobile robot navigation, surveillance, air-traffic control and intelligent vehicle operations.For multiple sensor fusion in a linear Gaussian environment, the information filter, which can be considered as the dual of the Kalman filter, has been a viable solution [1][2][3].Although the Kalman filter and the information filter are algebraically equivalent, the Kalman filter propagates a state vector and its error covariance whereas the information filter uses an information vector and an information matrix.This difference makes the information filter to be superior to the Kalman filter in fusion problems because computations are straight-forward and simple.Moreover, no prior information about the system state is required.
For nonlinear fusion problems however, it is difficult to obtain an optimal solution.In the past, researchers have closely followed the linear fusion theory to obtain a suboptimal solution for nonlinear fusion problems.Recently, Vercauteren et al. derived the sigma-point information filter using the statistical linear regression theory and the unscented transformation [4] whereas Kim et al. derived a similar set of steps using a minimum mean squared-error criterion [5].When we are confronted with an issue of striking the appropriate balance or trade-off between accuracy and computational complexity, the cubature Kalman filter (CKF) is considered to be the logical choice [6].The CKF is a more accurate and stable estimation algorithm than the unscented/sigma-point filter.However, due to the fact that the sigma-point filter and the CKF share a number common characteristics, the derivation of the Cubature Information Filter (CIF) is straightforward and hence trivial.In this paper, we focus on to derive the square-root version of the CIF for improved numerical stability.Unlike the CIF, the SCIF avoids numerically sensitive matrix operations such as matrix square-rooting and inversion.
The rest of the paper is organized as follows: Section 2 reviews information filtering in general.Section 3 derives the SCIF using the linear fusion theory and matrix algebra.In order to validate the formulation and reliability of the SCIF, it is applied to a highly maneuvering target tracking problem in a distributed sensor network with feedback in Section 4. Section 5 concludes the paper with final remarks.

Information Filtering: A Brief Review
The information filter is a modified version of the Kalman filter.The state estimates and their corresponding covariances in the Kalman filter are replaced by the information vectors and information matrices (inverse covariances), respectively, in the information filter.The updated covariance and the updated state take the information form, as shown by Similarly, the predicted covariance and the predicted state have equivalent information forms: At the heart of any one of information filters lies the information update, which now becomes a trivial sum.
Here, the information contribution matrix and the information contribution vector are defined as follows, respectively [4,5]:

Information Fusion
In the sensor fusion literature, there are a number of sensor networks with their own virtues and limitations [2].
In this paper, we specifically consider a distributed configuration with feedback [3].As shown in Figure 1, in this network, each local sensor has its own information processor.The locally processed results are then transmitted to the fusion center for computing a global estimate.The global estimate is broadcast so that all the local sensors utilize the global estimate for the purpose of processing the next measurement.The advantage of using the information filters within the local sensors is that the global estimate in the fusion center can be computed from ply summing the local information vectors and matrices: sensor measurements at each time step by sim- (10)  Note that the computations outlined in this section hold under the following assumptions: 1) The tracking problem at hand is described by a linear Gaussian system; 2) The sensor measurements are uncorrelated to each other; 3) There is no measurement origin of ambiguity; 4) The sensors are synchronized; 5) There is no receipt of out-of sequence measurements; 6) There is no communication loss among sensors.When one or more of these conditions are violated, various techniques have been proposed to get around them in the literature [2].

Square-Root Cubature Information Filtering
In each recursion cycle, it is important that we preserve the two properties of an information matrix, namely, its positive definitiveness and symmetry.Unfortunately, when the CIF is committed to an embedded system with limited word-length, numerical errors may lead to a loss of these properties.The accumulation of numerical errors may cause the information filter to diverge or otherwise crash.The CIF involves numerically sensitive operations such as matrix square-rooting and matrix inversion, which may combine to destroy the fundamental properties of an information matrix.The logical procedure to preserve both properties of the information matrix and to improve the numerical stability is to design a square-root version of the CIF.Although the square-root cubature information filter (SCIF) is reformulated to propagate the square-roots of the information matrices, both the CIF and the SCIF are algebraically equivalent to other.Copyright © 2013 SciRes.ICA Before deriving the SCIF, for convenience, we introduce the following notations:  We denote a general triangularization algorithm (e.g., , where S is a lower triangular matrix.The matrices A and S are related to each other as follows: Let R be an upper triangular matrix obtained from the QR decomposition on T A ; then Because many of the SCIF computations can be easily borrowed from the SCKF [7], we only derive the steps that require explicit treatments in the sequel.Like the SCKF, the SCIF also includes two steps, namely, the time update and the measurement update.Substituting the square-root factors on both side of (13) yields

S S S S (14)
We may therefore write the square-root of the error covariance matrix (15) We summarize the following important result from ( 13) and (15) as follows: y k k is a triangular matrix, the least-squares method can be used to avoid computing its inversion explicitly [8].From (2), we write the updated state estimate As shown in Figure 2, Path 3 is an inverse projection (from the state space to the information space) of Path 1.By closely following this idea and using (3), ( 4), the state space quantities can be projected back onto the information space to obtain Path 3.Because Path 2 is identical to the time update of the SCKF, the reader may refer to Section VII of [7] for a detailed derivation.

Measurement Update
In the measurement update step, we will see how to fuse a new measurement with the predicted information to obtain the updated information.As depicted in Figure 3, the measurement update step also includes three paths.Consider Path 1.Given

R k Rk
Substituting ( 18) into ( 7) and rearranging the right hand side, we get Therefore, from (19), we may write  in a factored form: where the square-root information matrix of dimension is defined as (see Equation (37) of [7]), we finally write (22) as To determining , we rewrite (4) as Because k k is a symmetric matrix, we may also write (24) as 1| ˆk k (25) Substituting ( 25) into (8) yields Substituting ( 21) into (26) and expanding the right hand side yields Consider Path 3 now.The updated information vector can be computed by substituting (27) into (6).To obtain the updated information matrix

Y
, we replace the right hand side of with square-roots and write

S S S S
Hence, the square-root of the updated information matrix is given by Note: When the local sensors employ the square-root information filtering algorithm, they are required to send the square-root information matrices to the fusion center (see Figure 1).For a distributed sensor network with s n sensors, we may obtain by augmenting the arguments on the right hand side of (28) with S s n squareroot information contribution matrices coming from s n 2) Compute the state estimate (31) 3) Use the time-update of the SCKF to compute from 2) Compute the square-root of the information contribution matrix , 1 , where the inverse of the measurement noise covariance matrix 3) Compute the information contribution vector .

Application to Decentralized Sensor Fusion
Context.We consider an air-traffic control scenario, where an aircraft executes maneuvering turn in a horizontal plane at a constant, but unknown turn rate .Figure 4 shows a representative trajectory of the aircraft.The kinematics of the turning motion can be modeled by the following nonlinear process equation [1]: where the state of the aircraft  te velocities x deno in the x and y directions, respectively; T is the time-interval between two consecutive measurements; the process noise , where q and 2 q are related to proca tion is given The scal ess by ar para eters noise intensities.The r dars were assumed to measure range and range-rate.For a radar located at , the measurement equa x w (39) where the measurement noise covariance R is given by To make this nonlinear tracking problem highly difficu dependent Monte Carlo runs (The reader may https://sites.google.com/site/haranarasaratnam/softwarefor a set of Matlab code used to generate the results).Th radars were randomly placed in a square-shaped surveillance region with the opposite vertices at (-4000, -4000) and (4000, 4000).In this experiment, the number of radars, s n , was varied from one to fifteen.We employed the following information filters for tracking the aircraft:  Extended Information Filter (EIF);  Cubature Information Filter (CIF);  Square-root Cubature Information Filter (SCIF).

own to rformance comparison, w
Note that the unscented information filter boils d the CIF when the free parameter  is forced to take zero.For this reason, the unscented information filter was excluded from comparison.
Performance Metrics.For pe e computed the accumulative root mean-squared error (ARMSE) in position and velocity.The ARMSE yields a combined measure of the bias and variance of a filter estimate.We define the ARMSE in position where      Figure 6 shows how many times each filter experien

Concluding Remarks
erically robust square-root ced divergence out of N = 100 independent Monte Carlo runs.As can be seen from Figure 6, the EIF diverges the most.However, its divergence rate decreases as n s increases.The CIF diverges a few times especially when n s is less.The SCIF diverges a few times only when n s = 2. Unlike the CIF, the SCIF does not diverge when n s = 3 and 4.This observation indicates that the SCIF is superior to other filters in terms of numerical robustness.None of the filters diverged when 7 s n  .
This paper has presented a num cubature information filter (SCIF) algorithm for multisensor fusion in a nonlinear Gaussian environment.Because the SCIF propagates the square-root matrices, it avoids to compute numerically sensitive matrix calculations.It was successfully demonstrated that the proposed SCIF algorithm is numerically robust using a computer experiment in which a target tracking problem in a distributed sensor network with feedback was considered.One of the interesting future research topics is to compare the computational demands of various information filters.

Figure 1 .
Figure 1.Information flow in a distributed sensor configuration with feedback.
depicted in Figure 2, the time update of the SCIF connects to  1| , 1| k k y k k   via three paths.Let us first consider how to derive Path 1, in which the information space is projected onto the state space.Taking inverse on both sides of (1), we get

Figure 2 .
Figure 2. Time update of the SCIF.

Figure 3 .
Figure 3. Measurement update of the SCIF.Let us derive Path 2 now.The end products of Path 2 are the information contribution vector  1 k  , and the square-root information contribution matrix i

5 )
Compute the square-root of the updated information matrix

4
target trajectory was made up of four segments, in each of which  was set to be Monte Carlo run.Similarly to the ARMSE in position, we may also write formula of the ARMSE in velocity.To check the numerical robustness of an information filter, the filter divergence rate was introduced.The filter was declared to diverge when   MSE pos of a specific Monte Carlo run exceeded 1 quently, those diverged runs were excluded from the final calculations of position and velocity, respectively, for the SCIF and EIF.As expected, as the number of sensors s n increases, the ARMSEs in position and velocity decrea .However, the performance gain quickly diminishes; as 5 s n  , the performance gain seems to be trivial.Mor , the SCIF consistently outperforms the EIF irrespective of se eover s n although the performance deviation between the tw reduces with o s n .The CIF is not included in Figures5(a)and (b) because its performance was almost identical to the SCIF.

Table 1
below summarizes the steps involved in the SCIF algorithm.

Table 1 . SCIF algorithm SCIF: Time Update
the square-root of the predicted information matrix