A Framework of Covariance Projection on Constraint Manifold for Data Fusion ^†

Abstract

A general framework of data fusion is presented based on projecting the probability distribution of true states and measurements around the predicted states and actual measurements onto the constraint manifold. The constraint manifold represents the constraints to be satisfied among true states and measurements, which is defined in the extended space with all the redundant sources of data such as state predictions and measurements considered as independent variables. By the general framework, we mean that it is able to fuse any correlated data sources while directly incorporating constraints and identifying inconsistent data without any prior information. The proposed method, referred to here as the Covariance Projection (CP) method, provides an unbiased and optimal solution in the sense of minimum mean square error (MMSE), if the projection is based on the minimum weighted distance on the constraint manifold. The proposed method not only offers a generalization of the conventional formula for handling constraints and data inconsistency, but also provides a new insight into data fusion in terms of a geometric-algebraic point of view. Simulation results are provided to show the effectiveness of the proposed method in handling constraints and data inconsistency.

1. Introduction

Data fusion is the process of obtaining a more meaningful and precise estimate of a state by combining data from multiple sources. The architecture of multisensor data fusion can be broadly categorized into two, depending on the way raw data are processed: (1) Centralized fusion architecture [1], where raw data from multiple sources are directly sent to and fused in the central node for state estimation and (2) Distributed fusion architecture [1,2], where data measured at multiple sources are processed independently at individual nodes to obtain local estimates before they are sent to the central node for fusion. In the centralized architecture, it is possible to apply data fusion methodology such as the Kalman filter [3] to all raw data received to yield optimal estimates in the sense of minimum variance. However, the centralized architecture can be costly especially for a large system in terms of infrastructure and communication overheads at the central node, let alone the issues of reliability and scalability. On the other hand, the distributed architecture is advantageous in reliability and scalability, with lower infrastructure and communication costs. Although advantageous, the distributed architecture needs to address statistical dependency among the local state estimates received from multiple nodes for fusion. This is due to the fact that local state estimates at individual nodes can be subject to the same process noise [4] and to double counting, i.e., sharing the same data sources among them [5]. Ignoring such statistical dependency or cross-correlation among multiple nodes leads to inconsistent results, causing divergence in data fusion [6]. In the case of known cross-correlation, the Bar-Shalom Campo (BC) formula [7] provides a consistent fusion result for a pair of redundant data sources, where the fused estimate is based on maximum likelihood [8]. A generalization to more than two data sources with known cross-correlations is given by weighted fusion algorithms of the generalized Millman’s formula (GMF) [9] and weighted Kalman filter (WKF) [10].

Sensors often provide spurious and inconsistent data due to unexpected situations such as short duration spike faults, sensor glitches, a permanent failure or slowly developing failure due to sensor elements [5,11]. Since these types of uncertainties are not attributable to the inherent noise, they are difficult to predict and model. The fusion of inconsistent sensor data with correct data can lead to severely inaccurate results [12]. For example, when exposed to abnormalities and outliers, a Kalman filter would easily diverge [13]. Hence, a data validation scheme is required to identify and eliminate the sensor faults/outliers/inconsistencies before fusion.

The detection of inconsistency needs either a priori information often in the form of specific failure model(s) or data redundancy [5]. Model-based approaches use the generated residuals between the model outputs and actual measurements to detect and remove faults. For instance, in [14], the Nadaraya–Watson estimator and a priori observations are used to validate sensor measurements. Similarly, a priori system model information as a reference is used to detect failures in filtered state estimates [15,16,17]. However, requirement of the prior information restricts the usage of these methods in the general case where prior information is not available or unmodeled failure occurs. A method to detect spurious data based on the Bayesian framework is proposed in [18,19]. The method adds a term to the Bayesian formulation which has the effect of increasing the covariance of the fused probability distribution when measurement from one of the sensor is inconsistent with respect to the other. However, the method is based on heuristics and assumes independence of sensor estimates in its analysis. In [20], the Covariance Union (CU) method is proposed where the fused covariance is enlarged to cover all local means and covariances in such a way that the fused estimate is consistent under spurious data. However, the method incurs high computational cost and results in an inappropriately large conservative fused result.

In some applications, the state variables observed in a multisensory system may be subject to additional constraints. These constraints can arise due to the basic laws of physics, kinematics or geometry consideration of a system or due to the mathematical relations to satisfy among states. For instance, the energy conservation laws in an undamped mechanical system; Kirchhoff’s laws in electric circuits; a road constraint in a vehicle-tracking scenario [21]; an orthonormal constraint in quaternion-based estimation [22] etc. These constraints if properly included can lead to improvement in state estimation and data fusion.

Various methods have been proposed to incorporate linear constraints among the state variables of dynamic systems [23,24,25,26,27,28]. For instance, the dimensionality reduction method converts a constrained estimation problem to an unconstrained one of lower dimension by eliminating some state variables using the constraints [25]. However, the state variables in a reduced dimension model may become difficult to interpret and their physical meaning may be lost [23]. The pseudo-measurement method satisfies the linear constraints among state variables by treating the state constraints as additional perfect/noise-free measurements [26,27,28]. However, this method increases the computational complexity of state estimation due to an increase in the dimension of augmented measurement. Furthermore, due to the singularity of augmented measurement covariance, the method may cause numerical problems [23,29]. A popular approach, the estimate projection method, projects the unconstrained estimate obtained from conventional Kalman filtering onto the constraint subspace using classical optimization methods [23,24]. Unfortunately, the method may not lead to the true constraint optimum since the projection method merely gives the solution as a feasible point that is closest to the unconstrained minimum.

This paper presents a unified and general data fusion framework, referred to as the Covariance Projection (CP) method to fuse multiple data sources under arbitrary correlations and linear constraints as well as data inconsistency. The method projects the probability distribution of true states and measurements around the predicted states and actual measurements onto the constraint manifold representing the constraints to be satisfied among true states and measurements. The proposed method also provides a framework for identifying and removing outliers in a fusion architecture where only sensor estimates may be available at the fusion center. This paper is an extended and improved version of the conference paper [30]. What was presented in [30] is a preliminary new framework of data fusion that we proposed. On the other hand, what is presented here represents a much more detailed implementation and refinement of the concept proposed in the conference paper. Specifically, this paper includes the following additions: (1) a detailed analysis of the equivalence of the proposed method to conventional methods for fusing redundant data sources; (2) handling linear constraints simultaneously under the proposed data fusion framework; (3) refining the mathematical formula and technical descriptions associated with them; and (4) detailed analysis of the method with additional simulations that deal with state estimation and data fusion in the presence of correlations, outliers and constraints.

2. Problem Statement

Consider a distributed sensor architecture [1], where each sensor system is equipped with a tracking system to provide local estimates of some quantity of interest in the form of mean and covariance. Assume the following linear dynamic system model for each local sensor system,

