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.


1 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!

  • 1
    $\begingroup$ The use of Madgwick algorithm requires none or very low presence of accelerations (or at least small durations of high accelerations). Since OP is using a high powered rocket, the algorithm alone will not work. $\endgroup$
    – brtiberio
    Mar 21, 2019 at 17:49
  • $\begingroup$ @brtiberio - True. I believe the Madgwick filter essentially relies on the fact that the only sustainable acceleration is gravity, so essentially all accelerations are treated as "gravity" and the resulting acceleration/thrust field is treated as "down." I didn't read that part of the question, actually. If this is "high power" as in large hobby-scale rocketry then maybe the burn duration is short enough relative to the filter's rate of convergence, but I would imagine it will still skew the data. If it's very high power (government-grade) then I would think the gyro is much higher quality. $\endgroup$
    – Chuck
    Mar 21, 2019 at 18:58
  • $\begingroup$ @Chuck, There are many different rotation matrices from the Wikipedia link you sent me. How do I know which one to use? I suppose i wont be using the Madgwick approach then. Also thanks for the help. $\endgroup$ Mar 21, 2019 at 19:06
  • $\begingroup$ @BrandonGorman - For IMUs it's generally roll, pitch, yaw angles, which would be processed with the ZYX form. $\endgroup$
    – Chuck
    Mar 21, 2019 at 19:30
  • $\begingroup$ @Chuck, So I implemented the rotation matrix Equation (3.42) from that link. The problem I am having now is I know at what time the rocket was in free fall back to Earth, but when I integrate the new accelerometer data to get velocity everything looks great until the rocket is coming back down in free fall. The integrated velocity values are saying the rocket increased its speed in free fall then decreased, then increased again which made the position data very inaccurate. How do I account for the constant acceleration, so velocity does not keep climbing? $\endgroup$ Mar 21, 2019 at 22:35

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.