Added more Kalman filter functionality