Finding the velocity and position for small distance from IMU sensors

Question

I am working on finding Velocity and Displacement from an IMU sensor (MPU9250). I have a Navio2 RPi4 setup, which gives me raw data (acceleration, Gyro, Magnetic).

As per my research this can be done in following method;

1. Finding the Attitude from the raw data. (Using quaternion method or Transformation Matrix)

2. Once the attitude data is available, calculate the gravity force acting on the acceleration data and eliminate the gravity from Acceleration to find the Linear Acceleration

3. Integrate the computed Linear Acceleration to find the velocity and Position.

Following the steps above I was able to compute the Attitude from the IMU sensor by following the example provided by Navio2. The example uses Quaternion method (Mahony filter) to fuse the raw data and give the quaternions.

The output is verified and works perfectly. Now I have to move to step 2 i.e, finding the gravity acting on the acceleration.

Using quaternions to find the gravity :

gx = (2 * (q1 * q3 - q0 * q2));
gy = (2 * (q0 * q1 + q2 * q3));
gz = ((q0 * q0) - (q1 * q1) - (q2 * q2) + (q3 * q3));

This gravity values are subtracted from the acceleration data to find the linear acceleration. Once the linear acceleration is available, integrating it with the time gives the velocity and position. The implementation is experimented by moving the IMU sensor to 100cm. However the position output is not as expected. The units of the raw data have been taken care just to have a sanity check with the implementation.

I have moved to convert the quaternions back to rotation matrix and compute the velocity and position data.

My question is how to find the gravity acting on the acceleration using the Rotation Matrix converted from Quaternion.

I request the community to help me with their valuable inputs and references.

Thanks.

Answer 1

This is the best damned reference on rotations: Representing Attitude: Euler Angles, Unit Quaternions, and Rotation by James Diebel of Stanford University (dated 2006-10-20).

You didn't specify, but you're almost certainly using the Tait-Bryan convention-- which is first yaw, then pitch, then roll-- for rotation axes. You're also possibly using NED for translation axes, but that's less clear.

You can see all the mappings in Sec. 5.6.1 in the above link. You might check that you have correctly mapped your quaternions to the rotation matrix. It's very easy to mess up a sign value. Pay special attention to the fact that the inverse of a rotation matrix is its transpose, so it's very easy to have the right rotation matrix values but in the transposed order.

To get to your specific question, using eqn. 35, you can see how to map an inertial frame acceleration into the body frame, using the rotation matrix. It's a simple matrix multiplication, and in your case, and assuming NED, the vector z is [0;0;9.805].

Lastly, I would suggest getting this to work in simulation first, before trying empirical. Rotations are tricky because they're so pedantic, and it's easy to approach them backwards.