Covariance Intersection Fusion Robust Steady-State Kalman Filter for Multi-Sensor Systems with Unknown Noise Variances

Abstract

For multi-sensor systems with uncertainties of noise variances, a local robust steady-state Kalman filter with conservative upper bounds of unknown noise variances is presented. Based on the Lyapunov equation, its robustness is proved. Further, the covariance intersection (CI) fusion robust steady-state Kalman filter is presented. It is proved that its robust accuracy is higher than that of each local robust Kalman filter. A Monte-Carlo simulation example shows its correctness and effectiveness.

95.1 Introduction

The multi-sensor information fusion has received great attentions and has been widely applied in many high-technology fields, such as tracking, signal proceeding, GPS position, robotics and so on. There are three optimal distributed weighted state fusers [1] which have the limitation to compute the optimal weights, the computation of the variances and cross-covariances of the local estimators are required. However, in many application problems, the systems have the uncertainty of model parameters or noise variances, so that the local filtering error variances and cross-covariances are unknown. To solve the filtering problems for uncertain systems, in recent years, several results have been derived on the design of robust Kalman filters that give an upper bound of the filtering error variances for any admissible uncertainty of model parameters [2–4], based on the Riccati equations. Recently, the covariance intersection (CI) fusion method has been presented by Julier and Uhlman [5, 6], which can avoid the computation of cross-covariances, but which requires the consistency of the local Kalman filters.

In this paper, the multi-sensor uncertain systems with uncertainties of noise variances are considered. First, we construct the local robust Kalman filters which give the upper-bounds of actual filtering error variances, their robustness is proved based on the Lyapunov equation. Secondly, the covariance intersection fusion robust Kalman filter is proposed by the convex combination of the local robust Kalman filters, whose robust accuracy is higher than that of each local robust Kalman filter. The geometric interpretation of these accuracy relations is given based on the variance ellipses.

95.2 Local Robust Steady-State Kalman Filter

Consider the multi-sensor uncertain system with unknown noise variances

$$ x\left( {t + 1} \right) = \Phi x\left( t \right) + \Gamma w\left( t \right) $$

(95.1)

$$ y_{i} \left( t \right) = H_{i} x\left( t \right) + v_{i} \left( t \right),\quad i = 1, \cdots ,L $$

(95.2)

where $t$ is the discrete time, $x\left( t \right) \in R^{n}$ is the state, $y_{i} \left( t \right) \in R^{{m_{i} }}$ is the measurement of the $i\text{th}$ subsystem, $w\left( t \right) \in R^{r}$, $v_{i} \left( t \right) \in R^{{m_{i} }}$ are uncorrelated white noises with zeros mean and unknown actual variances $\bar{Q}$ and $\bar{R}_{i}$, respectively. $\Phi$, $\Gamma$ and $H_{i}$ are known constant matrices. Assume that $Q$ and $R_{i}$ have conservative upper bounds $\bar{Q}$ and $\bar{R}_{i}$, respectively, i.e.

$$ \bar{Q} \le Q,\,\bar{R}_{i} \le R_{i} ,\,i = 1, \cdots ,L $$

(95.3)

in the sense that $A \le B$ means that $B - A \ge 0$ is a semi-positive definite matrix. And assume that each subsystem is completely observable and completely controllable.

Based on the $i\text{th}$ sensor, the local steady-state suboptimal Kalman filters with upper bound variances $Q$ and $R_{i}$ are given by Kailath et al. [7] and Jazwinski [8]

$$ \hat{x}_{i} \left( {t|t} \right) = \Psi_{i} \hat{x}_{i} \left( {t - 1|t - 1} \right) + K_{i} y_{i} \left( t \right) $$

(95.4)

$$ \Psi_{i} = \left[ {I_{n} - K_{i} H_{i} } \right]\Phi ,\,K_{i} = \Upsigma_{i} H_{i}^{\text{T} } \left( {H_{i} \Upsigma_{i} H_{i}^{\text{T} } + R_{i} } \right)^{ - 1} $$

(95.5)

where $\Psi_{i}$ is a stable matrix and $\Upsigma_{i}$ satisfies the steady-state Riccati equation

