Added Kalman filtering - need to clean up code