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).
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
Figure 2. Voronoi with center as z* = zGPS.
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 zi, that serve as generators for the Voronoi tessellations are themselves the mass centroids of those tessellations. We call such a tessellation Centroidal Voronoi tessellation . Partially filled Voronoi has the generator zi 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 zi is iterated till convergence is achieved. These iterations guarantee that the Voronoi gets filled to the maximum capacity with.
Once the dynamic Voronoi is filled with the given GPS measurement as the center zi, weights of the particles are updated according to Equation (9) where h is the measurement model.
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.
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 zi. 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.
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.
The field test data was collected by installing various equipments in a test vehicle . 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
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.
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.
*Both authors contributed equally.