Add simple kalman filter