One must wade through algebra and complex numbers to get to quaternions.
Very true.
That's a nice reference link. Still that is only the entrance to the rabbit hole of quaternions. It presents all the useful equations needed to manipulate quaternions in various ways but does not tell you where they came from.
I suspect that if you really want to go down that rabbit hole and understand quaternions you probably need some serious books. After all this is actually rocket science. Even if the guys actually building rockets in the 1960's did not know it. And it did take Hamilton ten years to devise.
I notice Sebastian Madgwick has the first of those books propping up a monitor in one of his videos
Don't dismiss the YouTube links, having someone like Bryan Duggan introduce the subject and tell you why you may be interested in it, what you can do with it and what you don't need to know is very helpful. Unlike a simple list of equations in a reference. Don't think of it as a YouTube video, think of it as sitting in on a lecture at DIT. Wonderful.
For myself, I'll be happy if I can just use quaternions. The struggle to understand what goes on under the hood may be too much.
There similar API's in many other languages and platforms.
No mention of complex numbers there. Such knowledge is not required to make use of what is available.
It's a bit harder in the embedded world, like the Propeller where such code may not be offered up on a plate and it probably helps to know something of the internals in order to get it running on your machine.
@Heater
I do understand the desire to have something working right now.. rather than making a long preamble of study.
We all learn differently. Some want to get something working right away and then learn what it does; others want to learn in steps where they accumulate background knowledge along the way and see the significance of the device while they are getting the device working.
And of course, you may view the challenge being to get a program up and running quickly as you are primarily a software person. A maths person might desire to savor the maths. I guess I am more of the latter.
=========
Having said all that, I have tested the devices I got from Hoverfly and they are working well. One is installed on the balancing bot, and the other is in plastic box that I can link to a PC via USB and wave about in the air.
So I guess I am ready to build a few software modules without fully understanding the maths.
Let's agree that the format is C in GCC via Simple IDE. (That was pretty much a tacit assumption as Madgwick code is in C).
Module A - The Raw data to Quaterion output (The Magwick Black Box)
We already have Raw data provided to USB serial, but it bounces around and runs past faster than one can see.
So the goal is to output 115,200 8N1 baud of Quaternion data and to observe if that is more stable.
{This might also spawn locating or developing a PC graphics program that converst the Quaterion output to animated display fo a floating box.}
Module B - Attach to Module A, a conversion of Madgwick Quaternion data to an appropriate scheme for motor control and output also via serial for capture and verification. (I still am trying to sort out if Euler Angles and Directional Cosine Matrix are the same thing or alternative solutions.)
Possible Module C - Combine the A and B and add motor control.
After those are done and verified good. I can look at getting the dual H-bridge responding to the Pitch axis, and ignore the Yaw and Roll. That should make a balancing robot viable... as a stationary balancing bot.
Possible Module D - Add IR Remote control sensor to the bot and use a small NEC 32bit IR remote ( a cheap Arduino device) to remotely control the ability of the robot to wander.
Possible Module E - Add your Raspberry Pi to provide more sophisticate wandering and avoidance features. (This seems to be a huge jump and yet very appropriate for the SOC's video abilities. More sensors can be attached via GPIO.
Above I listed a series of Modules as an approach to getting some real working code. Heater has introduced a lot of code that applies to the current status of Madjwick. And I think there is a general acceptance of the idea that Madjwick is optimal for the new 9DoF sensors.
What is missing here is a link to Propeller DCM, an effort that already accomplished a great deal with the 6DoF sensors and Propeller specific issues to optimizing the maths by possibly using fixed point, rather than floating point decimal.
And so I am back-tracking a little rather than pushing ahead independently.
Jason Dorie provided a substantial set of resources in two zip files.
One is his actual Propeller DCM code.
The second is his DCMvisualizer that is in C#. This provides a graphic demo on one's PC.
At this time, I am trying to sort out what to do with C# in Linux. Apparently there is an open-source application called Mono that might help to compile this for a Linux machine. That is an added bit of distraction for me. But the pay-off is likely to be handsome, as the HoverflyGimbal could then be demonstrated with Madjwick in graphic real-time via the USB serial on a PC.
I think the new learner runs into a series of obstacles (aside from learning to compile newer version of C) that are typical to development, and aside from learning the how and why of Quaternion use.
A. First a raw sensor data output. This was resolved by code provided by Hoverfly.
B. Getting the raw sensor data from a 9DoF device into a use-able form for control of Translation and Rotation of anything.
--- Quaternions are very handy and seem to be the way of the future as they do avoid slowness in calculations that the notorious 'Gimbal Lock' problem of Euler Angles (and maybe other trig based maths). But at some point, we need to get useful values on for servo motors in some other format THAT likely provide data for individual X, Y, and Z axis movements. One might arrive at dead ends or be tossed back into the Gimbal Lock problem by just converting for the sake of using someone else's somewhat successful code.
C. Translation and Rotation have their own math realm in the Translation Matrix. So far, I am unsure if the code has to go from Quaternions into a Translation Matrix and then into PID Motor Control and finally H-bridge or PWM output. Or if there is another simpler route.
Aside from this is the pro and con of an all C solution in floating-point versus a SPIN+PASM synthesis in either F32 or fixed-point. Personally I am intrigued by the Log and Trig Log tables that are resident in the Propeller -- but unsure of how useful they might be.
My gut feelings are to either choose an all C Floating-Point as a first attempt or to adapt what Jason Dorie has done with 6DoFto a 9DoF solution by replacement of its front ends. At this time, I think BOTH are valid approaches and need exploring.
But I am going to work on the side issue of getting Jason Dorie's DCM Visualizer compiled for my Debian Linux. It seems to be a helpful adjunct. I suspect it will be a helpful preamble to understanding is PropellerDCM and/or developing an alternative 9DoF DCM code.
More YouTube show and tell here. But I think you will like this one...
If you want to get into the theory of quaternions you will want watch a lecture series by Professor Norman Wildberger "The rotation problem and Hamilton's discovery of quaternions".
The first parts are a basic introduction to complex numbers, rotations, vectors and such which I thought would be a boring run through high school maths. Boy was I surprised. This is something else.
You see Wildberger refuses to use irrational numbers in his derivations. He does not want anything like the square root of two showing up in his maths. This is a bit radical and I first thought he must be some kind of crack pot.
The result is that he explains complex numbers, rotations and vectors, in 2D and 3D, in a way I have never seen before. There are no angles or sines and cosines etc. anywhere to be seen. Quite amazing.
All in all it makes a much easier explanation than the usual approach to complex numbers and such. And he claims it makes moving on to quaternions much more straight forward.
That remains to be seen as my brain shut down by the time I got to part three where the real quaternion stuff happens....
OMG, Four hours of YouTube. Maybe I am just old-fashioned, but I read the tutorial on Quaternions, learned quite a bit about gaps in my understanding of Imaginary numbers, Ordered pairs, the Rotator function, how matrices apply, and the utility of Quaternions.
I did all this in less that 2 hours. I just can't seem to grasp the need to seek a video presentation. I may be a bit old-school about this. it may work for others. But I tend to feel that I am being pushed into a Black Hole where time is consumed at exponential rates. (I have long disliked that financial news has switched over to video presentation from text. One just cannot survey the content of as much material as one can in text.)
I am sure some will appreciate the YouTube presentations. I just have my personal preferences about how I use the computer to learn. When in university, I got A grades on all my math exams by just studying the texts and not attending classes, and the same with my Statics and Strengths of Materials course. The only problem is that the Math Department refused to award me an A grade for lack of attendance, I had to settle for B+ as they felt I had to listen to a lecture and confused students muddle through Q & A.
Thinking back, I should have gone to class and slept. That is what I did in the Statics and Strengths class and got the A, while my classmates kept telling me I was going to flunk the course.
Maths are some the best written material for self-study. But I must say that having studied Chinese really helps with noticing how all the details of notation apply in a text where context might be shifting.
++++++++
BTW, there appears to me useful new information, so I just might have to watch the Four Hours of YouTube.
This morning I converted Madgwick to JavaScript, it now runs in the browser. I also knocked up a quick 3D viewer with webgl that displays a "brick" that is oriented according to the Madgwick filter's output quaternion.
The input to the filter is set to some fixed values of gyro, accelerometer and magnetometer with a bit of noise added to all of them.
From it's initial starting position it works out that the "brick" is upside down and rolls it over. Of course due to the noise input the way it actually rolls is different every time you reload the page.
You will notice that the brick quivers when it reaches it's resting position. The amount of shake can be reduced by decreasing the filters gain, look for betaDef at the top of MadgwickAHRS.js. Trouble is if you reduce the gain to reduce the shake the response gets slower.
You can play with the gyro, accel and magnetometer input values in the doTest function at the end of scene.js
Nothing to install, just unzip the attached file and point your browser at file:///where/ever/you/unzipeed/it/MadgwickAHRS_webgl/index.html
What I'd like to do is add a serial input to this and run it as a chrome app. Then you could plug your real live data into it.
What is the format of the serial data stream you get from your IMU?
Any chance of saving a minute or so of that data to a file and posting it here? I'd like to see how it looks on this brick.
Edit: Updated zip with new version that behaves nicely. Now has simulated magnetometer and accel rotations input to the filter so the yellow brick now rotates around the vertical axis whilst pitch and rolling.
Edit: Now runs a Chrome extension. Just unpack the zip somewhere and use "Load unpacked extension" in the Chrome extensions page. Then hit the "Launch button".
I was trying to tweak around with the accel and magnetometer inputs to Madgwick_webgl and get the "brick" to pitch, yaw and otherwise rotate. Remember the brick is oriented according to the filter output.
Try as I might I could not get the thing to move in the axis I wanted. Yaw, pitch and roll just did not anything sensible.
I tried swapping the axes around and changing signs of inputs, no good.
But now I think I have it. I think Madgwick has the numbers in the quaternion output in a different order to what the THREE.js 3D library expects. Certainly a different order than I have seen elsewhere.
A quaternion boils down to 4 numbers that represent a rotation. Three of those numbers are a vector indicating the direction if an axis about which something will rotate. The fourth number is an angle, the amount of rotation around the given axis.
Well normally this is expressed as (axis, angle) where axis is a vector so we have (vx, vy, vx, phi). And this just gets thrown around as the quaternion (q0, q1, q2, q3).
It seems to me that Madwick does it backwards (angle, axis) or (phi, vx, vy, vx). Which means his output (q0, q1, q2, q3) have to be fed into THREE.js as quaternion.set(q1, q2, q3, q0). Or at least that is what I am guessing from a paper Seb Madgwick has on quaternions where he has it backwards.
Grrr...
Anyway having done that my brick pitches and yaws just as I expect it to.
I now feed in a magnetometer vector that rotates around once every few seconds and some violent jiggles on the accelerometer's x and y axes (z is 1.0 for gravity). The brick behaves very well now, rotating and jiggling as expected.
I have updated the zip above with the new version.
I don't mean to intentionally ignore your progress, but last night I slipped and fell while walking the dog in rain. It seems as though I will be flat on my back for a few days in order to recover. I don't think I broke anything. Maybe bruised a spinal disk and related nerves. It is not even painful if I stay off my feet. But it does make it hard to keep up with all you have been posting. Tiger Balm seems to help out quite a bit.
Who knows, I may be back up to speed faster than expected.
Yes, I wish you had an IMU as well. I ordered a very cheap one from DealExtreme, but the delivery is slow if you elect Free Shipping.
Ouch, take care with that. Tiger Balm is great stuff.
No hurry here. I just remembered that I have a 3D accelerometer on my STM32 F4 Discovery board. If I can get serial output from that into my Madgwick_webgl viewer I can make some proper real-world testing. I can fake the magnetometer input for now.
Quite appropriate really, JavaScript at both ends, I could end up with the worlds first JavaScript controlled balancing bot!
Of course this is all heresy. What I really want is a Propeller controlled machine.
...but what will happen when JavaScript stops to garbage collect its heap? :-)
Bot falls over of course
On the other hand. Have you run Madgwick_webgl in your browser yet? I don't see any stuttering going on here.
Thing is, if you statically allocate all your objects at start up and never drop any references to them then the is no big garbage collection work going on. Basic real-time programming style as used by C++ programmers. Not that I have taken any trouble to do that here.
Of more concern is that I'm not sure an STM32 Discovery has the horse power to run the Madgwick algorithm at any reasonable speed. Perhaps a Raspi could do it though.
P.S. This thing now runs as chrome app. You can install it as a Chrome browser extension by going to the Tools->Extensions in the menu system and hitting the "Load unpacked extension" button. Then select the directory that you unpack the thing into and then hit the "launch" button.
Just now I'm trying to figure out how to get the serial port working there.
I don't mean to intentionally ignore your progress, but last night I slipped and fell while walking the dog in rain. It seems as though I will be flat on my back for a few days in order to recover. I don't think I broke anything. Maybe bruised a spinal disk and related nerves. It is not even painful if I stay off my feet. But it does make it hard to keep up with all you have been posting. Tiger Balm seems to help out quite a bit.
Who knows, I may be back up to speed faster than expected.
Yes, I wish you had an IMU as well. I ordered a very cheap one from DealExtreme, but the delivery is slow if you elect Free Shipping.
Thanks,
I am on the mend. I got up and out today -- walked the dog, picked up clean laundry, had a latte, and even rode the motor scooter a bit. I just have to be careful for a week or two and stay off my feet as much as possible.
Please don't stop making progress with the HoverFlyGimbal. I need to catch up on reading and viewing YouTube links. It seems that Ratronic has gotten the HoverFlyGimbal working with modification of Jason Dorie's code and his DCMViewer. I am sure anyone that bought one will want to try it.
I just remembered that I have a 3D accelerometer on my STM32 F4 Discovery board. If I can get serial output from that into my Madgwick_webgl viewer I can make some proper real-world testing. I can fake the magnetometer input for now.
Okay, I guess we have to go back to the beginning. I too have a 3D accelerometer, but it is supposedly not enough. You need a 3D gyro as well, but they tend to drift -- so you need both and a bit of code to blend the two. That was why the Kalman filter became popular.
That worked pretty well, except the accelerometer couldn't work on Yaw as measurement we pretty much wandering nonsense on that axis. In came combining input on the X, Y, Z from the earth's magnetic field and everything pretty much came together with the Madjwick filter.
I got excited when I realized that 9DoH sensors are now cheaper than my original 2 axis Memsic accelerometer that I got for my Toddler with the BS2.... long, long ago.
In other words, times have changed. Either get a HoverflyGIMBAL for a complete Propeller solution or by a cheapo 9DOH device and use a Propeller you already have. I don't think I will ever buy gyros, accelerometers, or compasses in separate components again. It just costs more and ends up with a less compact solution. And it even gets more tiresome if some are 5V and others are 3.3V. (The nicest new stuff can work with both.)
Yes indeed. Back to the beginning. What actually is the problem we are trying to solve with this? Basically we want to know which way up we are. Where are we pointing? Technically what is our orientation in 3D space.
A three axis accelerometer can do some of this. When you are stationary the only acceleration acting on you is gravity. So gx, gy, gz are a vector telling where down is (or up depending how you want to look at it). So we have up/down but no left/right.
Also an accelerometer is OK whilst you are stationary. As soon as you accelerate in the x, y, or z directions that changes gx, gy, gz and you can no longer tell what is gravity and what is other motion. Oh dear.
A three axis gyro can do all of this. Gyros measure rate of change of angle so if we know our orientation when we start we only have to integrate gx, gy and gz to get our orientation at any time in the future.
That's OK for a while but gyros drift and all that integration is going to accumulate errors, very soon we are lost again. Oh dear.
A three axis magnetometer can do some of this. At least the Earth's magnetic field gives us a fixed reference direction. Say due North. So we know which way North is but we don't know if we are tilting over some how.
Enter the Madgwick algorithm. Madgwick mashes accel, gyro and compass altogether and somehow produces a result that gives our 3D orientation. Ahh, magic!
I do hear that Kalman can do this as well. But from what little I know about Kalman it is not really the tool for the job and it takes a lot more processing power and it has a lot of parameters to tweak to get it to work nicely. Eeew.
Just now I can get good results out of Madgwick simulations using only three axis accelerometer and two axis compass inputs. Provided you are not rolling, pitching, and yawing too much (less than 45 degrees) and any translational acceleration is quite small to the Earths g. So for a balancing bot this may be quite sufficient. We will only find out by building it.
The FREEIMU device is nice. I had considered it with the 10DOF, but felt that I wasn't going to be flying anything soon that required a barometer. You can get a 9DOF for $10.00 USD.
Kalman and Madgwick are both doing essentially the same thing. From a high level, they're taking measurements and estimates and fusing them together. I believe the DCM is also referred to as a complimentary filter.
It boils down to this: Gyros can give you very precise readings in the short term. If you take your gyro values and sum them continuously (integration) you'll have a pretty accurate estimate of the changes in your orientation over time, however the longer you go without comparing against some sort of "real world" reference, the farther off you'll drift.
Accelerometers are very susceptible to noise, but averaged over time, their readings can give you a pretty decent measurement of the direction of gravity. Both the DCM and Kalman filters do basically the same thing - they use the gyro measurements for the lion's share of the work, and use a long-term average of the accelerometer readings to slowly pull the orientation estimated by the gyros into line with the real world measurement.
The quaternion and DCM solutions are approximately the same in terms of computational complexity - they just get their results using different math. Loopy, you are right in your statement that quaternions aren't subject to gimbal lock, but then neither are matrices, really. The gimbal lock issues occur when converting from a quaternion or a matrix back to angular measurements you can use to drive your motors. If you were tilted 179 degrees to the left, and tilted 2 more degrees, any quaternion-to-euler or matrix-to-euler conversion is going to return -179 degrees as the new result, not +181 degrees. Your code is going to have to be smart enough to recognize that you crossed that 'sign flip' threshold and behave accordingly.
Heater - Your point about Kalman vs Madgwick is pretty accurate. Most of the Kalman filter code that you'll find is 1D only, so people using them in quad rotors typically use 3 independent 1D filters. The problem with that is that the gyro readings are in the space of your object (quad), not the world. If you read the gyro's Y rotation value when you are level, it corresponds to your change in heading. If you are tilted 90 degrees to the right, the Y-axis of the craft is now aligned with the X-axis of the world, so it's not "heading" any more. The DCM code (or quaternion IMU) handles this by rotating the readings from the gyro into the current "space" of the craft, using the orientation estimate. A proper 3D Kalman filter does the same thing, but it's mathematically more complex.
The DCM code only gives you an accurate estimate of your orientation. What you do with it from there is still a bit tricky. :-)
The DCM code only gives you an accurate estimate of your orientation. What you do with it from there is still a bit tricky. :-)
Thanks, I have pretty much gotten clear of Euler Angles versus Quaternions. And for a two-wheeled balancing bot, I strongly suspect that converting the Pitch axis to Euler Angle for just the one will suffice. Even balancing in 2-dimensions might find Euler Angle conversion acceptable just because the actual range of motions is not going to be anywhere near 90 degrees ... so Gimbal Lock may be avoiding.
On the other hand, aerobatic flight or automatic recovery from sudden turbulence may need Quaternions and full 3-dimensional data. It also seems that this is very useful for the original purpose -- a camera gimbals that stays locked on one direction.
I really need to study Jason Dorie's code that has been revised for the HoverFlyGimal. It seems the most direct way of understanding what is involved. But understanding DCM seems to mean Directional Cosine Matrix, a third maths topic aside from the Euler Angles and the Quaternions. DIY Drones has a paper on this, maybe a bit rough... but worth reading.
The DCM code is probably harder to understand than the paper itself, in some ways, because it's been optimized and is using fixed point math. The basics of the DCM code are that it maintains a 3x3 matrix, where each row (or column) of the matrix represents one of the three basis vectors, X, Y, or Z, in the current orientation. If you have not applied any rotation to your craft, the matrix will be the identity matrix:
1 0 0
0 1 0
0 0 1
In this case, if you read the vectors as either rows or columns, they are identical. X is (1, 0, 0), which represents a unit vector whose sole non-zero value is in the X axis. Y and Z are similar. Maintaining the estimate of the current orientation is done by repeatedly doing a few steps:
- Take the current matrix and rotate it by the current gyro values
- Compute the difference between the current acceleration vector and the Y vector of the matrix, apply a small portion of that difference as a rotation
- Orthoganalize the matrix (make sure all vectors are perpendicular to each other)
- Normalize the matrix (make sure all vectors have unit length)
To add heading from a compass, you'd have to insert the following before the last two steps above:
- Figure out a vector for "north" using the compass
- Compute the difference between this north vector and your matrix Z axis vector
- Apply a portion of that difference to the matrix as a rotation
I'll take a crack at porting my current 28-bit version of the DCM code to the Hoverfly Gimbal board.
Edit: Hilarious - I just looked at the sample code provided with the board. The SensorDriver.spin code appears to be heavily based on my QuadX ITG3200.spin code. The "ITGComputeDrift" function being the dead giveaway.
@Jason Dorie
I appreciate your participation and patience. This is not a simple topic to just use Google and read up on. So your tips and comments are quite helpful.
I did locate the Direction Cosine entry in Google. And the short story is the Quaternion notation provides angles which need to be converted into cosines to be of any use for DCM. So that is a jump to different maths and different way of following rotation.
There are several papers on the Direction Cosine Matrix. At this point, I am not going to try to assert any favorite, though I suspect that there is always somebody that writes better than most.
My thinking at this point is just to assume the following for a quad-copter:
Raw data from 9DoF device ==> use Madjwick filter to convert to stable Quaternion output ==> import and convert to Direction Cosine Matrix for navigation and rotation control ==> output to quad-copter four motor speed control.
Of course, the Direction Cosine Matrix will have to take input from an operator via an R/C radio set up.
I haven't done enough reading to know what the Madgwick filter is doing, but I suspect that the DCM code and the Madgwick filter are doing the same thing, just that one is implemented using a rotation matrix, while the other is implemented with quaternions. It will most likely be necessary to implement one, but not both.
@Jason Dorie
I will risk a big jump in concept here. It seems that much of the Quad-copter community have accepted a Direction Cosine Matrix as the core of an inertial guidance.
In the back of my mind, I have been thinking an all Quaternion approach might be more efficient. Here is a link to a paper that addresses that idea. Please feel free to point out any obvious flaws.
I still think that putting a Madjwick filter front end on you Propeller code is a valid alternative and likely an easier step for most to achieve the upgrade to a 9DoF interial measurement unit..
Obviously an all Quaternion solution pretty much means removing the DCM and providing new code. At this point, it is just food for thought. I am not asking anyone to actually take on the task.
When it is loaded up hit "Build->Build all" or the build icon in the tool bar or just the F7 key.
Of course that won't run as it tries to use a Windows COMx port. Edit the file MainForm.cs and change the serial port that hard coded at line 87 to whatever port you are using.
comm = new SerialPort("/dev/ttyUSBxx", 115200, Parity.None, 8, StopBits.One);
Then build again and hit the run gear icon and it should run fine.
I've gotten my 28 bit DCM solution working on the HoverFly Gimbal board. It's attached.
The DCM code has a bunch of chunks in comment blocks. Remove the ' in front of the { to uncomment a block of code. One the blocks computes angles from the DCM matrix suitable for flight leveling using ASIN and prints the values to the serial port. Another block just sends raw binary gyro/accel values to the visualizer, and a third block sends the first 6 components of the DCM matrix computed on the Prop. Only one of these should be uncommented at any given time or the loop won't run fast enough.
I've also included an updated version of the DCM Visualizer code / executable. It draws using anti-aliasing, and only requires the first 6 values of the matrix (it computes the other 3) so the communication load is lighter, allowing the DCM to run faster. You'll need change the code to use your specific COM port for it to work. I should probably just make it auto-detect, but haven't done that yet.
I'd like to create a JS version of the DCM algorithm to put into my browser visualization. By way of comparison with Madgwick. Can I assume the C code you linked to previously is a good place to start?
Can you change the DCM Visualizer to prompt for a COM port, or use a command-line parameter to specify it? If I need to rebuild it for Windows where would I get the compiler/linker to do that?
Comments
That's a nice reference link. Still that is only the entrance to the rabbit hole of quaternions. It presents all the useful equations needed to manipulate quaternions in various ways but does not tell you where they came from.
I suspect that if you really want to go down that rabbit hole and understand quaternions you probably need some serious books. After all this is actually rocket science. Even if the guys actually building rockets in the 1960's did not know it. And it did take Hamilton ten years to devise.
Like: Quaternions-Rotation-Sequences-Applications-Aerospace
http://www.amazon.com/Quaternions-Rotation-Sequences-Applications-Aerospace/dp/0691102988/ref=sr_1_1?ie=UTF8&qid=1411029838&sr=8-1&keywords=quaternion
or Rigid-Body-Dynamics-For-Beginners:
http://www.amazon.com/Rigid-Body-Dynamics-For-Beginners/dp/1493598201/ref=sr_1_2?ie=UTF8&qid=1411029838&sr=8-2&keywords=quaternion
or A-Primer-Quaternions-Classic-Reprint
http://www.amazon.com/A-Primer-Quaternions-Classic-Reprint/dp/B008IXEU02/ref=sr_1_3?ie=UTF8&qid=1411029838&sr=8-3&keywords=quaternion
I notice Sebastian Madgwick has the first of those books propping up a monitor in one of his videos
Don't dismiss the YouTube links, having someone like Bryan Duggan introduce the subject and tell you why you may be interested in it, what you can do with it and what you don't need to know is very helpful. Unlike a simple list of equations in a reference. Don't think of it as a YouTube video, think of it as sitting in on a lecture at DIT. Wonderful.
For myself, I'll be happy if I can just use quaternions. The struggle to understand what goes on under the hood may be too much.
The maths is often Vector products (dot product and cross product).
I do agree that it is interesting to look into the maths and see how these things work. For sure I cannot help myself from being curious.
On the other hand if you want to get something done now one might just use the code that is available and skip wondering about how it works.
For example this API from a JavaScript 3d graphics package is all you need to move and draw stuff in the right place:
http://threejs.org/docs/#Reference/Math/Quaternion
There similar API's in many other languages and platforms.
No mention of complex numbers there. Such knowledge is not required to make use of what is available.
It's a bit harder in the embedded world, like the Propeller where such code may not be offered up on a plate and it probably helps to know something of the internals in order to get it running on your machine.
I do understand the desire to have something working right now.. rather than making a long preamble of study.
We all learn differently. Some want to get something working right away and then learn what it does; others want to learn in steps where they accumulate background knowledge along the way and see the significance of the device while they are getting the device working.
And of course, you may view the challenge being to get a program up and running quickly as you are primarily a software person. A maths person might desire to savor the maths. I guess I am more of the latter.
=========
Having said all that, I have tested the devices I got from Hoverfly and they are working well. One is installed on the balancing bot, and the other is in plastic box that I can link to a PC via USB and wave about in the air.
So I guess I am ready to build a few software modules without fully understanding the maths.
Let's agree that the format is C in GCC via Simple IDE. (That was pretty much a tacit assumption as Madgwick code is in C).
Module A - The Raw data to Quaterion output (The Magwick Black Box)
We already have Raw data provided to USB serial, but it bounces around and runs past faster than one can see.
So the goal is to output 115,200 8N1 baud of Quaternion data and to observe if that is more stable.
{This might also spawn locating or developing a PC graphics program that converst the Quaterion output to animated display fo a floating box.}
Module B - Attach to Module A, a conversion of Madgwick Quaternion data to an appropriate scheme for motor control and output also via serial for capture and verification. (I still am trying to sort out if Euler Angles and Directional Cosine Matrix are the same thing or alternative solutions.)
Possible Module C - Combine the A and B and add motor control.
After those are done and verified good. I can look at getting the dual H-bridge responding to the Pitch axis, and ignore the Yaw and Roll. That should make a balancing robot viable... as a stationary balancing bot.
Possible Module D - Add IR Remote control sensor to the bot and use a small NEC 32bit IR remote ( a cheap Arduino device) to remotely control the ability of the robot to wander.
Possible Module E - Add your Raspberry Pi to provide more sophisticate wandering and avoidance features. (This seems to be a huge jump and yet very appropriate for the SOC's video abilities. More sensors can be attached via GPIO.
Above I listed a series of Modules as an approach to getting some real working code. Heater has introduced a lot of code that applies to the current status of Madjwick. And I think there is a general acceptance of the idea that Madjwick is optimal for the new 9DoF sensors.
What is missing here is a link to Propeller DCM, an effort that already accomplished a great deal with the 6DoF sensors and Propeller specific issues to optimizing the maths by possibly using fixed point, rather than floating point decimal.
And so I am back-tracking a little rather than pushing ahead independently.
http://forums.parallax.com/showthread.php/131022-Propeller-DCM-Now-with-source
Jason Dorie provided a substantial set of resources in two zip files.
One is his actual Propeller DCM code.
The second is his DCMvisualizer that is in C#. This provides a graphic demo on one's PC.
At this time, I am trying to sort out what to do with C# in Linux. Apparently there is an open-source application called Mono that might help to compile this for a Linux machine. That is an added bit of distraction for me. But the pay-off is likely to be handsome, as the HoverflyGimbal could then be demonstrated with Madjwick in graphic real-time via the USB serial on a PC.
I think the new learner runs into a series of obstacles (aside from learning to compile newer version of C) that are typical to development, and aside from learning the how and why of Quaternion use.
A. First a raw sensor data output. This was resolved by code provided by Hoverfly.
B. Getting the raw sensor data from a 9DoF device into a use-able form for control of Translation and Rotation of anything.
--- Quaternions are very handy and seem to be the way of the future as they do avoid slowness in calculations that the notorious 'Gimbal Lock' problem of Euler Angles (and maybe other trig based maths). But at some point, we need to get useful values on for servo motors in some other format THAT likely provide data for individual X, Y, and Z axis movements. One might arrive at dead ends or be tossed back into the Gimbal Lock problem by just converting for the sake of using someone else's somewhat successful code.
C. Translation and Rotation have their own math realm in the Translation Matrix. So far, I am unsure if the code has to go from Quaternions into a Translation Matrix and then into PID Motor Control and finally H-bridge or PWM output. Or if there is another simpler route.
Aside from this is the pro and con of an all C solution in floating-point versus a SPIN+PASM synthesis in either F32 or fixed-point. Personally I am intrigued by the Log and Trig Log tables that are resident in the Propeller -- but unsure of how useful they might be.
My gut feelings are to either choose an all C Floating-Point as a first attempt or to adapt what Jason Dorie has done with 6DoFto a 9DoF solution by replacement of its front ends. At this time, I think BOTH are valid approaches and need exploring.
But I am going to work on the side issue of getting Jason Dorie's DCM Visualizer compiled for my Debian Linux. It seems to be a helpful adjunct. I suspect it will be a helpful preamble to understanding is PropellerDCM and/or developing an alternative 9DoF DCM code.
More YouTube show and tell here. But I think you will like this one...
If you want to get into the theory of quaternions you will want watch a lecture series by Professor Norman Wildberger "The rotation problem and Hamilton's discovery of quaternions".
https://www.youtube.com/watch?v=uRKZnFAR7yw
There are actually four parts of one hour each.
The first parts are a basic introduction to complex numbers, rotations, vectors and such which I thought would be a boring run through high school maths. Boy was I surprised. This is something else.
You see Wildberger refuses to use irrational numbers in his derivations. He does not want anything like the square root of two showing up in his maths. This is a bit radical and I first thought he must be some kind of crack pot.
The result is that he explains complex numbers, rotations and vectors, in 2D and 3D, in a way I have never seen before. There are no angles or sines and cosines etc. anywhere to be seen. Quite amazing.
All in all it makes a much easier explanation than the usual approach to complex numbers and such. And he claims it makes moving on to quaternions much more straight forward.
That remains to be seen as my brain shut down by the time I got to part three where the real quaternion stuff happens....
I did all this in less that 2 hours. I just can't seem to grasp the need to seek a video presentation. I may be a bit old-school about this. it may work for others. But I tend to feel that I am being pushed into a Black Hole where time is consumed at exponential rates. (I have long disliked that financial news has switched over to video presentation from text. One just cannot survey the content of as much material as one can in text.)
I am sure some will appreciate the YouTube presentations. I just have my personal preferences about how I use the computer to learn. When in university, I got A grades on all my math exams by just studying the texts and not attending classes, and the same with my Statics and Strengths of Materials course. The only problem is that the Math Department refused to award me an A grade for lack of attendance, I had to settle for B+ as they felt I had to listen to a lecture and confused students muddle through Q & A.
Thinking back, I should have gone to class and slept. That is what I did in the Statics and Strengths class and got the A, while my classmates kept telling me I was going to flunk the course.
Maths are some the best written material for self-study. But I must say that having studied Chinese really helps with noticing how all the details of notation apply in a text where context might be shifting.
++++++++
BTW, there appears to me useful new information, so I just might have to watch the Four Hours of YouTube.
Are you up for a few minutes of software testing?
This morning I converted Madgwick to JavaScript, it now runs in the browser. I also knocked up a quick 3D viewer with webgl that displays a "brick" that is oriented according to the Madgwick filter's output quaternion.
The input to the filter is set to some fixed values of gyro, accelerometer and magnetometer with a bit of noise added to all of them.
From it's initial starting position it works out that the "brick" is upside down and rolls it over. Of course due to the noise input the way it actually rolls is different every time you reload the page.
You will notice that the brick quivers when it reaches it's resting position. The amount of shake can be reduced by decreasing the filters gain, look for betaDef at the top of MadgwickAHRS.js. Trouble is if you reduce the gain to reduce the shake the response gets slower.
You can play with the gyro, accel and magnetometer input values in the doTest function at the end of scene.js
Nothing to install, just unzip the attached file and point your browser at file:///where/ever/you/unzipeed/it/MadgwickAHRS_webgl/index.html
What I'd like to do is add a serial input to this and run it as a chrome app. Then you could plug your real live data into it.
What is the format of the serial data stream you get from your IMU?
Any chance of saving a minute or so of that data to a file and posting it here? I'd like to see how it looks on this brick.
Edit: Updated zip with new version that behaves nicely. Now has simulated magnetometer and accel rotations input to the filter so the yellow brick now rotates around the vertical axis whilst pitch and rolling.
Edit: Now runs a Chrome extension. Just unpack the zip somewhere and use "Load unpacked extension" in the Chrome extensions page. Then hit the "Launch button".
I was trying to tweak around with the accel and magnetometer inputs to Madgwick_webgl and get the "brick" to pitch, yaw and otherwise rotate. Remember the brick is oriented according to the filter output.
Try as I might I could not get the thing to move in the axis I wanted. Yaw, pitch and roll just did not anything sensible.
I tried swapping the axes around and changing signs of inputs, no good.
But now I think I have it. I think Madgwick has the numbers in the quaternion output in a different order to what the THREE.js 3D library expects. Certainly a different order than I have seen elsewhere.
A quaternion boils down to 4 numbers that represent a rotation. Three of those numbers are a vector indicating the direction if an axis about which something will rotate. The fourth number is an angle, the amount of rotation around the given axis.
Well normally this is expressed as (axis, angle) where axis is a vector so we have (vx, vy, vx, phi). And this just gets thrown around as the quaternion (q0, q1, q2, q3).
It seems to me that Madwick does it backwards (angle, axis) or (phi, vx, vy, vx). Which means his output (q0, q1, q2, q3) have to be fed into THREE.js as quaternion.set(q1, q2, q3, q0). Or at least that is what I am guessing from a paper Seb Madgwick has on quaternions where he has it backwards.
Grrr...
Anyway having done that my brick pitches and yaws just as I expect it to.
I now feed in a magnetometer vector that rotates around once every few seconds and some violent jiggles on the accelerometer's x and y axes (z is 1.0 for gravity). The brick behaves very well now, rotating and jiggling as expected.
I have updated the zip above with the new version.
Now I wish I had an IMU here....
I have to take a bit of time out.
I don't mean to intentionally ignore your progress, but last night I slipped and fell while walking the dog in rain. It seems as though I will be flat on my back for a few days in order to recover. I don't think I broke anything. Maybe bruised a spinal disk and related nerves. It is not even painful if I stay off my feet. But it does make it hard to keep up with all you have been posting. Tiger Balm seems to help out quite a bit.
Who knows, I may be back up to speed faster than expected.
Yes, I wish you had an IMU as well. I ordered a very cheap one from DealExtreme, but the delivery is slow if you elect Free Shipping.
Ouch, take care with that. Tiger Balm is great stuff.
No hurry here. I just remembered that I have a 3D accelerometer on my STM32 F4 Discovery board. If I can get serial output from that into my Madgwick_webgl viewer I can make some proper real-world testing. I can fake the magnetometer input for now.
Quite appropriate really, JavaScript at both ends, I could end up with the worlds first JavaScript controlled balancing bot!
Of course this is all heresy. What I really want is a Propeller controlled machine.
Bot falls over of course
On the other hand. Have you run Madgwick_webgl in your browser yet? I don't see any stuttering going on here.
Thing is, if you statically allocate all your objects at start up and never drop any references to them then the is no big garbage collection work going on. Basic real-time programming style as used by C++ programmers. Not that I have taken any trouble to do that here.
Of more concern is that I'm not sure an STM32 Discovery has the horse power to run the Madgwick algorithm at any reasonable speed. Perhaps a Raspi could do it though.
P.S. This thing now runs as chrome app. You can install it as a Chrome browser extension by going to the Tools->Extensions in the menu system and hitting the "Load unpacked extension" button. Then select the directory that you unpack the thing into and then hit the "launch" button.
Just now I'm trying to figure out how to get the serial port working there.
New zip is attached in post #39 here.
Now all I need is a source of accel, gyro, and compass data...and then...
Twas bryllyg, and ye slythy toves
Did gyre and gymble in ye wabe:
All mimsy were ye borogoves;
And ye mome raths outgrabe.
George,
Hope you get better soon!
I am on the mend. I got up and out today -- walked the dog, picked up clean laundry, had a latte, and even rode the motor scooter a bit. I just have to be careful for a week or two and stay off my feet as much as possible.
Please don't stop making progress with the HoverFlyGimbal. I need to catch up on reading and viewing YouTube links. It seems that Ratronic has gotten the HoverFlyGimbal working with modification of Jason Dorie's code and his DCMViewer. I am sure anyone that bought one will want to try it.
Okay, I guess we have to go back to the beginning. I too have a 3D accelerometer, but it is supposedly not enough. You need a 3D gyro as well, but they tend to drift -- so you need both and a bit of code to blend the two. That was why the Kalman filter became popular.
That worked pretty well, except the accelerometer couldn't work on Yaw as measurement we pretty much wandering nonsense on that axis. In came combining input on the X, Y, Z from the earth's magnetic field and everything pretty much came together with the Madjwick filter.
I got excited when I realized that 9DoH sensors are now cheaper than my original 2 axis Memsic accelerometer that I got for my Toddler with the BS2.... long, long ago.
In other words, times have changed. Either get a HoverflyGIMBAL for a complete Propeller solution or by a cheapo 9DOH device and use a Propeller you already have. I don't think I will ever buy gyros, accelerometers, or compasses in separate components again. It just costs more and ends up with a less compact solution. And it even gets more tiresome if some are 5V and others are 3.3V. (The nicest new stuff can work with both.)
Yes indeed. Back to the beginning. What actually is the problem we are trying to solve with this? Basically we want to know which way up we are. Where are we pointing? Technically what is our orientation in 3D space.
A three axis accelerometer can do some of this. When you are stationary the only acceleration acting on you is gravity. So gx, gy, gz are a vector telling where down is (or up depending how you want to look at it). So we have up/down but no left/right.
Also an accelerometer is OK whilst you are stationary. As soon as you accelerate in the x, y, or z directions that changes gx, gy, gz and you can no longer tell what is gravity and what is other motion. Oh dear.
A three axis gyro can do all of this. Gyros measure rate of change of angle so if we know our orientation when we start we only have to integrate gx, gy and gz to get our orientation at any time in the future.
That's OK for a while but gyros drift and all that integration is going to accumulate errors, very soon we are lost again. Oh dear.
A three axis magnetometer can do some of this. At least the Earth's magnetic field gives us a fixed reference direction. Say due North. So we know which way North is but we don't know if we are tilting over some how.
Enter the Madgwick algorithm. Madgwick mashes accel, gyro and compass altogether and somehow produces a result that gives our 3D orientation. Ahh, magic!
I do hear that Kalman can do this as well. But from what little I know about Kalman it is not really the tool for the job and it takes a lot more processing power and it has a lot of parameters to tweak to get it to work nicely. Eeew.
Just now I can get good results out of Madgwick simulations using only three axis accelerometer and two axis compass inputs. Provided you are not rolling, pitching, and yawing too much (less than 45 degrees) and any translational acceleration is quite small to the Earths g. So for a balancing bot this may be quite sufficient. We will only find out by building it.
So hang on to your accelerometer.
My plan is to get a FreeIMU unit https://viacopter.eu/multirotor-shop/sensors/freeimu-0-4-3-multicopter-store. Comes with a barometer as well!
It boils down to this: Gyros can give you very precise readings in the short term. If you take your gyro values and sum them continuously (integration) you'll have a pretty accurate estimate of the changes in your orientation over time, however the longer you go without comparing against some sort of "real world" reference, the farther off you'll drift.
Accelerometers are very susceptible to noise, but averaged over time, their readings can give you a pretty decent measurement of the direction of gravity. Both the DCM and Kalman filters do basically the same thing - they use the gyro measurements for the lion's share of the work, and use a long-term average of the accelerometer readings to slowly pull the orientation estimated by the gyros into line with the real world measurement.
The quaternion and DCM solutions are approximately the same in terms of computational complexity - they just get their results using different math. Loopy, you are right in your statement that quaternions aren't subject to gimbal lock, but then neither are matrices, really. The gimbal lock issues occur when converting from a quaternion or a matrix back to angular measurements you can use to drive your motors. If you were tilted 179 degrees to the left, and tilted 2 more degrees, any quaternion-to-euler or matrix-to-euler conversion is going to return -179 degrees as the new result, not +181 degrees. Your code is going to have to be smart enough to recognize that you crossed that 'sign flip' threshold and behave accordingly.
Heater - Your point about Kalman vs Madgwick is pretty accurate. Most of the Kalman filter code that you'll find is 1D only, so people using them in quad rotors typically use 3 independent 1D filters. The problem with that is that the gyro readings are in the space of your object (quad), not the world. If you read the gyro's Y rotation value when you are level, it corresponds to your change in heading. If you are tilted 90 degrees to the right, the Y-axis of the craft is now aligned with the X-axis of the world, so it's not "heading" any more. The DCM code (or quaternion IMU) handles this by rotating the readings from the gyro into the current "space" of the craft, using the orientation estimate. A proper 3D Kalman filter does the same thing, but it's mathematically more complex.
The DCM code only gives you an accurate estimate of your orientation. What you do with it from there is still a bit tricky. :-)
Thanks, I have pretty much gotten clear of Euler Angles versus Quaternions. And for a two-wheeled balancing bot, I strongly suspect that converting the Pitch axis to Euler Angle for just the one will suffice. Even balancing in 2-dimensions might find Euler Angle conversion acceptable just because the actual range of motions is not going to be anywhere near 90 degrees ... so Gimbal Lock may be avoiding.
On the other hand, aerobatic flight or automatic recovery from sudden turbulence may need Quaternions and full 3-dimensional data. It also seems that this is very useful for the original purpose -- a camera gimbals that stays locked on one direction.
I really need to study Jason Dorie's code that has been revised for the HoverFlyGimal. It seems the most direct way of understanding what is involved. But understanding DCM seems to mean Directional Cosine Matrix, a third maths topic aside from the Euler Angles and the Quaternions. DIY Drones has a paper on this, maybe a bit rough... but worth reading.
In this case, if you read the vectors as either rows or columns, they are identical. X is (1, 0, 0), which represents a unit vector whose sole non-zero value is in the X axis. Y and Z are similar. Maintaining the estimate of the current orientation is done by repeatedly doing a few steps:
- Take the current matrix and rotate it by the current gyro values
- Compute the difference between the current acceleration vector and the Y vector of the matrix, apply a small portion of that difference as a rotation
- Orthoganalize the matrix (make sure all vectors are perpendicular to each other)
- Normalize the matrix (make sure all vectors have unit length)
To add heading from a compass, you'd have to insert the following before the last two steps above:
- Figure out a vector for "north" using the compass
- Compute the difference between this north vector and your matrix Z axis vector
- Apply a portion of that difference to the matrix as a rotation
I'll take a crack at porting my current 28-bit version of the DCM code to the Hoverfly Gimbal board.
Edit: Hilarious - I just looked at the sample code provided with the board. The SensorDriver.spin code appears to be heavily based on my QuadX ITG3200.spin code. The "ITGComputeDrift" function being the dead giveaway.
I appreciate your participation and patience. This is not a simple topic to just use Google and read up on. So your tips and comments are quite helpful.
I did locate the Direction Cosine entry in Google. And the short story is the Quaternion notation provides angles which need to be converted into cosines to be of any use for DCM. So that is a jump to different maths and different way of following rotation.
There are several papers on the Direction Cosine Matrix. At this point, I am not going to try to assert any favorite, though I suspect that there is always somebody that writes better than most.
My thinking at this point is just to assume the following for a quad-copter:
Raw data from 9DoF device ==> use Madjwick filter to convert to stable Quaternion output ==> import and convert to Direction Cosine Matrix for navigation and rotation control ==> output to quad-copter four motor speed control.
Of course, the Direction Cosine Matrix will have to take input from an operator via an R/C radio set up.
http://gentlenav.googlecode.com/files/DCMDraft2.pdf
http://www.starlino.com/dcm_tutorial.html
I haven't done enough reading to know what the Madgwick filter is doing, but I suspect that the DCM code and the Madgwick filter are doing the same thing, just that one is implemented using a rotation matrix, while the other is implemented with quaternions. It will most likely be necessary to implement one, but not both.
I will risk a big jump in concept here. It seems that much of the Quad-copter community have accepted a Direction Cosine Matrix as the core of an inertial guidance.
In the back of my mind, I have been thinking an all Quaternion approach might be more efficient. Here is a link to a paper that addresses that idea. Please feel free to point out any obvious flaws.
www.tu-berlin.de/.../Quaternions.pdf
I still think that putting a Madjwick filter front end on you Propeller code is a valid alternative and likely an easier step for most to achieve the upgrade to a 9DoF interial measurement unit..
Obviously an all Quaternion solution pretty much means removing the DCM and providing new code. At this point, it is just food for thought. I am not asking anyone to actually take on the task.
Your link above is busted. Managed to find the document here: http://www.tu-berlin.de/fileadmin/fg169/miscellaneous/Quaternions.pdf though.
Jason,
Thanks for all the tips and pointers. Interesting stuff.
How did you get on with compiling DCMVisualizer on Debian? I just managed to do it: When it is loaded up hit "Build->Build all" or the build icon in the tool bar or just the F7 key.
Of course that won't run as it tries to use a Windows COMx port. Edit the file MainForm.cs and change the serial port that hard coded at line 87 to whatever port you are using.
comm = new SerialPort("/dev/ttyUSBxx", 115200, Parity.None, 8, StopBits.One);Then build again and hit the run gear icon and it should run fine.
The DCM code has a bunch of chunks in comment blocks. Remove the ' in front of the { to uncomment a block of code. One the blocks computes angles from the DCM matrix suitable for flight leveling using ASIN and prints the values to the serial port. Another block just sends raw binary gyro/accel values to the visualizer, and a third block sends the first 6 components of the DCM matrix computed on the Prop. Only one of these should be uncommented at any given time or the loop won't run fast enough.
I've also included an updated version of the DCM Visualizer code / executable. It draws using anti-aliasing, and only requires the first 6 values of the matrix (it computes the other 3) so the communication load is lighter, allowing the DCM to run faster. You'll need change the code to use your specific COM port for it to work. I should probably just make it auto-detect, but haven't done that yet.
This is cool.
I'd like to create a JS version of the DCM algorithm to put into my browser visualization. By way of comparison with Madgwick. Can I assume the C code you linked to previously is a good place to start?