$$x_k=A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}+w_{k-1}$$

(1)

where $k$ is the discrete time, $A_k$ is the system matrix, $B_k$ is the input matrix, $u_k$ is the input vector and $x_k$ is the state vector. The system process is affected by zero mean Gaussian noise $w_k$ with covariance matrix $Q$. The sensor measurements are approximated as,

$$z_{k_i}=H_{k_i}{x}_k+v_{k_i},i=1,\dots ,n$$

(2)

where $H_k$ is the observation matrix and $n$ represents the number of sensors. $v_{k_i}$ is Gaussian noise with covariance matrices $R_i,i=1,2,\dots ,n$. Each sensor systems employs a Kalman filter to provide local state mean estimate ${\widehat{x}}_k$ and its covariance $P_k$ [31]. A prediction of the state estimate ${\widehat{x}}_k^{-}$ and its estimation error covariance $P_k^{-}$ can be computed based on process model (1),

$${\widehat{x}}_k^{-}=A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}$$

(3)

$$P_k^{-}=A_{k-1}{P}_{k-1}{A}_{k-1}^T+Q_{k-1}$$

(4)

The Kalman filter then provides the state estimate ${\widehat{x}}_k$ and its covariance $P_k$ as,

$${\widehat{x}}_k={\widehat{x}}_k^{-}+K_k\left(z_k-H_k{\widehat{x}}_k^{-}\right)$$

(5)

$$P_k=\left(I-K_k{H}_k\right)P_k^{-}{\left(I-K_k{H}_k\right)}^T+K_k{R}_k{K}_k^T$$

(6)

with the Kalman gain, $K_k=P_k^{-}{H}_k^T{\left(H_k{P}_k^{-}{H}_k^T+R_k\right)}^{-1}$. The estimates provided by sensor systems are assumed to be correlated due to the common process noise or double counting, that is, $P_{ij}=cov\left({\widehat{x}}_i,{\widehat{x}}_j\right)\ne 0$. To ensure optimality of the fused results, the cross-covariance $P_{ij}$ should be properly incorporated.

Due to the inherent nature of the sensor and environmental factors [32], the sensor measurements can also be perturbed by unmodel random faults $e_{k_i}$,

$$z_{k_i}=H_i{x}_k+v_{k_i}+e_{k_i},i=1,\dots ,n$$

(7)

Subsequently, the estimates computed by local sensor systems may be spurious and inconsistent. Therefore, validation of the sensor estimates is required to remove inconsistencies before the fusion process.

In addition, the states $x_k$ of the sensor systems may subject to linear constraints due to the geometry of the system environment or the mathematical description of the system [23], such that,

$$C{x}_k=c$$

(8)

where $C\in {ℝ}^{n\times m}$ and $c\in {ℝ}^n$ are both known. $C$ is assumed to have a full row rank. The constraints provide deterministic information about the state variables and can be used to improve the fusion accuracy.

These issues of correlations, data inconsistency and state constraints motivate the development of the Covariance Projection method, which is described next.

3. Proposed Approach

The proposed method first represents the probability of true states and measurements in the extended space around the data from state predictions and sensor measurements, where the extended space is formed by taking states and measurements as independent variables. Any constraints among true states and measurements that should be satisfied are then represented as a constraint manifold in the extended space. This is shown schematically in Figure 1a for filtering as an example (refer to Equations (1)–(6)). Data fusion is accomplished by projecting the probability distribution of true states and measurements onto the constraint manifold.

More specifically, consider two mean estimates, ${\widehat{x}}_1$ and ${\widehat{x}}_2$, of the state $x\in {ℝ}^N$, with their respective covariances as $P_1$, $P_2\in {ℝ}^{N\times N}$. Furthermore, the estimates are assumed to be correlated with cross-covariance $P_{12}$. The mean estimates and their covariances together with their cross-covariance in ${ℝ}^N$ are then transformed to an extended space of ${ℝ}^{2N}$ along with the linear constraint between the two estimates:

$$\widehat{x}=\left[\begin{array}{c}{\widehat{x}}_1\\ {\widehat{x}}_2\end{array}\right],P=\left[\begin{array}{cc}{P}_1& P_{12}\\ P_{12}^T& P_2\end{array}\right],C_1{\widehat{x}}_1=C_2{\widehat{x}}_2$$

(9)

where $C_1$ and $C_2$ are constant matrices of compatible dimensions. In the case where ${\widehat{x}}_1$ and ${\widehat{x}}_2$ estimate the same entity, $C_1$ and $C_2$ become identity matrix $I$. Figure 1b illustrates schematically the fusion of ${\widehat{x}}_1$ and ${\widehat{x}}_2$ in the extended space based on the proposed CP method. Fusion takes place by finding the point on the constraint manifold that represents the minimum weighted distance from $\widehat{x}$ in ${ℝ}^{2N}$, where the weight is given by $P$. As seen later, the proposed CP method with the minimum weighted distance is shown to be equivalent to the minimum variance estimates but advantageous for dealing with additional constraints and data inconsistency.

To find a point on the constraint manifold with minimum weighted distance, we apply the whitening transform (WT) defined as, $W=D^{-1/2}{E}^T$, where D and E are the eigenvalue and eigenvector matrices of $P$. Applying WT,

$${\widehat{x}}^W=W\widehat{x},P^W=WP{W}^T,M^W=WM$$

where the matrix $M={\left[C_1{C}_2\right]}^T$ is the subspace of the constraint manifold. Figure 2 illustrates the transformation of the probability distribution as an ellipsoid into a unit circle after WT. The probability distribution is then orthogonally projected on the transformed manifold $M^W$ to satisfy the constraints between the data sources in the transformed space as illustrated in Figure 2. Inverse WT is applied to obtain the fused mean estimate and covariance in the original space,

$$\tilde{x}=W^{-1}{P}_{r}W\widehat{x}$$

(10)

$$\tilde{P}=W^{-1}{P}_r{P_r}^T{W}^{-T}$$

(11)

where $P_r=M^W{\left(M^{W^T}{M}^W\right)}^{-1}{M}^{W^T}$ is the orthogonal projection matrix. Using the definition of various components in (10) and (11), a close form simplification can be obtained as,

$$\tilde{x}=M{\left(M^T{P}^{-1}M\right)}^{-1}{M}^T{P}^{-1}\widehat{x}$$

(12)

$$\tilde{P}=M{\left(M^T{P}^{-1}M\right)}^{-1}{M}^T$$

(13)

