Update measurment_model function in ParticleFilter.cc