Add Kalman filter implementation