Development of a reliable, risk-free course in a complex and dynamic environment for military or civilian applications requires a sense of positioning, tracking and navigation (constituting of position, velocity and attitude parameters). Global Positioning System (GPS) has been the prominent technology to fulfill the demands for reliable navigation, over extended periods of time, covering any part of the world during day or night [1]. However, standalone GPS signals may be completely lost for short durations when, for example, a vehicle goes through a tunnel or passes under a bridge or dense foliage [2] or can be deliberately jammed. Under these circumstances, alternate information sources need to be employed like Inertial Navigation System (INS), to bridge the non-GPS signal reception periods [3]. INS overcomes the shortcomings of GPS by providing continuous navigation data based on the self-contained measurements derived from inertial sensors (accelerometers and gyroscopes) [4]. INS can bridge GPS signal gaps, assist in signal reacquisition after an outage and reduce the search domain for detecting and correcting GPS cycle slips [3,4]. However, its solution accuracy decreases with time because of inherent sensor errors (biases, scale-factor errors, noises and drifts) that may render the uncorrected measurements useless, especially for low-cost sensors [5]. The INS error growth can be limited by utilizing external aiding sources like GPS derived position and velocity data, with bounded errors [2-5]. Therefore, to provide continuous, accurate and affordable navigation solution under all environmental scenarios, information coming from these multiple sensors (like GPS and INS) need to be fused or integrated together by accurate, robust and reliable algorithms/integration platforms [6]. Motivated by this scenario, this paper strives to develop a novel and reliable multi-sensor fusion algorithm based on integrating Particle filter with Voronoi tessellations called Voronoi based Particle Filter (VPF).

Generally, Kalman Filter (KF) and its modifications are the most widely used methods for integrating INS and GPS system because of their simplicity and ability to estimate past, current and even future states [7]. KF is an optimal filter for linear systems with Gaussian noise but is not applicable to non-linear systems. For non-linear models, Extended Kalman Filter (EKF) can be implemented, which is based on linearization of the non-linear models. Kalman filters and its variant assume the Gaussian distribution to obtain a closed form solution for the nonlinear model and propagate only mean and covariance of the state vector through approximate models (in case of EKF). However, the linearization process in EKF is often complicated, time consuming and may cause filter divergence [3,8]. To overcome these current limitations, other types of Bayesian filters like Unscented Kalman Filter (UKF) were developed [8]. UKF is based on the principle that it is easier to approximate a Gaussian distribution by fixed number of sigma points, than to approximate an arbitrary nonlinear function. However, when the nonlinearity is highly pronounced, even the best fitting Gaussian distribution becomes a poor approximation to the posterior distribution [9,10] as illustrated in Figure 1. As observed, neither an exact nor a finite-dimensional solution can be obtained for nonlinear filtering problem [10]. Hence, various numerical approximation methods (like sequential Monte Carlo or particle filters) are developed to address the intractability which will be discussed in Section 2.

This paper has been divided into 5 sections. Section 2 describes the various particles filters while Section 3 describes our proposed Voronoi based particle filter. Section 4 illustrates the results obtained with and without GPS outages and conclusions are drawn in Section 5.

Particle Filter (PF) was implemented in order to overcome the current limitations of EKF and UKF by various researchers [4,7,9-16]. The PF can deal with nonlinearities and does not require any assumption about the form of the posterior distributions. In PF the posterior distribution is represented by a cluster of random particles rather than a linearized function (EKF) or deterministically chosen few points around the mean value (UKF) [10]. According to the law of large numbers, larger the number of particles, closer is the distribution to the true posterior function. In PF, to avoid intractable integrals, the desired posterior density is represented by N independent, random samples with equal weights drawn from the distribution according to the Monte Carlo principle as given by Equation (1).

where represents the Dirac delta function. Since, it is not always feasible to sample from the true posterior distribution, it is common to sample from an easy to implement distribution called the proposal distribution , as per the Importance Sampling (IS) algorithm [4,7,10-16]. The importance weights are then given by Equation (2).

Based on the choice of this proposal density, different types of particle filters have emerged. In the simplistic Sequential Importance Sampling (SIS) Particle filter, the prior density function is selected as the importance or proposal density, so as to simplify the particle selection and weight calculations [10-12]. Consequently after a few iterations dispersion occurs, i.e., most of the samples exhibit negligible weights as information coming from the latest measurement is completely ignored while drawing out particles from the prior density [13-16]. The most common ways to avoid this problem is to use larger number of particles or to use Resampling method [10,11], leading to Sequential Importance Resampling (SIR) Particle filter. SIR filter constitutes of SIS + Resampling method at every time instant, which

eliminates particles with lower weights and multiplies particles with higher weights. Introducing resampling at every time step leads to sample impoverishment as similar particles will be repeated number of times [12]. An ad-hoc approach called jittering was suggested to alleviate the sample impoverishment issue [17]. This approach added a small amount of Gaussian noise to each resampled particles so as to increase their diversity. However, this comes at the cost of immediately introducing some additional variance [12]. The performance of the SIS or SIR PF is limited because of the choice of the proposal distribution, especially when the likelihood is too narrow in comparison to the transition prior density function [4, 7,10].

