Algorithm improvement for orientation based on accel+gyro