I have implemented a particle filter algorithm for the state estimation of a mobile robot.
There are several external range sensors(transmitters) in the environment which gives information on the distance (radius) of the robot based on the time taken for the receiver on the robot to send back its acknowledgement. So, using three or more such transmitters it will be possible to triangulate the position of the robot.
The particle filter is initialized with 15000 particles and the sensor noise is relatively low (0.02m).
Update Phase: At each iteration a range information from an external sensor is received. This assigns higher weights to the particles along the radius of the external sensor. Not all the particles are equally weighted since the process noise is low. Hence in most of the cases, the particle relatively closer to the robot gets lower weight than an incorrect one that happens to be along the radius. The weight is a pdf.
Resampling Phase: At this stage, the lower weighted particle(the correct one) that has negligible weight gets lost because the higher weighted particle gets picked up.
All this happens at the first iteration and so when the range information from another sensor arrives, the robot is already kidnapped.
Googling around, said that this problem is called as sample impoverishment and the most common approach is to resample only when the particle variance is low. (Effective Sample Size < number of particles / 2)
But, when the particles are assigned negligible weights and there are relatively very few particles with higher weights, the diversity of the particles are lost at resampling phase. So, when the variance is higher resampling is done which removes the lower weighted particle and hence the diversity of the particles is lost. Isnt this completely the opposite of the above idea of ESS?
Is my understanding of sample impoverishment correct? Is there a way this issue can be fixed?
Any pointers or help would be highly appreciated.