Attitude change - angles and axis issue - quaternion math

Question

I have an app that records angles as user is walking around an object, while pointing device (preferably) at the center of the object. Angle gets reset on user's command - so reference attitude gets reset.

Using Euler angles produces Gimbal lock, so I am currently using quaternions and calculating angles this way:

double angleFromLastPosition = acos(fromLastPositionAttitude.quaternion.w) * 2.0f;

This gives off good precision and it works perfectly IF device's pitch and yaw does not change. In other words, as the angle shows 360 degrees I end up in the same place as the start of the circle.

Problem 1: if device's yaw and pitch change slightly (user not pointing directly at center of the object), so does the angleFromLastPosition. I understand this part, as my angle formula just shows the angle in between two device attitudes in 3D space.

Scenario:

• I mark the start of rotation attitude and start moving in a circle around the object while pointing at the center
• I stop at, say, 45 degrees and change pitch of the device by pointing it higher or lower. Angle changes accordingly.
• What I would love to see is: angle stays at 45 degrees, even if pitch or yaw changes.

Question 1 is, how can I calculate only the Roll of the device using quaternions, and disregard changes in other two axes (at least within some reasonable number of degrees).

Problem 2: if I rotate for a bit and then freeze the device completely (on tripod so there's no shaking at all), the angleFromLastPosition drifts at a rate of 1 degree per about 10-20 seconds, and it appears not to be linear. In other words, it drifts fast at first, then slows down considerably. Sometimes I get no drift at all - angle is rock-solid if device is stationary. And this makes me lost in understanding what's going on.

Question 2, what is going on here, and how can I take care of the drift?

I went through quite a few articles and tutorials, and quaternion math is beyond me at the moment, hope someone will be able to help with a tip, link, or few lines of code.

Answer 1

I have tested this and it seems to work according to what you're looking for in Question 1, Andrei.

I set the homeangle initially 0, and immediately after the first pass I store the angle returned from walkaroundAngleFromAttitude:fromHomeAngle: in homeangle, for future use.

My testing included starting the device updates using a reference frame:

[_motionManager 
startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryZVertical 
toQueue:operationQueue 
withHandler:handler];

and using the following methods called within handler:

- (CMQuaternion) multiplyQuanternion:(CMQuaternion)left withRight:(CMQuaternion)right {

    CMQuaternion newQ;
    newQ.w = left.w*right.w - left.x*right.x - left.y*right.y - left.z*right.z;
    newQ.x = left.w*right.x + left.x*right.w + left.y*right.z - left.z*right.y;
    newQ.y = left.w*right.y + left.y*right.w + left.z*right.x - left.x*right.z;
    newQ.z = left.w*right.z + left.z*right.w + left.x*right.y - left.y*right.x;

    return newQ;
}

-(float)walkaroundRawAngleFromAttitude:(CMAttitude*)attitude {

    CMQuaternion e =  (CMQuaternion){0,0,1,1};
    CMQuaternion quatConj = attitude.quaternion;
    quatConj.x *= -1; quatConj.y *= -1; quatConj.z *= -1;
    CMQuaternion quat1 = attitude.quaternion;
    CMQuaternion quat2 = [self multiplyQuanternion:quat1 withRight:e];
    CMQuaternion quat3 = [self multiplyQuanternion:quat2 withRight:quatConj];

    return atan2f(quat3.y, quat3.x);
}
-(float)walkaroundAngleFromAttitude:(CMAttitude*)attitude fromHomeAngle:(float)homeangle {

    float rawangle = [self walkaroundRawAngleFromAttitude:attitude];
    if (rawangle <0) rawangle += M_PI *2;
    if (homeangle < 0) homeangle += M_PI *2;
    float finalangle = rawangle - homeangle;
    if (finalangle < 0) finalangle += M_PI *2;

    return finalangle;
}

This is using some modified and extended code from Finding normal vector to iOS device

Edit to deal with Question 2 & Problem 2:

This may not be solvable. I've seen it in other apps (360 pano for example) and have read about faulty readings in gyro and such. If you tried to compensate for it, of course you'll end up with a jittery experience when some authentic rotational movement gets tossed by the compensation code. So far as I've been interpreting for the last few years, this is a hardware-based issue.