To overcome these issues, a better proposal distribution is desired, which is conditioned on the latest measurement [11-14]. Kalman based filters incorporate the latest measurement into the updated posterior state. Therefore, if EKF or UKF is used to generate the importance distribution, latest measurement can be incorporated into the distribution through the local linearization around each particle [15,16]. This is the principle behind the Extended particle filter (EPF) [15,18] and the Unscented particle filter (UPF). The UPF is based on the UKF, an attractive alternative to EKF for highly nonlinear problems. However, the computational cost per particle is higher as each particle is individually propagated through EKF or UKF to form the proposal distribution. To overcome these limitations, a hybrid extended particle filter (HEPF) was proposed [19]. HEPF combines the advantages of EKF (less computational load) and EPF (more accuracy during highly nonlinear regions) by alternating between the two filters, based on system dynamic. Another option to alternating between two different filters is to marginalize out the linear states of the model from the nonlinear states as introduced in the RaoBlackwellized particle filter (RBPF) [20,21]. In RBPF the linear states are estimated by the KF while the nonlinear states by the PF portion of the algorithm. These strategies reduce the filter divergence issue and require less number of particles for outputting adequate navigation solution. However, since the EPF or classic PF are still used by the HEPF or RBPF for the non-linear state estimation, their respective problems remain associated with the filter design [22].

Another approach to alleviate sample impoverishment problem in SIS/SIR filter is addressed by Pitt and Shepard in the form of the Auxiliary particle filter (APF) [23]. APF enhances the effectiveness of the importance sampling step by augmenting the existing good particles

such that their predictive likelihood is high. Hence, the selected proposal density is a mixture of past states and the most recent observations. As long as the process noise is small, the performance of APF is superior to SIR PF. However, the moment the process noise increases, its accuracy decreases because of poor approximation of the [21].

Further, APF is computationally slower since the proposal is used twice in the implementation. In likelihood PF (LPF), the likelihood function is selected to be the proposal density based on the assumption that particles drawn from the likelihood function will be closer to the true posterior density in comparison to particles drawn from the state transition density [10,11]. This is effective only when the likelihood function is highly peaked and transition density is broad [21]. To combine the advantages of LPF and SIR filters, mixture particle filters (MPF) were developed. These MPF can be based on mixture posterior; where posterior is a weighted Gaussian mixture of parallel EKF or mixture proposal distributions [24], where distribution is based on mixture transition and likelihood functions. However, there are number of practical implementation limitations of these filters, for eg. the selection of Gaussian distribution parameters or decision about the number of samples to be used from the transition and likelihood function etc.

From all these methodologies discussed in literature, couple of prominent limitations like dispersion and sample impoverishment, are being addressed by our methodology. For a given finite set of generators, a Voronoi tessellation for each of these generators consists of all particles whose distance to a generator is not greater than to any other generator [25]. Motivated by this advantage of Voronoi tessellations, in this paper we have developed VPF method. In our methodology, Voronoi are created dynamically as and when new generators become available.

At previous epoch, N particles are generated from the prior distribution and assigned equal weights. Now each of these particles is passed through the developed system model as given by Equation (3).

where at time, is the state vector of dimension, is the independent dynamic noise vector, and the nonlinear function, of dimension, describes the state transition from discrete time to time m to get predicted state vector. The particles are now ready to be used for the creation of a dynamic Voronoi. This procedure follows different path for different scenarios which are explained below.

The field test data was collected by installing various equipments in a test vehicle [24]. These include a Crossbow IMU 300CC-100, reference high grade Honeywell IMU-HG1700, Novatel OEM GPS receivers and computer. The test trajectory covered a number of vehicle dynamics. Throughout the test, a minimum of seven satellites were visible, except for several short natural GPS signal outages. For testing purposes, we carried out many experiments (without GPS outage) and here we report a couple of cases with N = 15 (Figure 5) and N = 45 particles (Figure 6). We established the performance of the VPF algorithm by plotting the least square error (Figure 7) of the calculated trajectories (for N = 15 to 55) with respect to their reference solution. We also introduced several short GPS outages at various locations that are intentionally picked under diverse conditions. The position predicted by the VPF approach during one such

outage is compared with the reference differential GPS for N = 15 and N = 45 particles (Figure 7).

We have developed a Voronoi based particle filter to integrate GPS and INS data for the navigation purpose. In some scenarios creation of Voronoi involves redistribution of the particles indicating the modification of the proposal distribution. Using the concept of dynamic Voronoi, we made more particles to fall into the high likelihood region; thereby increasing the reliability of our proposed VPF algorithm. The robustness of the algorithm is illustrated by completing the whole trajectory with as minimum as fifteen particles with acceptable error. On increasing the number of particles to 45, we were able to achieve a smoother trajectory with less error compared to 15 particles case. Due to the redistribution principle, we are able to overcome the drawbacks of dispersion (maximum number of particles ending up with negligible weights) and sample impoverishment (many identical particles found in the posterior distribution), inherent in conventional particle filters. In case of GPS outages, we incorporated non-holonomic constraints to create dynamic Voronoi with virtual GPS center. Based on this method, we have obtained initial results and demonstrated the performance of the VPF algorithm. Future work will include larger GPS outages at various locations in the trajectory and this will be reported in later publication.