Multi-Sensor Fusion Positioning Method Based on Batch Inverse Covariance Intersection and IMM

: For mass application positioning demands, the current single positioning sensor cannot provide reliable and accurate positioning. Herein, we present batch inverse covariance intersection (BICI) and BICI with interacting multiple model (BICI-IMM) multi-sensor fusion positioning methods, which are based on the batch form of the sequential inverse covariance intersection (SICI) fusion method. Meanwhile, it is proved that the BICI is robust. Compared with SICI, BICI-IMM reduces estimation error variance of the motion model and has less conservativeness. The BICI-IMM algorithm improves the accuracy of local ﬁltering by interacting with multiple models and realizes global fusion estimation based on BICI. The validity of the BICI and BICI-IMM algorithm are demonstrated by two simulations and experiments in the open and semi-open scenes, and its positioning accuracy relations are shown. In addition, it is demonstrated that the BICI-IMM algorithm can improve the positioning accuracy in the actual scenes.


Introduction
With the advent of the era of Internet of everything, indoor and outdoor positioning information with high accuracy, high reliability, large capacity and low delay has became indispensable, which is vital for intelligent robots, unmanned driving and other unmanned platforms. The sensor positioning algorithm usually adopts time of arrival (ToA), difference of arrival (TDoA), angle of arrival (AoA), direction of arrival (DoA), frequency difference of arrival (FDoA), time of flight (ToF) and received signal strength indication (RSSI) [1][2][3][4]. The sensor measurement for positioning comes from global navigation satellite systems (GNSS), Wi-Fi [5], RFID [6], Bluetooth [7], vision [8], UWB [9], etc. GNSS has been widely applied in various scenes; unfortunately, it is difficult to position in indoor and canyon environments due to its inherent fragility [10]. Although many other sensors are available, they have are defects such as a small coverage and high cost. Therefore, a single sensor is hard to position with high precision, high reliability and availability in challenging scenarios.
Compared with a single sensor, multiple-sensor fusion can improve coverage, estimation accuracy and reliability because it combines the advantages of each information source. Multi-sensor fusion algorithms usually adopt Kalman filtering, support vector machine (SVM), Bayesian inference, Dempster-Shafer theory of evidence and artificial neural networks (ANN) [11]. The Kalman filter is frequently chosen for real-time fusion positioning in robotic applications because of its high computational efficiency. Multi-sensor fusion methods are mainly divided into centralized and distributed methods, where the difference is whether the original measurements are capable of direct fusion. The former can obtain a global optimal state estimation by expanded measurement equations and covariance matrix, but there are some drawbacks such as its computational complexity, fault tolerance and flexibility. The latter accesses a global optimal estimation from local filters. Distributed fusion methods include diagonal matrix, scalar weighted fusion and minimum covariance determinant (MCD) [12,13], etc., which need covariance among local filters. However, it is difficult to obtain covariance in the actual scene. For the multi-sensor fusion estimation problem with an unknown covariance, Julier and Uhlmann proposed a covariance intersection (CI) method, which uses the conservative error variance upper bound to avoid covariance calculation [14]. The accuracy of fusion positioning algorithms of batch covariance intersection (BCI) [15], sequential covariance intersection (SCI) [16] and parallel covariance intersection (PCI) [17] is limited because of the more conservative CI method. Sijs et al. showed an ellipsoid intersection (EI) algorithm, which used a common error to model the unknown correlation. The estimation accuracy of EI is better than CI [18]. However, EI has a worse robustness of fusion estimation because of its inconsistent estimation [19,20]. Noack et al. changed the parameters of EI to ensure consistency, and proposed an inverse covariance intersection (ICI) method [21]. Based on the ICI method, Chen et al. designed a sequential inverse covariance intersection (SICI) fusion method for packet dropouts, which solved the fusion problem with unknown covariances and had good estimation performance [22]. Tang et al. showed an information geometric fusion method [23], which adopted the information center of local posterior densities and had a high computational burden.
The current research on fusion positioning with an unknown motion model mainly focuses on model uncertainty and has precise noise variance. However, sensor noise variance is time-varying and unknown due to the signal blocking, electromagnetic interference, working temperature, etc., in actual scenes. Moreover, the limited number of models and uncertain noise variance reduce positioning reliability and accuracy in actual scenes. The multiple models' estimation (MME) algorithm is a research hotspot at present. However, it is hard to realize real-time positioning, because the rapid growth in computational complexity over time. The interacting multiple model (IMM) [24] has the same positioning accuracy and computational complexity as second-order and first-order pseudo Bayesian, respectively. It makes real-time high-precision reliable positioning feasible. Nevertheless, existing IMM filters [25,26] and variants [27,28] require process and measurement variance which cannot be accurately obtained in the actual scene [29].
The BICI-IMM multi-sensor positioning algorithm is presented for unknown covariance among local filters, uncertain time-varying noise variance and unknown motion models in challenging environments. The estimation accuracy is increased by the IMM algorithm. Meanwhile, we deduced the batch form of the ICI method which had less conservativeness. Then, the accuracy of global estimation is improved by BICI. The positioning accuracy of the BICI and BICI-IMM multi-sensor fusion positioning algorithms is demonstrated by simulations and experiments in open and semi-open areas.

