# Multi-Sensor Fusion Positioning Method Based on Batch Inverse Covariance Intersection and IMM
## 4 Multi-Sensor Fusion Positioning Algorithm Based on BICI and IMM
### Bicci-Imm Algorithm  
The structure of the BICI-IMM fusion algorithm is shown in Figure 6.  
Consider a multi-sensor fusion positioning system with an unknown motion model and uncertain time-varying noise variance as  
\[x_{k+1}^{i}= F_{k}^{i}x_{k}^{i}+B_{k}^{i}w_{k}^{i},\quad i=1,2,\ldots,M \tag{35}\] \[z_{k}^{h}= C_{k}^{h}x_{k}+v_{k}^{h},\quad h=1,2,\ldots,H \tag{36}\]  
where \(M\) represents the number of models, \(H\) represents the number of sensors. During one filtering update cycle, the BICI-IMM algorithm assumes that the motion model is transferred from model \(i\) to \(j\) with an a priori transfer probability \(\pi_{ij}\) defined as  
\[\pi_{ij}=P\{m_{k}^{j}|m_{k}^{i},s_{k}\in\mathbb{M}\} \tag{37}\]  
where \(\mathbb{M}=\{m_{k}^{i},\quad i=1,2,\ldots,M\}\) is the model set, \(\sum_{j}\pi_{ij}=1\). It is assumed that the local estimation of h-th sensor of j-th model at time \(k-1\) is \(\hat{x}_{k-1|k-1}^{j,h}\), the corresponding estimation error variance matrix is \(P_{k-1|k-1}^{j,h}\). The global estimation of j-th model is \(\hat{x}_{k-1|k-1}^{j}\) and error variance matrix is \(P_{k-1|k-1}^{j}\). The model probability is \(u_{k-1}^{i}\). The process of BICI-IMM algorithm at time \(k\) is  
(1) Model reinitialization  
For \(i=1,2,\ldots,M\), the model prediction probability is defined as  
\[u_{k|k-1}^{i}=P\Big{\{}m_{k}^{i}\Big{|}x^{k-1}\Big{\}}=\sum_{j}\pi_{ji}u_{k-1} ^{j}. \tag{38}\]  
The interaction weight is defined as  
\[u_{k-1}^{|ij|}=P\Big{\{}m_{k-1}^{j}\Big{|}m_{k}^{i},z^{k-1}\Big{\}}=\pi_{ji}u_{ k-1}^{j}/u_{k|k-1}^{i}. \tag{39}\]  
The interaction state estimation and variance matrix are  
\[g_{k-1|k-1}^{i,h}=E\Big{\{}x_{k-1}\Big{|}m_{k}^{i},\mathcal{M}_{k-1},z^{k-1} \Big{\}}=\sum_{j}u_{k-1}^{|ii|}\hat{x}_{k-1|k-1}^{j},\quad h=1,2,\ldots,H \tag{40}\]  
\[\hat{P}_{k-1|k-1}^{i,h}=\sum_{j}\biggl{[}p_{k-1|k-1}^{j,h}+\Big{(}\hat{x}_{k- 1|k-1}^{j,h}-\hat{x}_{k-1|k-1}^{j}\Big{)}\Big{(}\hat{x}_{k-1|k-1}^{j,h}-\hat{x }_{k-1|k-1}^{j}\Big{)}^{T}\biggr{]}u_{k-1}^{|ij|}\,. \tag{41}\]  
(2) Model local filter  
Figure 6: BICI-IMM multi-sensor fusion algorithm structure.  
The recursive formula of updating the model probability at time \(k\) through the model likelihood function based on the sequential method is  
\[U_{k}^{i,h}=\frac{U_{k|k-1}^{i,h-1}\Lambda_{k}^{i,h}}{\sum_{j}U_{k|k-1}^{j,h-1} \Lambda_{k}^{i,jh}} \tag{54}\]  
\[U_{k|k-1}^{i,h}=\sum_{j}\pi_{ji}U_{k}^{j,h},\quad h=1,2,\ldots,H-1 \tag{55}\]  
where the initial value is  
\[U_{k|k-1}^{i,0}=u_{k}^{i,1},\quad i=1,2,\ldots,M\,. \tag{56}\]  
The probability of the updated model is  
\[u_{k}^{i}=U^{i,H-1}\,. \tag{57}\]  
(5) Estimation fusion  
The weighted sum of state estimation of model filters is taken as the global estimation, so the global estimation and noise variance matrix are  
\[\hat{x}_{k|k}=\sum_{i}\hat{x}_{k|k}^{i}u_{k}^{i} \tag{58}\]  
\[P_{k|k}=\sum_{i}u_{k}^{i}\bigg{[}P_{k|k}^{i}+\Big{(}\hat{x}_{k}-\hat{x}_{k|k}^ {i}\Big{)}\Big{(}\hat{x}_{k}-\hat{x}_{k|k}^{i}\Big{)}^{T}\bigg{]}\,. \tag{59}\]