$$ \Upsigma_{i} = \Phi \left[ {\Upsigma_{i} - \Upsigma_{i} H_{i}^{\text{T} } \left( {H_{i} \Upsigma_{i} H_{i}^{\text{T} } + R_{i} } \right)^{ - 1} H_{i} \Upsigma_{i} } \right]\Phi^{\text{T} } + \Gamma Q\Gamma^{\text{T} } $$

(95.6)

where the symbol ${\text{T}}$ denotes the transpose. The local steady-state conservative filtering error variances satisfy the Lyapunov equation

$$ P_{i} = \Psi_{i} P_{i} \Psi_{i}^{\text{T}} + \left[ {{\rm I}_{n} - K_{i} H_{i} } \right]\Gamma Q\Gamma^{\text{T}} \left[ {I_{n} - K_{i} H_{i} } \right]^{\text{T}}\,+\,K_{i} R_{i} K_{i}^{\text{T}} $$

(95.7)

Defining the actual steady-state filtering error variance as

$$ \bar{P}_{i} = {\text{\rm E}}\left[ {\tilde{x}_{i} \left( {t|t} \right)\tilde{x}_{i}^{{\text{\rm T}}} \left( {t|t} \right)} \right],\,\tilde{x}_{i} \left( {t|t} \right) = x\left( t \right) - \hat{x}_{i} \left( {t|t} \right) $$

(95.8)

Theorem 95.1

The suboptimal conservative Kalman filters (95.4–95.7) is robust for all admissible actual variances$\bar{Q}$and$\bar{R}_{i}$, such that$\bar{Q} \le Q,\,\bar{R}_{i} \le R_{i}$in the sense that$\bar{P}_{i} \le P_{i}$. i.e.$P_{i}$is the upper bound variance.

Proof

Substituting (95.1) and (95.4) into $\tilde{x}_{i} \left( {t|t} \right) = x\left( t \right) - \hat{x}_{i} \left( {t|t} \right),$ we obtain that

$$ \tilde{x}_{i} \left( {t|t} \right) = \Phi x\left( {t - 1} \right) + \Gamma w\left( {t - 1} \right) - \Psi_{i} \hat{x}\left( {t - 1|t - 1} \right) - K_{i} y_{i} \left( t \right) $$

(95.9)

Substituting (95.2) into the above equation yields

$$ \tilde{x}_{i} \left( {t|t} \right) = \Psi_{i} \tilde{x}\left( {t - 1|t - 1} \right) + \left( {I_{n} - K_{i} H_{i} } \right)\Gamma w\left( {t - 1} \right) - K_{i} v_{i} \left( t \right) $$

(95.10)

Substituting (95.10) into (95.8) yields the actual steady-state filtering error variances as

$$ \bar{P}_{i} = \Psi_{i} \bar{P}_{i} \Psi_{i}^{\text{T}} + \left[ {I_{n} - K_{i} H_{i} } \right]\Gamma \bar{Q}\Gamma^{\text{T}} \left[ {I_{n} - K_{i} H_{i} } \right]^{\text{T}}\,+\,K_{i} \bar{R}_{i} K_{i}^{\text{T}} $$

(95.11)

Defining $\Updelta P_{i} = P_{i} - \bar{P}_{i}$, subtracting (95.11) from (95.7) yields the Lyapunov equation

$$ \Updelta P_{i} = \Psi_{i} \Updelta P_{i} \Psi_{i}^{\text{T}} + U_{i} $$

(95.12)

$$ U_{i} = \left[ {I_{n} - K_{i} H_{i} } \right]\Gamma \left( {Q - \bar{Q}} \right)\Gamma^{\text{T}} \left[ {I_{n} - K_{i} H_{i} } \right]^{\text{T}}\,+\,K_{i} \left( {R_{i} - \bar{R}_{i} } \right)K_{i}^{\text{T}} $$

(95.13)

Applying (95.3) and (95.13) yields that $U_{i} \ge 0$, noting that $\Psi_{i}$ is a stable matrix, applying the property of the Lyapunov equation [7], we have $\Updelta P_{i} \ge 0$, i.e.

$$ \bar{P}_{i} \le P_{i} $$

(95.14)

The proof is completed.

Remark 95.1

