Go Down

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


Hi there,

I'm working on a position tracking device and I'm using the Kalman library for smoothing my values. So far it all works fine, the only problem I have is the "wrap-around" when the angle jumps from 360 to 0 (or the other way around).
For example if I just look at the X axis, I get these values around the jump (red are the numbers which I can't explain):




So basically it jumps from 360 to a value near 180, then sinks to about 10 degrees, then goes to negative values of about -10 and then increases until it reaches 0 and then starts to behave normal again… The strange numbers are there for about 500 ms until it starts to behave normal again. This problem only appears in my values, that are filtered with the kalman "method". The angles given by my accelerometer are wrapping fine (from 360 directly to 0).
Any idea why this happens and how I can avoid it?

Thanks a lot in advance!


This is how I deal with it in the firmware for my balancing robot: https://github.com/TKJElectronics/Balanduino/blob/cc8beb5f3a78cfd615a00c490b6656bdcc1a9d8b/Firmware/Balanduino/Balanduino.ino#L290-L304.



You're a god! Works great, thank you for the quick response!

Now I have just one last problem with which I struggle:
EDIT: Since its difficult to describe, I recorded a few numbers, which explain my problem.
The left column are my roll values, the left column are my pitch values. They are the result of this calculation:
roll = (atan2(ay, az)+PI)*RAD_TO_DEG;
pitch = (atan2(ax, az)+PI)*RAD_TO_DEG;

I rotated my device around the X-axis (should only change my roll value). I did not rotate around the y-axis, so my pitch should theoretically stay at ca. 180 degrees all the time… but it doesn't!
Red is the part where it gets strange….

110.037712      177.94928;
108.725166     177.268387;
107.690384    178.456924;
105.609833    179.55899;
103.349213     181.768768;
102.630997    178.129913;
100.206619    180.742752;
97.836052      181.004349;
95.612274       180.824249;
94.362167       181.906158;
92.943687      180.99469;
91.328262       165.373825;
89.467445      57.259464;
88.114891        22.104139;
86.327652      15.940088;
85.284119       15.84662;
84.20356        12.40211;
82.488235      13.507222;
81.106842       7.860759;
80.041031      5.403023;
78.477669      7.589334;
77.705246      7.589335;
75.788643      7.558187;
75.097031      6.980302;
73.839981      5.246093;
73.038017      4.990366;

I hope you understand what I mean… Any ideas why this happens?
Again, I really appreciate your help!


This Video shows my problem:



This graph code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/Graph is for these codes:
While this one: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph is for the MPU-6050.

Can you tell me more detail how to use graph code. I copied the folder of MPU 6050 and Graph to Libraries of Arduino, but the problem is when I verified the code of Graph, it annonced there are errors with the code, some function isn't defined. Morever, when I clicked on Graph.exe, it opened for the first time, bút the second time, nothing showed up.
Looking  forward to ur reponse.


I am aware of the problem and have actually solved it in my local repo, but I need to clean it a bit before I push it. You should take a look at this app-note: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf for now ;)

You should not open the Graph code in the Arduino IDE, but in Processing: http://processing.org/.

You can find more information in the readme: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph#developed-by-kristian-lauszus-tkj-electronics-2012.


Feb 22, 2014, 06:42 pm Last Edit: Feb 22, 2014, 06:45 pm by PeterCheng Reason: 1

You should not open the Graph code in the Arduino IDE, but in Processing: http://processing.org/.

You can find more information in the readme: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph#developed-by-kristian-lauszus-tkj-electronics-2012.

It works perfectly thanks to ur help. And I'm also doing control motor DC (with encoder attached) by PID, so I'm looking for a program/graph apps which can show my signal response from my motor like the graph used for Mpu6050, can u suggest one ?


Hi Lausuz,

