[Math] How to get angular velocity from difference orientation quaternion and time

physics

I am using Bullet Physic library to program some function, where I have difference between orientation from gyroscope given in quaternion and orientation of my object, and time between each frame in milisecond. All I want is set the orientation from my gyroscope to orientation of my object in 3D space. But all I can do is set angular velocity to my object. I have orientation difference and time, that is why I need vector of angular velocity [Wx,Wy,Wz] from that.

After reading this:
angularVelocityArticle1 and this:
angularVelocityArticle2

I did something like:

btQuaternion diffQuater = gyroQuater - boxQuater;
btQuaternion diffConjQuater;

diffConjQuater.setX(-(diffQuater.x()));
diffConjQuater.setY(-(diffQuater.y()));
diffConjQuater.setZ(-(diffQuater.z()));

////////////////
//W(t) = 2 * dq(t) /dt * conj(q(t))

btQuaternion velQuater;

velQuater = ((diffQuater * 2) / d_time) * diffConjQuater;

But it is not working as I expect, I mean, there is written, vector part of the result quaternion should be vector of angular velocity, and scalar part should be 0, but my result is not like that.

angular velocity vector represented as quaternion with zero scalar part, i.e
W (t ) = (0, W x (t ), W y (t ), W z (t ))

Any ideas?

Best Answer

This question might go better over at the GameDev SE site; at first glance, though, this formula:

velQuater = ((diffQuater * 2) / d_time) * diffConjQuater;

is not representing what's in the comment above it. The formula you're looking for is $\omega(t) = 2q'(t)\bar{q}(t)$ (where as typical I've written $q'(t) = \frac{dq(t)}{dt}$), but what's written in that code is $\omega(t) = 2q'(t)\bar{q'}(t)$; in other words, you're not multiplying by the conjugate of your original orientation quaternion (as you should be) but by the conjugate of (the approximation of) the derivative. The line you want should be something like

velQuater = ((diffQuater * 2) / d_time) * conjBoxQuater;

but note that this is all predicated on another assumption - that gyroQuater and boxQuater represent the orientation of the same object at two nearby points in time. From your description it sounds like this might not be quite the case, and if it's not so then you may have to be more explicit about just what behavior you're after.