Gravity compensation of accelerometer measurements

This solution is only for normal mount of sensor package (see Fig 1)

When we read accelerometer values, we get measurements with Earth gravity. How to remove gravity amount?

Gravity is the vector in direction to center of Earth. Accelerometer values (for Ox, Oy, Oz axis) contains gravity vector's projections (projection, because of tilt of sensor: sensor can have pitch and roll angles not equals to 0!). We need to remove gravity projections to get clear values of acceleration.

Fig 1

 Fig 1 shows normal mount of the sensor and it's axis. First let's remove gravity projection from accelerometer value for Ox axis. When no tilt and roll, pitch angles are 0 - gravity projections on Ox and Oy are 0. But if we have some tilt, then we have also "gravity component" in accelerometer measurement value: Gx. See Fig 2 for Gx definition:

Fig 2
(Oy is "out from eyes")

Projection g_x = g * cos α. Angle p is the pitch angle. α = 180° - 90° - p = 90° - p.

So,




g_x = g * cos (90° - p) = g * sin p.

Ok, let's define gravity component on Oy axis. See Fig 3:

Fig 3
(Ox is "out from eyes")

Projection g_y = g * cos β. Angle r is the roll angle. β = 180° - 90° - r = 90° - r.

So,




g_y = g * cos (90° - r) = g * sin r

Ok, let's define gravity component on vertical, Oz axis.

Projection g_z has some unknown angle γ between Oz and vector of gravity g. And how to define this angle γ ? It's easy: we know the theorem about orths:




cos²p + cos²r + cos²γ = 1


And g_z = g * cos γ. We need to find cos γ. From this theorem: cos²γ = 1 - (cos²p + cos²r).

We know that cos²x = 0.5 * (1 + cos2x) so, cos²γ = 1 - (0.5*(1 + cos2p) + 0.5*(1 + cos2r)) = -0.5 * (cos2r + cos2p).

And now:




cos γ = ± SQRT(-0.5 * (cos2p + cos2r)) = ± 0.70710678118654752440 * SQRT(-(cos2p + cos2r))


So, we will use



g_z = g * 0.70710678118654752440 * SQRT(cos2p + cos2r)
with correction of sign (direction of g_z) by pitch/roll amounts.

These formulas are in g units, not in m/sec²! We suppose |g| = 1. So, you need to multiply by g constant (9.8). But more correct may be to use |g| = sqrt(Acc_x² + Acc_y² + Acc_z²) instead of 1

Here is the example in C:

#define DEG2RAD(D) ((D)*0.01745329251994329576)

typedef struct accv_t {
        double v[3]; // values of 3 axis of accelerometer
} accv_t;

accv_t corr_gravity(double ax, double ay, double az, double pitch, double roll) {
    /* XXX: for normal mount!!! Other not implemented */
    double gx, gy, gz, tmp;

    gx = sin(DEG2RAD(pitch));
    gy = sin(DEG2RAD(roll));
    tmp = cos(DEG2RAD(2*pitch)) + cos(DEG2RAD(2*roll));
    gz = 0.70710678118654752440 * sqrt(fabs(tmp));

    ax += gx;
    ay += gy;
    if (fabs(pitch) > 90.0 || fabs(roll) > 90.0) {
        az -= gz;
    }
    else {
        az += gz;
    }
    return ((accv_t){ .v = {ax, ay, az}});

}

NOTE: condition abs(pitch) > 90° || abs(roll) > 90° => invert g_z.

On real accelerometer values I got:

sensor:
  Ax =  0.287g
  Ay = -0.094g
  Az = -0.928g
corrected:
  Ax = -0.006g
  Ay = 0.006g
  Az = 0.022g

---

See also How to convert accelerometer values to path/coordinates