Guide to gyro and accelerometer with Arduino including Kalman filtering

What application are you trying to implement ?

The segway type scheme is a one dimensional implementation.

General flight control is a three dimensional implementation. Quaternions is the best way to go for this.

You program seems to be a two dimensional implementation, what is that ? I am not sure how quaternions would be useful there.

michinyon:
You program seems to be a two dimensional implementation, what is that ? I am not sure how quaternions would be useful there.

This is piece of code from original post, this is not my program, I've tried it, and yes, it works for 2D, and I using it for 2D stabilization. I want to have 3D stabilization, so I want to handle z axis as well, just want to add Z axis to listed code.
Lets forget about arcsin(2(q0q2-q1q3)) , this was something I did not know what I was talking about....

Question is simple: So what would be formula to convert Z values from accelerometer output to get angle, similar to

double accYangle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;

...in this particular piece of code??

It is not possible to calculate the rotation along the z-axis (yaw) using the accelerometer. More information can be found at the following comment on my blog: TKJ Electronics » A practical approach to Kalman filter and how to implement it.

Instead you should use the code that you used snippets of, which I believe is this one: i2cdevlib/Arduino/MPU6050 at master · jrowberg/i2cdevlib · GitHub, right?

But to get a precise yaw estimate you should use a magnetometer as well.

Hi, thanks for your explanation. i still have one question, does the complementary filtering solve drift issues "forever"? I mean lets say i let the device run for 2 days straight and move it up and down and turn it all the time and then i put it down on my table. Will i measure the same values as i did when i first turned it on two days ago and put it in the exact same space? or is there a remaining integration error that got bigger over time and now i read 2° offset on my x-axis angle where there should have been 0° or something like that? i hope you understand my question.

thanks in advance for your replies.

Pendax

@Pendax
I actually do not know. I have never tried to let it run for that long, but I believe it is able to do it.
Sorry for the delayed reply!

Hi Lauszus,
thank you for your answer. I tried it, it works.
I have another question:
Is it possible, with the MPU6050 or with any other sensor/sensor-fusion, to measure an exact angle of an object relative to the earth's coordinate system when the object is always affected by a changing acceleration.
For example: a bicycle that drives up a hill. The acceleration of this system is never just the gravity, but always (gravity) + (acceleration produced by the rider). So there would alwys be an incorrect calculated angle (by the accelerationsensors-signals) -> i cant correct the integrated gyro signal -> i always have a false inclination angle. It has to work somehow...
Please share your thoughts and ideas

best regards

Pendax

Great code, it is very helpful for our project.

The print data I get for the Complimentary filter and the Kalman Filter are both angles. Is it possible to get linear data as well?
We are trying to build a tracking system with GPS.

All help will be appreciated!
Thanks

@Pendax
That is the point of the sensor fusion. This example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050 shows how to do it.

@Ked85
It is very difficult to get linear data, as you will need to integrate the accelerometer, so the error will also be integrated.

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.