Got tracking working to a degree. Now need to implement a kalman filter thing