Added Kalman filtering, needs tweaking of paramters though