Gravity Compensation in Accelerometer Data

Question

Given an Accelerometer with 9 DOF (Accelerometer, Gyroscope and Magnetometer) I want to remove/compensate the effect of the gravity in accelerometer reading (Accelerometer can rotate freely). The sensor gives the orientation in quaternion representation relative to a (magnetic)north, west and up reference frame.

I found this http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus but couldn't understand the basis for the given equation.

How could I achieve this given above information?

Answer 1

I assume your accelerometer reading is in sensor body frame. First we need to represent accelerometer data with respect to inertial frame, and then subtract gravity. If you are directly using Euler angles rather than quaternion, then you need to compute rotation matrix

R = [
    ctheta*cpsi,
    -cphi*spsi + sphi*stheta*cpsi,
    sphi*spsi + cphi*stheta*cpsi;
    ctheta*spsi, cphi*cpsi + sphi*stheta*spsi,
    -sphi*cpsi + cphi*stheta*spsi;
    -stheta, sphi*ctheta, cphi*ctheta
]

(It's given with MATLAB notation). Here phi stands for roll angle, theta for pitch, and psi for yaw. This R matrix is from body to inertial frame. I think in flight dynamics it's also known as transpose of Direction Cosine Matrix (DCM).

When you apply matrix multiplication operation, now you need to subtract gravity from z direction in order to eliminate static acceleration, i.e., gravity.

Answer 2

You need to rotate the accelerometer reading by the quaternion into the Earth frame of reference (into the coordinate system of the room if you like), then subtract gravity. The remaining acceleration is the acceleration of the sensor in the Earth frame of reference often referred to as linear acceleration or user acceleration.

In pseudo-code, something like this

acceleration = [ax, ay, ay]  // accelerometer reading

q                            // quaternion corresponding to the orientation

gravity = [0, 0, -9.81]      // gravity on Earth in m/s^2

a_rotated = rotate(acceleration, q) // rotate the measured acceleration into
                                    // the Earth frame of reference

user_acceleration = a_rotated - gravity

You say that you can get q through the API. The only nontrivial step is to implement the rotate() function.

To compute the image of a vector v when rotated by q, the following formula should be applied: v_rotated = qvq^-1. To compute it with floating point numbers, you need to work out the formulas yourself; they are available at Using quaternion rotations.

As far as I can tell, the link you provided does exactly this, you see the expanded formulas there and now you know where they came from. Also, the linked content seems to measure gravity in g, that is, gravity is [0,0,-1].

Watch out for sign conventions (whether you consider gravity [0,0,-1] or [0,0,1]) and handedness of your coordinate systems!