How to transform raw accelerometer data into the Earth fixed frame to determine position

Question

I have data from a TeleMega and do not understand how to apply the rotation matrix for the accelerometer data so that I can get the more accurate position estimate from my accelerometer. My data comes from a high powered rocket if that helps. I don't understand quaternions so that wont be much help to me unless you can show me how to run the madgwick c++ code. I have raw accelerometer, magnetometer, and gyroscope data stored into arrays but I have no idea how to incorporate that into the madgwick code into c++. I am trying to follow this link but I am stuck on how to apply step 4. I am a decent coder in c++. I have integrated the gyroscope data already but don't understand what to do with it. please help. what is the algorithm to get position (Linear displacement and linear velocity ) using IMU (Like MPU6050)?

I wrote some code already that uses a Kalman filter to smooth the accelerometer data and then I applied numerical integration to obtain velocity and position and then implemented a 3D graph of the trajectory of the rocket but it was not what I was expecting which is why I am asking this question.

Answer 1

There are several questions here, so I'll try to address them all.

First, using the integrated gyro output. Integrating the gyro output, which provides angular velocity, gives you an orientation estimate. A lot of people that use the term "Euler Angles" generally actually mean Tait-Bryan angles. Proper Euler angles only use two axes, like an X-Z-X rotation, where the Tait-Bryan angles use all three axes, like an X-Y-Z rotation. Aviation/navigation angles like Roll-Pitch-Yaw (RPY) are Tait-Bryan angles.

If your question is how to convert those angles to a rotation matrix, then the answer is on the Wikipedia page for the Euler Angles under the "Rotation Matrix" section.

Step 4 in my other answer then uses that rotation matrix to transform the accelerometer outputs. It's done with a matrix multiplication:

$$ \left[\begin{matrix} \ddot{x}_w \\ \ddot{y}_w \\ \ddot{z}_w \\ \end{matrix}\right] = \left[R\right] \left[\begin{matrix} \ddot{x}_b \\ \ddot{y}_b \\ \ddot{z}_b \\ \end{matrix}\right] $$

The Eigen library would be a great starting place for getting the tools to easily do matrix math. You could do it all by hand, but you're reinventing the wheel and liable to make some math error that's going to cost you a lot of time.

The big alternative here is, as I mention in the other answer and you mention here, the Madgwick filter!. The C implementation available there (warning: direct download) has orientation in quaternion form, with q0, q1, q2, and q3 as global, and then there are two functions. The first is MadgwcikAHRSupdate, which uses gyro inputs (gx, gy, gz), accelerometer inputs (ax, ay, az), and magnetometer inputs (mx, my, mz).

If you don't have a magnetometer, use the second function, MadgwickAHRSupdateIMU. It only uses gyro and accelerometer outputs.

Be sure to update the sample frequency; the default is 512 Hz.

The Madgwick filter will do a better job of estimating orientation than just integration of the gyro output because the Madgwick filter uses the gravity vector from the accelerometer readings to eliminate orientation drift.

Once you have the output of the Madgwick filter, which again is the orientation in quaternion, you again convert that to a rotation matrix, transform your accelerometer readings, and then double integrate to get position. The conversion from quaternion to rotation matrix is also on Wikipedia.

Give these a shot and let us know how it goes! Best of luck!