0

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.

Brandon Gorman
  • 3
  • 1
  • 3

1 Answers1

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!

Chuck
  • 16,061
  • 2
  • 18
  • 47
  • 1
    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. – brtiberio Mar 21 '19 at 17:49
  • @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. – Chuck Mar 21 '19 at 18:58
  • @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. – Brandon Gorman Mar 21 '19 at 19:06
  • @BrandonGorman - For IMUs it's generally roll, pitch, yaw angles, which would be processed with the ZYX form. – Chuck Mar 21 '19 at 19:30
  • @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? – Brandon Gorman Mar 21 '19 at 22:35
  • @BrandonGorman - Gravity is an acceleration, so it would make sense to me that you would see speed increasing. I would imagine it would increase until either impact or terminal velocity. Terminal velocity would depend on a number of factors, but the biggest thing I could imagine is if the rocket is tumbling as it falls. As the rocket lays parallel to the ground the speed would slow, and then if it points at the ground the speed would increase, etc. What is the rotation doing during descent? Is it a fixed orientation and speed is varying or is it tumbling? – Chuck Mar 22 '19 at 13:11
  • @Chuck What happened during this launch is the tether became unattached to the back half of the rocket and came tumbling down to Earth mostly about its z-axis. Then once it hit the ground it slid a couple hundred meters on it side through some fields. After the free fall back down the acceleration goes from about 10m/s^2 to around 2 m/s^2 and the velocity just keeps going up from there from about 50m/s to 140m/s when the velocity should have been slowing down. Shouldn't the end velocity be close to zero? It seems like I need some way of telling the velocity to decrease as the acceleration does – Brandon Gorman Mar 22 '19 at 16:05
  • @BrandonGorman - Yes, the stopping velocity should be zero, but the sum of all accelerations should theoretically also give you that value. Are you removing the gravity vector before doing the numeric integration? The IMU at rest on a table would probably read ~g, or 9.81, but clearly it's not moving down. Similarly, if your accelerometer were reading zero, it would be because it's in free-fall; if you subtract 9.81 from that then you get that the device is actually accelerating -9.81 m/s^2, which would reflect how it's actually moving. Without any data though it's all kind of a guess. – Chuck Mar 22 '19 at 16:17
  • @Chuck I was told by the person who gave me this data that if I distributed It become a problem so I'm sorry that I cant give out the data. When I take the magnitude of the acceleration I subtract gravity out of it but I don't understand how to remove the gravity vector when its in all 3 dimensions at once. For example my data might read something like: Ax: 6.316, Ay: -11.82, Az: 1.841 m/s^2. So to answer your question I am not removing the gravity vector before numerical integration I don't see how. – Brandon Gorman Mar 22 '19 at 16:30
  • @Chuck I might not have said this before but I am taking the Ax and Ay to get Vx and Vy and then posX and posY estimates. I already have accurate height data so I don't need posZ estimates. I am trying to compensate for the 9.81 m/s^2 being broken up into all 3 dimensions. I don't understand how to remove the gravity contribution from each dimension based on the gyroscope readings – Brandon Gorman Mar 22 '19 at 16:39
  • @BrandonGorman - When you transform the accelerometer readings to the world frame you can just subtract 9.81 from the z-axis (assuming you're using +z as up). Double integrate the remainder and you should be left with the acceleration that's moving the rocket. Again, if it's on the launch pad then I would expect the transformed reading to be 9.81 exactly on the up axis. On takeoff, you'll see some value larger than 9.81 on the up axis; subtract the 9.81 and the remainder should be what's actually pushing the rocket up. This should correspond well with your existing height measurements. – Chuck Mar 22 '19 at 20:09
  • @Chuck the problem I am having is the gravity vector is not always in the z-direction. Before drogue the gravity vector is in the x direction and during decent the gravity vector is in all 3-dimension not just the z. The sum of the gravity vector is 9.81 but it is broken up into the x, y, & z dimensions on its way down. Then when it touched down the gravity vector was in the x-axis since the rocket slid for a couple hundred meters. How does subtracting 9.81 from only the z-axis account for the other dimension is what I am not getting? I tried that and the velocity goes way up to 1400m/s. – Brandon Gorman Mar 22 '19 at 21:05
  • @BrandonGorman - I'd like to move these comments to chat, but I don't get that option until there have been some threshold number of comments. I'm not suggesting you subtract 9.81 from the Z-axis output of the IMU, I'm suggesting you transform the IMU output to world coordinates first. Gravity then is 9.81 (or -9.81, depending on the sensor) on the z-axis, if the z-axis is up/down for you. Remove the gravity signal and then double integrate what's left and that should give you the rocket's motion in the world frame. – Chuck Mar 25 '19 at 15:09
  • @BrandonGorman - If possible, just start a new question. Restate the issue there, post your C++ code and it'll give everyone here on the site a chance to chip in :) – Chuck Mar 25 '19 at 18:41