Pages: [1]   Go Down
Author Topic: Digital gyro, acc, kalman, pitch, roll tutorial  (Read 4013 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Sr. Member
****
Karma: 1
Posts: 486
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

hey everybody, i finally got my digital pololu MiniIMU9 code working fairly well. currently, it has gyro, acc, mag, pitch,roll, kalman and yaw implemented(everything).  i wrote this post here so that otherd may understand it. let me know how it is!

I finally got around to finishing some code for my Arduino that interfaces with the Pololu MiniIMU9 to get gyro, accelerometer and magnetometer data. But, that would be boring alone and so i implemented pitch, roll and yaw(with mag), too. Also, I somehow managed to successfully implement Kalman filtering to combine gyro and acc data. But, i did have a few issues as i was writing this code with setting up the Wire interface and calculating pitch/roll/yaw from gyro, acc and mag data.

Hopefully, with this post i will explain how i did this so that others can apply it to other IMUs etc. First thing you need to do is enable the sensors through the registers. Most times this enable register (often the first register) is named similar to mine:  CTRL_REG1. Looking the datasheet for the sensor, you can find what value this register needs to be for your startup settings. This is how you send values to registers:
Code:
Wire.beginTransmission(address);
Wire.write(register);
Wire.write(value);
Wire.endTransmission();

In my code they were made into functions:
Code:
writeGyroReg(L3G4200D_CTRL_REG1, 0b10101111); //enable gyro, 0b10101111, 1010- 400hz ODR/50hz cutoff, 1111- default(enable all axis/normal mode)

writeAccReg(LSM303_CTRL_REG1_A, 0b00110111); //enable acc, 0b00110111, 001- normal mode, 10- 400hz ODR, 111- default(enable all axis)

writeMagReg(LSM303_MR_REG_M, 0x00); //enable mag

Next, you need to look for sensitivity; it will given for different modes (2000dps, 1000dps etc) for the gyro so you need to use the sensitivity factor that lines up with the dps. Same for AccGain, if you use 8 gauss mode, you need to use the sensitivity for that mode. This code is how i made sure my gyro/acc was on the right mode so that my Gyro/AccGain lined up in the calculation part of the code(explained later):
Code:
//gyro settings
  writeGyroReg(CTRL_REG4, 0b00110001); //0-continous update, 0- little endian, 11 2000dps, 0- blank,  00- self test disabled, 0 spi 4 wire(if used)

  //acc settings
  writeAccReg(CTRL_REG4_A, 0b00110000); //0- continuous update, 0- little endian, 11- 8g scale, 00 default, 0- blank, 0- self test disabled
So it seems that registers 1(enable, data rate, filters) and 4(update mode, sensitivity etc) are the most important. I still need to figure out how to choose the best  register settings for ODR and high and low pass filters.

The next thing you need to do is get some values from the sensors so that you can determine the level position values and use that as a bias throughout the code. Simply get xyz values from the Acc and Gyro a 100 times, or so, and then divide by 100 to get an average.

But, you need to read the values from the sensors first, and to do that something similar to enabling the sensors is used. It first begins the communication with the sensor, and then writes a value to the output register signalling that it wants data. Then it waits for the request data and then moves them to bytes. The bytes are then recombined into whole numbers into integers of the x/y/z axis.
Code:
Wire.beginTransmission(address);
{
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(address, 6);

  while (Wire.available() < 6);

  byte xla = Wire.read();
  byte xha = Wire.read();
  byte yla = Wire.read();
  byte yha = Wire.read();
  byte zla = Wire.read();
  byte zha = Wire.read();

  ACCy = -((xha << 8 | xla) >> 4); //reversed y axis
  ACCx = -((yha << 8 | yla) >> 4); //reversed x
  ACCz = (zha << 8 | zla) >> 4;
}

Now that you have the data into easy to work with integers, you can calculate the pitch, roll, and yaw etc. That in its self is easy; all you have to do is: pitchGyro = (GYROx - bGYROx) / GyroGain; or rollGyro = (GYROy - bGYROy) / GyroGain; but that wont provide very accurate results. GYROx is the value received from the gyro, bGYROx is the bias for the gyro calculated when the IMU was first turned on, and GyroGain is the factor that gives the value in DEG/s from the raw sensor value. This can be found in the data sheet as i mentioned before.

To provide accurate results, you need to combine acc and gyro values with a kalman filter. The Kalman filter has two main parts. One calculates pitch and roll individually based on acc and gyro:
Code:
Gyro pitch = (Gyro pitch  + ((GyroXvalue -  GyroXbias) / GyroGain)) * timeStep;
Accel pitch = (atan2((AccYvalue -  AccYbias) / AccGain, (AccZvalue  -  AccZbias) / AccGain) * 180.0) / PI;
Pitch prediction = Pitch prediction + ((GyroXvalue -  GyroXbias) / GyroGain) * timeStep;

Gyro roll = (rollGyro + ((GyroYvalue -  GyroYbias) / GyroGain)) * timeStep; //gyro roll in deg
Acc roll = (atan2(( AccXvalue -  AccXbias ) / AccGain, (AccZvalue -  AccZbias) / AccGain) * 180.0) / PI;
Roll prediction = rollPrediction - ((GyroYvalue -  GyroYbias) / GyroGain) * timeStep;
timestep is  how long one loop of the program is, and (*180.0) / PI is converting radians into degrees.

and the other, which is the actual filter which combines the gyro and acc data to get pitch/roll. i dont really know much about this, so i cant really explain.
Code:
void Kalman() //kalman filter for pitch / roll
{
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));

  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;

  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
}