Thanks for this amazing guide. I'm new to IMUs and have learned so much from your tutorials and your active replies to questions! I made it through 15 pages of the thread before I thought I'd ask my question (maybe you could point me in the right direction if it's been asked?)

I'm looking to send a signal when a certain rate is detected by the IMU. I've gotten the program to work and it is exactly what I hoped for. Now I'd like to utilize it to send a signal when "x" degrees/sec is reached. Is this something that would be possible?

Also, could the signal be sent when either the x or the y axis hit that rate? as of right now, when I tilt the x axis to 90 degrees, the y goes crazy and bounces the whole length of the graph very quickly; which would send false positives for the angle/s occurring.

Thanks for your help and look forward to your reply ! :D



You can get the unbiased rate using the following function: https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.h#L81.

I know what you are talking about and will upload a fix very soon. Please read this for now: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf.


I just updated the MPU-6050 code. The changes newest code can be found in the Github repository: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050.


HI Lauszus,

I finally finished reading the 34 pages of this post! VEry valuable information, but I have not managed to find anyone with the same problems that me.

I have written some doubts in your  Kalman filter article comments . I've thought about moving them to this thread so that if someone encounters the same problem, here you can find the information to solve it (if I can ever fix it ... ) :)

First of all, I use your code LPR530AL_LY530ALH_ADXL335. I only modified the data sensitivity to suit my gyroscopes, which are the IDG650 and ISZ650. Here are their datasheets: http://www.roboard.com/Files/G145/Datasheet_IDG650.pdf and http://www.roboard.com/Files/G145/Datasheet_ISZ650.pdf . Instead of the sensitivity data your code, I used 2.27, which I think are the sensitivity data of my gyros ... ((0.00227*1023)/3.3 = 0.7037)

This is your code:
Code: [Select]
void loop() {
  double gyroXrate = -((analogRead(gX) - zeroValue[0]) / 1.0323); // (gyroXadc-gryoZeroX)/Sensitivity - in quids - Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = -((analogRead(gY) - zeroValue[1]) / 1.0323);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);

This is mine:
Code: [Select]
void loop() {
  double gyroXrate = -((analogRead(gX) - zeroValue[0]) /0.7037); // (gyroXadc-gryoZeroX)/Sensitivity - in quids - Sensitivity = (0.00227*1023)/3.3 = 0.7037
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = -((analogRead(gY) - zeroValue[1]) /0.7037);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);

Is correct?

Everything works correctly when the  imu is static. IF I move the imu, I get the correct values in xAngle and yAngle variables. The problem occurs when the imu is in the airplane for which I am doing the tests. When the airplane takes off and starts flying, readings are totally erroneous.

IN the comments, you say me that :

It is most likely happening because your accelerometer values gets screwed up since I tuned the Kalman filter for my balancing robot and not for something like an airplane which is exposed to much larger g-forces than a balancing robot.

I went back to read the section "The measurement" one more time, and I understood there that  is need to adjust the values ??of variance. it Refers to Q_angle, Q_bias and R_measure variables? But, these variables do not affect what you told me to tune the data to adjust the filter for the small g forces that supports your robot or the bigs one that is supporting my airplane… or affects?

I'm not sure these are the values ??I need to change to adjust the kalman filter to the strongest forces g the imu has to endure in the plane.

And, in the comments, you say:

Yes they do. For instance try to increase R_measure and it should trust new measurements less.

I have only tried with the values ??of kalman filter. i´ll try to modify the R_measure.  Now, I'll try with the values ??of the complementary filter, to see if the same error occurs also.

Any help will be most welcome.


Hi Lauszus, first of all congrats for the great work that you have been done around here. I'm currently using your code for MPU6050 with the kalman filter and it works perfectly but now i have an issue because in your code you restrict the pitch angle to be from -90º to 90º. But I want that the both pitch and roll to be from -180º to 180º, i have already tried to change the code and use atan2 in both angles and use some extra restrictions but i end up with the same problem. Can you give me some tips??

PS: In the video I show the problem that i have, the roll is perfect(-180 to 180) but the pitch when hits the 90º or -90º flips the block...


Thanks a lot :D


I have answered your question at my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-4/#comment-486873.

I believe you can't use the angles directly, as the one of the angles is being restricted to +-90 degrees, you should instead modify your Processing code, so it shows the block correctly.


Mar 17, 2014, 07:46 pm Last Edit: Mar 18, 2014, 01:03 pm by juanvivo Reason: 1
Tks lauszus,

I tried, but I got no good result. Also try the complementary filter (even values ??ranging from 0.98 to 0.02 and 0.02 to 0.98 in a formula). The result is always the same. Works fine on the ground, if the imu is static, but when starts to fly, the imu does not mark well.

I have purchased a mpu 6050to see if I had better results with the updated code and the first test with the kalman has not gone well. I think I have a lot of vibration. I'll try to isolate the 6050 and I'll try again.

its correct this code to change the gyro and acc range to 2000 deg/s and +-8g ?

Code: [Select]
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x03; // Set Gyro Full Scale Range to ±2000deg/s
  i2cData[3] = 0x02; // Set Accelerometer Full Scale Range to ±8g

Go Up