The details of the simplification are provided in Appendix A. Due to the projection in extended space of ${ℝ}^{2N}$, (12) and (13) provide a fused result with respect to each data source. In the case where ${\widehat{x}}_1$ and ${\widehat{x}}_2$ estimate the same entity, that is, $M={\left[I_N{I}_N\right]}^T$, the fused result will be same for the two data sources. As such, a close form equation for fusing redundant data sources in ${ℝ}^N$ can be obtained from (12) and (13) as,

$$\tilde{x}={\left(M^T{P}^{-1}M\right)}^{-1}{M}^T{P}^{-1}\widehat{x}$$

(14)

$$\tilde{P}={\left(M^T{P}^{-1}M\right)}^{-1}$$

(15)

Given $n$ mean estimates ${\widehat{x}}_1$, ${\widehat{x}}_2$, …, ${\widehat{x}}_n$ of a state $x\in {ℝ}^N$ with their respective covariances $P_1,P_2,\dots ,P_n\in {ℝ}^{N\times N}$ and known cross-covariances $P_{ij},i,j=1,\dots ,n$, (14) and (15) can be used to obtain the optimal fused mean estimate and covariance with $M={\left[I_{N1},I_{N2},\dots ,I_{Nn}\right]}^T$.

For fusing correlated estimates from $n$ redundant sources, the CP method is equivalent to the weighted fusion algorithms [9,10], which compute the fused mean estimate and covariance as a summation of weighted individual estimates as,

$$\tilde{x}={\displaystyle \sum }_{i=1}^n{c}_i{\widehat{x}}_i$$

(16)

$$\tilde{P}={\displaystyle \sum }_{i,j=1}^n{c}_i{P}_{ij}{c}_j^T$$

(17)

with ${\sum }_{i=1}^n{c}_i=I$. Where the weights $c_i$ are determined by solving some cost function of (16) and (17) such that, ${\sum }_{i=1}^n{c}_i=I$. Equivalently, the CP fused mean and covariance can be written as,

$$\tilde{x}=L\widehat{x}$$

(18)

$$\tilde{P}=LP{L}^T$$

(19)

where $L=\left[L_1,L_2,\dots ,L_n\right]={\left(M^T{P}^{-1}M\right)}^{-1}{M}^T{P}^{-1}$ and ${\sum }_{i=1}^n{L}_i=I$. In the particular case of two data sources, the CP fused solution reduces to the well-known Bar-Shalom Campo formula [7],

$$\tilde{x}=\left(P_2-P_{21}\right){\left(P_1+P_2-P_{12}-P_{21}\right)}^{-1}{\widehat{x}}_1+\left(P_1-P_{12}\right){\left(P_1+P_2-P_{12}-P_{21}\right)}^{-1}{\widehat{x}}_2$$

(20)

$$\tilde{P}=P_1-\left(P_1-P_{12}\right){\left(P_1+P_2-P_{12}-P_{21}\right)}^{-1}\left(P_1-P_{21}\right)$$

(21)

Although equivalent to the traditional approaches in fusing redundant data sources, the proposed method offers a generalized framework not only for fusing correlated data sources but also for handling linear constraints and data inconsistency simultaneously within the framework.

The proposed method provides an unbiased and optimal fused estimate in the sense of minimum mean square error (MMSE).

Theorem 1.

For$n$unbiased mean estimates${\widehat{x}}_1,{\widehat{x}}_2,\dots ,{\widehat{x}}_n$, the fused mean estimate$\tilde{x}$provided by the CP method is an unbiased estimator of$x$, that is, $E\left(\tilde{x}\right)=E\left(x\right)$.

Proof. 

From (18), we can write,

$$\tilde{x}=\left[L_1,L_2,\dots ,L_n\right]\left[\begin{array}{c}{\widehat{x}}_1\\ \begin{array}{c}{\widehat{x}}_2\\ ⋮\\ {\widehat{x}}_n\end{array}\end{array}\right]$$

$$\tilde{x}=L_1{\widehat{x}}_1+L_2{\widehat{x}}_2+\dots +L_n{\widehat{x}}_n$$

$$\tilde{x}={\displaystyle \sum }_{i=1}^n{L}_i{\widehat{x}}_i$$

Taking the expectation on both sides, we get,

$$E\left(\tilde{x}\right)=E({\displaystyle \sum }_{i=1}^n{L}_i{\widehat{x}}_i)$$

$$E\left(\tilde{x}\right)={\displaystyle \sum }_{i=1}^n{L}_{i}E({\widehat{x}}_i)$$

Since the sensor estimates ${\widehat{x}}_1,{\widehat{x}}_2,\dots ,{\widehat{x}}_n$ are unbiased, we have $E({\widehat{x}}_1)=E({\widehat{x}}_2)=\cdots E({\widehat{x}}_n)=E\left(x\right)$,

$$E\left(\tilde{x}\right)=E\left(x\right)$$

where ${\sum }_{i=1}^n{L}_i=I$. This concludes that the fused state estimate $\tilde{x}$ is an unbiased estimate of $x$. ☐

Theorem 2.

The fused covariance$\tilde{P}$of the CP method is smaller than the individual covariances, that is,$\tilde{P}\le P_i,i=1,2,\dots ,n$.

Proof. 

From equation (15), we can write,

$$\tilde{P}={\left(M^T{P}^{-1}M\right)}^{-1}$$

By Schwartz matrix inequality, we have,

$$\begin{gathered} \tilde{P}={[{(P^{-\frac{1}{2}}M)}^T(P^{\frac{1}{2}}{M}_i)]}^T\times {[{(P^{-\frac{1}{2}}M)}^T(P^{-\frac{1}{2}}M)]}^{-1}\times [{(P^{-\frac{1}{2}}M)}^T(P^{\frac{1}{2}}{M}_i)] \\ \le {\left(P^{\frac{1}{2}}{M}_i\right)}^T\left(P^{\frac{1}{2}}{M}_i\right)=P_i \end{gathered}$$

where $M$ is the constraint between data sources and $M_i={\left[I_{Ni},0,\dots ,0\right]}^T$ is the constraint matrix for $P_i$. The equality holds for $P_i=P_{ij},$ that is, $\tilde{P}=P_i,$ when $P_i=P_{ij},j=1,2,\dots ,n$.

It can be observed from (14) and (15) that computation of cross-covariance $P_{ij}$ is needed to compute the fused mean and covariance. Cross-covariance among the local estimates can be computed as [9,10,33],

$$P_{ij}=\left[I-K_i{H}_i\right]\left[A{P}_{ij}^{k-1}{A}^T+BQ{B}^T\right]{\left[I-K_j{H}_j\right]}^T$$

(22)

where $K_i$ and $K_j$ are the Kalman gain of source $i$ and $j$ respectively for $i,j=1,\dots ,n$ and $P_{ij}^{k-1}$ represent the cross covariance of the previous cycle between source $i$ and $j$. ☐

4. Fusion in the Presence of Spurious Data

