# 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.

Drones and Model Aircraft Asked by TheLazy on August 19, 2021

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.

Answered by Kenn Sebesta on August 19, 2021

## Related Questions

### Will this set up fry my components?

2  Asked on August 19, 2021

### What is a good 3” BNF that can be used indoors and outdoors?

1  Asked on August 19, 2021

### How to control a gimbal remotely?

2  Asked on August 19, 2021

### FPV lag on visuo xs812

1  Asked on August 19, 2021

### Is WiFi drone control protocol standardized?

1  Asked on August 19, 2021 by patrykk

### Motor size and Kv advice for ZD550?

1  Asked on August 19, 2021

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

1  Asked on August 19, 2021 by thelazy

### What are the steps to connect the Pixhawk to Auvidea J120 board using a telemetry cable?

0  Asked on August 19, 2021 by desert_ranger

### Can motors be “too big” for any reason?

2  Asked on August 19, 2021

### Is Naze32 capable of autopilot?

1  Asked on August 19, 2021

### Can “toy” drone FC firmware be tweaked?

2  Asked on August 19, 2021 by galaxy

### F450 frame all legs are not stable

1  Asked on August 19, 2021 by muyustan

### How do aerodynamics for small model aircraft differ from that of larger aircraft?

1  Asked on December 8, 2020 by flux

### Will connecting my Mavic Pro to a an iPad Litchi app try to update my Mavic Pro firmware?

1  Asked on August 25, 2020 by cliffclof

### Transmit video from drone to laptop via WiFi

2  Asked on August 15, 2020 by user1876484