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

Low-pass filter

One of the low-pass filters software algorithm is described on http://en.wikipedia.org/wiki/Low-pass_filter. Here is the example of Python implementation:

# Low-pass filter
import random

RC = 0.05 # time constant (inertion)
DT = 0.02 # sampling time

A = DT/( RC + DT)
#A = 0.45
xk_1 = 0
print "Measured;Filtered"
for i in xrange(0, 200):
    xm = random.randint(0, 2)
    # Both are equals:
    #xk = A*xm + (1-A)*xk_1
    xk = xk_1 + A * (xm - xk_1)
    print "%d;%f" % (xm, xk)
    xk_1 = xk

With RC (tau) we can control inertia of the filter

RC=0.5

RC=0.05

Here is the example how looks low-pass filter with tilt angles (F-16 seems to be more stable):

HMC6343 demo 1 from bapcyk on Vimeo.

If you try to filtering tilt angles then you can see funny effect: turning of 3D model! It's happens due to error in sensor data which filter try to smooth, see image:

Near the scale edges error seems to be small, but angles may be, for ex., 10°, 350°, and filter will smooth their to 10°, 20°, 30°... 340°. And you can see rotation of the model on the screen.
One of the approach to avoid this problem is to normalize angles. For example, 10°, 350° may be converted to 10°, -10°. After such normalization angles great then 360° should be normalized to canonical range. Algorithm is easy:

absdiff = abs(v1 - v0)
if absdiff > 180.:
  d = 360. - absdiff
  if v0 > v1:
    v1 = v0 + d
  else:
    v1 = v0 - d

And after filtering to avoid values great then 360° we can use something like this:

if abs(v) >= 360.:
  v = v % 360.