Guide to gyro and accelerometer with Arduino including Kalman filtering

@Pendax
Try to use the Kalman filter I have provided and see if that helps. You could also try to implement a low-pass filter of the accelerometer reading, to get rid of some of the noise.

@ninor
This graph code: Example-Sketch-for-IMU-including-Kalman-filter/Graph at master · TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter · GitHub is for these codes: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/ITG3205_ADXL345 and https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/LPR530AL_LY530ALH_ADXL335.

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

@ceciliazy13
You can find the code for my balancing robot here: GitHub - TKJElectronics/Balanduino: Git repository for the Balanduino balancing robot.

@siddhu99199
You need to download the I2C.ino and Kalman.h files in this directory: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050 as well. Then simply click on the file name MPU6050.ino.

Thanks for the graph details.
Is there a way to get the axis displacement in mm?

@ninor
What do you mean by displacement?

Once started, the sensor is located in X,Y,Z = 0,0,0 and as it moves in space, the serial output shows relative physical location in space measured in millimeters.

@ninor
You will need some other sensor like a GPS sensor to get the position. See my previous reply to Ked85: Guide to gyro and accelerometer with Arduino including Kalman filtering - #485 by Lauszus - Sensors - Arduino Forum.

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):

355.4
357.3
359.4
195.2
140.3
80.1
35.3
10.6
-8.3
-4.0
-1.5
0.5
1.5
3.9
6.4

etc.

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!

@kirky
This is how I deal with it in the firmware for my balancing robot: Balanduino/Balanduino.ino at cc8beb5f3a78cfd615a00c490b6656bdcc1a9d8b · TKJElectronics/Balanduino · GitHub.

@Lauszus

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:

Lauszus:
This graph code: Example-Sketch-for-IMU-including-Kalman-filter/Graph at master · TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter · GitHub 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.

@kirky
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 :wink:

@PeterCheng
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.

Lauszus:
@PeterCheng
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 ! :smiley:

Mark

@mbedfordm
You can get the unbiased rate using the following function: KalmanFilter/Kalman.h at master · TKJElectronics/KalmanFilter · GitHub.

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.

@mbedfordm
I just updated the MPU-6050 code. The changes newest code can be found in the Github repository: Example-Sketch-for-IMU-including-Kalman-filter/IMU/MPU6050 at master · TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter · GitHub.

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 ... ) :slight_smile:

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:

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:

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 :smiley:

@juanvivo
I have answered your question at my blog: TKJ Electronics » A practical approach to Kalman filter and how to implement it.

@andremorais
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.

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 ?

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

very clearn, thanks for sharing .