Converting 9 degrees of freedom data to a quaternion or rotator
I'm attempting to add support for a custom piece of hardware that has an IMU with 9 degrees of freedom, in which I'm trying to convert to a rotator or quaternion. I decided to try implementing the Madgwick's orientation filter to convert the accelerometer, magnetometer, and gyroscope readings into Unreal world space.
Overall the result is very jittery. Pitch and roll seem mostly okay, but yaw doesn't appear to function. I'm honestly not sure I'm implementing the algorithm correctly, because it works with a sample of frequencies. For my implementation, I'm calculating the data on every tick and using the DeltaTime as the frequency.
Any ideas? Or does Unreal have some functionality like this already available? I'm also not certain I'm calculating the bias correctly either...
Here is the function I implemented:
And here's a small sample of the data for reference:
Overall the result is very jittery. Pitch and roll seem mostly okay, but yaw doesn't appear to function. I'm honestly not sure I'm implementing the algorithm correctly, because it works with a sample of frequencies. For my implementation, I'm calculating the data on every tick and using the DeltaTime as the frequency.
Any ideas? Or does Unreal have some functionality like this already available? I'm also not certain I'm calculating the bias correctly either...
Here is the function I implemented:
Code:
FRotator MyClass::GetDeviceRotation(float DeltaTime)
{
SensorData data;
FQuat q;
FRotator rot;
if (GetData(device, OUT &data) == SUCCESS)
{
// Get accelerometer values
float ax = data.imu_acc[2] * -MilimeterScale; // Scale value is 0.00001f
float ay = data.imu_acc[0] * MilimeterScale;
float az = data.imu_acc[1] * MilimeterScale;
// Get gyro values
float gx = FMath::DegreesToRadians(data.imu_gyro[0]); // Device gives gyro values in degrees
float gy = FMath::DegreesToRadians(data.imu_gyro[1]);
float gz = FMath::DegreesToRadians(data.imu_gyro[2]);
// Get magnetometer values
float mx = data.imu_mag[2] * -Sixense_MilimeterScale;
float my = data.imu_mag[0] * Sixense_MilimeterScale;
float mz = data.imu_mag[1] * Sixense_MilimeterScale;
float q1 = q.W, q2 = q.X, q3 = q.Y, q4 = q.Z; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (shown as 3 deg/s)
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.f * q1;
float _2q2 = 2.f * q2;
float _2q3 = 2.f * q3;
float _2q4 = 2.f * q4;
float _2q1q3 = 2.f * q1 * q3;
float _2q3q4 = 2.f * 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 = (float)FGenericPlatformMath::Sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.f) return rot; // handle NaN
norm = 1 / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = (float)FGenericPlatformMath::Sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.f) return rot; // handle NaN
norm = 1 / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.f * q1 * mx;
_2q1my = 2.f * q1 * my;
_2q1mz = 2.f * q1 * mz;
_2q2mx = 2.f * 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 = (float)FGenericPlatformMath::Sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.f * _2bx;
_4bz = 2.f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.f * q2q4 - _2q1q3 - ax) + _2q2 * (2.f * 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.f * q2q4 - _2q1q3 - ax) + _2q1 * (2.f * q1q2 + _2q3q4 - ay) - 4.f * q2 * (1 - 2.f * q2q2 - 2.f * 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.f * q2q4 - _2q1q3 - ax) + _2q4 * (2.f * q1q2 + _2q3q4 - ay) - 4.f * q3 * (1 - 2.f * q2q2 - 2.f * 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.f * q2q4 - _2q1q3 - ax) + _2q3 * (2.f * 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 = 1.f / (float)FGenericPlatformMath::Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
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 * DeltaTime;
q2 += qDot2 * DeltaTime;
q3 += qDot3 * DeltaTime;
q4 += qDot4 * DeltaTime;
norm = 1.f / (float)FGenericPlatformMath::Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
q.W = q1 * norm;
q.X = q2 * norm;
q.Y = q3 * norm;
q.Z = q4 * norm;
rot = FRotator(q);
}
return rot;
}
Code:
delta time : 0.008333 accelerometer : 2098, -110, 20 gyroscope : -21, 0, 2 magnetometer : 220, -188, -152 delta time : 0.008333 accelerometer : 2098, -112, 24 gyroscope : -21, 0, 2 magnetometer : 225, -189, -170 delta time : 0.008334 accelerometer : 2093, -109, 14 gyroscope : -21, 0, 2 magnetometer : 225, -189, -170 delta time : 0.008334 accelerometer : 2089, -111, 26 gyroscope : -21, 0, 2 magnetometer : 225, -189, -168 delta time : 0.008333 accelerometer : 2100, -103, 23 gyroscope : -21, 0, 2 magnetometer : 213, -181, -157 delta time : 0.008333 accelerometer : 2099, -102, 21 gyroscope : -21, 0, 3 magnetometer : 213, -181, -157 delta time : 0.008333 accelerometer : 2094, -96, 23 gyroscope : -21, 0, 4 magnetometer : 223, -189, -158 delta time : 0.008333 accelerometer : 2086, -106, 22 gyroscope : -21, 0, 4 magnetometer : 223, -185, -158 delta time : 0.008334 accelerometer : 2099, -103, 15 gyroscope : -21, 0, 4 magnetometer : 223, -185, -158 delta time : 0.008333 accelerometer : 2097, -106, 31 gyroscope : -21, 0, 4 magnetometer : 216, -185, -156 delta time : 0.008334 accelerometer : 2080, -113, 12 gyroscope : -21, 0, 4 magnetometer : 221, -192, -158
Comment