How to convert accelerometer values to path/coordinates

To get path or coordinates from accelerometer values you need to integrates them. From physics we know:

v = dr/dt
a = dv/dt = d²r/dt²

If a (acceleration) is constant, motion is called uniform accelerated:

v(t)=v(0)+at
r(t)=r(0)+v(0)t+0.5*at²

v(0), r(0) - initial velocity and displacement. If a != const, these formulas can not be used. If we know how a depends on t (for ex., a(t) = b + c*t), we can use:

v(t) = v(0) + b*t + 1/2*c*t²
r(t) = r(0) + v(0)*t + 1/2*b*t² + 1/6*c*t³

But if we don't know a(t), we can only use common rules:

          t
r(t)=r(0)+∫v(t)dt
          0

          t
v(t)=v(0)+∫a(t)dt
          0

and calculate this with some numerical integration method
Examples (models with plots) of such methods (in SciLab) you can found there [in integral.sci file: Simpson's rule, Trapezoid rule, Leo Tick's rule]. But we should know about 2 problems:

1. If there is some constant "component" in acceleration (or noise), this will be reason of BIG error in result
2. Each numerical integration method is filter and has transfer function, which give distortions on 0.2 - 0.4 Nyquist's frequency even [see R. W. Hamming, Digitals filters, 1977]

So, you can use more smart methods. One of them is complex (chaining) integration with filters. Idea is to filter each dataflow: between each integrator and to filter output result too:

a(t)->FILT->af(t)->INT->v(t)->FILT->vf(t)->INT->x(t)->FILT->xf(t)

   where FILT - is a filter, INT - is an integrator.

It can be implemented on dataflow model (as I done it in hmc-demo). You can found example of such model in SciLab source file "path.sci" where you can combain different integration methods with different filters and see result in plots.
And remember, eventually, you never get correct result. Use sensors of velocity instead.

High-pass filter

In one of my posts I wrote about low-pass IIR filter (of 1^st order). Here is the simple high-pass filter in form of IIR of 1^st order. See more info here.
In my application HMC demo I poll sensor (accelerometer) and filter Ax signal:

HPFilter hpf
hpf configure -datatype flt -datatypes {phys}
Saver saver1
saver1 listen -on
saver listen -on
listen phvals:hpf
listen hpf:saver1
serial open 4
sensor poll -loop

This will create 2 CSV files with native and with filtered signals (after saving we rename them). We can render signals with GLE tool:

begin graph
    title "Accelerations"
    xtitle "Time [sec]"
    ytitle "Ax [g]"
    axis grid
    ticks color grey40
    subticks on
    subticks lstyle 2
    set font "texcmr"

    d1 file "c:/tmp/saver.csv,0,5"
    let d2 = d1
    d1 line color red
    d2 deresolve 10 average line color red lwidth 0.1

    d3 file "c:/tmp/saver1.csv,0,5"
    let d4 = d3
    d3 line color blue
    d4 deresolve 10 average line color blue lwidth 0.1
end graph

begin key
    set fill yellow
    line color red text "Native"
    line color blue text "Filtered"
end key

and get this image:

 Blue line is signal after high-pass filtering: it shows effect of removing slow drift.
 Removing of slow drift is used in position estimation of sensor (from acceleration) in multicascading scheme:

HPF -> INTEGR -> HPF -> INTEGR -> HPF

Here is the C code (for SWIG) of this kind of filter:

double* filter(double *v) {
        int i;
        int vlen;

        vlen = (int)v[0];
        if (vlen != $self->n) {
            return (NULL);
        }

        for (i=1; i <= vlen; i++) {
            $self->v[i] = $self->a * ($self->v[i] + v[i] - $self->x[i]);
            $self->x[i] = v[i];
        }
        return ($self->v);
    }

$self is the pointer to CHPFilter struct:

typedef struct CHPFilter {
        int n; // length of v
        double a; // alpha
        double *v; // calculated values (keep prev. before filter)
        double *x; // prev. input
} CHPFilter;

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