CMDeviceMotion yaw values unstable when iPhone is vertical

Question

In a iOS prototype I use a combination of CMDeviceMotion.deviceMotion.yaw and CLHeading.trueHeading to make stable compass heading that is responsive and accurate. This work

Answer 1

If somebody is interested in the implementation in iOS Swift the code is given below:

    let queue = NSOperationQueue()
    motionManager.startDeviceMotionUpdatesToQueue(queue) {
        [weak self] (data: CMDeviceMotion!, error: NSError!) in
    var yawDegrees: Double = self!.motionManager.deviceMotion.attitude.yaw * (180.0 / M_PI)
        var pitchDegrees: Double = self!.motionManager.deviceMotion.attitude.pitch * (180.0 / M_PI)
        var rollDegrees: Double = self!.motionManager.deviceMotion.attitude.roll * (180.0 / M_PI)


        if(rollDegrees < 0 && yawDegrees < 0){
            self!.rotationDegrees = 360 - (-1 * (yawDegrees + rollDegrees))
        }
        else {
            self!.rotationDegrees = yawDegrees + rollDegrees
        }
   }

However I am having some problems and I hope @blkhp19 can help me with this because at certain points the angles go into negative values which then messes up the entire calculation and I can't figure out what the problem is.

Answer 2

I was just searching for an answer to this problem. It broke my heart a bit to see that you posted this over a year ago, but I figured maybe you or someone else could benefit from the solution.

The issue is gimbal lock. When pitch is about 90 degrees, yaw and roll match up and the gyro loses a degree of freedom. Quaternions are one way of avoiding gimbal lock, but I honestly didn't feel like wrapping my mind around that. Instead, I noticed that yaw and roll actually match up and can simply be summed to to solve the problem (assuming you only care about yaw).

SOLUTION:

    float yawDegrees = currentAttitude.yaw * (180.0 / M_PI);
    float pitchDegrees = currentAttitude.pitch  * (180.0 / M_PI);
    float rollDegrees = currentAttitude.roll * (180.0 / M_PI);

    double rotationDegrees;
    if(rollDegrees < 0 && yawDegrees < 0) // This is the condition where simply
                                          // summing yawDegrees with rollDegrees
                                          // wouldn't work.
                                          // Suppose yaw = -177 and pitch = -165. 
                                          // rotationDegrees would then be -342, 
                                          // making your rotation angle jump all
                                          // the way around the circle.
    {
        rotationDegrees = 360 - (-1 * (yawDegrees + rollDegrees));
    }
    else
    {
        rotationDegrees = yawDegrees + rollDegrees;
    }

    // Use rotationDegrees with range 0 - 360 to do whatever you want.

I hope this helps someone else!

Answer 3

The problem is a bit confusing because there are at least two different ways to think about Yaw. One is from the phone's perspective, and one from the world perspective.

I'll use this image from Apple to explain further:

If the phone is flat on a table:

• Rotations along the phone's yaw (or Z axis): change the compass heading.
• Rotations along the phone's roll (or Y axis): do not change compass heading.

If the phone is flat against a wall:

• Rotations along the phone's yaw (or Z axis): do not change the compass heading.
• Rotations along the phone's roll (or Y axis): change the compass heading.

If you want the phone's compass heading while vertical

Use blkhp19's code above which sums up yaw and roll. If you import SwiftUI, you can leverage the Angle struct to make radian + degrees conversion easier:

let yaw = Angle(radians: deviceMotion.attitude.yaw)
let roll = Angle(radians: deviceMotion.attitude.roll)

var compassHeading: Angle = yaw + roll
if roll.degrees < 0 && yaw.degrees < 0 {
  compassHeading = Angle(degrees: 360 - (-1 * compassHeading.degrees))
}

If you want rotations along the phone's yaw axis while vertical

You'll need to use atan2 and inspect gravity as in this example.

let rotation = atan2(data.gravity.x,
                             data.gravity.y) - .pi

Also note that if you don't need the actual angle, and all you need is the relationship (e.g. isPhoneUpright), you can simply read gravity values for those.