CMDeviceMotion yaw values unstable when iPhone is vertical - iphone

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 works well when the iPhone is held flat, where I have a graphical arrow that point to a stable compass heading.
The problem appear when the iPhone is held vertical in portait mode. The UIDeviceOrientation constantly changes from UIDeviceOrientationFaceDown to UIDeviceOrientationFaceUp and back. This makes the yaw value to skip back and forth +/-180 degrees based on small changes of the pitch. Is it possible to lock the device to one orientation that gives a stable yaw value, predict the change without glitches or compute the gyro yaw (or roll in this orientation) in other ways?
This poor guy have the same problem, with no answers. Double points possible people! :)
https://stackoverflow.com/questions/10470938/euler-angle-yaw-not-working-when-iphone-orientation-changes

Answer 1

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 2

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 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.
Rotations along the phone's pitch (or X axis): do not change compass heading.
If the phone is flat against a wall:
Rotations along the phone's yaw (or Z axis): change the compass heading.
Rotations along the phone's roll (or Y axis): change the compass heading.
Rotations along the phone's pitch (or X axis): do not change compass heading.
For the remainder of this answer, I'll assume the phone is upright and yaw, pitch, and roll refer to exactly what's in the photo above.
Yaw
You'll need to use atan2 and inspect gravity as in this example.
let yaw = -Angle(radians: .pi - atan2(motion.gravity.x, motion.gravity.y))
Pitch
Similar to the above, I primarily just swapped x and z and it seems to be returning the correct values:
let pitch = Angle(radians: .pi - atan2(motion.gravity.z, motion.gravity.y))
Roll (aka Compass Heading)
Use blkhp19's code above which sums up the attitude yaw and roll. If you import SwiftUI, you can leverage the Angle struct to make radian + degrees conversion easier:
func roll(motion: CMDeviceMotion) -> Angle {
let attitudeYaw = Angle(radians: motion.attitude.yaw)
let attitudeRoll = Angle(radians: motion.attitude.roll)
var compassHeading: Angle = attitudeYaw + attitudeRoll
if attitudeRoll.degrees < 0 && attitudeYaw.degrees < 0 {
compassHeading = Angle(degrees: 360 - (-1 * compassHeading.degrees))
}
return compassHeading
}
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.
extension CMDeviceMotion {
var yaw: Angle {
-Angle(radians: .pi - atan2(gravity.x, gravity.y))
}
var pitch: Angle {
Angle(radians: .pi - atan2(gravity.z, gravity.y))
}
var roll: Angle {
let attitudeYaw = Angle(radians: attitude.yaw)
let attitudeRoll = Angle(radians: attitude.roll)
var compassHeading: Angle = attitudeYaw + attitudeRoll
if attitudeRoll.degrees < 0 && attitudeYaw.degrees < 0 {
compassHeading = Angle(degrees: 360 - (-1 * compassHeading.degrees))
}
return compassHeading
}
}