Improved both Kalman and Particle filter, added statistics.