working version of kalman filter tracking