Fix sensor listener
Add kalman filter