Add KalmanFilterTracker and fix bugs