Improved particle fusing geometric relation between particles in FastSLAM

FastSLAM is a framework for simultaneous localization and mapping using a Rao-Blackwellized particle filter (RBPF). But, FastSLAM is known to degenerate over time due to the loss of particle diversity, mainly caused by the particle depletion problem in resampling phase. In this work, improved particle filter using geometric relation between particles is proposed to restrain particle depletion and to reduce estimation errors and error variances. It uses a KD tree (k-dimensional tree) to derive geometric relation among particles and filters particles with importance weight conditions for resampling. Compared to the original particle filter used in FastSLAM, this technique showed less estimation error with lower error standard deviation in computer simulations.