The robustness (95.14) is also called the consistency or non-divergent estimation [5, 6]. If $P_{i}^{ * }$ is another upper bound variance for all admissible $\bar{Q} \le Q$ and $\bar{R}_{i} \le R_{i}$. Taking $\bar{Q} = Q,\,\bar{R}_{i} = R_{i}$ yields $P_{i} \le P_{i}^{ * }$. This shows that $P_{i}$ is also the minimum upper bound variance.

95.3 CI Fusion Robust Steady-State Kalman Filter

Applying the CI fused algorithm [5, 6], the CI fusion robust steady-state filter is presented as following

$$ \hat{x}_{CI} \left( {t|t} \right) = P_{CI} \sum\limits_{i = 1}^{L} {\omega_{i} } P_{i}^{ - 1} \hat{x}_{i} \left( {t|t} \right) $$

(95.15)

$$ P_{CI} = \left[ {\sum\limits_{i = 1}^{L} {\omega_{i} } P_{i}^{ - 1} } \right]^{ - 1} ,\,\sum\limits_{i = 1}^{L} {\omega_{i} } = 1,\,\omega_{i} \ge 0 $$

(95.16)

The weighting coefficients $\omega_{i}$ is obtained by minimizing the performance index

$$ \mathop {\hbox{min} }\limits_{{\omega_{i} }} \text{tr}\,P_{CI} = \mathop {\hbox{min} }\limits_{\begin{subarray}{l} \omega_{i} \in \left[ {0,1} \right] \\ \omega_{1} + \cdots + \omega_{L} = 1 \end{subarray} } \text{tr} \left\{ {\left[ {\sum\limits_{i = 1}^{L} {\omega_{i} P_{i}^{ - 1} } } \right]^{ - 1} } \right\} $$

(95.17)

where the symbol ${\text{tr}}$ denotes the trace of matrix. For Eq. (95.17), the optimal weights $\omega_{i}$ can be obtained by “fimincon” function in Matlab.

Theorem 95.2

The covariance intersection fused filter (95.15) and (95.16) has the actual error variance$\bar{P}_{CI}$as

$$ \bar{P}_{CI} = {\text{E}}\left[ {\tilde{x}_{CI} \left( {t|t} \right)\tilde{x}_{CI}^{\text{T}} \left( {t|t} \right)} \right] = P_{CI} \left[ {\sum\limits_{i = 1}^{L} {\sum\limits_{j = 1}^{L} {\omega_{i} P_{i}^{ - 1} \bar{P}_{ij} P_{j}^{ - 1} \omega_{j} } } } \right]P_{CI} $$

(95.18)

where$\tilde{x}_{CI} \left( {t|t} \right) = x\left( t \right) - \hat{x}_{CI} \left( {t|t} \right),\,\bar{P}_{ij} = {\text{\rm E}}\left[ {\tilde{x}_{i} \left( {t|t} \right)\tilde{x}_{j}^{{\text{\rm T}}} \left( {t|t} \right)} \right]$are unknown actual cross-covariances among the local filtering errors, and it can be computed by the following Lyapunov equation

$$ \bar{P}_{ij} = \Psi_{i} \bar{P}_{ij} \Psi_{j}^{\text{T}} + \left[ {I_{n} - K_{i} H_{i} } \right]\Gamma \bar{Q}\Gamma^{\text{T}} \left[ {I_{n} - K_{j} H_{j} } \right]^{\text{T}} ,\,i,\,j = 1, \cdots ,L,\,i \ne j $$

(95.19)

$$ \bar{P}_{ii} = \bar{P}_{i} $$

(95.20)

Proof

From Eq. (95.16), we have

$$ x\left( t \right) = P_{CI} \left[ {\sum\limits_{i = 1}^{L} {\omega_{i} P_{i}^{ - 1} } } \right]x\left( t \right) $$

(95.21)

Subtracting (95.15) from (95.21), we easily obtain the CI fused actual filtering error

$$ \tilde{x}_{CI} \left( {t|t} \right) = P_{CI} \sum\limits_{i = 1}^{L} {\omega_{i} } P_{i}^{ - 1} \tilde{x}_{i} \left( {t|t} \right) $$

