Design of Multi-sensor Fusion Architectures Based on the Covariance Intersection Algorithm—Estimating Calculation Burdens

Abstract

This paper addresses the problem of multi-sensor fusion and estimation for a system composed of several collaborative subsystems. A multi-sensor fusion approach based on the Kalman filter and the covariance intersection algorithm is proposed. Moreover, centralized and distributed architectures are presented and discussed—the breakdown of calculation burdens on each system component is determined. The purpose is to help in the choice of the best fusion architecture for a system composed of several collaborative subsystems, especially systems with a large number of sensors. Finally, the approach is experimentally illustrated in the context of collaborative mobile robotics. A numerical study is provided to illustrate the efficiency of each proposed architecture. Compared to the centralized architecture, the partially distributed architecture showed good stability and low requirements on the communication capacity and computing speed of the system.

Appendices

Appendix A: Calculation Burden of Matrix Operations

Matrix operation	Matrix dimension	Calculation burden
A + B = C	[n × m] + [n × m]	nm
A + B = S	[n × n] + [n × n]	$\frac {n^{2}}{2}+ \frac {n}{2}$
I + A = B	[n × n] + [n × n]	n
A.B = C	[n × m].[m × l]	2nml − nl
A.B = S	[n × m].[m × n]	$n^{2} m+nm- \frac {n^{2}+n}{2}$
A− 1 = B	[n × n]	$\frac {1}{6} (16n^{3}-3n^{2}-n)$

Appendix B: Calculation Burden of Kalman Filter

Matrix operation	Matrix dimension	Calculation burden
Prediction step
${F_{k}^{i}} X_{k-1/k-1}^{i}$	[ni × ni].[ni × 1]	2(ni)2 − ni
${F_{k}^{i}} P_{k-1/k-1}^{i}$	[ni × ni].[ni × ni]	2(ni)3 − (ni)2
${F_{k}^{i}} P_{k-1/k-1}^{i}({F_{k}^{i}})^{T}$	[ni × ni].[ni × ni]	$(n^{i})^{3}+\frac {(n^{i})^{2}-n^{i}}{2}$
${F_{k}^{i}} P_{k-1/k-1}^{i}({F_{k}^{i}})^{T}$	[ni × ni] + [ni × ni]	$\frac {(n^{i})^{2}+n^{i}}{2}$
$+{Q_{k}^{i}}$
Update step
${H_{k}^{i}} P_{k/k-1}^{i}$	[mi × ni] + [ni × ni]	2(ni)2mi − nimi
${H_{k}^{i}}P_{k/k-1}^{i}({H_{k}^{i}})^{T}$	[mi × ni].[ni × mi]	ni(mi)2 + nimi
		$-\frac {(m^{i})^{2}+m^{i}}{2}$
${S_{k}^{i}}={R_{k}^{i}}+{H_{k}^{i}} P_{k/k-1}^{i}$	[mi × mi]	$\frac {(m^{i})^{2}+m^{i}}{2}$
$({H_{k}^{i}})^{T}$	+ [mi × mi]
$({S_{k}^{i}})^{-1}$	[mi × mi]	$\frac {1}{6}[16(m^{i})^{3}$
		− 3(mi)2 − mi]
${K_{k}^{i}}=P_{k/k-1}^{i}({H_{k}^{i}})^{T}$	[ni × mi].[mi × mi]	2ni(mi)2 − nimi
$({S_{k}^{i}})^{-1}$
${K_{k}^{i}}{H_{k}^{i}}$	[ni × mi].[mi × ni]	2(ni)2mi − (ni)2
$I-{K_{k}^{i}}{H_{k}^{i}}$	[ni × ni] + [ni × ni]	n i
${K_{k}^{i}}{Z_{k}^{i}}$	[ni × mi].[mi × 1]	2nimi − ni
$(I-{K_{k}^{i}}{H_{k}^{i}})X_{k/k-1}$	[ni × ni].[ni × 1]	2(ni)2 − ni
$(I-{K_{k}^{i}}{H_{k}^{i}})X_{k/k-1}^{i}$	[ni × 1] + [ni × 1]	n i
$+{K_{k}^{i}}{Z_{k}^{i}}$
$(I-{K_{k}^{i}}{H_{k}^{i}})P_{k/k-1}^{i}$	[ni × ni].[ni × ni]	$(n^{i})^{3}+\frac {(n^{i})^{2}-n^{i}}{2}$