Due to the inherent nature of sensor devices and the real-world environment, the sensor observations may also be affected by random faults. Subsequently, the local estimates provided by sensor systems in a distributed architecture may be spurious and inconsistent. This may cause the fusion methodologies to fail since they are based on the assumption of consistent input sensor estimates. Therefore, a validation scheme is required to detect and remove the spurious estimates from the fusion pool. The proposed approach exploits the constraint manifold among sensor estimates to identify any data inconsistency. The identification of inconsistent data is based on the distance from the constraint manifold to the mean of redundant data sources in the extended space that provides a confidence measure with the relative disparity among data sources. Assuming a joint multivariate normal distribution for the data sources, the data confidence can be measured by computing the distance from the constraint manifold as illustrated in Figure 3.

Consider the joint space representation of $n$ sensor estimates $\left({\widehat{x}}_{N1},P_1\right),\left({\widehat{x}}_{N2},P_n\right),\dots ,\left({\widehat{x}}_{Nn},P_1\right)$,

$$\widehat{x}=\left[\begin{array}{c}{\widehat{x}}_{N1}\\ \begin{array}{c}{\widehat{x}}_{N2}\\ ⋮\\ {\widehat{x}}_{Nn}\end{array}\end{array}\right],P=\left[\begin{array}{cc}{P}_1& \begin{array}{ccc}{P}_{12}& \dots & P_{1n}\end{array}\\ \begin{array}{c}{P}_{12}^T\\ ⋮\\ P_{1n}^T\end{array}& \begin{array}{ccc}\begin{array}{c}{P}_2\\ ⋮\\ \dots \end{array}& \begin{array}{c}\dots \\ \ddots \\ \dots \end{array}& \begin{array}{c}⋮\\ ⋮\\ P_{nn}\end{array}\end{array}\end{array}\right]$$

where $N$ is the dimension of the state vector. The distance $d$ from the constraint manifold can be calculated as,

$$d={\left(\widehat{x}-\tilde{x}\right)}^T{P}^{-1}\left(\widehat{x}-\tilde{x}\right)$$

(23)

where $\tilde{x}$ is the point on the manifold and can be obtain by using (12). In the case of two data sources with mean ${\widehat{x}}_1$, ${\widehat{x}}_2$ ∈ ${ℝ}^N$ and respective covariance matrices $P_1$ and $P_2$ ∈ ${ℝ}^{N\times N}$. The distance $d$ can be obtained as,

$$d=\left[\begin{array}{cc}{\left({\widehat{x}}_1-\tilde{x}\right)}^T& {\left({\widehat{x}}_2-\tilde{x}\right)}^T\end{array}\right]{\left[\begin{array}{cc}{P}_1& 0\\ 0& P_2\end{array}\right]}^{-1}\left[\begin{array}{c}{\widehat{x}}_1-\tilde{x}\\ {\widehat{x}}_2-\tilde{x}\end{array}\right]$$

The point on the manifold is given as,

$$\tilde{x}=P_2{\left(P_1+P_2\right)}^{-1}{\widehat{x}}_1+P_1{\left(P_1+P_2\right)}^{-1}{\widehat{x}}_2$$

Simplifying, we get,

$$d={\left[{\widehat{x}}_1-{\widehat{x}}_2\right]}^T{\left(P_1+P_2\right)}^{-1}\left[{\widehat{x}}_1-{\widehat{x}}_2\right]$$

(24)

The details of simplifications are provided in Appendix B. From (24), it can be observed that distance $d$ is a weighted distance between the two data sources and it can provide a measure of nearness or farness of the two data sources to each other. A large value of $d$ implies a large separation while a small $d$ signifies closeness of the data sources. In other words, the distance from the manifold provides an indication of the relative disparity among data sources.

Theorem 3.

For$n$data sources of$N$dimension, the$d$distance (23) follow a chi-squared distribution with$nN$degrees of freedom (DOF), that is,$d~{\chi }^2\left(Nn\right)$.

Proof. 

From (23) we have,

$$d={\left(\widehat{x}-\tilde{x}\right)}^T{P}^{-1}\left(\widehat{x}-\tilde{x}\right)$$

(25)

Under the whitening transformation, $W{P}^{-1}W=I$, $W\widehat{x}={\widehat{x}}^W$ and $W\tilde{x}={\tilde{x}}^W$. Thus, we can write,

$$\begin{gathered} {\left(\widehat{x}-\tilde{x}\right)}^T{P}^{-1}\left(\widehat{x}-\tilde{x}\right)={({\widehat{x}}^W-{\tilde{x}}^W)}^T({\widehat{x}}^W-{\tilde{x}}^W) \\ \Rightarrow {\left(W\left(\widehat{x}-\tilde{x}\right)\right)}^T\left(W\left(\widehat{x}-\tilde{x}\right)\right)=y^{T}y \end{gathered}$$

(26)

where for normal distribution $\left(\widehat{x}-\tilde{x}\right)$, $y=W\left(\widehat{x}-\tilde{x}\right)$ is an independent standard normal distribution $\mathcal{N}\left(0,1\right)$. For N dimensions of the state vector, the right-hand side of (26) is ${\sum }_{i=1}^N{y}_i^2$, thus distance d follows a chi-square distribution with $N$ DOF, that is, $d~{\chi }^2\left(N\right)$. For $n$ data sources with $N$ states,

$$d~{\chi }^2\left(Nn\right)$$

Since $d$ is a chi-square distribution with $Nn$ DOF, then for any significance level $\alpha \in \left(0,1\right)$, ${\chi }_{\alpha }^2\left(Nn\right)$ is defined such that the probability,

$$P\left\{d\ge {\chi }_{\alpha }^2\left(Nn\right)\right\}=\alpha$$

This is depicted in Figure 4. Hence, to have a confidence of 100 × (1 − $\alpha$) percent, $d$ should be less than respective critical value as illustrated in Figure 4. A chi-square table [34] can be used to obtain the critical value for the confidence distance with a particular significance level and DOF. A value of $\alpha$ = 0.05 is assumed in this paper unless specified. ☐

4.1. Inconsistency Detection and Exclusion

To obtain reliable and consistent fusion results, it is important that the inconsistent estimates be identified and excluded before fusion. For this reason, at each time step when the fusion center receives computed estimates from sensor nodes, distance $d$ is calculated. A chi-square table is then used to obtain the critical value for a particular significance level and DOF. A computed distance $d$ less than the critical value mean that we are confident about the closeness of sensor estimates and that they can be fused together to provide a better estimate of the underlying states. On the other hand, a distance $d$ greater than or equal to the critical value indicate spuriousness of the sensor estimates. At least one of the sensor estimate is significantly different than the other sensor estimates. To exclude the outliers, a distance from the manifold is computed for every estimate and compared with the respective critical values. For $n$ mean estimates ${\widehat{x}}_1,{\widehat{x}}_2,\dots ,{\widehat{x}}_n$ with respective covariances $P_1,P_2,\dots ,P_n$ and cross-covariances $P_{ij}\mathrm{for}i,j=1,\dots ,n$, the hypothesis and decision rule are summarized as follows,

