- added simulation test, started debugging the particle filter