Update measurement_model function in ParticleFilter.cc