This article is about handling data from a MEMS IMU. There are used abundantly in alot of consumer devices and as a robotics engineer its important to understand important concepts and handling of such data. Often prone to noise, various filtering techniques are needed to refine this data.
See Madgwick filter for details
Useful Links:
https://stackoverflow.com/questions/60492369/gravity-compensation-in-imu-data
https://stackoverflow.com/questions/18252692/gravity-compensation-in-accelerometer-data
An IMU resting on a surface will have gravity acting on it along the gravitational axis. Gravitational axis is device centric and depends upon what the vertical axis has been defined by IMU’s reference frame ( could be x,y,z). I am assuming that its the z axis so all the calculations and code will be presented keeping this assumption.
In order for any sensor fusion algorithm to be able to use acceleration data we want to compensate for this gravitational effect. The algorithm is quite simple and if applied properly will give zero acceleration vector when the device is stationary no matter what orientation it has been put into. Algorithm :
Get IMU’s orientation ( Quaternion)
Convert quaternion to rotation matrix
The code is highly ROS(Robot Operating System agnostic) but should give an idea
Rotation Matrix layout
{ m[0][0] m[0][1] m[0][2] }
{ m[1][0] m[1][0] m[1][0] }
{ m[2][0] m[2][0] m[2][0] }
(Quaternion should have been normalized)
getX() --> x component of quaternion
getY() --> y component of quaternion
getZ() --> z component of quaternion
getW() --> w component of quaternion
m[0][0] = 1 - 2 * pow(q.getY(), 2) - 2 * pow(q.getZ(), 2);
m[0][1] = 2 * q.getX() * q.getY() - 2 * q.getZ() * q.getW();
m[0][2] = 2 * q.getX() * q.getZ() + 2 * q.getY() * q.getW();
m[1][0] = 2 * q.getX() * q.getY() + 2 * q.getZ() * q.getW();
m[1][1] = 1 - 2 * pow(q.getX(), 2) - 2 * pow(q.getZ(), 2);
m[1][2] = 2 * q.getY() * q.getZ() - 2 * q.getX() * q.getW();
m[2][0] = 2 * q.getX() * q.getZ() - 2 * q.getY() * q.getW();
m[2][1] = 2 * q.getY() * q.getZ() + 2 * q.getX() * q.getW();
m[2][2] = 1 - 2 * pow(q.getX(), 2) - 2 * pow(q.getY(), 2);
Use that rotation matrix to transform gravity vector [0, 0 , -9.8] from inertial to world frame.
Subtract transformed gravity vector from raw acceleration vector from IMU.
a is the gravity vector [0,0,-9.8]
a_x,a_y,a_z --> compensated gravity vector
auto a_x = msg->linear_acceleration.x - a.getX() * m[0][0] + a.getY() * m[1][0] + a.getZ() * m[2][0];
auto a_y = msg->linear_acceleration.y - a.getX() * m[0][1] + a.getY() * m[1][1] + a.getZ() * m[2][1];
auto a_z = msg->linear_acceleration.z - a.getX() * m[0][2] + a.getY() * m[1][2] + a.getZ() * m[2][2];