Dynamic creation of a Voronoi is withheld until the GPS data is available, as GPS is the Voronoi generator (illustrated by Figure 2). Till the arrival of GPS, the incoming INS particles are stored in. Bythe time GPS data becomes available, storage might have none, some or huge set of INS particles. The decision to cluster an INS particle into the Voronoi is based on Equation (5). If the criterion is met, the INS particle joins with that Voronoi otherwise the INS particles are stored in for the next Voronoi. This procedure is continued till the Voronoi gets the set number (N) of INS particles. With this, Voronoi is closed and the algorithm looks for the dynamic creation of next Voronoi. Thus theVoronoi tessellations are formed such that particles with similar weights that satisfy Equation (5) are grouped together in a tessellation.

3.1.2. Voronoi Gets Partially Filled

If sufficient number of particles did not satisfy Equation (5) then Voronoi gets partially filled. In this case, the data in storage is redistributed depending on the distance function (Equation (8)) and thus results in a dynamic Voronoi. Given a tessellation and a density function, defined in V, the mass centroid of V is defined by Equation (6).

(6)

In our case, the arithmetic mean (Equation (7)) corresponds to the mass centroid of V, where n is the number of particles in the tessellation V

(7)

Figure 2. Voronoi with center as z^{*} = z_{GPS}.

Given a set of k particles which are generators, where; we can define their associated Voronoi tessellations. On the other hand, given the tessellation we can define their mass centroids as illustrated by Figure 2. Here, we are interested in the situation where, i.e., the particles z_{i}, that serve as generators for the Voronoi tessellations are themselves the mass centroids of those tessellations. We call such a tessellation Centroidal Voronoi tessellation [25]. Partially filled Voronoi has the generator z_{i} and one can compute the mass centroid for the data in storage. Now the process of simultaneously redistributing the particles in the storage and checking distance function (Equation (8)) between the new mass centroid and z_{i} is iterated till convergence is achieved. These iterations guarantee that the Voronoi gets filled to the maximum capacity with.

(8)

Once the dynamic Voronoi is filled with the given GPS measurement as the center z_{i}, weights of the particles are updated according to Equation (9) where h is the measurement model.

(9)

The posterior density function (required density) will be represented by a collection of these predicted particles along with their updated weight as given in Equation (10). The complete Voronoi based particle filter algorithm is illustrated in Figure 3.

(10)

3.2. Situation II: GPS Data Is Unavailable

In case of non-availability of GPS, the previous GPS data point acts as the starting point for z_{i}. The INS data that is already collected is divided into arbitrary number of balls and their mass centroids are computed. They then follow a sequence of iterations to simultaneously correct the centers and redistributing the arbitrary balls with reference to The resulting balls are ordered in an increasing error threshold (i.e., is more closer to than) and the Voronoi starts getting filled with the first ball and incase it is not filled to its maximum capacity, second ball in row is used. This ensures creation of an appropriate Voronoi with propagated GPS () as the center. Further, incorporation of the non-holonomic conditions, given by Equation (11) guarantees the vehicle progresses in the correct path.

(11)

where and denote the body frame velocities in Y and Z directions respectively. The created Voronoi has all the particles with equal weights, which are later updated according to the new GPS center. The posterior density function (required density) will be represented by a collection of these predicted particles along with their updated weights.

4. Results

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

Figure 3. Voronoi based particle filter.

Figure 4. VPF with 15 particles.

Figure 5. VPF with 45 particles.

Figure 6. Least square errors vs particles.

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

4.1. Without GPS Outage

In this section we demonstrate the successful completion of the 52 minute trajectory with as small as 15 particles by our proposed Voronoi based particle filter. The trajectory data is shown using the GPS Visualizer toolbox. We observe that as the number of particles increases, the trajectory becomes smoother and smoother. Figures 4 and 5 illustrate the complete trajectories with N = 15 and N = 45 particles. We observe that trajectory with 45 particles has lesser error and is smoother than the trajectory with 15 particles. We have highlighted a section of the trajectory to illustrate this observation. Figure 6 illustrates the performance of the VPF with different number of particles ranging from 15 to 55. The least square error is calculated by comparing each trajectory with the reference solution. We observe that as the number of particles increases, the error decreases.

4.2. With GPS Outage

We carried out some initial experiments with GPS outages at various intentionally chosen locations. We could successfully bridge the GPS gap with as small as 15 particles. This shows that our algorithm works in the GPS outage scenario as well. We report here the preliminarily

(a) (b)

Figure 7. Performance of VPF with (a) 15 and (b) 45 particles during GPS outage.

results for 15 and 45 particles as given in Figure 7, where GPS gap is highlighted. We can clearly see that the error with 45 particles (circled region in Figure 7(b)) is less than the error with 15 particles (circled region in Figure 7(a)). More work will be put in this scenario with longer GPS outages under diverse conditions in our future publications.

5. Conclusion

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.

REFERENCES

