Gravity Compensation in IMU Data

问题

I'm trying to do gravity compensation on my accelerometer data. Given an Accelerometer, with 6 DOF (Accelerometer, Gyroscope) I want to remove/compensate the effect of the gravity in accelerometer reading (Accelerometer can rotate freely).

Following is the way I store raw sensor values to a struct called sample :

uint8_t *p=data; // p is a pointer to the sensor data

    int i;
    for(i=0; i<4; i++)  // quaternion
    {
        sample.quaternion[i]=((float)get_int32(p))/(1<<29);
        len+=snprintf(s+len, sizeof(line)-len, "\t%9.6f", sample.quaternion[i]);
        p+=4;
    }

    for(i=0; i<3; i++) // euler213_degrees
    {
        sample.euler213_degrees[i]=get_int16(p);
        len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler213_degrees[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // euler123_degrees
    {
        sample.euler123_degrees[i]=get_int16(p);
        len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler123_degrees[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // acceleration_g
    {
        sample.acceleration_g[i]=(2.0*get_int16(p))/(1<<15);
        len+=snprintf(s+len, sizeof(line)-len, "\t%6.3f", sample.acceleration_g[i]);
        p+=2;
    }

    for(i=0; i<3; i++) // gyroscope_dps
    {
        sample.gyroscope_dps[i]=(2000.0*get_int16(p))/(1<<15);
        len+=snprintf(s+len, sizeof(line)-len, "\t%6.1f", sample.gyroscope_dps[i]);
        p+=2;
    }

Can you show me a way to get gravity compensated Accelerometer data?

回答1:

The quaternion computed by the IMU (6 DOF device) is basically the device attitude (3D-orientation) with respect to the local Earth reference frame; so it can be used to rotate the acceleration measurement from the local body reference frame (the values directly measured by the IMU) to the local Earth reference frame (the coordinate system with x-y plane tangential to the ground). You can use quaternion multiplication to do this

v' = q . v . q *

where q is the quaternion and v is the acceleration vector (please look up quaternion-vector multiplication for further details*). Since we know that gravity is the vector u = (0,0,g) in the local Earth reference frame (where g ≈ 9.81 m/s^2, assuming that the IMU measurements are also in m/s^2), we can then subtract this vector from v'

v'' = v' - u

v'' is the gravity compensated/removed vector. However, it is not in the IMU's local body reference frame (remember we rotated it to the local Earth reference frame). So to convert it back, you can multiply it by the inverse quaternion

v''' = q-1 . v . q-1 *

v''' will be the original acceleration measurement minus gravity. To confirm that the result is correct, you can take measurements while the device is stationary. No matter how the device is rotated, the acceleration measurements should be close to (0,0,0).

If the IMU did not provide a quaternion measurement, it could have been computed via the fusion of accelerometer and gyroscope readings (which is what the IMU is doing anyway). If you are interested in this, I suggest looking up Madgwick, Mahony or Extended Kalman Filter algorithms for attitude estimation.

*Helpful resources

Quaternion multiplication: https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm

Rotating a vector with a quaternion (especially the third answer if you'd like to see some code): https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion

来源：https://stackoverflow.com/questions/60492369/gravity-compensation-in-imu-data

标签

c++

quaternions