[Ether]

The code for trapezoidally integrating an acceleration to get distance was given in this thread in an earlier post.

If your acceleration is in a plane (the plane of the floor), use the same concept to get your position in the plane:

Given t, x, y, v_x, v_y, a_x, and a_y at some point in time, and a_xnew a_ynew at some later point in time t_new^*,
compute v_xnew v_ynew x_new and y_new as follows:

dt = t_new - t;

v_xnew = v_x + dt*(a_xnew+a_x)/2;
x_new = x + dt*(v_xnew+v_x)/2;

v_ynew = v_y + dt*(a_ynew+a_y)/2;
y_new = y + dt*(v_ynew+v_y)/2;

... where x,y is the location of the accelerometer in the fixed plane of the floor. Note that you will have to convert your accelerometer signal from the vehicle reference frame to the fixed x,y reference frame of the floor, using the gyro to do the coordinate rotation.

As stated earlier, the errors will accumulate quickly and the computed position will diverge from the true position.


^*Just to be absolutely clear for those who may be new to this: t_new is not one giant step from t. It is a very small integration time step (say 20ms) later than t. The repetition of this calculation over time is known as numerical integration.