Hypotheses: 

$$H_0:{\widehat{x}}_1={\widehat{x}}_2=\cdots ={\widehat{x}}_n$$

$$H_1:{\widehat{x}}_1\ne {\widehat{x}}_2\ne \cdots \ne {\widehat{x}}_n$$

Compute: 

$d={\left(\widehat{x}-\tilde{x}\right)}^T{P}^{-1}\left(\widehat{x}-\tilde{x}\right)$

Decision Rule: 

Accept $H_0$ if $d<{\chi }_{\alpha }^2\left(Nn\right)$

       Reject $H_0$ if $d\ge {\chi }_{\alpha }^2\left(Nn\right)$

If the hypothesis $H_0$ is accepted, then the estimates are optimally fused using (14) and (15). On the other hand, rejection of null hypothesis means that at least one of the sensor estimates is significantly different than the other estimates. Then, a distance from the manifold is computed for each of the estimates as,

$$d_i={\left({\widehat{x}}_i-\tilde{x}\right)}^T{P}_i^{-1}\left({\widehat{x}}_i-\tilde{x}\right),i=1,2,\dots ,n$$

with $\tilde{x}$ computed using (14). The outliers are identified and eliminated based on the respective critical value, that is, if $d_i\ge {\chi }_{\alpha }^2\left(N\right)$, they are rejected, where $N$ is the dimension of an individual data source.

4.2. Effect of Correlation on d Distance

Since the estimates provided by multiple data sources are correlated, it is important to consider the effect of cross-correlation in the calculation of confidence distance. Consider two sensor estimates ${\widehat{x}}_1\in {ℝ}^1$ and ${\widehat{x}}_2\in {ℝ}^1$ with respective variances ${\sigma }_1^2$ and ${\sigma }_2^2$ and cross-covariance ${\sigma }_{12}^2=\rho \sqrt{{\sigma }_1^2{\sigma }_2^2}$, where $\rho \in \left[-1,1\right]$ is the correlation coefficient. The $d$ distance for the pair of multivariate Gaussian estimates $\left({\widehat{x}}_1,{\sigma }_1^2\right)$ and $\left({\widehat{x}}_2,{\sigma }_2^2\right)$, with cross-covariance ${\sigma }_{12}^2$ can be written as,

$$d=\frac{{\left[{\widehat{x}}_1-{\widehat{x}}_2\right]}^2}{{\sigma }_1^2+{\sigma }_2^2-{\sigma }_{12}^2-{\sigma }_{21}^2}$$

(27)

It is apparent that the distance between the mean values is affected by the correlation between the data sources. Figure 5 illustrates the dependency of confidence distance $d$ on the correlation coefficient. Figure 5a shows the distance $d$ with changing correlation coefficient from −1 to 1. It can be observed that a positive correlation between the data sources results in a large $d$ distance. This means that a slight separation between the positively correlated data sources indicates spuriousness with high significance as compared to negatively correlated and uncorrelated data sources. Figure 5b shows the scenario in which a data source (with changing mean and constant variance) is moving away from another data source (with constant mean and constant variance). The distance $d$ is plotted for various values of correlation coefficients. The y-axis shows the percentage of rejection of the null hypothesis $H_0$. It can be noted that ignoring the cross-correlation in distance $d$ results in underestimated or overestimated confidence and may lead to an incorrect rejection of the true null hypothesis (Type I error) or incorrect retaining of false null hypothesis (Type II error). The proposed framework inherently takes care of any cross-correlation among multiple data sources in the computation of distance $d$.

Example: 

Consider a numerical simulation with the constant state,

$$x_k=10$$

Three sensors are used to estimate the state $x_k$, where the measurements of the sensors are affected with respective variances of $R_1,R_2\mathrm{and}{R}_3$. The values for the parameters assumed in the simulation are,

$$Q=2,R_1=0.5,R_2=1,R_3=0.9$$

The sensor measurements are assumed to be cross-correlated. It is also assumed that the sensor 1, sensor 2 and sensor 3 measurements are independently corrupted by unmodeled noise and produce inconsistent data for 33%, 33% and 34% of the time respectively. The sensors compute local estimates of the state and send it to the fusion center. Three strategies for combining the local sensor estimates are compared: (1) CP, which fuses the three sensor estimates using (14) and (15) without removing outliers; (2) CP WO-d means the outliers were identified and rejected before fusion based on (27) with ${\sigma }_{12}^2=0$, that is, correlation in computation of $d$ is ignored and; (3) CP WO-dC, reject the outliers based on (27) with taking into account the cross-correlation. Figure 6 shows the fused solution of three sensors when the estimate provided by sensor 2 is in disagreement with sensor 1 and 3. It can be observed from Figure 6 that neglecting the cross-correlation in CP WO-d results in Type II error, that is, all the three estimates are fused despite the fact that estimate 2 is inconsistent. CP WO-dC correctly identifies and eliminates the spurious estimate before the fusion process. Figure 7 shows the estimated position after the fusion of the three sensors’ estimates for 100 samples. It can be seen that the presence of outliers greatly affects the outcome of multisensor data fusion. As depicted in Figure 7, eliminating outliers before fusion improves the estimation performance. The fused samples of CP WO-d and CP WO-dC on average lies closer to the actual state. Figure 7 also shows the difference in fusion performance when outliers are identified with and without considering cross-correlation. It can be noted that neglecting the correlation affects the estimation quality because of Type I and Type II errors.

5. Fusion under Linear Constraints

The system model of a linear dynamic system takes into account the relation and dependencies among components of the state vector. In some applications, however, the state variables may be subject to additional constraints due to the basic laws of physics, geometry of the system environment or due to the mathematical description of the state vector. Imposing such certain information in an otherwise probabilistic setting should yield a more accurate estimate that is guaranteed to be feasible.

Consider a linear dynamic system model,

$$x_k=A_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}+w_{k-1}$$

(28)

$$z_{k_i}=H_{k_i}{x}_k+v_{k_i},i=1,\dots ,n$$

(29)

where $k$ represents the discrete-time index, $A_k$ is the system matrix, $B_k$ is the input matrix, $u_k$ is the input vector and $x_k$ is the state vector. The system process noise $w_k$ with covariance matrix $Q$ and measurement noise $v_k$ with covariance $R$ are assumed to be correlated, that is,

$$E\left\{\left[\begin{array}{c}{w}_k\\ v_k\end{array}\right]{\left[\begin{array}{c}{w}_k\\ v_k\end{array}\right]}^T\right\}=\left[\begin{array}{cc}Q& P_{QR}\\ P_{QR}^T& R\end{array}\right]$$

