Updated with gyro and kalman filter