- P. Misra and P. Enge, “Global Positioning System: Signals, Measurements and Performance,” Ganga-Jamuna Press, Massachusetts, 2010.
- M. Grewal, L. Weill and A. Andrews, “Global Positioning Systems Inertial Navigation, and Integration,” 2nd Edition, Wiley-Interscience, New Jersey, 2007. doi:10.1002/0470099720
- N. El-Sheimy, “Inertial Techniques and INS/DGPS Integration,” ENGO 623-Course Notes, University of Calgary, Calgary, 2006.
- P. Aggarwal, Z. Syed, A. Noureldin and N. El-Sheimy, “Integrated MEMS Based Navigation Systems,” Artech House, Norwood, 2010.
- P. Aggarwal, Z. Syed, X. Niu and N. El-Sheimy, “A Standard Testing and Calibration Procedure for Low Cost MEMS Inertial Sensors and Units,” Journal of Navigation, Vol. 61, No. 2, 2007, pp. 323-336.
- C. Hide, “Integration of GPS and Low-Cost INS Measurements,” Ph.D. Thesis, University of Nottingham, Nottingham, 2003.
- B. Ristic, S. Arulampalan and N. Gordon, “Beyond the Kalman Filter: Particle Filters for Tracking Applications,” Artech House, 2004.
- S. Julier, J. Uhlmann and H. Durrant, “A New Approach for Nonlinear Transformations of Means and Covariances in Filters and Estimators,” IEEE Transactions on Automatic Control, Vol. 45, No. 3, 2000, pp. 477-482. doi:10.1109/9.847726
- N. Bergman, “Recursive Bayesian Estimation: Navigation and Tracking Applications,” Ph.D. Thesis, Linköping University, Linköping, 1999.
- A. Doucet, N. Freitas and N. Gordon, “Sequential Monte Carlo Methods in Practice,” Springer, New York, 2001.
- M. Arulampalam, S. Maskell, N. Gordon and T. Clapp, “A Tutorial on Particle Filters for Online Nonlinear/NonGaussian Bayesian Tracking,” IEEE Transactions on Signal Processing, Vol. 50, No. 2, 2002, pp. 174-188. doi:10.1109/78.978374
- A. Doucet and A. Johansen, “A Tutorial on Particle Filtering and Smoothing: Fifteen Years Later,” In: D. Crisan and B. Rozovsky, Eds., Handbook of Nonlinear Filtering, Oxford University Press, Oxford, 2011.
- F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson and P. Nordlund, “Particle Filters for Positioning, Navigation and Tracking,” IEEE Transactions on Signal Processing, Vol. 50, No. 2, 2002, pp. 425-437. doi:10.1109/78.978396
- G. Kitagawa, “Monte Carlo Filter and Smoother for NonGaussian Nonlinear State Space Models,” Journal of Computational and Graphical Statistics, Vol. 5, No. 1, 1996, pp. 1-25.
- R. Merwe, A. Doucet, J. Freitas and E. Wan, “The Unscented Particle Filter,” University of Cambridge, Cambridge, 2000.
- A. Haug, “A Tutorial on Bayesian Estimation and Tracking Techniques Applicable to Nonlinear and Non-Gaussian Processes,” MITRE Technical Report, MTR 05W- 0000004, MITRE Corporation, 2005.
- N. Gordon, D. Salmond and A. Smith, “Novel Approach to Nonlinear and Non-Gaussian Bayesian State Estimation,” Proceedings of the IEEE, Vol. 140, 1993, pp. 107- 113.
- P. Aggarwal, D. Gu and N. El-Sheimy, “Extended Particle Filter (EPF) for Land Vehicle Navigation Applications,” International Global Navigation Satellite Systems (IGNSS), Sydney, 4-6 December 2007.
- P. Aggarwal, Z. Syed and N. El-Sheimy, “Hybrid Extended Particle Filter for Integrated Navigation and Global Positioning System,” Measurement Science and Technology, Vol. 20, No. 5, 2009.
- A. Doucet, N. Freitas, K. Murphy and S. Russell, “RaoBlackwellised Particle Filtering for Dynamic Bayesian Networks,” Proceedings of the 16th Conference on Uncertainty in Artificial Intelligence, Stanford, 30 June-3 July 2000, pp. 176-183.
- Z. Chen, “Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond,” Adaptive Systems Laboratory Technical Report, McMaster University, Hamilton.
- Y. Jianjun, Z. Jianqiu and M. Klaas, “The Marginal RaoBlackwellized Particle Filter for Mixed Linear/Nonlinear State Space Models,” Chinese Journal of Aeronautics, Vol. 20, No. 4, 2007, pp. 346-352. doi:10.1016/S1000-9361(07)60054-5
- M. Pitt and N. Shephard, “Filtering via Simulation: Auxiliary Particle Filters,” Journal of the American Statistical Association, Vol. 94, No. 446, 1999, pp. 590-599. doi:10.1080/01621459.1999.10474153
- J. Georgy, A. Noureldin, M. Korenberg and M. Bayoumi, “Low Cost 3-D Navigation Solution for RISS/GPS Integration Using Mixture Particle Filter,” IEEE Transactions on Vehicular Technology, Vol. 59, No. 2, 2010, pp. 599- 615. doi:10.1109/TVT.2009.2034267
- Q. Du, V. Faber and M. Gunzburger, “Centroidal Voronoi Tesselations: Applications and Algorithms”, SIAM Review, Vol. 41, No. 4, 1999, pp. 637-676. doi:10.1137/S0036144599352836

NOTES

^{*}Both authors contributed equally.