Next up is to calculate yaw accurately with the help of the mag. MAGx/y/z are the raw values from the sensor.
Code:
 //YAW!
  //coonvert raw acc to g.
  float newACCx = ACCx - bACCx;
  float newACCy = ACCy - bACCy;

  newACCx = newACCx / pow(2, 15) * 8;
  newACCy = newACCy / pow(2, 15) * 8;

  float pitch = asin(newACCx);
  float roll = asin(newACCy/cos(pitch));
  
  //this part is required to normalize the magnetic vector
  if (MAGx>xMAGMax) {
    xMAGMax = MAGx;
  }
  if (MAGy>yMAGMax) {
    yMAGMax = MAGy;
  }
  if (MAGz>zMAGMax) {
    zMAGMax = MAGz;
  }

  if (MAGx<xMAGMin) {
    xMAGMin = MAGx;
  }
  if (MAGy<yMAGMin) {
    yMAGMin = MAGy;
  }
  if (MAGz<zMAGMin) {
    zMAGMin = MAGz;
  }

  //Map the incoming Data from -1 to 1
  xMAGMap = float(map(MAGx, xMAGMin, xMAGMax, -30000, 30000))/30000.0;
  yMAGMap = float(map(MAGy, yMAGMin, yMAGMax, -30000, 30000))/30000.0;
  zMAGMap = float(map(MAGz, zMAGMin, zMAGMax, -30000, 30000))/30000.0;

  //normalize the magnetic vector
  float norm = sqrt( sq(xMAGMap) + sq(yMAGMap) + sq(zMAGMap));
  xMAGMap /=norm;
  yMAGMap /=norm;
  zMAGMap /=norm;

  //new calcs:
  float xh = xMAGMap * cos(pitch) + zMAGMap * sin(pitch);
  float yh = xMAGMap * sin(roll) * sin(pitch) + yMAGMap * cos(roll) - zMAGMap * sin(roll) * cos(pitch);
  float zh = -xMAGMap * cos(roll) * sin(pitch) + yMAGMap * sin(roll) + zMAGMap * cos(roll) * cos(pitch);

  yawRaw = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
  {
    //do nothing, yaw value is ok
  }
  else
  {
    yawRaw += 360;
  }

code attached(too big smiley-sad)

the Processing serial data grapher is also attached so you can see the results.

Next is to implement some PID and make some balance bot magic!

* IMUdataGrapher.pde (3.67 KB - downloaded 61 times.)
* workingIMUdata.ino (9.35 KB - downloaded 91 times.)
* IMU SerialChart conf.scc (0.47 KB - downloaded 43 times.)
« Last Edit: August 26, 2012, 03:57:37 pm by sirbow2 » Logged

http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

Greenville, IL
Offline Offline
Edison Member
*
Karma: 15
Posts: 1328
Warning Novice on board! 0 to 1 chance of errors!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


  The blog post should be handy to get my MiniIMU9 working well! Thanks for the post, I will have to test it out soon!
Logged


Offline Offline
Sr. Member
****
Karma: 1
Posts: 486
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

ahh, finally found it useful! yay!
Logged

http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

Florida
Offline Offline
Newbie
*
Karma: 0
Posts: 26
Engineer at Jaycon Systems LLC
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hey guys, we did one too with the MinImu-9, but we used Avr Studios. All Code/explanation and video here smiley

http://www.jayconsystems.com/forum/viewtopic.php?f=22&t=40

Our sensor fusion code works really well.

Jay
Logged

Jaycon Systems LLC
   Automate Your Life!

www.jayconsystems.com

Offline Offline
Sr. Member
****
Karma: 1
Posts: 486
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

ive done a lot of changes (fixed yaw, moved the content into this post etc). enjoy!
Logged

http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

Offline Offline
Full Member
***
Karma: 0
Posts: 101
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Do you mind explaining why you are using the map function for the magnetometer? Is that really neccessary?
And where did you get the variable
Quote
bACCx
from ? Cant seem to find it anywhere else in your code.

Cheers
« Last Edit: March 28, 2013, 02:03:52 pm by freak174 » Logged

Offline Offline
Sr. Member
****
Karma: 1
Posts: 486
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

i dont quite know, haha. i copied that code. bACCx  is probably in a different function. do you want it?
Logged

http://dduino.blogspot.com all my Arduino/electronic projects!!!

{NEW} Getting Started, Learning, Reference + FAQ PDF!!:
[url]http://ar

Offline Offline
Full Member
***
Karma: 0
Posts: 101
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

i dont quite know, haha. i copied that code. bACCx  is probably in a different function. do you want it?

It got sort out when I downloaded your code. It was simply an average of 100 samples when you start the sensor, with other word offset/bias value.

Thanks anyway mate smiley-wink
Logged

Pages: [1]   Go Up
Jump to: