Go Down

Topic: Combining quaternion orientation with gravity-free acceleration measurements (Read 10132 times) previous topic - next topic

Jeff Rowberg

I have successfully implemented a working motion fusion algorithm (from the Madgwick report) using a SparkFun 6DOF board. The native calculation results in a quaternion representation, but I can easily convert this to Euler angles or yaw/pitch/roll values as well. I also have calculated the gravity vector so that acceleration due to gravity can be removed regardless of the orientation, so that I'm left with acceleration due only to things other than gravity. There is no magnetometer involved, so the "yaw" value (heading) is estimated from the gyroscope, but it's reasonably accurate and good enough for what I need.

There is only one thing that I'm still missing, and I feel like the trig involved to make it happen really shouldn't be that difficult. I just can get my head around it (in large part because I also can't really get my head around much of the IMU filter code and quaternion calculation--I just used and tweaked code from the above linked PDF and a few other places).

I need to combine the orientation info with the measured non-gravity acceleration to get an absolute left/right - forward/backward - up/down acceleration measurement, relative to the initial state.

Does anyone grasp 3D spatial math enough to point me in the right direction here? I promise to release all of this as a clean, documented, plug-and-play 6DOF IMU example sketch as soon as I have it working. My current code, if it's helpful, is available here:

(currently the important bits are in the update_motion() function @ line 136)

I'm SO close...it's just that darn absolute acceleration that's giving me fits.

Jeff Rowberg

In the interest of sharing info in relevant channels, I got this response from a friend on Facebook to whom I asked the same basic question from my original post:

The sensor fusion algorithm will give you a quaternion, representing the orientation of the IMU with respect to the starting position. It's important to note that you have three 3D frames here. The sensor frame, the starting frame and the monitor frame. If you read the accelerometer data you will obtain a 3D vector expressed in the sensor frame but you actually need the accelerations in the starting frame so that you can integrate it and compute displacements. You can obtain it using quaternion multiplications as:

^E{\boldsymbol{a}} = {^S_E q_e} \otimes {^S{\boldsymbol{a}}} \otimes {^S_E q_e^*}

(latex syntax: copy and paste the above to http://www.codecogs.com/latex/eqneditor.php)

...where ^S_E q_e is the sensor fusion output, ^S_E q_e^* is it's conjugate, \otimes represents quaternion multiplication and \boldsymbol{a} represent the quaternion extension of a which is:

^S\boldsymbol{a} = 0 + \boldsymbol{i} \,^Sa_x + \boldsymbol{j} \,^Sa_y + \boldsymbol{k} \,^Sa_z

Still working on the final solution, but I'm closer now, and maybe this will help someone here conceptually.


Wow, 10 references within the first 3 lines. That is some serious stuff. Downloaded and saved for future reference. Seems like it's a lot of math, something that requires me to sit down and work on it a couple hours at a time. Maybe I can offer some help.

Off topic: what's up with inmojo? They seem to have stopped making any updates, blogs tweats. Hope they keep the website running. We're both make some sales on it :)
Serial LCD keypad panel,phi_prompt user interface library,SDI-12 USB Adapter

Jeff Rowberg

Wow, 10 references within the first 3 lines. That is some serious stuff.

It is definitely some serious stuff. I had 26 semester units of math in college (and got an AA in Mathematics on my way to a BS in Computer Science), but there's still a whole lot that goes over my head because it's been too long. The most beneficial part for me is at the end, where there are working sample implementations of both the 6DOF IMU filter and the 9DOF MARG filter. Excellent code, that.

On a different note, I think I finally solved the problem here, thanks to a lot of experimenting, searching, and direct help from others. It's too complicated to explain in detail here, so I'll summarize the steps necessary to pull it off:

1. Obtain orientation of device and store in quaternion (accomplished by the IMU filter).
2. Calculate estimated gravity vector from quaternion.
3. Remove acceleration due to gravity from raw accelerometer x/y/z measurements.
4. Store gravity-free acceleration as a quaternion where w (vector rotation) is zero.
5. Multiply orientation quaternion [w, x, y, z] by gravity-free acceleration quaternion [0, accX, accY, accZ].
6. Multiply previous result quaternion by the conjugate of the acceleration quaternion [w, -x, -y, -z].
7. Final result quaternion now contains [0, realX, realY, realZ] acceleration in the starting (non-rotated) frame of reference.

It appears to be working correctly. A bit more visualization testing and I'll know for sure, but the math matches what I've read and been told, and the raw numbers appear to be at least close to what they should be, which is always a good sign.

Final IMU example SparkFun 6DOF-friendly Arduino sketch to come.

And yeah, InMojo has been great lately, except they do seem a bit quiet on the PR front. I don't pay too much attention to that though, honestly--not out of apathy, but out of busy-ness. :-)

Go Up