I have a custom piece of hardware that has an IMU sensor with 9 degrees of freedom. I want to convert the accelerometer, gyroscope, and magnetometer data to a rotator or quaternion in 3D world space in Unreal Engine.
I decided to use the Madgwick's orientation filter in my C++ code, but the results aren't quite right. It's very jittery, and while the pitch and roll looks somewhat okay, the yaw doesn't seem to work.
I'm hoping someone here can take a look at my function to see if anything rings a bell. Usually Madgwick uses a frequency (Hz) for calculation, but since I'm calculating on every frame tick, I'm using the DeltaTime. I'm also not sure if I'm calculating the bias correctly.
Here's my code:
FRotator MyClass::GetDeviceRotation(float DeltaTime)
{
DeviceData data;
FQuat q;
FRotator rot;
if (deviceGetData(device, OUT &data) == SUCCESS)
{
// Get accelerometer values
float ax = data.imu_acc[2] * -MilimeterScale; // Scale 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 data 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] * -MilimeterScale;
float my = data.imu_mag[0] * MilimeterScale;
float mz = data.imu_mag[1] * 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;
}
And here's a small sample of the data for reference:
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
Aucun commentaire:
Enregistrer un commentaire