Particle Filter Commit 3

- Some helper functions
- Development of PF continued
- Bug fixes
- Some DEBUG MODE visualizations