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.

can drift be related to [Drifting Yaw](http://stackoverflow.com/questions/13613239/drifting-yaw-angle-after-moving-fast?rq=1)? if yes, finding a formula for question 1 will take care of both issues :) — Andrei G., May 20 '13 at 16:45
If you're using CMAttitude, is there any particular reason you can't just use fromLastPositionAttitude.roll to get the roll of the device? I appreciate this isn't using the quaternion property, but its there to be used! — David Doyle, May 22 '13 at 14:40
yes, started with that - using euler angles produces a gimbal lock and loss of degrees of freedom. full rotation is not 360 degrees when device is not aligned with Z-axis. at least this is my understanding of the effect.... — Andrei G., May 22 '13 at 15:28

score 4 · Accepted Answer · edited May 23 '17 at 12:34

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.

thanks for your help, with some tweaking I got your code working, and it solves my problem_1. Need more time to test and tweak, but i am sure i got everything I need to finish it up. Drift is behaving differently now, it drifts for few degrees and then stops - angle is rock solid with just 10th of degree variation. Which is perfectly acceptable for this app. I'll play with it and will post if I figure out what it depends on, until then, it's in hardware blame box and it's good enough :) cheers — Andrei G., May 27 '13 at 19:45
@AndreiG. Good stuff, glad I could give you a hand. Also, this gave me the much-needed exercise to get back into quaternion math :) Cheers — Tom Pace, May 27 '13 at 20:48
@TomPace Hi, I am also trying to find a solution to something similar to this, Could you please take a look at my question here:[Link](http://stackoverflow.com/questions/19239482/using-quaternion-instead-of-roll-pitch-and-yaw-to-track-device-motion).I think i have found the change in angle part, But am not sure it I am on the right track. I still need to figure out how to track phone movement in different axes. My current progress is [here](http://pastebin.com/faPWUVE2). Thanks in advance. — iSeeker, Oct 16 '13 at 04:50
@TomPace Thats great! I appreciate it :). So is it possible to get and measure the motion of iphone in different axes like i found for the angle? — iSeeker, Oct 17 '13 at 04:22
@TomPace Hello again, still looking forward for that GitHub project that you said you would create.. could you give me the link if you have done it already?.. :) — iSeeker, Feb 06 '14 at 03:38

Attitude change - angles and axis issue - quaternion math

1 Answers1