Added Kalman filter, use Angles for sensor measure