Guide to gyro and accelerometer with Arduino including Kalman filtering

Hello Lauszus,
i tried to get an accurate angle based on a sensor fusion of accelerometer and gyroscopes using a complementary filter which should, to my understanding, output a signal similiar to a kalman filter output. the formulas im using to calculate the x angle is also in the image, gyro_y is the present gyro-rate.
i rode a bicycle on a completely flat surface (which should return 0 degree relative to earths coordinate system on the x axis) with the MPU6050 attached to it. my calculated angles look horrible, as you can see in the attached image. while i was accelerating the bicycle, of course the acceleromtere readings were biased by that (yellow) so the calculated accelerometer angle gets wrong (up to 10 degrees). Problem is, the filtered angle follows, so i dont get a "true" angle, but my sensor sees a hill where everything is flat. At 13 seconds i hit the brakes and the angle goes negative (again like 10 degrees). after that i accelerate pretty hard and the signals get even worse as you can see. Do you have any idea what i am doing wrong or why the x-angle doesnt stay at 0 degrees, as it is in reality? maybe there is something you can read out of these signals and help me with.

thanks in advance

Pendax

Hi Lauszus,

I downloaded your code and I'm able to get raw data which to be honest means very little to me. Also tried to run Graph but no data shown, I am not sure where to find instructions about relevant .ino and/or other configuration.

As mentioned, my application is quite simple, I would like to know the displacement in X and Y in 1000/mm, Z would be a nice bonus. The application is a servo platform which I am trying to follow it's location very precisely for a long period of work, say hours.

As a new Arduinoist with no math/physics background I am having difficulties establishing the starting point.

Thanks in advance for your (other?) help.

Nino

Hi Lauszus,

Thank you for all the demo and explanation. It is exactly something I'm looking for right now. However the Github site seems to be down from my place. Would you mind send me the codes you wrote for collecting gyro data and balance the robot through email?

My email is : ceciliazy13@gmail.com

Thank you in advance!

Yi

I will email you the code.

we are doing our project (self balancing robot ) using arduino uno and MPU6050 . we got struck with the programming of i2c interfacing [ http://www.bajdi.com/page/3/ ] , we are encountering the following errors in our programme . so we request you to help us solve the errors or to provide us with a new complete programme for interfacing .. we would be glad if u can help us =( =( :blush: :relaxed:

sketch_feb07a.ino: In function 'void setup()':
sketch_feb07a:55: error: 'i2cWrite' was not declared in this scope
sketch_feb07a:56: error: 'i2cWrite' was not declared in this scope
sketch_feb07a:58: error: 'i2cRead' was not declared in this scope
sketch_feb07a:67: error: 'i2cRead' was not declared in this scope
sketch_feb07a.ino: In function 'void dof()':
sketch_feb07a:140: error: 'i2cRead' was not declared in this scope

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