Remove gravity from raw accelerometer data of IMU--> please approve math and algo
I am using this device (http://www.sparkfun.com/products/10724) and have successfully implemented an quite well working orientation estimation based on a fusion of magnetometer, accelerometer and gyroscope data based on this http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms implementation. Now I want to calculate the dynamic acceleration (measures acceleration without static gravity acceleration). For doing this I came to the following idea.
Calculate a running average of the raw accelerometer data. If the raw acceleration is stable for some time (small difference between running average and current measured raw data) we assume the device does not move and we are measuring the raw gravity. Now save the gravity vector and also current orientation as quaternion. This approach assumes that our device could not be accelerated constantly without gravity.
For calculating the acceleration without gravity I am now doing following quaternion calculation:
RA = Quaternion with current x,y,z raw acceleration values
GA = Quaternion with x,y,z raw acceleration values of estimated gravity
CO = Quaternion of current orientation
GO = saved gravity orientation
DQ = GO^-1 * CO // difference of orientation between last gravity estimation and current orientation
DQ = DQ^-1 // get the inverse of the difference
SQ = DQ * GA * DQ^1 // rotate gravity vector
DA = RA - SQ // get the dynamic acceleration by subtracting the rotated gravity from the raw acceleration
Could someone check if this is correct? I am not sure because on testing it I get some high acceleration on rotating my sensor board, but I am able to get some acceleration data (but is is much smaller than the accelration during rotation) if the device is moved without rotating it.
Moreover I have the question if the accelerometer is also measuring acceleration if it is rotated on place or not!
Answer
Another way is to differentiate accel to give jerk (using finite difference, j = (a2 - a1) / dt). Run the jerk through a decay/leakage function (use a half life decay calc value rather than a simple multiplier). Then integrate the jerk (trapezoidal rule, a = dt * (j1 + j2) * 0.5) and it will remove the DC offset (gravity). Again run this signal through a decay function.
The decay functions avoid the value spiraling off but will reduce the magnitude of dynamic acceleration values that you see and will introduce some shaping to the signal. So you won't get values that are 'accurate' m/s/s readings any longer. But it is useful for short-time movements.
Of course you could just use a highpass filter instead but that generally requires a fixed sampling rate and is probably more computationally expensive if you are using convolution (finite impulse response filter).