Filtering/fusing multiple sensor data in max? (Kalman/Complimentary/Marg)

    Sep 25 2011 | 6:31 pm
    Hello, first post.
    I'm working on a multi-sensor setup (3axis gyro and 3axis accelerometer) that I plan on using in a multimedia performance project(
    I'm using an Arduino compatible board with built in sensors, sending that data wirelessly using XBee modules, and getting it into max using the serial object. The sensors will be mounted on my wrists to gather angle/orientation/motion data while I paint. (see videos on webpage linked above)
    I have the data coming in fine (though not calibrated at the moment).
    Now I've been doing lots of reading up on doing something meaningful with the numbers. The primarily goal would be to determine absolute angles of the sensors and mapping that to processing in max. In order to do this I've read that you need to use some kind of software filtering. I've run into several examples/types including Kalman filtering, Complimentary filtering and Marg filtering. I know that to determine three absolute angles I would need to incorporate a magnetometer, which is something I'm considering, but given the example below, I think I can compensate for the drift/error accumulation.
    And there is the (C) code posted on the project page:
    There's also a (lengthy) paper covering the math involved.
    I must admit most of the math is waaaay over my head, particularly in the paper.
    So basically I have some C code that does exactly what I need. (Convert 6DOF sensor data into 3 absolute angles, with drift along the yaw). I tried building the math in max, but am at a loss on how to do most of it due to order of operation combined with max's right to left paradigm.
    Is it possible to do this kind of calculation in straight max or do I need to build an external to do the math?
    If it can't happen in max, is there a more max/object oriented solution for determining absolute angles from a sensor array?
    A much simpler question here. What is the best way to calibrate the sensors? Leave it sitting flat and get a [mean] from that, then offset the value by that amount? Would I lose range in one direction by doing it that way? (For example, my gyro data seems to hover around 0.2 and goes from 0. to 1. if I center it to 0.5 will it only go from 0.3 to 1. after calibration?)
    Here is the main body of the C code:
    void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez;
    // normalise the measurements norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm;
    // estimated direction of gravity vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    // error is sum of cross product between reference direction of field and direction measured by sensor ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx);
    // integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki;
    // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt;
    // integrate quaternion rate and normalise q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
    // normalise quaternion norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; }
    Here is my max code (which doesn't work due to stack overflow):

    • Sep 27 2011 | 9:05 am
      I found this these which shows how to handle some of the math:
      The main bit is this: expr sqrt ( $f1*$f1 + $f2*$f2 + $f3*$f3 )
      That doesn't address the C code stuff or calibration however.
    • Sep 27 2011 | 10:09 am
      I attempted something along these lines a while back (we probably even have the same sparkfun 6dof board). I was attempting to get most of the math running on the arduino itself, and only output the 3 axis rotational data via the serial object (along with an absolute 3-dimensional spatial number).
      That C-code looks promising, it probably wouldn't be TOO hard to convert to Processing to use on an arduino.
      If you're interested in collaborating on getting something like this to work, I'm all for it! I'm sure we have different needs for the end result but it's something that I think would come in handy for a lot of people.
    • Sep 27 2011 | 11:56 am
      That would be great. I (we, as Angela and myself are working on this together) have an Arduimu board which has the same exact sensors as the sparkfun 6dof board.
      I've seen the code you're talking about (if it's the code posted on the arduino forum) and it is pretty straight forward looking. I wanted to avoid doing the processing on the arduino in order to bring latency down to a minimum (working on the assumption that a laptop can crunch the numbers tons faster than the arduino) and wanted to also avoid involving Processing. Basically to have the smallest amount of stuff "in between" as possible. So for me it would be ideal to turn that C code into straight Max so that it goes from sensor data right to processing in max or whatever.
      Definitely up for collaborating on figuring this stuff out. With the amount of wiimote action happening I'm surprised more people aren't doing this kind of software filtering on the data.
    • Sep 27 2011 | 1:32 pm
      I've been playing a bit with the Sparkfun 9DOF but I haven't been able to get a stable 3D position tracking without serious drifting. So any development to get a good Kalman filtering or such within Max would be nice !
    • Sep 27 2011 | 2:48 pm
      The version of the video/code posted in the first post (that uses 9dof instead of 6dof) is really impressive. Solid tracking and very little latency (at least visible).
      Check it:
    • Sep 27 2011 | 6:44 pm
      I found a 6dof Kalman filter designed specifically for the sparkfun 6dof Razor board that I own - which seems to be working pretty well - sadly it doesn't cover yaw, as you need a magnetometer to do that.
      I suppose I don't really see the necessity of running all the filtering from within Max, when you've got a more than powerful enough microprocessor on any arduino. That way your hardware is putting out useful information, and all you have to do with max is interpret it. In my experience, trying to do 'heavy' math like a 6 or 9 dimensional kalman filter is either too slow, too complex (which leads to 'slow' again), or downright impossible - and you'd need an external written in C. At which point, why not just run that code on your microprocessor?
      I'm of the opinion that utilizing the microprocessor is for the best - the arduino can output at a bit rate/baud rate of 115200 bps. If we have 16 bytes worth of data coming out, that's about 900 samples per second. That's like running a metro set to 1.1 - which in my experience can cause some issues with Max (CPU spikes, crashing metros, etc). For contrast, MIDI runs at 31250 baud, which is almost a quarter of the speed of the Arduino's max output.
      Basically, the benefit of a microprocessor is that you can devote the entirety of it's processing to doing something very specific - implementing a Kalman, DCM, or Complimentary filter in Max would likely be slower merely because the CPU is doing other things - and unless we can write a C external that devotes a thread to the filtering, I'd expect error handling would start to get maddening.
    • Sep 27 2011 | 9:10 pm
      Hmm, you make some compelling points. At the moment I'm polling serial at 10ms with a qmetro, so it's not super fast.
      What looked good about the video in the first post is that the yaw was there (even with 6dof), it was just a bit prone to error.
      For our particular usage, the artists hands would often return to her side, or rather, straight ahead, and she would never turn completely around, so I think that could be used to periodically 'reset' the gyro by sensing if its been relatively stationary for 3sec, then reset that as zero, or something like that.
      Having the numbers crunched in max could make that kind of autocorrection more difficult, as the drift would be reset post-calculations. There are 9dof arduino filters I think too (or something like the C code above).
    • Sep 27 2011 | 9:31 pm
      Hmm, so as long as the Arduino can read, process, and send over wireless faster than I can poll serial comfortably (10ms?) it's better. Wireless also adds a bit of delay/bottlenecking too I would think.
      Are you sending both the raw and processed values across? Just wondering about the resetting error correction (though I guess that could happen in the Arduino as well, much more difficultly).
    • Sep 28 2011 | 2:27 am
      Wireless is something I still need to work out myself (Xbee is physically too large for what I need in the end), but I don't think it would add too much overhead. Possibly a delay, sure, but I don't think it would eat into the baud rate at all.
      Resetting error correction wouldn't be too hard to code into an arduino - in the main loop, you just keep a running average (or two, or four) that resets every second or so. So, say you have averages A, B, C, and D - A resets at 0.25 seconds, B at .5, C at .75, and D at 1.0, as a cycle. Then you average those four numbers, which gets you an average of the past 1 second without any huge hiccups. If all of the averages are within a certain range of each other, reset to 0 (or whatever numbers make sense). Then run the main loop.
      OK so I guess maybe that is a bit complicated. My personal ideal would be to not even have to reset arbitrarily, as it's more precise in the long run. That would need a 9dof, though, I'm rather certain. Sadly I can't afford to buy one of those for a few more months.
    • Sep 28 2011 | 4:26 am
      FreeIMU seems to be the way to go! The Sparkfun 9DOF boards use the exact same chips, far as I can tell.
    • Sep 28 2011 | 7:46 am
      Adding wireless isn't bad. Just sticking a bunch of Serial.print lines where relevant. The XBee wireless modules I'm using work just like a straight serial connection.
      I've been considering adding a magnetometer module:
      As on their own they are quite cheap and easy to incorporate via i2c (in terms of bussing, not sure about the code side of it).
      I saw the freeimu thing but I don't think I looked at the sketches. It uses the marg filtering like in the link I posted, which is promising.
      Do you rely on Processing at all with the freeimu thing? (from the look of the text it's just some libraries with all of the sketches being Processing (heading out to work so no time to test things out now).
    • Sep 28 2011 | 7:15 pm
      Did run into a snag that the freeIMU library looks to interface via I2C to the arduino, meaning I can't access the individual pins. My sensor board is hardwired to pins on the arduino (straight to the analog pins).
      In looking at the library files for the freeIMU the core of the freeIMU library is the exact code I linked above by Madgwick. It's just buried amongst oodles of other library stuff.
    • Aug 06 2012 | 8:12 pm
      So I dusted off my sensor stuff and got it up and working, but I'm just looking at raw numbers in max. I had run into a big bottleneck (which made me shelve the project for 10 months) which was about having 2 XBees sending data to 1 XBee receiver. In the end, after some consultation with a hardcore hardware dude, he suggested using one receiver per transmitter. More gear, but less hassle/problems. For the time being I'm only using sender and one receiver (once I get everything working I'll just duplicate the stuff).
      The software/calibration problem is still the same.
      There's this very useful thread on the Arduino forum (,58048.0.html), although that primarily is just 6DOF (as opposed to 9DOF (3axis accel, 3axis gyro, 3axis magnetometer)). Everything is still about doing things in the Arduino/Processing. I want to do everything in Max, so the Arduino is literally just doing ADC and piping the raw data over XBee/serial.
      Has anyone done sensor filtering/fusing in straight max or externals?
    • Aug 06 2012 | 8:24 pm
      Yes, but very simple. Calculating pitch and roll from my MiniBees' 3-axis accelerometer (you already know it but might be useful to others: ), filtering/smoothing and velocity/acceleration calculation using FTM/MNM objects.
      The MiniBees use a Python interface though. It sends the raw sensor data to Max over OSC.
    • Aug 06 2012 | 9:01 pm
      The gist would be the same though, working with the raw values in Max (mine are coming in over serial directly).
      Can you post your filtering/smoothing code? I don't have FTM/MNM (ircam webpage is currently down? (, but I can maybe get some ideas from it.
    • Aug 07 2012 | 9:34 am
      here you go, it's really basic use of mnm.alphafilter and objects:
    • Aug 07 2012 | 11:23 am
      I've been hesitant to install ftm in the past as I've been trying to limit my dependencies/externals for maximum future-proofability, I might give it a spin to take a look at this.
    • Aug 07 2012 | 11:44 am
      i guess you could look up the algorithms and roll your own
    • Aug 07 2012 | 3:52 pm
      Ok sat down and messed with this for a bit and got some kind of OK results.
      Here's the code:
      I followed the instructions from that arduino forum post above (,58048.0.html) to do the calibration and some of the math.
      I also noticed that I'm getting some pretty extreme sensor noise (some of the axis jumping from 0 to 250 over and over) which is mucking up all the math down the line. Here's a video of the problem:
      In the video it's only happening to the Accel X axis and magnetometer Z axis, but shortly after filming the vid it was happening to both the Accel X and Y axis.
      I made this thread on the arduino forum with more details about the problem.,117638.0.html
    • Aug 10 2012 | 9:45 pm
      So it seems that the problem I'm having is because I'm sending 10bit and 16bit data across an 8bit pipe. I know the Arduino does 10bit ADC and I'm guessing the i2c bus does 16bit, but all of it is going across serial/XBees as bytes (which are 8bit data?). So the jumping around I'm seeing is because all my top bits are getting truncated.
      So with that being the case, I'm not sure how to send 10bit data across serial using the XBee (if not using Serial.write, then what?).
    • Aug 11 2012 | 8:07 am
      Maybe you should have a look at how SenseStage does it in their python interface. It does 10bit.
    • Aug 11 2012 | 3:16 pm
      I think the part that handles it is the Arduino code to xbee. The Python interface (if I understand correctly) is just receiving that data, and then sending it to whatever app you want.
      Looking through the sensestage files it looks like they don't use a standard arduino firmware/bootloader so it's hard to gauge what's going on, and more specifically what command is being used to send the data across.
      Looking through some of my older code it looks like I was sending each sensor value as two bytes, a highByte and a lowByte, like so:
      Serial.write(highByte(z)); Serial.write(lowByte(z));
      Don't remember/know why I was doing that back then, and why i stopped doing it, but I'm guessing it was to do with sending the first half of the message, then the second half, but I don't know what to do with that once it gets in Max.
    • Aug 11 2012 | 3:18 pm
      There's also "Serial.println" which sends the value as ASCII instead of Bytes. I do want to keep the messages super tight/short for minimum latency and less chance of buffer overflow/crashes.
    • Aug 11 2012 | 3:41 pm
      Ok so that's it, I need to send each sensor as two bytes, a highByte and a lowByte. Now I need to figure out how to combine each pair into a single value in max.
      I'm using the serial call-response method so I only send values when I ask for them, to keep everything nice and clumped together, but I don't know how to fuse the bytes together particularly since some values are 10bit (accel/gyro) and some are 16bit (magnetometer).
      Here's the relevant bit of the Arduino code:
      // if we get a valid byte, read analog ins: if (Serial.available() > 0) { inByte =;
      AccX = analogRead(aX); AccY = analogRead(aY); AccZ = analogRead(aZ); GyroX = analogRead(gX); GyroY = analogRead(gY); GyroZ = analogRead(gZ);
      Serial.write(highByte(AccX)); Serial.write(lowByte(AccX)); Serial.write(highByte(AccY)); Serial.write(lowByte(AccY)); Serial.write(highByte(AccZ)); Serial.write(lowByte(AccZ)); Serial.write(highByte(GyroX)); Serial.write(lowByte(GyroX)); Serial.write(highByte(GyroY)); Serial.write(lowByte(GyroY)); Serial.write(highByte(GyroZ)); Serial.write(lowByte(GyroZ)); Serial.write(highByte(x)); Serial.write(lowByte(x)); Serial.write(highByte(y)); Serial.write(lowByte(y)); Serial.write(highByte(z)); Serial.write(lowByte(z));
    • Aug 11 2012 | 4:04 pm
      Bitwise operations are one of my many coding dark spots but I know there are some objects in Max...
    • Aug 11 2012 | 4:23 pm
      You and me both..
      I tried messing around with & and | and got nothing joyful. From what I'm getting as raw bytes I'm getting a value between 0-255, so I figured sending two ints between 0-255 into &, but that doesn't work like I thought it would.
      Also did some searching and found how to convert a value into two bytes, but don't know how to do it the other way around.
    • Aug 16 2012 | 5:54 am
      To reverse the process bitshift up by 8 the dmx hi and add it back into the dmx low, divide the total by 65536.0 to put the number back into the range of 0.0 to 1.0
      I know you don't want to use extra externals, but the bit and byte externals from Peter Elsea's Lobjects can help to visualize how bitshifting works
    • Aug 16 2012 | 3:32 pm
      I do want to stay external free (whenever possible).
      I ended up doing pretty much the same thing with this:
    • May 07 2013 | 5:38 am
      I'm trying to get a similar output in openGL but using an iphone's accel/gyro data via C74. Having a bear of a time getting it calibrated. Would love to hear if anyone has had any luck with this. Thanks!
    • May 11 2015 | 2:12 pm
      Hi Rodrigo
      Any chance you can share what you did to achieve useful data from the sensor array into Max? polling the serial port right? I'm using the Sparkfun 9DOF. What did you have running in the Arduino? Most of my Arduino experience is using Maxuino with the 'Standard Firmata', which pours out useful info for you, no polling, no actual Arduino coding. This is not possible afaik with the 9dof board. I can get data into max on the serial port, it's just unintelligible, and I'm just seeking some help with how to Poll the Serial port for useful data.
      I know it's an old post but here's hoping! Surprising using this type of Sensor with Max doesn't seem to be that common...
      Cheers in advance for any help
    • May 11 2015 | 2:23 pm
      Balls, my typed out response evaporated...
      Here goes again!
      This kind of stuff ended up being a real pain in the butt, so I abandoned using serial for this type of stuff years ago. I moved on to a couple of x-io products (, mainly the x-OSC and the x-IMU. The x-OSC is ridiculously easy to use (you get OSC data pouring right in to Max). The x-IMU has amazing onboard sensor fusion algorithms but is harder to get data in to Max with.
      For the sake of sharing, here is the last thing I was using for parsing with Max. There's some clunky hack in the serial parsing section, and to be honest, I don't remember why it was needed, but it was, hehe. Hope this is of some help.
    • May 12 2015 | 10:51 am
      Thanks Rodrigo- massively useful response in lightning time!
      Got me all keen for the x-io products now- x-OSC looks like a user friendly dream, but the onboard AHRS stuff on the x-IMU looks worth the extra effort. Any chance you can give me a really brief headsup on how hard it was to get rumbling with Max? on the face looks like I would hit similar issues ot the Sparkfun board- no?
      Also- just searching before I post this and found this thread of yours:
      Suggests that would be exactly what the doctor ordered regarding the x-IMU? did that work out for you?
      Thanks again for the helping hand!
    • May 12 2015 | 12:19 pm
      I used the IMU2OSC app, but it had (still has?) a memory leak, and it didn't have all the outputs, specifically the fused version that I wanted. Hence me looking for a dedicated external to parse/receive everything natively in Max.