The state $x_k\in {ℝ}^N$ is known to be constrained as,

$$C{x}_k=c=0$$

(30)

For $c\ne 0$, the state space can be translated by a factor $c$ such that $C{\overline{x}}_k=0$. After constrained state estimation, the state space can be translated back by the factor c to satisfy $C{x}_k=\mathrm{c}$. Hence, without loss of generality, the $c=0$ case is considered for analysis here. The matrix $C\in {ℝ}^{n\times m}$ is assumed to have a full row rank. A row deficient matrix $C$ signifies the presence of redundant constraints. In such a case, we can simply remove the linearly dependent rows from $C$. In the following, the estimate projection (EP) method is briefly reviewed which is followed by the Covariance Projection (CP) method for linear constraints among state variables.

5.1. Estimate Projection Method

The estimate projection (EP) method [23,24] projects the unconstrained estimate obtained from Kalman filtering onto the constraint subspace to satisfy the linear constraints among state variables. Let us denote the unconstrained filtered estimate and constrained estimate as $\left({\widehat{x}}^u,P^u\right)$ and $\left({\widehat{x}}^p,P^p\right)$, respectively. Then the following optimization problem is solved to obtain the constrained estimate,

$$\underset{{\widehat{x}}^p}{\min }{\left({\widehat{x}}^p-{\widehat{x}}^u\right)}^{T}U\left({\widehat{x}}^p-{\widehat{x}}^u\right)\mathrm{such}\mathrm{that}C{\widehat{x}}^p=0$$

(31)

where $U$ is any symmetric positive definite weighting matrix. Solving (31) using Lagrange multipliers results in a constrained mean and covariance,

$${\widehat{x}}^p=J{\widehat{x}}^u$$

(32)

$$P^p=J{P}^u{J}^T$$

(33)

where $J$ is the projector on the null space of constrained matrix $C$, defined as,

$$J=I-U^{-1}{C}^T{\left(C{U}^{-1}{C}^T\right)}^{-1}C$$

Any symmetrical positive definite matrix can be used as a weighting matrix $U$ to obtain the constrained estimate but the two most common choices are identity matrix $I$ and inverse of unconstrained covariance $P^{u^{-1}}$.

5.2. Covariance Projection Method for Linear Constraints

The CP framework incorporates any linear constraints among states without any additional processing. Let us denote the constrained filtered estimate of the CP method as $\left({\widehat{x}}^c,P^c\right)$. The extended space representation of the state predictions and measurements of multiple sensors can be written as,

$$\widehat{x}=\left[\begin{array}{c}{\widehat{x}}_k^{-}\\ z_{k_1}\\ \begin{array}{c}⋮\\ z_{k_n}\end{array}\end{array}\right],P=\left[\begin{array}{cc}{P}_k^{-}& \begin{array}{ccc}{P}_{P_k^{-}{R}_1}& \dots & P_{P_k^{-}{R}_n}\end{array}\\ \begin{array}{c}{P}_{P_k^{-}{R}_1}^T\\ ⋮\\ P_{P_k^{-}{R}_n}^T\end{array}& \begin{array}{ccc}\begin{array}{c}{R}_1\\ ⋮\\ \dots \end{array}& \begin{array}{c}\dots \\ \ddots \\ \dots \end{array}& \begin{array}{c}⋮\\ ⋮\\ R_n\end{array}\end{array}\end{array}\right]$$

Then the CP estimate in the presence of linear constraints among states can be obtained using (12) and (13) as,

$${\widehat{x}}^c=M_c{\left(M_c^T{P}^{-1}{M}_c\right)}^{-1}{M}_c^T{P}^{-1}\widehat{x}$$

(34)

$$P^c=M_c{\left(M_c^T{P}^{-1}{M}_c\right)}^{-1}{M}_c^T$$

(35)

where the $M_c$ matrix is the subspace of the constraint among the state prediction $x_k^{-}$ and sensor measurements $z_{k_i}$ as well as linear constraints $C$ among state variables. The subspace of the linear constraint among state prediction and sensor measurements can be written as,

$$M={\left[I_N,H_1,H_2,\dots ,H_n\right]}^T$$

Then, $M_c$ is a combination of $M$ and $C$, that is,

$$M_c\in \left(M,C\right)$$

The projection of the probability distribution of true states and measurements around the predicted states and actual measurements onto the constraint manifold $M_c$ in the extended space provide the filtered or fused estimate of state prediction and sensor measurements as well as completely satisfying the linear constraints among the states directly in one step.

It can be observed from (31) that the EP method forces the unconstrained estimate on the linear constraint under some norm to satisfy the linear constraints among state variables. Consequently, the true optimality cannot be guaranteed due to the fact that the projected point close to the unconstrained estimate does not imply that it is close to the true constrained state [29]. On the other hand, the unified constraint matrix $M_c$ of the CP method ensures that the linear constraints among state variables are exactly satisfied. Furthermore, as compared to the EP method that needs the online projection steps (32) and (33) after filtering, the $M_c$ matrix for the CP method can be computed offline. This means that the EP method is computationally less efficient. Additionally, the proposed CP method is inherently suitable for taking care of any cross-correlation in the constrained estimation process.

6. Simulation Results

In this section, illustrative examples are provided to demonstrate the effectiveness of the theoretical results derived in the previous sections. We use the Monte Carlo technique [35], a method extensively used in a wide variety of fields such as physical science, computational biology, statistics, computational geometry, artificial intelligence, engineering, decision theory, and quantitative finance (see, e.g., the recent works [36,37,38,39,40]).

6.1. Tracking in the Presence of Correlations and Outliers

Consider a target tracking scenario characterized by the following dynamic system model,

$$x_k=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]x_{k-1}+\left[\begin{array}{c}\raisebox{1ex}{$T^2$}\!\left/ \!\raisebox{-1ex}{$2$}\right.\\ T\end{array}\right]w_{k-1}$$

(36)

with the state vector $x_k={\left[x,\dot{x}\right]}^T$. Where $x$ and $\dot{x}$ are the position and velocity of the target at time $k$, respectively. T = 0.5 s is the sampling period. The system process is affected by zero mean Gaussian noise with covariance matrix $Q$. Four sensors are employed to track the movement of the target, where the sensor measurements are approximated by the following equation,

$$z_{k_i}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]x_k+v_{k_i},i=1,2,3,4$$

(37)

The measurements of the sensors are affected by noise $v_{k_i}$ with respective covariances of $R_1,R_2,R_3\mathrm{and}{R}_4$. The covariances of the process noise and sensor measurement noises used in the simulation are,

