# Finding the velocity and position for small distance from IMU sensors

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.

• Maybe try to ask this in Electronics.SE if you don't receive any answers here. You will need the gyroscope part of this sensor in addition to the acceleration data. You then have to calculate your current rotation relative to the earth's gravitation vector and then you have to take it away from your measured acceleration. Since there is no way of measuring speed, position or the current rotation directly, you can only assume that when the total acceleration is 9.81 +/- 2% it is probably still and facing downward, so you can calibrate it that way. – Bastian Springer Jul 19 at 15:38

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.

• Here is the pdf version of the paper you cited from the University of Groningen. – sempaiscuba Aug 1 at 2:17
• Looks good, although I prefer the CiteSeer link (which also has the PDF) because I feel, without empirical evidence, that it is more likely to be operational in 10 or 20 years. – Kenn Sebesta Aug 1 at 13:15
• Fair enough. I've suggested an edit to have the link display the title of the paper - just in case CiteSeer also goes down! – sempaiscuba Aug 1 at 16:31
• That's a great addition, thanks! – Kenn Sebesta Aug 2 at 3:02