Guide to gyro and accelerometer with Arduino including Kalman filtering

See this code I wrote a while back: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino for the MPU6050

And here is some code for the ITG3205/ITG3200 gyroscope and ADXL345 accelerometer: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino

That's the whole point of the filter

Well no. The whole point of the filter is dealing with noise. The problem of distinguishing true acceleration from the effect of gravity on the sensor remains. I've yet to see a convincing implementation of the Kalman filter approach which considers more than one dimension.

michinyon:

That's the whole point of the filter

Well no. The whole point of the filter is dealing with noise. The problem of distinguishing true acceleration from the effect of gravity on the sensor remains. I've yet to see a convincing implementation of the Kalman filter approach which considers more than one dimension.

You should look at the FreeIMU's implentation of MARG orientation fusion algorithm. I am using a modified version of this and it seems to work well.

@Lauszus
Thanks for your guide.
I'm working with an MPU6050 for my balancing robot project. But the model required yaw measurement. I wonder if the MPU6050 is capable of measure the yaw accurate enough or it needs additional magnetometer. Please help :smiley:

longlongvn:
@Lauszus
Thanks for your guide.
I'm working with an MPU6050 for my balancing robot project. But the model required yaw measurement. I wonder if the MPU6050 is capable of measure the yaw accurate enough or it needs additional magnetometer. Please help :smiley:

It can do short term yaw measurements just fine, but it drifts over time. So if you just need to know incremental yaw measurements during yaw events it will work fine. However if you are using to it maintain a specific orientation for extended periods of time, it will not work well unless you integrate a magnetometer.

thank you very much. I'm gonna try integrating an magnetometer to my IMU.

The MPU-6050 supports auxillary SCL and SDA. My GY-521 breakout board broke the pins out and called them XCL and XDA. If the magnetometer you choose uses I2C, you can plug it directly into the MPU-6050.

I used a modified FreeIMU library to get mine all working.

@Lauszus Sir I have a L3g4200d. It just a 3 axis Gyro sensor. My problem is to how to use kalman filtering to Arduino Uno. And I want to get the reading values to a degree(x,y,z). To able to control my Servo. Thanks in Advance.

thank you for your help ,that's kind of you
the code that you give me, I'll bring in the card and it will arduino controller actuators? is that the code is complete?

@michinyon
If true acceleration is not noise in the system, what is it then?

@longlongvn
As jjspierx pointed out you will need a magnetometer as well if you need a precise yaw measurement.

@wturn
To translate the gyro rate in to an angle simply multiply the reading by the delta time as I described in the blog post, but it will properly drift a lot if you use a gyro only.

@aziz-qessate_arduino
What do you mean? Please describe it in more detail.

Lauszus do you know what specifically I could change within your code to further minimize the noise from shaking the sensor in the z axis. There has to be something that can be done. Iv'e read through all your blogs on calibration and such but nothing is jumping out at me. You say I'm relying on the accelerometer values too much, but me being bad at math it doesn't jump out at me very quick exactly where I can adjust its reliance on the accelerometer or the gyro. It seems that the Q_angle, Q_bias variables are what I adjust and those correspond to the variance and the bias respectively from your blog. (TKJ Electronics » A practical approach to Kalman filter and how to implement it) The filter will filter out relatively small vibration, but not when the road gets really bumpy. If I shake the MPU6050 relatively strong up and down it just messes it slowly drifts the camera to one side or the other. It seems that no matter what I tune these variables too, it still doesn't work properly. Is there possibly something else that can be done with this chip, or are there other sensors that would be better suited for what I'm trying to accomplish. My gopro camera mount is soooo close to working properly.. Any extra help would be greatly appreciated. I'll continue working through your blog in the meantime.

It seems there is also a way to change the sensitivity of the gyro and accelerometer but I can't find any information on how to do that for a newbie like myself... =/

So basically I have limited the issue with the motorcycle gyro cam down to the accelerometers z value. What I did to help reduce issues with vibrations was filter the incoming values by just loading them into an array and averaging the values. By increaing the amount of values loaded into the array it helps smooth it more, but also slows down responsiveness. But for the application, the responsiveness isn't too important. Seems to be working pretty well so far.