Local Filter
Considering the multi-sensor fusion positioning discrete system with uncertain time-varying noise variance, this is defined as where k ∈ R represents discrete time, x k ∈ R n represents the state of fusion positioning system. F k , B k and C h k represent the time-varying state transition matrix, input noise matrix and observation matrix with known appropriate dimensions, respectively. H is the number of sensors. z h k represents the measurement of h-th sensor at time k . w k and v h k are model and observation noise of the h-th sensor, respectively. w k and v h k have unknown noise , respectively, and the conservative upper bound ofQ k andR h k are Q k and R h k , respectively.
Based on the minimax estimation [30,31] and linear unbiased minimum variance rule, the local filters under time-varying noise variance one-step predictors are given by [31] x where the filtering gains K h k are the estimation error covariance matrices are where ψ h k = [I − K h k C h k ]F k , I represents the identity matrix. The local estimation variance matrix satisfies the Lyapunov equation to ensure the robustness [32] The key issue of multi-sensor fusion positioning algorithm with an unknown motion model, unknown covariance and uncertain time-varying noise variance is how to realize the track fusion efficiency of local filters and design an accurate smoother.

Fusion Algorithms Considering Accuracy and Robustness
Different from [33], the multi-sensor fusion positioning method with unknown covariance is more practical, because covariance is inessential. The fusion method with unknown covariance is mainly based on the CI method, which is a conservative global estimation method for local filters. Although it can guarantee a robust estimation, the accuracy of positioning is poor.
Inspired by SCI and BCI, we deduced the batch form of the ICI fusion positioning algorithm. Then the BICI fusion positioning algorithm is proposed and the robustness is certified. Meanwhile, the robustness of SICI is also verified. The BICI and SICI algorithms are divided into two phases: local filtering and global fusion. In the local filtering phase, the local optimal resultx (h) k and variance P h k of each sensor are obtained by a local filter in Section 2. Then, the global fusion is employed for the local resultx (h) k and P h k , which is described in detail as follows.

SICI Fusion Algorithm
The SICI multi-sensor fusion algorithm is realized by the (H−1) two-sensor ICI method. The structure of the SICI fusion algorithm is displayed in Figure 1 as follows. The SICI multi-sensor fusion estimator is given by [22] x where the filtering gains K SICI(h) k are obtained by the optimal ω h minimizes trace of covariance matrix P SICI(h) k|k as objective : min where tr(·) represents the matrix traces. It is necessary to solve the H − 1 one-dimensional optimization problems in one filtering cycle for SICI. The optimization problem can be solved by the MATLAB f minbnd function [16].
Theorem 1. SICI fusion algorithmx SICI k can be represented as the batch form of a weighted mean wherex h k are local filters. Weighting coefficients θ can be obtained recursively as with initial values are θ where K l−1 and ω l−1 can be calculated from Equations (13) and (14), respectively.
Proof of Theorem 1. We chose mathematical induction to prove the Theorem 1.
Step 2: Assuming that Theorem 1 holds when the number of sensors is H − 1, then We have to prove that Theorem 1 holds when the number of sensors is H. The fusion of thex It can be seen that when the number of sensors is H, Theorem 1 still holds. The proof is completed.

BICI Fusion Algorithm
The batch forms of ICI method and the BICI multi-sensor fusion positioning algorithm are proposed based on two-sensor ICI. Meanwhile, the noise variance upper bound of error variance matrix is derived to prove the robustness of BICI. The structure of the BICI fusion algorithm is displayed in Figure 2 as follows. Inspired by Theorem 1, the batch form of ICI is a convex combination of local filters, given as where filtering gains K h are where ω h fulfills the condition 0 The selection of the coefficients ω h is the minimum trace of the variance matrix P BICI k as [15] objective : min The above optimization problem can also be solved by the f mincon function in the MATLAB optimization toolbox.

