Added more debugging output to KalmanFastTracking