Guide to gyro and accelerometer with Arduino including Kalman filtering

@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

Hi Lauszus,

I've been learning a lot from your code, thanks very much for putting it on here.

I have a board with single axis gyro with 3 axis accelerometer, analogue outputs.

I'm looking to find the Roll angle, so the single gyro board will need to be orientated up on its end so that the Roll axis goes through the middle of the gyro. The accelerometers with then be: X=up/down, Y=left/right, Z=forward/backward.

In this configuration which combination of the X,Y and Z accelerometers should I be using in the code?

I am guessing that it's the X and Y as the Z axis accelerometer is not effected by roll when the board is mounted on it's end

Your code uses X and Z and Y and Z in the arctan function, so I should use X and Y, correct?

@richardtheboffin
It depends on your gyro - some have to be orientated up others don't.
Regarding which axis you should use on your accelerometer: I can't say for sure. It depends on how it's orientated. Either it's X and Z or Y and Z.
Try to modify this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/LPR530AL_%20LY530ALH_%20ADXL335/LPR530AL_%20LY530ALH_%20ADXL335.ino, so it's uses the correct sensitivity values for your setup.
Then afterwards you can easily see which angles should be fused together.
You might also need to invert some of the readings, as I have done here: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/LPR530AL_%20LY530ALH_%20ADXL335/LPR530AL_%20LY530ALH_%20ADXL335.ino#L51

Hi Lauszus,

I have your code running ok with the single gyro and x,y,z accelerometers.

I'll dump the intermediate angle calculation values out and see if I can figure which pair I need to combine with the gyro.

Cheers,

Richard.

Hi Lauszus,

All sorted out now. I had everything coded correctly but missed the initial offset from zero for the down axis accelerometer due to gravity.

It works very well and rejects linear movements really well.

Just need to feed the angle into my motor controller now.

Thanks for everything!

please , can you share with us all the program?

i was able to implement the filter thanks for this guide it was really helpful...thank you very much
but i dont know how to relate these values to motor's rpm of a quadcopter so i will stabilize, can anybody post links to guides like this?

tensa:
i was able to implement the filter thanks for this guide it was really helpful...thank you very much
but i dont know how to relate these values to motor's rpm of a quadcopter so i will stabilize, can anybody post links to guides like this?

You will need to use a PID controller to give roll,pitch,yaw outputs when the orientation deviates from 0,0,0. However, I have used these filters on a quadcopter and they suffer from the problem of linear acceleration affecting the pitch and roll. The filters are fast enough, smooth and very stable (calculation time was 80micros for twin Kalmans on an Arduino Due) but you'd probably be better off using Mahonys DCM or Madgwicks filter - they're less sensitive to linear acceleration but a bit more prone to noise (vibration from the motors).

You'll find plenty of links via Google for any of this stuff.

i want to work with MPU-6050-Triple Axis Accelerometer Gyro can it be done

i want the same guide for MPU-6050-Triple Axis Accelerometer Gyro