Go Down

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


Please have a look at the following example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/ITG3205_ADXL345.



hello Lauszus,

Can i use this example with a 10dof (accelero, gyro, magneto) ?




Thanks for the response Lauszus,

the reason I got confused probably was that either my Arduino due or my Imu are broken. I tried different codes from different people including yours and after almost two weeks of failing I'm now more or less able to write my own code - which also doesn't work. If I print the raw readings I get values around 65k for the Accelerometers in X an Y direction and for the Gyros while the X Accelerometer changes to low values around 20 - 200 when moving it in the right way. The Accelerometer in Z shows constant values of 1023.

If something similar occured to you or you can tell me if this probably is because of broken gear or not, I would be helped very much.

Thanks again,

edit: The Gyro readings change from values between 65k and 0 when moving the Imu but return to around 65k when holding it still. Sometimes the values switch from values around 65k to values around 0 very fast when holding the IMU still.


@ Lauszus
I tried the same code (the Kalman FIltering part) that you've given the link to. Not as a header file, but as a loop in the main sketch.
Initially, I put the same values for the Q_angle and R_angle. The response of the Kalman FIlter was very slow as a result of it. It took forever to catchup with the angle of the accelerometer(which is the value that I am comparing the filter with). You change the direction of the tilt, and it doesn't even move from positive to negative quickly, it takes a lot of time.
As I try to decrease the values of the Q_angle and R_angle, even when one of the values reaches the 5th decimal, the output of the filter gets stuck on the initial value(that of the accelerometer) and does not change. How ever the same code in C(I printed the values of the accelerometer and gyroscope in a file and read it in the program) produces a very fast response and works.
So. my problem is that I'm sure the lower values of Q_angle and R_angle will do the trick and I will just need to tune them, but the output gets stuck in Arduino.
What do you think should I do?
Arduino 2560,
IMU=MultiWii MWC Flight Control Module,Gyro= ITG3205, Accelerometer= BMA180.


I wish to add a lean sensor readout to my motorcycle. However I dont have strong physics on me. I can tinker with the tutorial codes a bit to get the angle readout but will this work on a moving motorcycle? From what limited research Ive done on the internet, it seems to me calculating the angle of a stationary object is fairly simple but when it comes to a motorcycle its a different thing altogether. If Ive understood it correctly then I think the combination of both the gyro and accelerometer will work, but again I am not too good at this.


Hello Lauszus and others!

I too am making a balancing bot with PID controlling (I will get a Bachelors degree for it!) and good PID loop relies on good input data so I tried out your sample sketch for ADXL/ITG combo (got mine of dealextreme for pennies). For some reason, the Kalman filter seemed to be too reliant on accelerometer so the output data was rather noisy for me and jumped all over the place during rotation.

First column is Gyro, second is Accelerometer, third is Kalman and fourth is Complementary. As you can see, Kalman pretty much follows the accelerometer. Since I could not figure this out, I gave up on Kalman and looked into ways to improve complementary filter and I did succeed in this!
First I changed tuning to 99.7 and 0.03, which improved the stability a bit. Second, I implemented two instances of running median filters to smooth out spikes in raw data - I had problems with large spikes coming in from both gyro and accelerometer, gyro ones making the result drift more than 10 degrees ever so often and afterwards rolling back to right results over the course of several seconds - unacceptable. Sudden odd spikes from the accelerometer on the other hand were compromising the static stability, with fast jumps of +/- 1.5 degrees.
Now, the simple median filter works by looking at three readings and organizing them from smallest to biggest value, returning the middle one; so if we get three readings i.e. 190; 25; 192 it would be simple to see which of them is the bogus one and by rearranging them to 25; 190; 192 the 190 would be pretty much on spot. Unless we get more than one spike in a row, simple median filter will do the trick just fine. And since it is rolling, the correct value of 192 will also pass - it wont be omitted permanently, just used in the next calculation! If it is right, it will pass. But you can use more elaborate median filters with i.e 13 data points also, there is a lot written about implementing it on the web.
This is more precise than rolling average filter, which would've given us the result of 135.66. I also tried that and I also tried running complementary filter's result through Kalman, the second integer being gyro rate.
I will stick by the improved complementary from now on, saves up a few process cycles for me!

The microcontroller I am running my sketch on for now is a humble, honest Arduino Micro. And the data looks damn sexy on that Processing graph :)

PS: code is attached to this post! I left Kalman in for reference. If it works on your processor just as well as it should and is up par with complementary or even better then good, it is something I did wrong then :D


First sorry for my bad english.
I used mpu6050 + kalman filter+ servo control cameras on a motorcycle.I want to keep the camera level, but to consider the centrifugal force or acceleration component dynamic when it?
Now with mpu6050 + kalman filter seem to shake not work on a motorcycle, for a mpu6050 dmp6 static may be normal, but the actual loading on the car but can not achieve the desired angle. I have to modify the program? You can give directions? I do not know how write program. there are examples to follow?
I really need it.
Please provide comments to help me.
Thank you.


Hi Lauszus

I'm trying to use ITG3205 and ADXL335 (not ADXL345) and I have a doubt about your code:

