Hey lauszus, first off thanks for writing this great guide- I'm also using kas's approach and both have helped me get much farther than I would have thought- however I have hit a roadblock and through you might be able to help. btw- I'm using a 2-axis XY gyro (

http://www.pololu.com/catalog/product/1266/specs) and 3-axis accelerometer (

http://shop.moderndevice.com/products/mma7260qt-3-axis-accelerometer) in tandem. If it's needed I can also connect a standalone single-axis gyro for z-axis measurement, but I don't think it will be needed for angle measurement now.

Like yours, my bot also has the requirement of incline-sensing in both X and Y axes- therefore, I used the force-vector method as you did. My raw acc-angles look great, showing incline measurements independent of one another (essential for my bot to properly balance, as it's a biped). Here's a shot of my acc-calculate angles :

[img=http://img43.imageshack.us/img43/9821/scrn1c.th.jpg]red line = y-axis inclination, black line = x-axis inclination. As you can see, the angle measurements are clearly independent of one another when the imu is tilted in a cardinal direction. (imu board tilted 90deg right with no y-axis inclination)

However, after applying kalman filtering, I get this:

[img=http://img85.imageshack.us/img85/8039/scrn2n.th.jpg]Here, the board is in the same orientation as in the previous screenshot, but it is clear that the two measurements are not independent of each other after applying the kalman filter. What's more, the angle estimation jumps and then settles at the true angle. Finally, a screenshot with both acc and kalman data shown:

[img=http://img689.imageshack.us/img689/6681/scrn3m.th.jpg]The highlighted box shows how the x-axis measurement jumps and settles in response to the y-axis tilt, while the accelerometer reading is relatively unchanged.

Here is the relevant code:

source of gyro data

int getGyroRateX() { // ARef=3.3V, Gyro sensitivity=2.5mV/(deg/sec)

return int(sensorValue[GYR_X] / 0.775); // sensitivity = 0.0025 / 3.3 * 1023

}

int getGyroRateY() { // ARef=3.3V, Gyro sensitivity=2.5mV/(deg/sec)

return int(sensorValue[GYR_Y] / 0.775); // i'm guessing this value is in degrees

}

force vector calcs

R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));

xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1; //unless I'm not mistaken, this angle is in rads

xACC_angleDeg = xACC_angle * RAD_DEG;

xACC_angleQuid = xACC_angle * RAD_DEG * DEG_QUID;

yACC_angle = (acos(sensorValue[ACC_Y]/R)-pi/2)*-1;

yACC_angleDeg = yACC_angle * RAD_DEG;

yACC_angleQuid = yACC_angle * RAD_DEG * DEG_QUID;

kalman calculations

xGYRO_rate = getGyroRateX();

xGYRO_angle = xGYRO_angle + xGYRO_rate*loopTime/1000;

yGYRO_rate = getGyroRateY();

yGYRO_angle = yGYRO_angle + yGYRO_rate*loopTime/1000;

xfinAngleDeg = kalmanCalculateX(xACC_angleDeg, xGYRO_rate, loopTime);

yfinAngleDeg = kalmanCalculateY(yACC_angleDeg, yGYRO_rate, loopTime);

These angle measurements will be feeding a PID that drives an IK calculator to get joint angles for the bot, so it is a must that these angles are nearly dead-on

Could you help me figure this out? Thanks