Theorem 2. The error variance matrix of BICI fusion algorithm isP
, and noise variance upper bound is P BICI .
Proof of Theorem 2.
Using the inequality is where r > 0. The noise variance matrix upper bound P BICI k can be subtracted by The proof is completed.
Using Theorem 2, we demonstrated that the BICI multi-sensor fusion positioning method with unknown covariance is robust. Combined with Theorem 1, the robustness of the SICI fusion positioning method is indirectly verified as Corollary 1. The error variance matrix of the SICI fusion algorithm isP SICI , and noise variance upper bound is

Simulation
The goal of this section is to evaluate the performance of the BICI with uncertain time-varying noise variance and unknown covariance. The numerical example provides two representative cases, which are fixed and time-varying noise. The SCI, BCI, SICI and BICI multi-sensor positioning algorithms are taken for comparison. In the presentation of the simulation result, the root mean square (RMS) of positioning error is utilized for positioning accuracy and robustness evaluation.
The simulation trajectory is shown in Figure 3, and the simulation time is 100 s. A 3-sensor fusion positioning system with uncertain time-varying noise variance can be represented as where T = 100 ms is the sampling period of sensor, x k = p x , v x , p y , v y , p z , v z T is the positioning system state. p x , p y and p z are the position of X, Y and Z directions, respectively. v x , v y and v z are the velocity of X, Y and Z directions, respectively. w k and v h k are white Gaussian noise with zero mean and variance matrixQ k ,R h k , respectively. In the simulation, we take Q = 3I,Q k = 0.8 and the conservative accuracy of three sensors as 1 m, 2 m and 3 m, respectively. The observation and process noise of sensors 1 and 2 have random  Through 50 Monte Carlo simulations, the cumulative distribution functions (CDF) of SCI, BCI, SICI, BICI, SCI (fix), BCI (fix), SICI (fix) and BICI (fix) with uncertain time-varying noise are shown in Figure 5. It is indicated that SCI, BCI, SICI, and BICI multi-sensor fusion positioning algorithms have the same positioning accuracy under uncertain fixed and time-varying noise variance, respectively. Therefore, the four algorithms have good robustness under the time-varying noise variance. The positioning accuracy relation is SICI < BCI ≈ SCI < BICI in Table 1. BICI has less conservativeness and improved the positioning accuracy by at least 26.7% compared with the other three algorithms.

Multi-Sensor Fusion Positioning Algorithm Based on BICI and IMM
The motion pattern of platform is typically hard to obtain in actual scene. The filter based on a single model sharpens the deterioration of positioning accuracy and divergences when motion model noise variance exceeds the conservative upper bound in the strong mobility scenario. The accurate estimation can be obtained by a multi-model algorithm with known covariance in actual scene. There are few studies on the estimation with uncertain time-varying noise variance. Combined with BICI and IMM, BICI-IMM multi-sensor fusion positioning algorithm improves the positioning accuracy.
The process of the traditional IMM algorithm is divided into four steps: model reinitialization, model filter, model probability updating and estimation fusion [34]. The model filter designs for a single sensor cannot meet the multi-sensor fusion positioning. Therefore, we introduced a local model and global filter to replace model filter based on BICI algorithm. Meanwhile, a sequential method was employed for the likelihood function in the model probability updating phase.