$$Q=3.5,R_1=diag\left(5,3.5\right),R_2=diag\left(2,8\right),$$

$$R_3=diag\left(7,2.1\right),R_4=diag\left(2.5,5\right)$$

Starting from an initial value of [100, 3], in each time step the individual sensor uses (36) to predict the state of the target and then update the state prediction by its own sensor measurements. The local estimates are assumed to be correlated and (22) is used to compute the cross-correlation among local estimates. The estimated mean and covariances of the states by each sensor are sent to the fusion center, where they are fused by the CP method.

Table 1. The accuracy comparison in terms of trace of matrices.

$\mathit{t}\mathit{r}\mathit{a}\mathit{c}\mathit{e}{\mathit{P}}_{\mathbf{1}}$	$\mathit{t}\mathit{r}\mathit{a}\mathit{c}\mathit{e}{\mathit{P}}_{\mathbf{2}}$	$\mathit{t}\mathit{r}\mathit{a}\mathit{c}\mathit{e}{\mathit{P}}_{\mathbf{3}}$	$\mathit{t}\mathit{r}\mathit{a}\mathit{c}\mathit{e}{\mathit{P}}_{\mathbf{4}}$	$\mathit{t}\mathit{r}\mathit{a}\mathit{c}\mathit{e}\tilde{\mathit{P}}$
4.3271	2.9109	3.9656	3.4321	1.2674

summarizes the trace of covariance of local sensors along with the trace of fused covariance provided by the CP method. As seen from Table 1, the trace of covariance provided by the CP method is less than the individual local state estimates, that is, $trace\tilde{P}\le$ $trace{P}_i$, $i=1,\dots ,4$. This means that the fused result is better than each of the local state estimates.

In order to further verify this theoretical result, we compute the mean square error (MSE) as,

$$S_{MSE}\left(k\right)=\frac{1}{V}{\displaystyle \sum }_{i=1}^V{\left[{\widehat{x}}_i\left(k\right)-x_i\left(k\right)\right]}^T\left[{\widehat{x}}_i\left(k\right)-x_i\left(k\right)\right]$$

where V is the number of Monte Carlo trials and ${\widehat{x}}_i\left(k\right)$ and $x_i\left(k\right)$ are the estimated and actual state vector respectively. Since,

$$trace\left(P_i\right)=E[{\left({\widehat{x}}_i-x\right)}^T\left({\widehat{x}}_i-x\right)]$$

Then we have [41],

$$S_{MSE}\left(k\right)=trace\left(P_i\right),k\to \infty ,V\to \infty$$

(38)

The simulation is carried out for 1000 Monte Carlo runs and the local estimates provided by four sensor nodes along with the fused result of the CP method are shown in Figure 8. The straight lines in Figure 8 denote the trace of error covariance matrices and the solid curve represents the MSE of local and fused estimates. It can be observed from Figure 8 that the MSE of the individual sensor node fluctuates around the trace, which is consistent with (38). Furthermore, the accuracy relation of local sensor estimates and fused estimates in terms of MSE in Figure 8 is coincident with the theoretical results in Table 1.

Consider the same dynamic example of four sensors with the following system and measurement equations,

$$x_k=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]x_{k-1}+\left[\begin{array}{c}\raisebox{1ex}{$T^2$}\!\left/ \!\raisebox{-1ex}{$2$}\right.\\ T\end{array}\right]u_{k-1}+w_{k-1}$$

(39)

$$z_{k_i}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]x_k+v_{k_i}+e_{k_i},i=1,2,3,4$$

(40)

where the process and measurement noise parameters are the same. Now, it is also assumed that the sensor 1, sensor 2, sensor 3 and sensor 4 measurements are independently affected by unmodeled random noise $e_{k_i}$ for 5%, 15%, 20% and 10% of the time, respectively, and thus the estimates provided by the sensors are spurious. The control input alternate between 1 and −1 and set to a value of 1 if $\dot{x}<30$ otherwise it is changed to −1 until $\dot{x}<5$. Starting from an initial value of [100, 3], in each time step the individual sensor node compute local filtered estimates. The estimated mean and covariances by each sensor are sent to the fusion center, where they are fused. The three fusion strategies of CP (fusion without outlier removal), CP WO-d (outlier removal without considering cross-correlation) and CP WO-dC (taking care of correlation in outlier removal) are compared based on root mean squared error (RMSE) between the actual state value and fused estimate of the state. The inconsistency is detected with significance level $\alpha$ = 0.05. Figure 9a,b illustrate the RMSE of the target position and velocity respectively versus time for 1000 Monte Carlo runs.

Table 2. Average RMSE for 1000 Monte Carlo Runs.

Average RMSE	CP	CP WO-d	CP WO-dC
Position (m)	101.1605	93.0824	89.6491
Velocity (m/s)	43.0339	36.0899	31.2033

summarizes the average RMSE of position and velocity. It can be observed from Figure 9 and Table 2 that the presence of outliers deteriorates the performance of multisensor data fusion. Eliminating the outliers before fusion greatly improves the estimation quality. Figure 9 and Table 2 also shows the difference in fusion performance of CP WO-d and CP WO-dC, when outliers are identified with and without consideration of cross-correlation in distance $d$ respectively. It can be noted that consideration of correlation in distance $d$ improves the estimation quality in presence of outliers by avoiding Type I and Type II errors.

6.2. Target Tracking in the Presence of Linear Constraints

Consider a 2D target tracking problem [24] with the following system equation,

$$x_k=\left[\begin{array}{ccc}1& 0& \begin{array}{cc}T& 0\end{array}\\ 0& 1& \begin{array}{cc}0& T\end{array}\\ \begin{array}{c}0\\ 0\end{array}& \begin{array}{c}0\\ 0\end{array}& \begin{array}{cc}\begin{array}{c}1\\ 0\end{array}& \begin{array}{c}0\\ 1\end{array}\end{array}\end{array}\right]x_{k-1}+\left[\begin{array}{c}0\\ 0\\ \begin{array}{c}Tsin\theta \\ Tcos\theta \end{array}\end{array}\right]u_{k-1}+w_{k-1}$$

(41)

where $x_k={\left[x_1,x_2,{\dot{x}}_1,{\dot{x}}_2\right]}^T$ is the state vector, with the first two states as the north and east position with the last two states as the north and east velocity of the target respectively. A sensor measures the position and velocity of the system as,

$$z_k=H{x}_k+v_k$$

(42)

with $H=\left[\begin{array}{ccc}1& 3& \begin{array}{cc}2& 1\end{array}\\ 3& 1& \begin{array}{cc}0& 1\end{array}\\ \begin{array}{c}2\\ 2\end{array}& \begin{array}{c}1\\ 2\end{array}& \begin{array}{cc}\begin{array}{c}3\\ 0\end{array}& \begin{array}{c}3\\ 1\end{array}\end{array}\end{array}\right]$. $w_k$ and $v_k$ are the Gaussian process and measurement noise, respectively. Suppose that we have additional information that the vehicle is moving on the road with a heading angle of $\theta$ from the east $x_2$, then we can write,

