Updated the Particle filter for a 3D implementation, and related
plotting code