Code: [Select]
void loop() {
  double gyroXrate = -(((double)readGyroX() - zeroValue[3]) / 14.375);
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = (((double)readGyroY() - zeroValue[4]) / 14.375);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double accXangle = getXangle();
  double accYangle = getYangle();

  compAngleX = (0.93 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle);
  compAngleY = (0.93 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);

  double xAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter
  double yAngle = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter

  timer = micros();

  /* print data to processing */
  Serial.print(gyroXangle); Serial.print("\t");
  Serial.print(gyroYangle); Serial.print("\t");

  Serial.print(accXangle); Serial.print("\t");
  Serial.print(accYangle); Serial.print("\t");

  Serial.print(compAngleX); Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");

  Serial.print(xAngle); Serial.print("\t");
  Serial.print(yAngle); Serial.print("\t");



What is the purpose of gyroXangle / gyroYangle variables? You get the gyro rate (º/sec) over time (and it will generate drift) but yo dont use it to calculate the angles, or in complementary/kalman filter, isnt it?

I think you use it only for graphic / processing purposes... Am I right?

Another question that I have is that when you roll or pitch PCB close to 90º, the values get erroneous... this code only work for rotations < 90º in X and Y, is that correct? (I made a tipical 3D box with processing and using rotateX/rotateZ with the angles that I obtain with your code)

Thank you very much for your code and help.


Dear Lauszus,
You said that hopefuly we have millis() in order to measure the time lapse between readings...
I'm searching the way to measure delta time with millis or micros all around the forum and the playground and i just  get dispersed clues but nothing useful for a newbie like me...
Could you please take a minute, or any other samaritan soul, to extend the explanation on how to calculate d-time from a sensor reading? Or suggest a paper or a link to work it out by myself?
Thanks in advance!


Hi Lauszus,

Great guide on IMUs and the Kalman filter!

I seem to have a problem that some of the others faced but there is no mention of a solution in the thread.

I'm using a SEN10121 board (https://www.sparkfun.com/products/10121) which has an ADXL345 and an ITG-3200 with an Arduino Mega ADK.
I'm using your code from here - https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/ITG3205_ADXL345

Although you mention that the angle range at the output end of the Kalman filter should be -90 to 90, I'm getting values as follows -
(Let us consider anticlockwise rotation starting from 0 degrees on y axis)

Actual Rotation (Degrees)        Output from Kalman Filter (Degrees)
0                                                        0/360
90                                                      22-23
180                                                    0/360
270                                                    342-343
360                                                    0/360

Is there a way this can be corrected?
Your response will be really helpful.

Thank you!


Dear Lauszus,
You said that hopefuly we have millis() in order to measure the time lapse between readings...
I'm searching the way to measure delta time with millis or micros all around the forum and the playground and i just  get dispersed clues but nothing useful for a newbie like me...
Could you please take a minute, or any other samaritan soul, to extend the explanation on how to calculate d-time from a sensor reading? Or suggest a paper or a link to work it out by myself?
Thanks in advance!
I always think that examples are easier to understand than a long-winded old man. So, you can take a look at this project: HERE and this code is responsible for setting up the timing loop for the graph:

In the ScrnFuncts Tab, the function:
Code: [Select]

void drawGraph(unsigned long Pressure)
  const unsigned long GraphResponsemS = 514286 ; // full screen of 84 slots every 12 hours

To use the time as an event, this code makes it happen:
Code: [Select]

  PlotTime = TimeMarker + GraphResponsemS;    // this is our trigger to plot

  if (PlotTime < millis() )  // machine clock has advanced beyond trigger, so react

So, what we are really doing is setting up a situation where we wish something to happen at a future time. Then we check with each iteration of the main loop() to see if that has happened; that is we compare the current time (milliseconds) to that future time - when the "future" event is current or past, it is time to act.

After acting, we need to set our trigger event into the future again...
Code: [Select]

    PlotTime = millis();              // Reset with current time for future event triggering

I have a lots more examples of simple projects on my project page.

Sorry for hijacking the topic.



Hi ,
Firstly i thank you a lot for your effort and for sharing your knowledge
i'm actually working on a project for indoor localisation so i need to know how to use
the MPU9060 (interpretating accceleration anf gyroscope values ) to do so ...
 and please another question : how can i save all values in matlab workspace

Thanks a lot


@ Lauszus Hi I'm using the ITG 30205 and ADXL345 and want to know how you got the values of bias zeroValue [5] = {-200, 44, 660, 52.3, -18.5} to your ITG3205_ADXL345.ino code, I made a code just for this, the more the values do not agree especially in the case of the gyro, you can help me.


Hi all, I am new to the accelerometer/gyro world but am eager to learn.  I have the ITG30205/ADXL345 IMU with I2C.  I have used Lauszus's code (from Github) and Vpooler's code.  I get data values only of 225, 270, 180, and 325-335.  I was expecting 0, 45, 90, 135, 180, 225, 270, 315, 360 or 90 degree combinations something to that effect.  Is this correct?  Could there be something wrong?  Also my goal is to have the output "stable" (1 to 5 degree accuracy)  I hope that's not a pipe dream.  Could there be a way to get the code to go to 1 degree increments if not real-time output in degrees?  Any help is appreciated.



Hi friends,

Thanks for your all sharings. I need to ask a few questions. If you help me, I appreciate

To find 'gyroRate' , we will use this formula " gyroRate = (gyroAdc-gyroZero)/sensitivity "

Is this 'sensitivity' value for my choice?

How can I find the 'gyroZero' value?

To find 'accVal' we will use this formula " accVal = accAdc-accZero "

How can I find zero voltage and also accZero? Is not sensitivity needed to calculate acc value?


Edit: This data belongs to MPU6050

Go Up