Go Down

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


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?


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

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

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.

What do you mean? Please describe it in more detail.


Feb 01, 2013, 04:05 am Last Edit: Feb 02, 2013, 12:57 am by ezikiel12 Reason: 1
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. (http://blog.tkjelectronics.dk/2012/09/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.


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

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;
if(buf != 0x68) { // Read "WHO_AM_I" register
 Serial.println(F("Error reading sensor"));

delay(100); // Wait for sensor to get ready
uint8_t data[4];
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:
Code: [Select]

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

void i2cWrite(uint8_t registerAddress, uint8_t data) {
 Wire.endTransmission(); // Send stop
uint8_t i2cRead(uint8_t registerAddress, uint8_t nbytes, uint8_t * data) {
 unsigned long timeOutTime = micros();
 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();
       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 ,


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.

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: http://arduino.cc/forum/index.php?action=profile;u=48118
You can find some of his code here: http://arduino.cc/forum/index.php/topic,68755.msg507679.html#msg507679


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!!





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

I would be gratefull for help

Go Up