Digital gyro, acc, kalman, pitch, roll tutorial

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:

Wire.beginTransmission(address);
Wire.write(register);
Wire.write(value);
Wire.endTransmission();

In my code they were made into functions:

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

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

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:

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.

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.

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

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)

workingIMUdata.ino (9.35 KB)

IMU SerialChart conf.scc (485 Bytes)

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

ahh, finally found it useful! yay!

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

Our sensor fusion code works really well.

Jay

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

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

bACCx

from ? Cant seem to find it anywhere else in your code.

Cheers

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

sirbow2:
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 :wink: