Particle filter resampling when weights are equal

[Dave Blumstein]

In: package com.cra.figaro.algorithm.filtering, trait ParticleFilter, updateBeliefState, the function checks if weights are all zero or all one, and if so, does not resample the particles. Per discussion with Brian, suggest this is changed to test if all weights are same value. This would cover the case we have now where constraints have been applied and the weights do not happen to be zero or one. Here resampling simply terminates reasonable particles and accelerates preferential attachment.

For reference, my local changes are:

private[figaro] def updateBeliefState(weightedParticles: Seq[ParticleFilter.WeightedParticle]) {
// If all the particles have weight 1, there is no need to resample
// If all the particles have weight 0, none of them satisfy the conditions, so the best we can do is produce a uniform distribution over them.
// And if all the particles have the same weight, there is limited value in resampling.
// the more robust approach to pick tolerance is to scale by magnitude, but this seems a reasonable approximation
val tolerance = 1e-9
val weightOfParticleZero = weightedParticles(0)._1

if (weightedParticles.forall( particle => math.abs(particle._1 - weightOfParticleZero) < tolerance)) {
  val weightedParticleArray = weightedParticles.toArray

...