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?
The DCMVisualizer zip file contains a full C# implementation of the DCM code in fixed point. It shouldn't be hard to convert to using floats - Most of the stuff in it is done with vector and matrix classes I wrote to make it easier to see the math that's happening. I'd start there.
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?
I can, and plan to, I just didn't have time to do that this morning before posting it. If you want to rebuild it, it should work with Visual C# Express, which is a free download from Microsoft.
I've started looking at the Madgwick stuff - It's basically an implementation of the DCM filter stuff, but using quaternions instead of matrices to represent the orientation. As such, their performance should be very similar. I've done a quick scan of the math, and it looks like quaternion math may come out slightly ahead in terms of performance due to operation count:
It seems to be easier to extract the up / left / forward vectors from a matrix than from a quat, and there's still the matter of extracting directional values from the orientation estimate. For matrices I know how to do it, but I don't have a grip on it yet for quats, so I'll have to do more reading.
Here's a video with some source code links to a few implementations:
In the video, he's obviously not using the compass in the DCM, so it drifts badly. My own version does not drift nearly this much, and once the compass is integrated in, there won't be any heading drift at all.
Jason, I modified your code to print the rotation matrix to the Parallax Serial Terminal, and it's works pretty well. PST seems to have a problem with high data speeds, but I was able to print with an update rate of 50 times per second. I had to modify the Gyro scaler to handle the slower update rate, so I added a routine to set the scaler based on the update rate. I could pick up the HoverFly, move it around a bit, and then put it back where it started and the matrix would return back to the identity matrix. I even tried disabling the Accelerometer correction, and the gyro drifted at a fairly slow rate.
Thanks for posting the DCM code. It should work well for my application, which is to control the gimbal servos on a finless rocket.
Dave - I've accomplished this without having to lower the update rate by using a counter and printing only part of the output per cycle. For example, use a counter & 3, increment it each loop. For counter values of 0, 1, or 2, print that row of the matrix, and for counter 3, reset the position of the cursor. The difference won't be perceptible, and it doesn't require changing the update rate.
@everyone
Thanks for a lot of great input. I am pretty confident that I can get the HoverFly to preform as well as the Wii input device demo that includes Madjwick. I don't think I have much more to ask of everyone. I need to get more into study and application.
@Heater
I am really slow at taking on compiler tasks in new languages.. thanks for pointing out what to do with Mono in Linux to get C# code to compile.
++++++++
Up to this point, I have been jumping around quite a bit looking for material that would help explain all the terminology (which can be confusing to a new user) and apply very directly to robotics.
I think I can settle in to actually getting some code active. I may try to write up a Glossary of terms, just to help other new users get up to speed quicker. I would also cut through any remaining confusion I have.
@Jason
Yes, the Quaternion is a different visualization and takes a bit of effort to sink in. It does seem that if motion control at the end of the line is done by motors or control surfaces specifically aligned with the X, Y, and Z axis of whatever, the idea of an all Quaternion code versus a jump to Direction Cosine Matrix may not be worthwhile.
Why so? Well I see that operator or auto-pilot inputs to the Direction Cosine Matrix are easy. I am not sure how to make similar inputs -- as of yet -- to an all Quaternion scheme.
Dave - thinking about your application, won't using an accelerometer be problematic? I would expect the G forces present in even a small rocket to saturate any normal accelerometer pretty easily. You might be better going with a gyro only solution, unless you want it for after the initial acceleration is done.
In my application I'll use the accelerometer to determine the initial alignment with the zenith and for launch detection. To be honest I could get by with just a 2D gyro, which is what I used a few years ago when I build my first rocket with active control. For that rocket I used a magnetically activated switch for launch detection. The accelerometer will also allow me to detect when the rocket has reached its highest point in flight, at which point it will fire a charge to eject the parachute. However, I'll probably use it only for logging purposes for the first few flights, and rely on a time delay to eject the parachute.
In rocket flight the accelerometer cannot be used to compensate the gyro for drift. When the rocket motor is burning it's difficult to separate out the acceleration due to gravity from the acceleration due to the motor. After motor burnout the rocket is in free-fall, and it is essentially weightless. The accelerometer will only sense the drag-force on the rocket. Once the chute has fully deployed the drag force will equal the weight of the rocket, and it will be able to sense the downward force of gravity again.
The motor I use only burns for about 8 seconds, so gyro drift is really not a problem during that time. The rocket could have small controllable fins that would guide it after motor burnout, which would require using the gyro for another 10 or 20 seconds. However, I'm not planning on do that anytime soon.
EDIT: I should mention that the weight of the rocket will be about half of the thrust of the motor. So the accelerometer will sense around 2Gs during launch. I believe this is well within the range of the accelerometer that is used in the HoverFly. It is true that many model rockets experience 10 Gs or more during launch, but I'll be avoiding such high accelerations in my rocket.
Have you considered combining the gyros with magnetic compass to get better readings. I have been studying Madjwick's 2007 paper. Heater dislikes the code, says it is flawed. But the text does present how everything works together. And you might be able to improve the gyro's performance a bit even if drift is not an issue..
Have you considered combining the gyros with magnetic compass to get better readings?
Yes, I want to add that. The hard part is the matrix multiply - The magnetometer reading needs to be "realigned", rotating it by your current orientation estimate. I figure I'll need to do the following:
- On power up, verify that the craft is level, or close to it, by waiting for the DCM code without magnetometer readings to settle
- Take a magnetometer reading and use that reading as the "north reference vector"
Then, in flight, either per update or every N updates:
- Multiply the original "north reference vector" by the craft orientation matrix to produce an "oriented reference vector"
- Multiply the current magnetometer vector by the craft orientation matrix to produce an oriented measurement
- Compute the difference between the oriented reference and the oriented measurement
- Apply a portion of that difference to the craft orientation matrix, much in the way that the accelerometer reading is applied now (could even be part of the same correction vector)
I haven't done enough reading to know if this is right or not. I'll likely implement on the PC in the DCM Visualizer first, then port the code to the Prop once it's been debugged.
Heater does not dislike the Madgwick code at all. I am in no position to say if the algorithm is good or not or if it suits ones application or not.
However, the code as presented in the Madgwick paper does suffer from divide by zero problems which then produces NaN outputs. Not good which ever way you look at it.
I have been playing around with the Magwick JavaScript and webgl IMU viewer.
I made a JS version of Madgwick's version of the MahoneyIMU filter and included that in there to enable comparison.
I cheekily borrowed the axis pointer images from Magwick's viewer to paint on the brick.
There is now Chrome serial port API handling code in there, it's works but is not hooked up to anything in the program yet.
If someone could tell me the format/protocol they are using with their IMU's I'd like to add the ability to use that. If any one has a recording of such a serial stream from their IMU that would be great.
Formats for input? From what I have read, there are mainly 3 -- [a] Euler Angles, Direction Cosine Matrix, and [c] Quaternion.
That is the short reply.
+++++++++++++++++++
And the long reply is........
It seems that even if the animation is in something other than Euler Angles, numerical display of Euler Angles is commonly provided for reference as people easily understand it. But, if the actual input format is Euler Angles, you will suffer gimbal lock in your display performance (a worth-while demo?).
Several papers that I have read supported practical use of Euler Angles or Direction Cosine Matrix while openly admitting that the main reason those were chosen is that the format was just easier for new users to visualize and follow the code.
Quaternions for Madgwick is a bit of a separate topic from Quaternions fro ARHS control. But both seem to process data with less code.
So it seems each has a role to play
1. Euler Angles - Helpful as final numerical output for the public about rotational position, not good for animation or control over wide ranges.
2. Direction Cosine Matrix - Works well for control and is less of a learning curve than going directly to Quaternions. Widely in popular use.
3. Quaternions - Possibly the optimal solution, but steepest learning curve if new to the maths. It is likely best to study them as two separate topics -- Sensor fusion and AHRS management.
Madjwick filter simply fuses sensor data and outputs Quaternions (which can be converted to Euler Angles or DCM if preferred).
Key elements of Madjwick take advantage of Quaternion format [1] to use a simpler regression via a Gradient Decent algorithm than the linear regression used in Kalman and [2] to use a Jacobian partial derivative matrix to calculate. Some quaternion maths is involved, but may not be all that is needed for navigation and control.
Quaternion for AHRS is a separate topic that I am just beginning to learn.
Steps are Rotation and Transition. And a key feature is the 'transition angle'. I haven't yet gotten my head around this very different point of view.
My main points are:
1. All three formats will remain valid and popular for a variety of reasons. Many AHRS hardware packages support more than one format as output.
2. Quaternions as applied to the Madjwick filters seems to just be the most convenient result optimization via the Jacobian Matrix operations and a Gradient-decent algorithm.
3. Quaternions as applied to actual AHRS computations is a separate topic from Madjwick's application. Nothing about Jacobian Matrix or a Gradient-decent algorithm, but other maths to learn.
Heater - I'm using a very simple protocol for the DCM Visualizer.
The first output type uses two bytes, 0x77, 0x77, as the signature, followed by 6 bytes of gyro (x, y, z0, in high byte, low byte order), followed by 6 bytes of accelerometer data. Same order. The values for them are raw, right out of the sensors, so they're not in normal "units".
I have a different output that uses 0x78, 0x78 as the signature, followed by 6 high/low byte pairs which are the first 6 fixed-point values of the computed DCM matrix. The last vector can be computed as the cross product of the first two, so it's not sent.
I did not phrase my request for formats/protocols clearly enough.
Firstly we have IMUs. They might output raw gyro, accel, and compass measurements. We have nine variables N times per second coming out. If those numbers are put out on a serial line then how? What order I they in? Are they 8 or 16 bit or what? Are they signed? Scaled? Are they even ints? What do these messages look like, does it have a header, length, checksum, whatever? Is it binary or ASCII?
With that in place you could plug your IMU directly into the madgwick.js Chrome App and then see what the Madgwick and Mahony filters can do with it. I hope to add DCM at some point.
Secondly your IMU might have some intelligence, like a Prop, and be performing it's own filtering. OK whats the format of that output data? Euler angles or what? Again what is the actual on the wire protocol?
It's not a big deal to have to draw that brick from quaternion or Euler or whatever. The three.js library can handle them or perhaps some conversion code needs putting in between.
I just want that you and others can actually try out this app.
In the mean time I can attempt to get the accelerometer on my STM32 F4 Discovery to output the same formats.
I did not phrase my request for formats/protocols clearly enough.
Firstly we have IMUs. They might output raw gyro, accel, and compass measurements. We have nine variables N times per second coming out. If those numbers are put out on a serial line then how? What order I they in? Are they 8 or 16 bit or what? Are they signed? Scaled? Are they even ints? What do these messages look like, does it have a header, length, checksum, whatever? Is it binary or ASCII?
With that in place you could plug your IMU directly into the madgwick.js Chrome App and then see what the Madgwick and Mahony filters can do with it. I hope to add DCM at some point.
Secondly your IMU might have some intelligence, like a Prop, and be performing it's own filtering. OK whats the format of that output data? Euler angles or what? Again what is the actual on the wire protocol?
It's not a big deal to have to draw that brick from quaternion or Euler or whatever. The three.js library can handle them or perhaps some conversion code needs putting in between.
I just want that you and others can actually try out this app.
In the mean time I can attempt to get the accelerometer on my STM32 F4 Discovery to output the same formats.
Gawk! You desire the raw data. I missed that.
Do you really desire to survey the marketplace for each and every permutation of sensor components out there?? Obviously there are a few standard MEMS sensor solutions that are being sold in volume today, but there are a lot of legacy devices that might be packed together by a DiY using what they have on hand.
I would go with a short list of currently popular ones sold by popular vendors- such as SparkFun, LadyADA, Pololu, DealExtreme, and EBay. Maybe HobbyKing as well. But the list could grow into a full-time occupation. You also mentioned FreeIMU.
=====
Would it not be easier to propose your own input format and have others manipulate the data to fit that?
Raw data or filtered. Quaternian, DCM, Euler whatever. Jason has defined messaging for both so I will start with that.
No I don't desire to try every IMU solution out there. What gave you that idea?
I already have my eye on the IMU's you listed. However I have my heart set on a FreeIMU board first and foremost. In the mean time I can play with the accelerometer on the STM32 F4 Discovery board that I have already.
Should I specify my own input format? That might me easier for me but the thing is, I was hoping you and Jason could just plug your IMU's into this app without having to do any hacking around. So far you guys seem to be the only ones who may be interested in checking out this little effort so I should not make work for you.
P.S. I woke up this morning with yet another dumb *** idea...How about hacking up a real good old fashioned mechanical rate gyro out of an old hard drive? Use the motor and platter as the flywheel. One could attach that to the end of the read/write head arm making use of that really good bearing that it has...
I've been working on a simple visualizer that uses another Prop driving a VGA screen. It's based on the 512x384 bitmapped display object. For now I'm just drawing the 3D XYZ axes as a 2D projection. I found that if I use 2 columns from the 3x3 rotation matrix it gives me the 2D projection from the point of view of the missing column. So if I want to look from the direction of the X axis I would use the Y and Z columns to get the 2D coordinates for the three axes.
I haven't interfaced it to the HoverFly board yet, but once I get it working I'll post the code.
P.S. I woke up this morning with yet another dumb *** idea...How about hacking up a real good old fashioned mechanical rate gyro out of an old hard drive? Use the motor and platter as the flywheel. One could attach that to the end of the read/write head arm making use of that really good bearing that it has...
Heater, I thought of using a disc drive for a gyro at one point. I found that if I picked up an external mechanical USB drive while it was running and rotate it I could feel a slight gyroscopic resistance to rotation. The disc platters are very light, so I think they would need to be weighted a bit with a metal ring attached to the platter. A gyroscopically stabilized platform could be built by mounting it in a 3D gimbal and attaching two drives at right angles to each other to the platform.
This would create a platform that would always point in the same direction independent of the direction of the external housing that attaches to the gimbal. That's the way the original gyro-controlled rockets worked. I think Robert Goddard's rockets used mechanical linkage attached to vanes that word divert the rocket thrust. I believe the V2 used potentiometers as sensors, and controlled the thrust vanes with electrical motors. These days we could use optical encoders to read the rotation on each axis of the gimbal. Of course today it's just easier to use rate gyro chips and do the math to get the orientation readings.
Raw data or filtered. Quaternian, DCM, Euler whatever. Jason has defined messaging for both so I will start with that.
No I don't desire to try every IMU solution out there. What gave you that idea?
I already have my eye on the IMU's you listed. However I have my heart set on a FreeIMU board first and foremost. In the mean time I can play with the accelerometer on the STM32 F4 Discovery board that I have already.
Should I specify my own input format? That might me easier for me but the thing is, I was hoping you and Jason could just plug your IMU's into this app without having to do any hacking around. So far you guys seem to be the only ones who may be interested in checking out this little effort so I should not make work for you.
P.S. I woke up this morning with yet another dumb *** idea...How about hacking up a real good old fashioned mechanical rate gyro out of an old hard drive? Use the motor and platter as the flywheel. One could attach that to the end of the read/write head arm making use of that really good bearing that it has...
So far, it seems I have to first install Chrome to get Java Script working, so what's a few more steps between friends? It seems if I go left, you tend to go right. I really don't want to burden you with my development path which is likely to be slower.
My current situation is that I have the HoverFly that provides raw data right out of the box, but likely has a few ASCII strings that would need to be removed. And I ordered 3 items from DealExteme that I have yet to really sort out. One is a GY-85 9DoF unit (aka MARG), another is a GY-521 6DoF unit (aka IMU) specified as a MPU-6050 [very popular], and the third is 3-Axis digital gyro unit intended to be combined with a 3-Axis accelerometer unit I have had on hand for ages.
Thus, the Arg.... Raw Data formats. I actually have four very different schemes in which two are 9Dof and two 6Dof. I thought I'd wait to see what actually arrives from DealExtreme as they have previously been months behind on delivery to me and I felt uncertain that I'd get anything.
And I have not been very keen of starting early as I have an Oct 15 deadline to file my 2013 income tax. That pretty much has my first priority.
I seem to be off in the weeds once again. I thought that Jason Dorie's DCM Visualizer might take my HoverFly code after it was put through the Madjwick filter and converted the Directional Cosine Matrix.
I suppose that if you want the HoverFly Raw Data for your project, the existing demo can be converted to deliver an binary packet similar to Jason Dorie's or in any other fashion you want.
Or are you really interested in accepting ASCII? You do realize that many of these devices have multiple ranges of output - 1X, 4X, 8X and so on. And it seems some are 16bit while others are 24 bit.
Your 10DoF choice from FreeIMU is a MPU6050 with an add-on Magnetic compass from another MEMS outfit, plus the altimeter from a third; then there is a 9Dof MPU 9050 that has it all in one package that I'm a bit curious about.
In sum, I am hanging back and just focusing on the HoverFly for now. That could be making life simpler for you.
...it seems I have to first install Chrome to get Java Script working, so what's a few more steps between friends?
Perhaps I am being presumptuous in assuming everyone has a Chrome browser installed already? How come you have not? Any way that step will only take you a minute: Just get the download from here https://support.google.com/chrome/answer/95346?hl=en and follow the short instructions.
I don't recommend using "apt-get install google-chrome" or whatever the Debian package is called as it did not post to this forum very well for me.
It seems if I go left, you tend to go right.
I don't mean to. It's just that the only other way I would do this is by using Qt and OpenGl. But then you would be having to install the Qt libs and compiling the thing yourself. Besides there is no point in that as we already have Jason's C# IMU viewer or Seb Chadwick's if we want actual Windows or Linux or Mac apps.
Besides this is all new ground form me, chrome apps, chrome serial port API etc etc.
My current situation is that I have the HoverFly that provides raw data right out of the box, but likely has a few ASCII strings that would need to be removed
This is great. If you let me know the on the wire format of the HoverFly serial communications I can add that to madgwick.js as an option. A sample of data for testing would be great too.
Or are you really interested in accepting ASCII?
ASCII, binary, I'm not fussy.
You do realize that many of these devices have multiple ranges of output - 1X, 4X, 8X and so on. And it seems some are 16bit while others are 24 bit.
No doubt. However the actual scaling and units of accelerometer values is of no consequence. Give it to me in furlongs/hour/week if you like. These are vector quantities that immediately get normalized so it does not matter. Same for the compass. I guess the gyro had better be in radians/second though.
In sum, I am hanging back and just focusing on the HoverFly for now. That could be making life simpler for you.
As I said, I'd like to accommodate the HoverFly as well. Let's do it.
Should I specify my own input format? That might me easier for me but the thing is, I was hoping you and Jason could just plug your IMU's into this app without having to do any hacking around. So far you guys seem to be the only ones who may be interested in checking out this little effort so I should not make work for you.
Heater,
I downloaded "9DOFHoverflyExperimenterBoard.spin" & "SensorDriver.spin" from the HoverFly site and compiled them as a SimpleIDE project and loaded the binary onto the board (I bought one of these boards from the initial thread post). That code spits out the following to the serial terminal, while it also manipulates the onboard RGB LED with changing colors when you move the board:
I could mod the code and have it send data in whatever format you need for the Madgwick IMU Chrome extension. As you can see, the code repeatedly sends a screenful of formatted text and data, but could easily just send data out the USB port to the Google Chrome app or extension, right?
What stream of bytes would you like to receive?
EDIT: I used spin2cpp to create a C version of this code. I modified that code to use print statements versus FullDuplexSerialPlus functions, which simplified the print section. Should be easier to format an alternate data-only output version.
Just now, for the first time, I'm getting data from my Espruino accelerometer into madgwick.js formatted like so:
{"a":[32,17,51]}
That is an ASCII string terminated with 0x0d, 0x0a. It may look horribly verbose to those who like binary or terse ASCII formats but it's JSON which gives us some advantages. For a start it's dead easy and fast to generate in my Espruino and also fast and easy to parse in madgwick.js. It allows expansion, for example we want gyro data which will look like this:
In case you are not familiar with JSON it basically says, we have an object {...}, the object has components like "a": that have values. Those values are arrays [..] of values or a single value in the case of b.
The arrays are the vector components of the sensors in x, y, z order.
@Heater
Whatever you use, it seems that you are going to need RS232 input -- likely 57600Baud or 115200Baud 8N1. Anything slower is not using the full potential of the MEMS devices 400Kbit I2C.
But try to consider that these gyros, accelorometers, and compasses do come in analog, SPI,and I2C outputs. With the anlog devices, wide range of ADC chips might be chosen with their own SPI or I2C set up.
Conversion to RS232 appears to be a must.
So why not take better advantage of the device that is doing the downloading to at least tidy up the form into your USB or RS232 port to one expected format?. After all, something must go inbetween. And a BS2 with 19200 baud half-duplex may be too slow. The inbetween device can at least clean up so you don't have to write a complicated series of input choices.
==========
Of course that just led me in a different direction from the start. I just figured the Madjwick would also be in the microcontroller, not the PC. The PC would just handle the graphics that it does best.
So I figured that as long as a Propeller was necessary between the computer and the MEMS devices, I might as well have Madjwick operating in the Propeller so that I could affirm its adequacy in its eventual target platform. Output for a PC display seemed obviously to be best in DCM or Quaternion data. Madjwick provides the Quternion, and there are good examples of conversion to DCM.
And so, I am having a bit of difficulty grasping why demo Madjwick in the PC from modified raw data? It just confounds me. I desire to depoly Madjwick withing the Propeller for robotics, not in the PC.
I do understand that compiling the code on a computer is a proof of its soundness. But we migrate farther afield.. to JavaScript and into Chrome.
FYI, I tried Chrome and didn't like it so I went back to Firefox. You have a very grand scheme with a lot of layers that you may enjoy building. I am just taking a simpler approach. And yes, I can load Chrome and get this all working, but I am still going to have to go back an make the HoverFlyGimal include Madjwick and anything else.
+++++++
Happy to see you got your 10Dof FreeIMU and that it is working well.
I connected a serial link between the HoverFly and a Prop Demo Board, and I got the Visualizer working earlier this morning. I still need to tweak it so that the XYZ axes on the HoverFly match up with the XYZ axes in the visualizer. Currently the axes are in a different order, but I didn't have time to fix it before going to work. I hope to get it working correctly this evening, and I'll post the code then.
When I initially ran this code on the 9DOFHoverflyExperimenter board and your Madgwick Chrome app, I got an error about the serial port (expected as I'm on Mac OS X), and I changed the code in serial.js to this, which uses the same device designation as SimpleIDE did in uploading the program:
// Connect to the serial port
//chrome.serial.connect("/dev/ttyUSB0", {bitrate: 115200}, onConnect);
chrome.serial.connect("/dev/cu.usbserial-A601YBMT", {bitrate: 115200}, onConnect);
That error went away...
Then, I got an error for this line in serial.js "str = convertArrayBufferToString(info.data);" claiming convertArrayBufferToString was undefined. I replaced that with a function that should do the same. The error went away.
At this point I get no errors, but it does not appear that the data is being pulled from the serial port. Within onReceiveCallback, Info.data appears to be an empty array. Javascript debugging is new for me, so I'm learning as I go. I've set breakpoints within onReceiveCallback, but have not made progress in determining why there's no data...
I have been making many changes today. That "convertArrayBufferToString" thing has gone away.
I have been having a lot of trouble getting my STM32 F4 Discovery talking to this program at all. After some hours it turns out that most of it is due to a bad USB cable. Some time it worked some time not. Often confusing the Linux USB system so much that a reboot was required! Confused me anyway.
Any way just now I have pushed a new version to github that works. At least here. It uses /dev/ttyACM0 which I'm sure is not correct for other machines. I am going to need a bit of a user interface to sort that out.
On top of that my parsing of the input data is very fragile. Works if the timing is right! I'll try and have a look at it again tomorrow.
The HoverFly has large mounting holes that accept rubber grommets. Does anyone know of a good source for getting the rubber grommets?
Hi Dave,
I got grommets over-the-counter retail in Taiwan. If you are stuck, I can send you some via post. I thin you just have to target the 'smallest grommet in stock' with most retailers. Some won't go that small.
At first, I just used grommets provided for mounting R/C servos. They work, but are a sloppy fit.
There is not and probably never will be any RS232 in my set up. RS232 is an electrical signalling standard. I suspect you mean "some kind of byte stream" so let's continue...
I don't have to "try to consider", we all know the variety of devices and interfaces they have that are out there. As you say there will be an MCU handling that hardware interface and providing the byte stream to any PC based application.
So why not take better advantage of the device that is doing the downloading to at least tidy up the form into your USB or RS232 port to one expected format?
Quite inadvertently that is what I have done. See posts above. That only happened because it was the easiest way to get my STM32 F4 Discovery running Espruino to talk to a Chrome app.
The "complicated series of input choices" may come about as well. If there is no means of changing the code in the MCU at the sensor end. For example if I ever get hold of a commercial IMU like the x-sens http://www.xsens.com/ (Much too expensive by the way).
=====
Yes, you are right. At the end of the day we want Madgwick or any other fusion code, running in the micro controller. Of course, we don't want a PC or laptop bolted to our balancing bots and quad copters.
I might as well have Madjwick operating in the Propeller...
Yes indeed. I was hoping you were working on that:)
It's not yet clear to me that the Propeller has the chops to run Madgwick at any reasonable rate, what with all that floating point stuff going on. Will it even fit in memory if compiled with propgcc? One would probably want to use fixed point maths to increase speed and decrease size. It gets messy and tedious.
And so, I am having a bit of difficulty grasping why demo Madjwick in the PC from modified raw data? It just confounds me. I desire to depoly Madjwick withing the Propeller for robotics, not in the PC.
Because.
Why confounded? It's not just me who was attracted to the idea of running the fusion code in a viewer on the PC. Magwick has done it, see his YouTube videos. JasonDorie has done it in C#. There are many others.
When you are developing such an algorithm, or just tinkering around with it like me, it's much easier to do the work on a PC with a test harness, like these viewers.
In my case it was a simple half hour job to create a JS version of the algorithms from the original C. So why not throw it in there? Besides I don't have an IMU with onboard MCU to hand yet.
You have a very grand scheme with a lot of layers that you may enjoy building.
Hardly a "grand scheme", just some JS hacked up in a short time. More an experiment or exploration. I have been having fun with this and learned a few things along the way:
1) I learned my favorite 3D library has adopted quaternians since I used it last.
2) I found out what a quaternian even is and a bit about how to use them
3) I figured out how to make Chrome apps and use the serial API.
4) I got my Espruino board to read it's accelerometers.
As for the "layers", well, as I said, this kind of viewer could be done in C++ or C# or whatever. Always lots of layers. Languages, compilers, libraries etc etc. At least using JS ensures everyone can run the thing without messing around with any of that.
If you really want no layers it looks like Dave Hein has the thing for you. A 3D visualiser running on the Prop itself.
Comments
The DCMVisualizer zip file contains a full C# implementation of the DCM code in fixed point. It shouldn't be hard to convert to using floats - Most of the stuff in it is done with vector and matrix classes I wrote to make it easier to see the math that's happening. I'd start there.
I can, and plan to, I just didn't have time to do that this morning before posting it. If you want to rebuild it, it should work with Visual C# Express, which is a free download from Microsoft.
http://www.visualstudio.com/en-us/products/visual-studio-express-vs.aspx
You should be able to use the 2010 version, which you can get as just C#, or the latest one, which includes C/C++ and others as well.
Multiply rotations together:
Matrix : 18 mults, 18 adds
Quat : 16 mults, 12 adds
It seems to be easier to extract the up / left / forward vectors from a matrix than from a quat, and there's still the matter of extracting directional values from the orientation estimate. For matrices I know how to do it, but I don't have a grip on it yet for quats, so I'll have to do more reading.
The quaternion math is boiled down relatively well here: http://www.cprogramming.com/tutorial/3d/quaternions.html
And I've found this page comparing a few different orientation systems: http://www.olliw.eu/2013/imu-data-fusing/
Here's a video with some source code links to a few implementations:
In the video, he's obviously not using the compass in the DCM, so it drifts badly. My own version does not drift nearly this much, and once the compass is integrated in, there won't be any heading drift at all.
Thanks for posting the DCM code. It should work well for my application, which is to control the gimbal servos on a finless rocket.
J
Thanks for a lot of great input. I am pretty confident that I can get the HoverFly to preform as well as the Wii input device demo that includes Madjwick. I don't think I have much more to ask of everyone. I need to get more into study and application.
@Heater
I am really slow at taking on compiler tasks in new languages.. thanks for pointing out what to do with Mono in Linux to get C# code to compile.
++++++++
Up to this point, I have been jumping around quite a bit looking for material that would help explain all the terminology (which can be confusing to a new user) and apply very directly to robotics.
I think I can settle in to actually getting some code active. I may try to write up a Glossary of terms, just to help other new users get up to speed quicker. I would also cut through any remaining confusion I have.
@Jason
Yes, the Quaternion is a different visualization and takes a bit of effort to sink in. It does seem that if motion control at the end of the line is done by motors or control surfaces specifically aligned with the X, Y, and Z axis of whatever, the idea of an all Quaternion code versus a jump to Direction Cosine Matrix may not be worthwhile.
Why so? Well I see that operator or auto-pilot inputs to the Direction Cosine Matrix are easy. I am not sure how to make similar inputs -- as of yet -- to an all Quaternion scheme.
In rocket flight the accelerometer cannot be used to compensate the gyro for drift. When the rocket motor is burning it's difficult to separate out the acceleration due to gravity from the acceleration due to the motor. After motor burnout the rocket is in free-fall, and it is essentially weightless. The accelerometer will only sense the drag-force on the rocket. Once the chute has fully deployed the drag force will equal the weight of the rocket, and it will be able to sense the downward force of gravity again.
The motor I use only burns for about 8 seconds, so gyro drift is really not a problem during that time. The rocket could have small controllable fins that would guide it after motor burnout, which would require using the gyro for another 10 or 20 seconds. However, I'm not planning on do that anytime soon.
EDIT: I should mention that the weight of the rocket will be about half of the thrust of the motor. So the accelerometer will sense around 2Gs during launch. I believe this is well within the range of the accelerometer that is used in the HoverFly. It is true that many model rockets experience 10 Gs or more during launch, but I'll be avoiding such high accelerations in my rocket.
Yes, I want to add that. The hard part is the matrix multiply - The magnetometer reading needs to be "realigned", rotating it by your current orientation estimate. I figure I'll need to do the following:
- On power up, verify that the craft is level, or close to it, by waiting for the DCM code without magnetometer readings to settle
- Take a magnetometer reading and use that reading as the "north reference vector"
Then, in flight, either per update or every N updates:
- Multiply the original "north reference vector" by the craft orientation matrix to produce an "oriented reference vector"
- Multiply the current magnetometer vector by the craft orientation matrix to produce an oriented measurement
- Compute the difference between the oriented reference and the oriented measurement
- Apply a portion of that difference to the craft orientation matrix, much in the way that the accelerometer reading is applied now (could even be part of the same correction vector)
I haven't done enough reading to know if this is right or not. I'll likely implement on the PC in the DCM Visualizer first, then port the code to the Prop once it's been debugged.
However, the code as presented in the Madgwick paper does suffer from divide by zero problems which then produces NaN outputs. Not good which ever way you look at it.
On the other however, the Madgwick code you find here http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ tries to avoid that problem.
On the yet another however, I have managed to get even that code to produce NaN output.
Anyway, I am running that code in my JavaScript simulations and it works very well.
How it works in real life is another story I guess.
I made a JS version of Madgwick's version of the MahoneyIMU filter and included that in there to enable comparison.
I cheekily borrowed the axis pointer images from Magwick's viewer to paint on the brick.
There is now Chrome serial port API handling code in there, it's works but is not hooked up to anything in the program yet.
If someone could tell me the format/protocol they are using with their IMU's I'd like to add the ability to use that. If any one has a recording of such a serial stream from their IMU that would be great.
The whole caboodle is up on github https://github.com/ZiCog/madgwick.js
That is the short reply.
+++++++++++++++++++
And the long reply is........
It seems that even if the animation is in something other than Euler Angles, numerical display of Euler Angles is commonly provided for reference as people easily understand it. But, if the actual input format is Euler Angles, you will suffer gimbal lock in your display performance (a worth-while demo?).
Several papers that I have read supported practical use of Euler Angles or Direction Cosine Matrix while openly admitting that the main reason those were chosen is that the format was just easier for new users to visualize and follow the code.
Quaternions for Madgwick is a bit of a separate topic from Quaternions fro ARHS control. But both seem to process data with less code.
So it seems each has a role to play
1. Euler Angles - Helpful as final numerical output for the public about rotational position, not good for animation or control over wide ranges.
2. Direction Cosine Matrix - Works well for control and is less of a learning curve than going directly to Quaternions. Widely in popular use.
3. Quaternions - Possibly the optimal solution, but steepest learning curve if new to the maths. It is likely best to study them as two separate topics -- Sensor fusion and AHRS management.
Madjwick filter simply fuses sensor data and outputs Quaternions (which can be converted to Euler Angles or DCM if preferred).
Key elements of Madjwick take advantage of Quaternion format [1] to use a simpler regression via a Gradient Decent algorithm than the linear regression used in Kalman and [2] to use a Jacobian partial derivative matrix to calculate. Some quaternion maths is involved, but may not be all that is needed for navigation and control.
Quaternion for AHRS is a separate topic that I am just beginning to learn.
Steps are Rotation and Transition. And a key feature is the 'transition angle'. I haven't yet gotten my head around this very different point of view.
My main points are:
1. All three formats will remain valid and popular for a variety of reasons. Many AHRS hardware packages support more than one format as output.
2. Quaternions as applied to the Madjwick filters seems to just be the most convenient result optimization via the Jacobian Matrix operations and a Gradient-decent algorithm.
3. Quaternions as applied to actual AHRS computations is a separate topic from Madjwick's application. Nothing about Jacobian Matrix or a Gradient-decent algorithm, but other maths to learn.
The first output type uses two bytes, 0x77, 0x77, as the signature, followed by 6 bytes of gyro (x, y, z0, in high byte, low byte order), followed by 6 bytes of accelerometer data. Same order. The values for them are raw, right out of the sensors, so they're not in normal "units".
I have a different output that uses 0x78, 0x78 as the signature, followed by 6 high/low byte pairs which are the first 6 fixed-point values of the computed DCM matrix. The last vector can be computed as the cross product of the first two, so it's not sent.
I did not phrase my request for formats/protocols clearly enough.
Firstly we have IMUs. They might output raw gyro, accel, and compass measurements. We have nine variables N times per second coming out. If those numbers are put out on a serial line then how? What order I they in? Are they 8 or 16 bit or what? Are they signed? Scaled? Are they even ints? What do these messages look like, does it have a header, length, checksum, whatever? Is it binary or ASCII?
With that in place you could plug your IMU directly into the madgwick.js Chrome App and then see what the Madgwick and Mahony filters can do with it. I hope to add DCM at some point.
Secondly your IMU might have some intelligence, like a Prop, and be performing it's own filtering. OK whats the format of that output data? Euler angles or what? Again what is the actual on the wire protocol?
It's not a big deal to have to draw that brick from quaternion or Euler or whatever. The three.js library can handle them or perhaps some conversion code needs putting in between.
I just want that you and others can actually try out this app.
In the mean time I can attempt to get the accelerometer on my STM32 F4 Discovery to output the same formats.
Thanks. Actually I just stumbled across your C# code that reads that, now it is clear what is going on.
That's a really odd start of message header:)
Gawk! You desire the raw data. I missed that.
Do you really desire to survey the marketplace for each and every permutation of sensor components out there?? Obviously there are a few standard MEMS sensor solutions that are being sold in volume today, but there are a lot of legacy devices that might be packed together by a DiY using what they have on hand.
I would go with a short list of currently popular ones sold by popular vendors- such as SparkFun, LadyADA, Pololu, DealExtreme, and EBay. Maybe HobbyKing as well. But the list could grow into a full-time occupation. You also mentioned FreeIMU.
=====
Would it not be easier to propose your own input format and have others manipulate the data to fit that?
Raw data or filtered. Quaternian, DCM, Euler whatever. Jason has defined messaging for both so I will start with that.
No I don't desire to try every IMU solution out there. What gave you that idea?
I already have my eye on the IMU's you listed. However I have my heart set on a FreeIMU board first and foremost. In the mean time I can play with the accelerometer on the STM32 F4 Discovery board that I have already.
Should I specify my own input format? That might me easier for me but the thing is, I was hoping you and Jason could just plug your IMU's into this app without having to do any hacking around. So far you guys seem to be the only ones who may be interested in checking out this little effort so I should not make work for you.
P.S. I woke up this morning with yet another dumb *** idea...How about hacking up a real good old fashioned mechanical rate gyro out of an old hard drive? Use the motor and platter as the flywheel. One could attach that to the end of the read/write head arm making use of that really good bearing that it has...
I haven't interfaced it to the HoverFly board yet, but once I get it working I'll post the code.
This would create a platform that would always point in the same direction independent of the direction of the external housing that attaches to the gimbal. That's the way the original gyro-controlled rockets worked. I think Robert Goddard's rockets used mechanical linkage attached to vanes that word divert the rocket thrust. I believe the V2 used potentiometers as sensors, and controlled the thrust vanes with electrical motors. These days we could use optical encoders to read the rotation on each axis of the gimbal. Of course today it's just easier to use rate gyro chips and do the math to get the orientation readings.
So far, it seems I have to first install Chrome to get Java Script working, so what's a few more steps between friends? It seems if I go left, you tend to go right. I really don't want to burden you with my development path which is likely to be slower.
My current situation is that I have the HoverFly that provides raw data right out of the box, but likely has a few ASCII strings that would need to be removed. And I ordered 3 items from DealExteme that I have yet to really sort out. One is a GY-85 9DoF unit (aka MARG), another is a GY-521 6DoF unit (aka IMU) specified as a MPU-6050 [very popular], and the third is 3-Axis digital gyro unit intended to be combined with a 3-Axis accelerometer unit I have had on hand for ages.
Thus, the Arg.... Raw Data formats. I actually have four very different schemes in which two are 9Dof and two 6Dof. I thought I'd wait to see what actually arrives from DealExtreme as they have previously been months behind on delivery to me and I felt uncertain that I'd get anything.
And I have not been very keen of starting early as I have an Oct 15 deadline to file my 2013 income tax. That pretty much has my first priority.
I seem to be off in the weeds once again. I thought that Jason Dorie's DCM Visualizer might take my HoverFly code after it was put through the Madjwick filter and converted the Directional Cosine Matrix.
I suppose that if you want the HoverFly Raw Data for your project, the existing demo can be converted to deliver an binary packet similar to Jason Dorie's or in any other fashion you want.
Or are you really interested in accepting ASCII? You do realize that many of these devices have multiple ranges of output - 1X, 4X, 8X and so on. And it seems some are 16bit while others are 24 bit.
Your 10DoF choice from FreeIMU is a MPU6050 with an add-on Magnetic compass from another MEMS outfit, plus the altimeter from a third; then there is a 9Dof MPU 9050 that has it all in one package that I'm a bit curious about.
In sum, I am hanging back and just focusing on the HoverFly for now. That could be making life simpler for you.
I don't recommend using "apt-get install google-chrome" or whatever the Debian package is called as it did not post to this forum very well for me. I don't mean to. It's just that the only other way I would do this is by using Qt and OpenGl. But then you would be having to install the Qt libs and compiling the thing yourself. Besides there is no point in that as we already have Jason's C# IMU viewer or Seb Chadwick's if we want actual Windows or Linux or Mac apps.
Besides this is all new ground form me, chrome apps, chrome serial port API etc etc. This is great. If you let me know the on the wire format of the HoverFly serial communications I can add that to madgwick.js as an option. A sample of data for testing would be great too. ASCII, binary, I'm not fussy. No doubt. However the actual scaling and units of accelerometer values is of no consequence. Give it to me in furlongs/hour/week if you like. These are vector quantities that immediately get normalized so it does not matter. Same for the compass. I guess the gyro had better be in radians/second though. As I said, I'd like to accommodate the HoverFly as well. Let's do it.
Heater,
I downloaded "9DOFHoverflyExperimenterBoard.spin" & "SensorDriver.spin" from the HoverFly site and compiled them as a SimpleIDE project and loaded the binary onto the board (I bought one of these boards from the initial thread post). That code spits out the following to the serial terminal, while it also manipulates the onboard RGB LED with changing colors when you move the board:
I could mod the code and have it send data in whatever format you need for the Madgwick IMU Chrome extension. As you can see, the code repeatedly sends a screenful of formatted text and data, but could easily just send data out the USB port to the Google Chrome app or extension, right?
What stream of bytes would you like to receive?
EDIT: I used spin2cpp to create a C version of this code. I modified that code to use print statements versus FullDuplexSerialPlus functions, which simplified the print section. Should be easier to format an alternate data-only output version.
dgately
OK. Cool.
Just now, for the first time, I'm getting data from my Espruino accelerometer into madgwick.js formatted like so:
{"a":[32,17,51]}That is an ASCII string terminated with 0x0d, 0x0a. It may look horribly verbose to those who like binary or terse ASCII formats but it's JSON which gives us some advantages. For a start it's dead easy and fast to generate in my Espruino and also fast and easy to parse in madgwick.js. It allows expansion, for example we want gyro data which will look like this:{"a":[32,17,51],"g":[45,23,3]}Then magnetometer data:{"a":[32, 17, 51], "g":[45, 23, 3], "m":[45, 23, 3]}Then for the FreeIMU we get barometer data:{"a":[32, 17, 51], "g":[45, 23, 3], "m":[45, 23, 3], "b":120}In case you are not familiar with JSON it basically says, we have an object {...}, the object has components like "a": that have values. Those values are arrays [..] of values or a single value in the case of b.
The arrays are the vector components of the sensors in x, y, z order.
The spaces are optional.
Whatever you use, it seems that you are going to need RS232 input -- likely 57600Baud or 115200Baud 8N1. Anything slower is not using the full potential of the MEMS devices 400Kbit I2C.
But try to consider that these gyros, accelorometers, and compasses do come in analog, SPI,and I2C outputs. With the anlog devices, wide range of ADC chips might be chosen with their own SPI or I2C set up.
Conversion to RS232 appears to be a must.
So why not take better advantage of the device that is doing the downloading to at least tidy up the form into your USB or RS232 port to one expected format?. After all, something must go inbetween. And a BS2 with 19200 baud half-duplex may be too slow. The inbetween device can at least clean up so you don't have to write a complicated series of input choices.
==========
Of course that just led me in a different direction from the start. I just figured the Madjwick would also be in the microcontroller, not the PC. The PC would just handle the graphics that it does best.
So I figured that as long as a Propeller was necessary between the computer and the MEMS devices, I might as well have Madjwick operating in the Propeller so that I could affirm its adequacy in its eventual target platform. Output for a PC display seemed obviously to be best in DCM or Quaternion data. Madjwick provides the Quternion, and there are good examples of conversion to DCM.
And so, I am having a bit of difficulty grasping why demo Madjwick in the PC from modified raw data? It just confounds me. I desire to depoly Madjwick withing the Propeller for robotics, not in the PC.
I do understand that compiling the code on a computer is a proof of its soundness. But we migrate farther afield.. to JavaScript and into Chrome.
FYI, I tried Chrome and didn't like it so I went back to Firefox. You have a very grand scheme with a lot of layers that you may enjoy building. I am just taking a simpler approach. And yes, I can load Chrome and get this all working, but I am still going to have to go back an make the HoverFlyGimal include Madjwick and anything else.
+++++++
Happy to see you got your 10Dof FreeIMU and that it is working well.
The code now outputs:
When I initially ran this code on the 9DOFHoverflyExperimenter board and your Madgwick Chrome app, I got an error about the serial port (expected as I'm on Mac OS X), and I changed the code in serial.js to this, which uses the same device designation as SimpleIDE did in uploading the program:
// Connect to the serial port //chrome.serial.connect("/dev/ttyUSB0", {bitrate: 115200}, onConnect); chrome.serial.connect("/dev/cu.usbserial-A601YBMT", {bitrate: 115200}, onConnect);That error went away...
Then, I got an error for this line in serial.js "str = convertArrayBufferToString(info.data);" claiming convertArrayBufferToString was undefined. I replaced that with a function that should do the same. The error went away.
At this point I get no errors, but it does not appear that the data is being pulled from the serial port. Within onReceiveCallback, Info.data appears to be an empty array. Javascript debugging is new for me, so I'm learning as I go. I've set breakpoints within onReceiveCallback, but have not made progress in determining why there's no data...
dgately
Thanks for trying that out.
I have been making many changes today. That "convertArrayBufferToString" thing has gone away.
I have been having a lot of trouble getting my STM32 F4 Discovery talking to this program at all. After some hours it turns out that most of it is due to a bad USB cable. Some time it worked some time not. Often confusing the Linux USB system so much that a reboot was required! Confused me anyway.
Any way just now I have pushed a new version to github that works. At least here. It uses /dev/ttyACM0 which I'm sure is not correct for other machines. I am going to need a bit of a user interface to sort that out.
On top of that my parsing of the input data is very fragile. Works if the timing is right! I'll try and have a look at it again tomorrow.
Hi Dave,
I got grommets over-the-counter retail in Taiwan. If you are stuck, I can send you some via post. I thin you just have to target the 'smallest grommet in stock' with most retailers. Some won't go that small.
At first, I just used grommets provided for mounting R/C servos. They work, but are a sloppy fit.
There is not and probably never will be any RS232 in my set up. RS232 is an electrical signalling standard. I suspect you mean "some kind of byte stream" so let's continue...
I don't have to "try to consider", we all know the variety of devices and interfaces they have that are out there. As you say there will be an MCU handling that hardware interface and providing the byte stream to any PC based application. Quite inadvertently that is what I have done. See posts above. That only happened because it was the easiest way to get my STM32 F4 Discovery running Espruino to talk to a Chrome app.
The "complicated series of input choices" may come about as well. If there is no means of changing the code in the MCU at the sensor end. For example if I ever get hold of a commercial IMU like the x-sens http://www.xsens.com/ (Much too expensive by the way).
=====
Yes, you are right. At the end of the day we want Madgwick or any other fusion code, running in the micro controller. Of course, we don't want a PC or laptop bolted to our balancing bots and quad copters. Yes indeed. I was hoping you were working on that:)
It's not yet clear to me that the Propeller has the chops to run Madgwick at any reasonable rate, what with all that floating point stuff going on. Will it even fit in memory if compiled with propgcc? One would probably want to use fixed point maths to increase speed and decrease size. It gets messy and tedious. Because.
Why confounded? It's not just me who was attracted to the idea of running the fusion code in a viewer on the PC. Magwick has done it, see his YouTube videos. JasonDorie has done it in C#. There are many others.
When you are developing such an algorithm, or just tinkering around with it like me, it's much easier to do the work on a PC with a test harness, like these viewers.
In my case it was a simple half hour job to create a JS version of the algorithms from the original C. So why not throw it in there? Besides I don't have an IMU with onboard MCU to hand yet. Hardly a "grand scheme", just some JS hacked up in a short time. More an experiment or exploration. I have been having fun with this and learned a few things along the way:
1) I learned my favorite 3D library has adopted quaternians since I used it last.
2) I found out what a quaternian even is and a bit about how to use them
3) I figured out how to make Chrome apps and use the serial API.
4) I got my Espruino board to read it's accelerometers.
As for the "layers", well, as I said, this kind of viewer could be done in C++ or C# or whatever. Always lots of layers. Languages, compilers, libraries etc etc. At least using JS ensures everyone can run the thing without messing around with any of that.
If you really want no layers it looks like Dave Hein has the thing for you. A 3D visualiser running on the Prop itself.
How cool is that? Well done Dave.