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

Sep 25, 2011 at 6:31pm

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

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(www.takahashisshellfishconcern.com).

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.

This looks ideal:

http://www.youtube.com/watch?v=Egl75nv9E7s

And there is the (C) code posted on the project page:

http://code.google.com/p/imumargalgorithm30042010sohm/downloads/list

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):

– Pasted Max Patch, click to expand. –
#59042
Sep 27, 2011 at 9:05am

I found this these which shows how to handle some of the math:

http://cycling74.com/forums/topic.php?id=35218

The main bit is this:
expr sqrt ( $f1*$f1 + $f2*$f2 + $f3*$f3 )

That doesn’t address the C code stuff or calibration however.

#212130
Sep 27, 2011 at 10:09am

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.

#212131
Sep 27, 2011 at 11:56am

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.

#212132
Sep 27, 2011 at 1:32pm

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 !

#212133
Sep 27, 2011 at 2:48pm

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:

http://www.youtube.com/watch?v=fOSTOnQzZC

#212134
Sep 27, 2011 at 6:44pm

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.

#212135
Sep 27, 2011 at 9:10pm

Hmm, you make some compelling points. At the moment I’m polling serial at 10ms with a qmetro, so it’s not super fast.

Is this what you’re using?

http://arduino.cc/forum/index.php/topic,58048.0.html

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).

#212136
Sep 27, 2011 at 9:31pm

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).

#212137
Sep 28, 2011 at 2:27am

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.

#212138
Sep 28, 2011 at 4:26am

http://www.varesano.net/projects/hardware/FreeIMU#library

FreeIMU seems to be the way to go! The Sparkfun 9DOF boards use the exact same chips, far as I can tell.

#212139
Sep 28, 2011 at 7:46am

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:

http://www.sparkfun.com/products/10530

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).

#212140
Sep 28, 2011 at 7:15pm

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.

#212141
Aug 6, 2012 at 8:12pm

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 (http://arduino.cc/forum/index.php/topic,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?

#212142
Aug 6, 2012 at 8:24pm

Yes, but very simple. Calculating pitch and roll from my MiniBees’ 3-axis accelerometer (you already know it but might be useful to others: http://www.sensestage.eu/?page_id=48 ), 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.

#212143
Aug 6, 2012 at 9:01pm

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? (http://ftm.ircam.fr/index.php/MnM)), but I can maybe get some ideas from it.

#212144
Aug 7, 2012 at 9:34am

here you go, it’s really basic use of mnm.alphafilter and mnm.delta objects:

– Pasted Max Patch, click to expand. –
#212145
Aug 7, 2012 at 11:23am

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.

#212146
Aug 7, 2012 at 11:44am

i guess you could look up the algorithms and roll your own

#212147
Aug 7, 2012 at 3:52pm

Ok sat down and messed with this for a bit and got some kind of OK results.

Here’s the code:

– Pasted Max Patch, click to expand. –

I followed the instructions from that arduino forum post above (http://arduino.cc/forum/index.php/topic,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:

http://rodrigoconstanzo.com/sensors/sensors.MOV

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.

http://arduino.cc/forum/index.php/topic,117638.0.html

#212148
Aug 10, 2012 at 9:45pm

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?).

#212149
Aug 11, 2012 at 8:07am

Maybe you should have a look at how SenseStage does it in their python interface. It does 10bit.

#212150
Aug 11, 2012 at 3:16pm

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.

#212151
Aug 11, 2012 at 3:18pm

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.

#212152
Aug 11, 2012 at 3:41pm

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 = Serial.read();

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));

#212153
Aug 11, 2012 at 4:04pm

Bitwise operations are one of my many coding dark spots but I know there are some objects in Max…

#212154
Aug 11, 2012 at 4:23pm

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.

– Pasted Max Patch, click to expand. –

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.

– Pasted Max Patch, click to expand. –
#212155
Aug 16, 2012 at 5:54am

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

– Pasted Max Patch, click to expand. –

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

#212156
Aug 16, 2012 at 3:32pm

I do want to stay external free (whenever possible).

I ended up doing pretty much the same thing with this:

– Pasted Max Patch, click to expand. –
#212157
May 7, 2013 at 5:38am

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!

#212158

You must be logged in to reply to this topic.