(95.22)

Applying (95.10) and (95.11) yields (95.19) and substituting (95.22) into (95.18) yields the actual fused error variance (95.18). The proof is completed.

Remark 95.2

References [5, 6, 9] proved that when the local filter is robust or consistent, then the CI fusion filter is also robust or consistent, i.e.

$$ \bar{P}_{CI} \le P_{CI} $$

(95.23)

Remark 95.3

From (95.23), we can see that $P_{CI}$ is a common upper bound of the unknown actual fused variances $\bar{P}_{CI}$ for all possible $\bar{P}_{i}$ satisfying the relation $\bar{P}_{i} \le P_{i}$, $\left( {i = 1, \cdots ,L} \right)$ and all possible unknown $\bar{P}_{ij}$. From 95.16, we see that $P_{CI}$ is independent of actual variances $\bar{P}_{i}$ and cross-covariances $\bar{P}_{ij}$. So that the accuracy of the CI fuser has the robustness with respect to unknown $\bar{P}_{i}$ and $\bar{P}_{ij}$, or equivalently, the CI fuser is robust with respect to uncertainty of $\bar{Q}$ and $\bar{R}_{i}$ satisfying (95.3).

95.4 Accuracy Analysis

Theorem 95.3

The accuracy comparison of the local and the CI fusion robust filter is given by

$$ \text{tr} \bar{P}_{i} \le \text{tr} P_{i} ,\quad i = 1, \cdots ,L $$

(95.24)

$$ \text{tr} \bar{P}_{CI} \le \text{tr} P_{CI} \le \text{tr} P_{i} ,\,i = 1, \cdots ,L $$

(95.25)

Proof

From the robustness (95.14), (95.24) holds. From (95.23), the first inequality of (95.25) holds. From (95.17), taking $\omega_{i} = 1$ and $\omega_{j} = 0\,\left( {j \ne i} \right)$ yield $\text{tr} P_{CI} = \text{tr} P_{i} ,$ hence we have the accuracy relations $\text{tr} P_{CI} \le \text{tr} P_{i} ,\,i = 1, \cdots ,L.$ The proof is completed.

Remark 95.4

Equation (95.24) means that the actual accuracy of the local filter for all admissible $\bar{Q}$ and $\bar{R}_{i}$ satisfying (95.3) is globally controlled by $\text{tr} P_{i}$, therefore $\text{tr} P_{i}$ is called the robust accuracy of the local filter. From (95.25) we see that the actual accuracy of CI fuser is globally controlled by $\text{tr} P_{CI}$, hence $\text{tr} P_{CI}$ is also called the robust accuracy of the CI fuser. The second inequality of (95.25) means the robust accuracy of the CI fuser is higher than that of each local filter. The robustness of the local and CI fused filters means that the robust accuracies $\text{tr} P_{i}$ and $\text{tr} P_{CI}$ are independent of arbitrarily variances satisfying $\bar{Q} \le Q$ and $\bar{R}_{i} \le R_{i}$, i.e., $\text{tr} P_{i}$ and $\text{tr} P_{CI}$ are insensitive to uncertain $\bar{Q}$ and $\bar{R}_{i}$.

95.5 Simulation Example

Consider the two-sensor tracking system with uncertain variances

$$ x\left( {t + 1} \right) = \Phi x\left( t \right) + \Gamma w\left( t \right) $$

(95.26)

$$ y_{i} \left( t \right) = H_{i} x\left( t \right) + v_{i} \left( t \right),i = 1,2 $$

(95.27)

$$ \Phi = \left[ {\begin{array}{*{20}c} 1 & {T_{0} } \\ 0 & 1 \\ \end{array} } \right],\Gamma = \left[ {\begin{array}{*{20}c} {0.5{T_0}^{2} } \\ {T_{0} } \\ \end{array} } \right],\,H_{1} = \left[ {\begin{array}{*{20}c} 1 & 0 \\ \end{array} } \right],\,H_{2} = I_{2} $$

(95.28)

