Go Down

Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering (Read 313 times) previous topic - next topic

Lauszus

#batista1987
Try to send a PM to Kashif and ask him for his code :)


asrinivasan

#227
Jan 04, 2012, 09:03 pm Last Edit: Jan 04, 2012, 09:19 pm by asrinivasan Reason: 1
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
Code: [Select]

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
Code: [Select]

 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
Code: [Select]

 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

elwylly

Hi Lauszus,

Quick question. What's the other gyro in this IMU for? You use the Gx4 but, what's the Gx1? I just don't get why you have 2 gyros but only use this one.

Thanks!

Lauszus

#asrinivasan
You are doing the conversion from radians to degrees wrong:
Code: [Select]

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;


Instead it should be:

Code: [Select]

xACC_angleRad = (acos(sensorValue[ACC_X]/R))*-1;
xACC_angleDeg = xACC_angleRad * RAD_DEG;


I guess you are multiplying by -1, to transform the angle, so it has the same sign as the gyro :)

#elwylly
The Gx4 output the Gx1 output multiplied by 4. It just allows the user to get either higher or lower sensitivity depending on what you need it for.

Go Up