I wish to get my phone's current orientation by the following method:

  1. Get the initial orientation (azimuth) first via the getRotationMatrix() and getOrientation().
  2. Add the integration of gyroscope reading over time to it to get the current orientation.

Phone Orientation:

The phone's x-y plane is fixed parallel with the ground plane. i.e., is in a "texting-while-walking" orientation.

"getOrientation()" Returnings:

Android API allows me to easily get the orientation, i.e., azimuth, pitch, roll, from getOrientation().

Please note that this method always returns its value within the range: [0, -PI] and [o, PI].

My Problem:

Since the integration of the gyroscope reading, denoted by dR, may be quite big, so when I do CurrentOrientation += dR, the CurrentOrientation may exceed the [0, -PI] and [o, PI] ranges.

What manipulations are needed so that I can ALWAYS get the current orientation within the the [0, -PI] and [o, PI] ranges?

I have tried the following in Python, but I highly doubt its correctness.

rotation = scipy.integrate.trapz(gyroSeries, timeSeries) # integration
if (headingDirection - rotation) < -np.pi:
    headingDirection += 2 * np.pi
elif (headingDirection - rotation) > np.pi:
    headingDirection -= 2 * np.pi
# Complementary Filter
headingDirection = ALPHA * (headingDirection - rotation) + (1 - ALPHA) * np.mean(azimuth[np.array(stepNo.tolist()) == i])
if headingDirection < -np.pi:
    headingDirection += 2 * np.pi
elif headingDirection > np.pi:
    headingDirection -= 2 * np.pi
<
Edit
Report