where $T_{0} = 0.25$ is the sampled period, $x\left( t \right) = \left[ {x_{1} \left( t \right),x_{2} \left( t \right)} \right]^{\text{T}}$ is the state, $x_{1} \left( t \right)$ and $x_{2} \left( t \right)$ are the position and velocity of target at time $tT_{0}$. $w\left( t \right)$ and $v_{i} \left( t \right)$ are independent Gaussion white noises with zero mean and unknown variances $Q$ and $R_{i}$ respectively. In the simulation, we take $Q = 1,\,R_{1} = 0.8,\,R_{2} = diag(8,0.36)$, $\bar{Q} = 0.8$, $\bar{R}_{1} = 0.65$, $\bar{R}_{2} = diag(6,0.25)$.

According to the Kalman filtering, the variances of the local and CI fused filter are obtained as

$$ \begin{aligned} P_{1} = \left[ {\begin{array}{*{20}c} {0.2492} & {0.1855} \\ {0.1855} & {0.3046} \\ \end{array} } \right],\,P_{2} = \left[ {\begin{array}{*{20}c} {0.4035} & {0.0645} \\ {0.0645} & {0.121} \\ \end{array} } \right],\,\bar{P}_{1} = \left[ {\begin{array}{*{20}c} {0.2019} & {0.1497} \\ {0.1497} & {0.2447} \\ \end{array} } \right], \hfill \\ \bar{P}_{2} = \left[ {\begin{array}{*{20}c} {0.2922} & {0.0448} \\ {0.0448} & {0.0892} \\ \end{array} } \right],\,P_{CI} = \left[ {\begin{array}{*{20}c} {0.2645} & {0.0977} \\ {0.0977} & {0.1668} \\ \end{array} } \right],\,\bar{P}_{CI} = \left[ {\begin{array}{*{20}c} {0.0986} & {0.0386} \\ {0.0386} & {0.0944} \\ \end{array} } \right] \hfill \\ \end{aligned}. $$

The accuracy of the local and CI fuser is defined as the trace of their error variance matrix, the smaller trace means the higher accuracy and the larger trace means the lower accuracy. The traces of the error variance of the local and CI fused Kalman filters are compared in Table 95.1. From Table 95.1, we see that the accuracy relations (95.24) and (95.25) hold.

Table 95.1 The accuracy comparison of local and fused filters

In order to give a geometric interpretation of the accuracy relations, the covariance ellipse is defined as the locus of points $\left\{ {x:x^{\text{T} } P^{ - 1} x = c} \right\}$, where $P$ is the variance matrix and $c$ is a constant. Generally, we select $c = 1$. It has been proved in [9] that $P_{1} \le P_{2}$ is equivalent to that the covariance ellipse of $P_{1}$ is enclosed in that of $P_{2}$.

The accuracy comparison of the covariance ellipses is shown in Fig 95.1. From Fig 95.1, we see that the ellipse of the actual variance $\bar{P}_{1}$ or $\bar{P}_{2}$ is enclosed in that of the upper bound variance $P_{1}$ or $P_{2}$, respectively, which verify the consistent Eq. (95.14). The ellipse of actual CI fused variance $\bar{P}_{CI}$ is enclosed in that of $P_{CI}$, which verifies the robustness of the Eq. (95.23), and the ellipse of $P_{CI}$ encloses the intersection of the variance ellipses formed by $P_{1}$ and $P_{2}$, and passes through the four points of intersection of the local ellipses for $P_{1}$ and $P_{2}$ [9].

Fig. 95.1

The covariance ellipses of Kalman filters

In order to verify the above theoretical accuracy relations, taking $N = 200$ runs, the curves of the mean square error (MSE) of local and fused Kalman filters are shown in Fig. 95.2.

Fig. 95.2

The MSE curves of local and fused filters

From Fig. 95.2, we see that the $\text{MSE}_{i} \left( t \right)$ values of the local and CI fused filters are close to the corresponding theoretical trace values, which also verifies the accuracy relations (95.24), (95.25) and the accuracy relations in Table 95.1.

95.6 Conclusion

For the multi-sensor systems with uncertainties of noise variances, using the Kalman filtering the local steady-state robust Kalman filter and the CI robust fuser have been presented, and the robustness of the local filtering estimates is proved based on the Lyapunov equation. The corresponding CI fuser is also robust, and its robust accuracy is higher than that of each local robust filter.