Simple gravity compensation for 9 DOM IMUs

An accelerometer is subject to dynamic (or external) and static (gravity) accelerations (see my thesis, chapter 5.1). This means that whenever you are interested in measuring dynamic accelerations rather than gravity you have to compensate for gravity.

I implemented a very simple approach for doing so, assuming that you have a 9 degrees of measurement inertial measurement unit as my FreeIMU, from which you are able to compute its orientation quaternion representing the orientation of the Earth frame with respect to the Sensor frame and that you have the readings coming from the accelerometer.

This is the code, implemented in Python:

# compensate the accelerometer readings from gravity. 
# @param q the quaternion representing the orientation of a 9DOM MARG sensor array
# @param acc the readings coming from an accelerometer expressed in g
#
# @return a 3d vector representing dinamic acceleration expressed in g
def gravity_compensate(q, acc):
  g = [0.0, 0.0, 0.0]
  
  # get expected direction of gravity
  g[0] = 2 * (q[1] * q[3] - q[0] * q[2])
  g[1] = 2 * (q[0] * q[1] + q[2] * q[3])
  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]
  
  # compensate accelerometer readings with the expected direction of gravity
  return [acc[0] - g[0], acc[1] - g[1], acc[2] - g[2]]

The idea behind this is pretty simple. With the quaternion we can compute the expected direction of gravity and then subtract that from the accelerometer readings. The result is the dynamic acceleration.

I'm sure that there could be a better solution but this is just the result of a 10 minutes coding and seems working pretty good.