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;
Finding the Attitude from the raw data. (Using quaternion method or Transformation Matrix)
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
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.