implemented kalman filter, a lot of tweaking needed