Add DEBUG to control printing of KalmanFilter

(KalmanFilter) : Add DEBUG to control printing of KalmanFilter