include a drawing function in RobotSimulator.cc; add TestParticleFilter.cc to test the particle filter implementation