Added a simple kalman filter for smoothing out tracking. Working on improving accuracy still