$$\tan \theta =\frac{x_1}{x_2}\Rightarrow x_1-x_2\tan \theta =0$$

$$\tan \theta =\frac{{\dot{x}}_1}{{\dot{x}}_2}\Rightarrow {\dot{x}}_1-{\dot{x}}_2\tan \theta =0$$

Due to the heading of the vehicle, the states are dependent on each other, thus providing us additional constraints,

$$\left[\begin{array}{c}\begin{array}{ccc}1& -\tan \theta & \begin{array}{cc}0& 0\end{array}\end{array}\\ \begin{array}{ccc}0& 0& \begin{array}{cc}1& -\tan \theta \end{array}\end{array}\end{array}\right]x_k=\left[\begin{array}{c}0\\ 0\end{array}\right]$$

(43)

Based on the constraint matrix $M={\left[\begin{array}{cc}{I}_4& H\end{array}\right]}^T$ between the state prediction and sensor measurement and linear constraints (43) among the state variables, the unified constraint matrix $M_c$ can be obtained as,

$$M_c={\left[\begin{array}{cccc}\begin{array}{c}\tan \theta \\ 0\end{array}& \begin{array}{c}1\\ 0\end{array}& \begin{array}{c}0\\ \tan \theta \end{array}& \begin{array}{cccc}\begin{array}{c}0\\ 1\end{array}& \begin{array}{c}3+\tan \theta \\ 1+2\tan \theta \end{array}& \begin{array}{c}1+3\tan \theta \\ 1\end{array}& \begin{array}{cc}\begin{array}{c}1+2\tan \theta \\ 3+3\tan \theta \end{array}& \begin{array}{c}2+2\tan \theta \\ 1\end{array}\end{array}\end{array}\end{array}\right]}^T$$

(44)

The covariance of the process and measurement noise are assumed to be,

$$Q=\left[\begin{array}{ccc}100& 20& \begin{array}{cc}0& 0\end{array}\\ 20& 100& \begin{array}{cc}0& 0\end{array}\\ \begin{array}{c}0\\ 0\end{array}& \begin{array}{c}0\\ 0\end{array}& \begin{array}{cc}\begin{array}{c}30\\ 10\end{array}& \begin{array}{c}10\\ 30\end{array}\end{array}\end{array}\right],R=\left[\begin{array}{ccc}500& 10& \begin{array}{cc}20& 30\end{array}\\ 10& 500& \begin{array}{cc}15& 10\end{array}\\ \begin{array}{c}20\\ 30\end{array}& \begin{array}{c}15\\ 10\end{array}& \begin{array}{cc}\begin{array}{c}100\\ 20\end{array}& \begin{array}{c}20\\ 100\end{array}\end{array}\end{array}\right]$$

The target is set to an initial state of $\left[\begin{array}{ccc}0& 0& \begin{array}{cc}15\tan \theta & 15\end{array}\end{array}\right]$. The sampling time T is set to 1 s, heading angle $\theta =\pi /3$ and control input $u_k=1$ if ${\dot{x}}_{1}or{\dot{x}}_2<30$ otherwise it is changed to −1 until ${\dot{x}}_{1}or{\dot{x}}_2<5.$ The process and measurement noise are assumed to be correlated with covariance,

$$P_{QR}=\rho F_1{F}_2^T$$

where $F_1$ and $F_2$ are the cholesky decomposition of $Q$ and $R$ respectively. $\rho$ is the correlation coefficient and assumed as 0.4 in the simulation.

Starting from the initial value, in each time step (41) is used to predict the states of the vehicle. The states are then updated with the sensor measurements (42). Using $M_c$ from (44) in (34) and (35), the proposed method directly satisfies the constraint between the perdition and measurement as well as the linear constraints among states due to the heading of the vehicle. On the other hand, the EP method first employs a Kalman filter to obtain the unconstrained state estimates and then project the unconstrained state estimates on the linear constraints subspace to satisfy the constraints among states. The performance of the proposed method is compared with the unconstrained estimate and estimate provided by the EP method in terms of RMSE.

Table 3. Average RMSE for 1000 Monte Carlo Runs.

Methods	Average RMSE
	${\mathit{x}}_{\mathbf{1}}$ (m)	${\mathit{x}}_{\mathbf{2}}$ (m)	${\dot{\mathit{x}}}_{\mathbf{1}}$ (m/s)	${\dot{\mathit{x}}}_{\mathbf{2}}$ (m/s)
Unconstrained	95.479	87.496	52.413	49.972
CP	50.39	29.093	36.624	21.145
EP (I)	55.823	32.229	39.648	22.891
EP ($P^{u^{-1}}$)	53.023	30.613	37.186	21.469

summarizes the average RMSE of the individual states for 1000 Monte Carlo runs. It can be observed from Table 3 that the RMSE of the CP method is lower than the other estimators for all states. For the EP method, the use of $P^{u^{-1}}$ as a weighting parameter provides better results than using $I$. The comparative results of different methods can be also seen in Figure 10a,b, where Figure 10a is the RMSE of the northerly position and Figure 10b is the RMSE of northerly velocity. It can be seen that the proposed method performs better as compared to the unconstrained state estimation and EP method. Figure 11a,b show the variance of northerly position and northerly velocity of the vehicle, respectively. It is proven that the EP method with weighting parameter $P^{u^{-1}}$ has the smallest covariance [24]. This can also be observed from Figure 11a,b where the EP method with weight $P^{u^{-1}}$ provides smaller variance than the unconstrained estimate and EP method by identitying matrix $I$ as the weighting parameter. However, the CP method results in even smaller state variance than all the competing methods as seen in Figure 11a,b.

7. Conclusions

In this paper, we propose a general approach to data fusion under arbitrary correlations and linear constraints as well as data inconsistency. The proposed method provides an unbiased and optimal solution in the sense of MMSE for fusing data from multiple sources. The method improves the fusion accuracy by automatically detecting and removing inconsistent estimates from the fusion pool by a statistical confidence measure. Moreover, it is shown that by considering the cross-correlation among local estimates, the proposed method avoids the deterioration of the fusion accuracy due to Type I and Type II errors. Without any additional manipulation, the proposed method completely satisfies any linear constraints among state variables. The method improves the accuracy of constrained state estimation by satisfying the linear constraints among state variables and provides better results than the unconstrained state estimation and constrained state estimation using the estimate projection method.

Future work includes the extension of the proposed method for state estimation of non-linear dynamic systems. Another avenue is examining the proposed method for incorporating non-linear constraints among state variables.