Appendix C: Calculation Burden of Covariance Intersection Algorithm

	Calculation burden
Calculation of ${w_{p}^{i}}$
$tr({R_{p}^{i}})~~~~(p=1 \to {N_{r}^{i}})$	${N_{r}^{i}} (n^{i}-1)$
${\sum }_{q=1}^{N} \frac {1}{tr({R_{q}^{i}} )}$	$Divisions: N_{r}^{i}$
	Additions :
	$(N_r^i-1 )$
$\frac {\frac {1}{tr({R_{p}^{i}})}}{{\sum }_{q=1}^{N} \frac {1}{tr({R_{q}^{i}} )}}$	$Divisions : {N_{r}^{i}}$
$Total: {w_{p}^{i}}$	${N_{r}^{i}} (n^{i}+2)-1$
Calculation of $R_{p}^{i}$
${w_{p}^{i}} {R_{p}^{i}}$	${N_{r}^{i}} \times \frac {n^{i} (n^{i}+1)}{2}$
$(R_p^i)^{-1}$	$\frac {N_r^i}{6} \times [16(n^i)^3$
[ni × ni]− 1;	− 3(ni)2 − ni]
$(p=1 \to N_r^i)$
$w_p^i (R_p^i)^{-1};$	${N_{r}^{i}} \times \frac {n^{i} (n^{i}+1)}{2}$
$(p=1\to N_r^i)$
${\sum }_{p=1}^{{N_{r}^{i}}}{w_{p}^{i}} ({R_{p}^{i}})^{-1};$	$(N_r^i-1)\times \frac {n^i (n^i+1)}{2}$
[ni × ni] + [ni × ni]
$[{\sum }_{p=1}^{N_r^i}w_p^i (R_p^i)^{-1}]^{-1}$	$\frac {1}{6} \times [16(n^i)^3-3(n^i)^2-n^i]$
Total : Ri	$(3N_r^i-1)\times \frac {n^i(n^i+1)}{2}+\frac {N_r^i+1}{6}$
	× [16(ni)3 − 3(ni)2 − ni]
Calculation of Zi
${w_{p}^{i}}({R_{p}^{i}})^{-1} {Z_{p}^{i}}$	${N_{r}^{i}} (2(n^{i} )^{2}-n^{i})$
$(p=1\to {N_{r}^{i}} );$
[ni × ni].[ni × 1]
${\sum }_{p=1}^{{N_{r}^{i}}}{w_{p}^{i}}({R_{p}^{i}})^{-1} {Z_{p}^{i}}$	$({N_{r}^{i}}-1)n^{i}$
$R^{i} {\sum }_{p=1}^{{N_{r}^{i}}}{w_{p}^{i}}({R_{p}^{i}})^{-1} {Z_{p}^{i}};$	2(ni)2 − ni
[ni × ni].[ni × 1]
Total : Zi	$2n^i (n^i N_r^i+n^i-1)$

Appendix D: Calculation Burden of the EKF Prediction Step

Matrix operation	Matrix dimension	Calculation burden
$A_k^iU_k^i$	[ni × li].[li × 1]	2nili − ni
$X_{k-1}^i+A_k^iU_k^i$	[ni × 1]	n i
	+ [ni × 1]
$F_k^iP_{k-1/k-1}^i{(F_k^i)}^T$	[ni × ni].	${(n^i)}^3+\frac {{(n^i)}^2}{2}$
	[ni × ni]	$-\frac {n^i}{2}$
$G_k^i(Q_u^i)_k$	[ni × li].[li × li]	2ni(li)2 − nili
$G_k^i(Q_u^i)_k{(G_k^i)^T}$	[ni × li].[li × ni]	(ni)2li + nili
		$-\frac {{(n^i)}^2+n^i}{2}$
$F_k^iP_{k-1/k-1}^i{(F_k^i)}^T$	[ni × ni]	$\frac {{(n^i)}^2+n^i}{2}$
$+G_k^i(Q_u^i)_k{(G_k^i)^T}$	+ [ni × ni]
$F_k^iP_{k-1/k-1}^i{(F_k^i)}^T$	[ni × ni]	$\frac {{(n^i)}^2+n^i}{2}$
$+G_k^i(Q_u^i)_k{(G_k^i)^T}$	+ [ni × ni]
$+Q_k^i$