@ezikiel12
That exactly what I would have recommended or you could try to look at the difference between the current angle and the new angle from the accelerometer, if the difference is larger that a certain value, then decrease the value or even just drop it.

I'm actually working on a project with the MPU-6050 right now, but unfortunatly I can't show the full source code, but I will just copy paste the relevant sections:

This goes into setup:

Wire.begin();
i2cWrite(0x1B,0x00); // Set Full Scale Range to ±250deg/s
i2cWrite(0x1C,0x00); // Set Full Scale Range to ±2g
i2cWrite(0x19,0x27); // Set the sample rate to 200Hz
i2cWrite(0x6B,0x01); // PLL with X axis gyroscope reference and disable sleep mode

uint8_t buf;
i2cRead(0x75,1,&buf);
if(buf != 0x68) { // Read "WHO_AM_I" register
  Serial.println(F("Error reading sensor"));
  while(1); 
}

delay(100); // Wait for sensor to get ready
uint8_t data[4];
i2cRead(0x3D,4,data);  
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;

kalman.setAngle(accAngle); // Set starting angle

And put this in your loop:

uint8_t data[8];
while (!i2cRead(0x3D,8,data));
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
gyroX = ((data[6] << 8) | data[7]);

accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
gyroRate = (double)gyroX/131.0;

pitch = kalman.getAngle(accAngle, gyroRate, (double)(micros()-timer)/1000000.0); // Calculate the angle using a Kalman filter
timer = micros();

And these are the i2cRead and i2cWrite functions:

void i2cWrite(uint8_t registerAddress, uint8_t data) {
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t nbytes, uint8_t * data) {
  unsigned long timeOutTime = micros();
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++) {
    if (Wire.available())
      data[i] = Wire.read();
    else {
      timeOutTime = micros();
      while (((micros() - timeOutTime) < I2C_TIMEOUT) && !Wire.available());
      if (Wire.available())
        data[i] = Wire.read();
      else
        return 0; // Error in communication
    }
  }
  return 1;
}

If true acceleration is not noise in the system, what is it then?

The accelerometer sensor reading is part of the measurement of the state of your system, just
as the gyroscope sensor reading is.

So it should be part of the measurement vector, not the input vector

If you have a system model like this

x(k) = F.x(k-1) + B.u(k) + noise
z(k)= C.x(k) + noise

then any quantities you are actually able to measure to enable identification of the best estimate
of the state of your system, x, go into the z vector not the u vector.

first i will thank you for your help , my project is the same one like the project of RIVELLO; so i need the all code please ,

@michinyon
What do you mean? z is the measurement of the state - in this case the measurement is the angle measured using the accelerometer. The x in your equation has nothing to do with the estimate of the state. It is the state of the system which is not observable.

@aziz-qessate_arduino
As I have already wrote to you in my email. I don't have the code for the self stabilizing platform, as I didn't write it. But you could try to contact him here at the forum or youtube and ask for the code: Arduino Forum
You can find some of his code here: Self balancing waiter tray. (or almost it) - #4 by Rivello - Exhibition / Gallery - Arduino Forum

Thanks laz again. Very helpful, taught me quite a bit about I2C that I didnt understand in just a little snippet of code. Thanks to your help though I have successfully got it working extremely well. The biggest issue was not so much the bumpyness of the road, I was able to tune it out and use a band pass filter + some averaging to get that taken care of. It was the high frequency engine vibration that was killing it. The quadcopter guys have the same issues with there motors and sensors. I was able to completely fix this by mounting the system on the bike with some padding and velcro.

Check it out!!

@ezikiel12
It looks really nice. Glad I could help :slight_smile:

Hi,

someone can share the file with code for self positioning platform on e-mail ?

I would be gratefull for help

He simple added the following to my code: Self balancing waiter tray. (or almost it) - #4 by Rivello - Exhibition / Gallery - Arduino Forum

You can also send him a personal message if you like: Arduino Forum