BICI-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 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 π ij defined as where M = {m i k , i = 1, 2, . . . , M} is the model set, ∑ j π ij = 1. It is assumed that the local estimation of h-th sensor of j-th model at time k − 1 isx The interaction weight is defined as The interaction state estimation and variance matrix arē (2) Model local filter For h-th sensor (h = 1, 2, . . . , H), the local state estimation process is The state prediction isx where the variance of positioning prediction error is The measurement residual isz The measurement noise variance matrix is The filtering gain is Update the state estimation and variance matrix tô (3) Global filter The global estimation of j-th model based on the BICI algorithm iŝ with error variance matrix is (14) and (26) The likelihood function is obtained through multi-dimensional normal distribution distribution by The recursive formula of updating the model probability at time k through the model likelihood function based on the sequential method is where the initial value is The probability of the updated model is (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 arê

Simulation
In order to evaluate the positioning accuracy of the BICI-IMM multi-sensor fusion positioning algorithm with uncertain noise variance, unknown covariance and motion model, we simulated the unmanned platform with constant velocity (CV) and constant turn (CT) models. The positioning accuracy evaluation is the same as in Section 3.3. The multi-sensor positioning system model is shown as (1) The CV model It is assumed that the unmanned platform moves in a straight line with constant speed and the system state is x k = p x , v x , p y , v y T ; then (2) The CT model It is assumed that unmanned platform moves in a turning motion at constant angular speed ψ and the system state is x k = p x , v x , p y , v y T , then The trajectory of the unmanned platform is displayed in Figure 7, which contains CV and CT models in Table 2. The initial state is X 0 = [10, 5, 10, 0] T . In the simulation, we take Q = 3I,Q k = 0.8, and the conservative noise variance upper bounds of the three sensors are 1 m, 2 m and 3 m, respectively. The actual positioning accuracies of the three sensors are 0.8 m, 1.2 m and 2 m, respectively. The correlation coefficients of sensor 1 and 2 are the same as in Section 3.3.  Through 50 Monte Carlo simulations, the positioning results of BICI, SICI and BICI-IMM are exhibited in Figure 8. Figure 9 is the positioning error CDF of algorithms. It is indicated that BICI has a higher positioning accuracy compared with other single model algorithms. In contrast, the BICI-IMM has higher positioning accuracy. The algorithm based on CV model positioning accuracy is slightly higher than the model based on CT in Table 3.
The error variance of system model based on CV is relatively, small because 71% and 29% of the motion model are CV and CT. Therefore, the positioning accuracy is also slightly higher. The positioning accuracy of BICI-IMM algorithm is about 14.3% and 52%, increased by the BICI and SICI algorithm, respectively.

Experiment
In order to verify the positioning accuracy of the BICI-IMM and BICI fusion positioning algorithms, we designed a multi-sensor fusion positioning equipment and carried out the fusion positioning algorithms experiments in the open and semi-open areas. As shown in Figure 10, the multi-sensor fusion positioning equipment includes two GPS, IMU, co-processor and embedded CPU. The two GPS receivers adopt ZED-F9P and MAX-7Q of u-blox company. The IMU sensor is the ADIS16470. The co-processor adopts the STM32F407 with uC/OS-III for data acquisition and time synchronization between sensors. The collected sensors data are sent to the embedded processor through rosserial. The embedded CPU is Allwinner H6 and carries a Linux operating system with a robot operating system. Allwinner H6 is the sensors' data-processing unit for data recording and fusion processing. During the experiment, we used the high-precision real-time kinematic (RTK) receiver of CHCNV E91 as the reference positioning and the continuously operating reference stations (CORS) adopted the FindCM service of Qianxun.

Open Area Experiment
We carried out the positioning accuracy of multi-sensor fusion positioning experiment based on fusion positioning equipment in the playground, as in Figure 11 Table 4 and Figure 13. Compared with single model BICI and SICI algorithms, the positioning accuracies of the BICI-IMM multi-sensor fusion algorithm are improved by 18.5% and 14.4%, respectively. Therefore, the BICI-IMM multi-sensor fusion algorithm has better positioning accuracy in open area.

Semi-Open Area Experiment
We  Table 5. Compared with other algorithms, the accuracy of the BICI fusion positioning algorithm decreases the least, so BICI has better robustness and positioning accuracy. However, the performance of BICI-IMM is limited by the precision motion model, which is hard to obtain.

Discussion
With respect the local filter, the sensor subsystem is a linear system. For the nonlinear system, the extending Kalman is one of the solutions. The robustness and other research need to be further improved for complex scenarios.
The H−1 dimension optimization is inevitable for BICI. The fast calculation method of the CI method can be used as a reference [35], which reduces the computational complexity of the BICI algorithm and fits the real-time positioning and navigation for the embedded platform with computing and power consumption limitation.
In multi-model algorithm, BICI-IMM utilizes a fixed model set. The size of the model set will be very large in the actual scene. It will boost the computational complexity and positioning error due to the conflicts among the motion models. The variable-structure multi-model algorithm can achieve a balanced positioning accuracy, model set size and computational complexity. Therefore, it is necessary for the further study of a fusion positioning algorithm based on a variable-structure multiple-model, in order to enhance positioning accuracy in challenging scenes.

Conclusions
A BICI multi-sensor fusion positioning algorithm is proposed based on the batch form of ICI. Compared with SICI, BCI and SCI algorithm, the positioning accuracy of the BICI algorithm increases by 26.7%, at least in the simulation. We presented the BICI-IMM algorithm with unknown motion model, uncertain time-varying noise variance and unknown covariance based on an IMM algorithm structure. The positioning accuracy increased by 14.3% compared with BICI. The BICI-IMM multi-sensor fusion algorithm has better positioning accuracy than SICI in the open and semi-open scenes. The BICI-IMM can provide a robust positioning for the distributed control of robots and assist in the rapid construction of dynamical network topologies [36]. It helps to decrease the average time taken to organize the group in a coordinated behavior and increase the moving speed of robots in real applications. Institutional Review Board Statement: Not applicable.

Informed Consent Statement: Not applicable.
Data Availability Statement: The study did not report any data.

Conflicts of Interest:
The authors declare no conflict of interest.

Abbreviations
The following abbreviations are used in this manuscript: