koobas.hobune.stream
The question is answered, right answer was accepted
IMU Sensor and Quaternion
Hi,
I'm currently working on a Unity Game where an object is orientated according to an IoT sensor ( IMU). I programmed the sensor so it sends a quaternion to the game via mqtt. I have something that work without quaternion but is subject to gimbal-lock and I think that my magnometer is not well calibrated ( the heading would not turn regularly ) So I tried some algorithm that would not use Roll Pitch and Yaw.
My problem is that the object do move, but if I want to do a 360° with my cube, then I need to turn the sensor of, let's say 1500° ( did not check the true number).
I don't know if the quaternion calculated by the sensor is wrong or if I don't use it correctly in Unity.
I receive this kind of message : quaternion 0.23 -0.19 -0.91 0.30 Here's my code in unity:
String[] parsed = message.Split();
q.w = float.Parse(parsed[1],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
q.x = float.Parse(parsed[2],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
q.z = float.Parse(parsed[3],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
q.y = float.Parse(parsed[4],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
Just some parsing when I receive it, and I exchange Z & Y axis.
Then in the update :
_3Dmodel.transform.rotation = Quaternion.Inverse(q);
On the sensor side, I use this algorithm :
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f / norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
That I call with this command in my loop:
updateImu();
MadgwickQuaternionUpdate(imu.ax, imu.ay, imu.az, imu.gx * PI / 180.0f, imu.gy * PI / 180.0f, imu.gz * PI / 180.0f, imu.mx, imu.my, imu.mz);
publishQuaternion();
Then I directly publish the quaternion without any other calculation.
Ok, I sended my raw data from my sensor directly to Unity and then I applied madgwick filter. It was worse. I played a bit with the samplingFrequency and it was the source of my problem. Also used a fixedUpdate instead of Update.
The frequency totaly impact the quaternion calculation which can give "under movement" or "crazy spinning" just with noise.
My last problems are that I have drift, which is probably (again) a setting issue, and the order of x y z w... which I still think should be q0 = w, q1 = x, q2 = z , q3 = y
Here's my code in unity:
String[] parsed = message.Split();
q.w = float.Parse(parsed[1],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
q.x = float.Parse(parsed[2],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
q.z = float.Parse(parsed[3],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
q.y = float.Parse(parsed[4],
System.Globalization.CultureInfo.InvariantCulture.NumberFormat);
Just some parsing when I receive it, and I exchange Z & Y axis.
Quaternions don't have axes, but they're represented using four real numbers generally denoted as xyzw. Now, assuming that's how your sensor is supplying them, your code above appears to map them as w=x, x=y, z=z, y=w. Why are you doing that rather than a straight mapping of xyzw => xyzw?
I read a bunch of stuff about $$anonymous$$adgwick quaternion and Unity quaternion: At first I did it straight. Here is the post that made me change this: http://answers.unity3d.com/comments/1319576/view.html
I m still unsure about this, but I think the order will ultimately, only change the "direction" it turns, not the distance it rotate itself, so I could always test at the end if , when i turn my sensor and change its roll/pitch/yaw, if the object move the same direction.
Edit : I just check again, if I left the order untouched, it doesn't solve my problem
I'm not familiar with the particular sensor you're using, but I'm guessing the output format must be documented somewhere? I'd expect transposing the xyzw values should do far more than simply changing the direction of rotation - x + yi + zj + wk is very different to y + wi + zj + xk. And when you said you changed the order back to default it "didn't solve" your problem, did it actually not make a difference? Or was it just differently wrong? It seems to be the wrong approach to just arbitrarily swap variables without some reasoning as to why!
The problem is that the library given with the sensor ( sparkfun 9DOF stick) don't give any quaternion calculation.
So ins$$anonymous$$d I decided to use madgwick, which is here ( https://github.com/xioTechnologies/Open-Source-AHRS-With-x-I$$anonymous$$U ) which use a NED ( north east down) system to compute quaternion, which is a different reference from Unity If i'm not mistaking.
I tried a bunch of changement in the order of x y z w and it does not really change the way it moves, meaning when I turn my sensor of 360 degree on itself ( yaw ) , my cube in unity turn of maybe 90 degree in a direction ( which change if I change the parameter order).
$$anonymous$$y understanding of the problem is that I must have missed something with madgwick. Problem is that i basically know nothing about quaternion, I just understand that it represents a movement in 4D space ( or something like that).
I think that my next step will be to move the quaternion calculation from the sensor to the computer itself, so I can use existing c# madgwick code.
I do think that my problem may come from the frequency used to calculate the quaternion. Right now, I think the sensor compute it every 10ms. $$anonymous$$aybe Unity Update is going at a faster/slower pace which may cause loss of message resulting in a partial rotation.
