Added StateParticleFilter