Guide to gyro and accelerometer with Arduino including Kalman filtering

Lauszus:
I don't understand why you want to calculate the sensitivity like that. Simply just divide the output by the one given in the datasheet: http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf page 10.

Also please try to run the example code: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if it works.

The newest version can be found at Github: GitHub - TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter: Software for "Guide to gyro and accelerometer with Arduino including Kalman filtering", as I explain in the guide.

Sorry, I wasn't very clear. When I run https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino, it gets values from the gyro fine, but I get the raw output below (with the words 'angular velocity:' removed because the post was too big...):

The values coming from the X-axis are in the first column. When it's at rest they hover around 15 (gyroXZero), when I pick it up and spin it clockwise they go up to >200, when I spin anti-clockwise they also go up to >200 (Min is 0 and Max is 255). Is this what is expected from the raw output?

To convert these values to Degrees Per Second, it seems that I take (gyroXRaw - gyroXZero)/70 (sensitivity from the datasheet).

Sorry for all the questions, I've found a lot of information on google, but I can't seem to find anything to clear this up.

Thanks again.

Initializing I2C devices...
Testing device connections...
L3G4200D connection successful
	183	213	32
	8	253	19
	9	255	23
	11	0	26
	8	252	17
	11	255	24
	11	253	16
	7	252	20
	8	253	17
	12	254	17
	8	1	18
	10	255	17
	12	250	19
	5	251	19
	13	0	22
	10	254	19
	13	254	18
	10	254	18
	13	254	15
	13	1	25
	10	254	20
	14	255	17
	13	250	20
	18	255	21
	11	0	19
	14	1	14
	14	255	16
	12	0	17
	7	255	19
	10	252	19
	13	254	18
	10	248	14
	7	254	15
	10	252	20
	8	253	19
	10	251	14
	11	254	18
	11	3	19
	10	255	12
	9	252	18
	13	3	20
	9	255	18
	9	4	24
	13	1	18
	8	4	17
	16	0	16
	10	0	22
	7	1	12
	15	255	16
	10	1	21
	11	255	23
	6	0	19
	9	252	23
	11	4	15
	7	252	20
	8	1	20
	5	1	20
	11	254	15
	7	252	16
	15	255	15
	11	254	18
	14	250	20
	14	254	20
	6	255	25
	8	250	21
	7	255	22
	12	253	23
	11	0	14
	11	252	20
	8	253	24
	10	251	16
	5	251	18
	12	254	20
	18	254	18
	15	254	24
	3	255	22
	5	251	15
	9	0	18
	7	249	20
	12	255	22
	7	252	21
	12	3	19
	11	253	22
	13	253	17
	13	1	15
	13	1	21
	8	251	16
	7	255	16
	13	250	23
	8	255	16
	10	252	18
	9	1	20
	10	255	17
	14	252	18
	9	251	13
	11	254	19
	12	253	22
	9	1	15
	13	251	19
	9	255	20
	6	249	16
	8	253	17
	10	1	24
	8	253	18
	9	253	16
	10	248	22
	7	251	21
	8	3	21
	10	253	19
	10	0	23
	10	250	19
	5	255	19
	11	250	20
	11	252	26
	10	250	19
	11	251	12
	10	254	15
	12	0	18
	10	255	23
	7	0	21
	9	250	22
	13	251	16
	8	248	22
	9	0	18
	9	255	24
	15	254	17
	7	255	25
	10	255	19
	14	253	19
	13	252	25
	11	2	17
	9	255	18
	12	5	21
	13	254	20
	13	254	17
	13	0	20
	12	0	20
	11	250	25
	6	255	16
	16	1	17
	14	253	19
	10	255	19
	11	255	19
	9	254	24
	10	1	17
	12	250	18
	7	5	13
	9	255	21
	4	249	20
	15	255	15
	9	245	16
	7	248	18
	9	1	17
	9	252	21
	7	0	16
	14	252	17
	9	255	18
	10	1	20
	12	251	17
	14	253	19
	10	1	17
	12	255	13
	10	252	14
	10	254	16
	11	255	17
	7	255	21
	10	0	21
	10	254	21
	9	0	20
	9	254	16
	10	0	15
	12	254	13
	8	250	16
	14	0	11
	12	255	16
	12	3	19
	7	251	25
	11	255	18
	10	255	22
	9	255	15
	11	254	19
	12	251	17
	13	255	18
	8	253	19
	14	0	17
	9	254	19
	16	250	19
	7	1	20
	11	1	17
	8	255	20
	7	1	16
	11	0	15
	17	1	19
	11	0	22
	12	254	20
	13	253	17
	12	255	17
	12	254	28
	11	252	21
	13	252	20
	14	254	18
	12	251	18
	11	251	22
	10	253	18
	9	0	23
	15	248	20
	13	0	15
	10	254	20
	10	255	19
	10	254	18
	7	249	21
	10	251	21
	7	1	23
	7	252	15
	6	253	19
	9	254	18
	13	253	18
	8	246	21
	11	255	14
	12	0	13
	7	254	22
	12	252	20
	7	254	20
	13	251	18
	12	254	13
	9	249	17
	8	253	20
	13	255	20
	11	253	17
	14	254	21
	16	2	19
	12	255	15
	10	255	17
	11	0	20
	9	253	17
	12	0	21
	11	247	16
	11	0	22
	8	0	15
	8	254	17
	13	253	20
	9	1	14
	13	253	17
	8	252	19
	13	255	18
	11	252	23
	9	252	18
	14	253	16
	9	4	24
	18	253	25
	9	251	18
	6	0	18
	12	1	21
	18	254	17
	10	2	19
	14	254	17
	10	255	24
	9	251	18
	10	0	16
	11	3	18
	7	254	24
	8	1	16
	7	255	18
	10	1	16
	9	254	20
	15	249	20
	6	0	14
	13	252	23
	10	253	18
	7	254	20
	10	254	19
	10	1	19
	11	255	20
	13	250	17
	17	253	15
	13	251	18
	6	255	19
	7	249	15
	11	253	16
	12	252	16
	9	254	18
	10	2	19
	7	3	23
	10	252	19
	8	2	12
	6	255	17
	14	254	15
	11	251	16
	14	2	14
	7	1	22
	9	253	17
	14	0	17
	15	252	16
	11	250	21
	5	255	16
	8	253	18
	8	253	25
	11	253	18
	10	253	13
	9	252	15
	7	0	20
	11	255	19
	11	253	21
	5	255	16
	13	251	15
	13	254	19
	10	5	9
	9	1	17
	4	254	15
	12	254	11
	9	1	17
	10	254	19
	11	254	22
	13	254	16
	12	5	20
	11	1	20
	13	252	21
	13	252	18
	8	254	22
	16	253	20
	10	253	22
	8	252	17
	9	253	15
	14	255	17
	11	0	14
	8	252	14
	11	0	17
	8	249	20
	8	247	17
	12	1	20
	9	255	21
	11	0	26
	10	255	22
	10	3	15
	9	0	18
	8	253	17
	10	252	14
	6	255	15
	11	248	21
	10	255	25
	5	254	20
	8	254	17
	13	252	21
	13	254	17
	9	254	17
	7	1	23
	4	253	20
	13	254	20
	9	254	18
	5	251	21
	10	2	17
	9	2	21
	16	252	16
	15	253	16
	6	255	15
	12	254	20
	11	0	19
	10	254	17
	13	1	19
	11	255	17
	11	251	21
	10	0	19
	9	2	21
	10	0	21
	14	249	18
	14	250	16
	14	250	18
	12	1	14
	13	254	16
	9	254	16
	10	252	19
	9	250	13
	6	255	17
	7	251	16
	16	2	23
	13	253	20
	8	2	20
	9	3	22
	12	0	27
	16	255	94
	11	1	120
	13	3	104
	238	88	101
	0	226	240
	52	142	52
	242	158	234
	66	128	179
	137	235	17
	234	154	7
	117	1	152
	25	76	231
	189	157	44
	1	121	104
	122	198	127
	102	126	232
	216	201	119
	95	174	108
	255	122	49
	115	129	78
	108	56	218
	140	135	143
	121	206	161
	61	187	0
	13	216	107
	80	45	51
	165	187	158
	69	135	29
	44	97	142
	9	171	119
	6	49	157
	81	109	232
	41	185	148
	5	232	242
	146	242	217
	20	252	146
	103	211	101
	27	135	253
	235	202	30
	173	252	118
	41	127	59
	203	43	63
	102	45	105
	189	218	142
	121	158	213
	15	190	44
	39	82	34
	173	69	128
	239	221	38
	13	123	193
	110	170	216
	114	32	34
	144	115	36
	17	76	104
	251	224	5
	1	240	197
	74	61	36
	166	236	64
	3	59	236
	163	120	138
	123	107	236
	192	27	0
	147	51	196
	118	125	191
	9	227	224
	220	170	24
	61	189	61
	13	249	96
	143	116	131
	21	32	56
	15	3	44
	18	10	40
	25	3	52
	23	251	148
	18	249	88
	18	250	64
	136	168	78
	9	250	170
	19	238	162
	12	247	52
	6	220	15
	10	15	19
	12	17	16
	7	1	20
	8	247	20
	8	248	23
	15	253	16
	12	12	18
	10	255	17
	9	254	18
	13	2	19
	14	249	17
	9	0	18
	11	252	16
	10	4	18
	13	251	14
	11	0	18
	6	0	20
	12	0	24
	8	255	17
	14	252	22
	12	252	20
	11	0	21
	8	255	19
	11	252	19
	8	0	14
	6	0	19
	5	6	19
	11	254	16
	4	250	207
	13	0	25
	9	250	26
	13	252	25
	9	254	20
	17	7	40
	10	0	246
	15	2	245
	12	248	18
	5	249	25
	10	0	20
	11	3	35
	10	6	251
	8	255	62
	11	0	75
	10	245	95
	9	238	47
	11	249	18
	14	245	23
	40	225	47
	128	229	66
	224	64	96
	196	252	72
	41	230	164
	162	125	48
	227	241	27
	226	245	249
	237	230	232
	120	96	120
	162	100	67
	7	9	241
	112	172	13
	30	224	140
	102	162	244
	63	53	56
	89	228	166
	248	173	152
	14	94	190
	153	6	96
	137	227	199
	124	199	77
	56	146	228
	68	250	158
	3	5	135
	165	210	67
	13	13	78
	32	17	225
	150	208	168
	129	112	69
	184	240	152
	236	231	29
	42	129	193
	207	8	32
	70	206	85
	57	223	14
	111	228	242
	176	6	141
	121	130	167
	55	103	163
	245	62	71
	133	246	243
	69	210	46
	51	182	4
	118	126	223
	14	60	213
	223	71	224
	237	16	252
	10	13	11
	29	66	20

Just updating this for anyone with the same issue, I tried out the https://github.com/pololu/L3G/tree/master/L3G library instead of the i2cdev library and I'm now getting values between -65,535 and 65,536 with negative values for anti-clockwise rotations instead of 0 - 255 with no difference between anti-clockwise and clockwise.

There's either an issue in the i2cdev library for the L3G4200D or I was just using it incorrectly.

Okay good you got it working then :wink:
Maybe it would be a good idea to open to fill in an issue: Issues · jrowberg/i2cdevlib · GitHub, so he can see there is an issue.

Can u suggest me with these data, that I initially received with my GY-521 module of MPU6050 IMU...
I'm not able to distinguish, whether why there's so much of drift...
The connections made are:
vcc=5v
GND=GND
SDA=SDA //dedicated to arduino
SCL=SCL //dedicated to arduino

These are the values, which I'm getting...

InvenSense MPU-6050
June 2012
WHO_AM_I : 68, error = 0
PWR_MGMT_2 : 0, error = 0

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 1804, 1256, 15184
temperature: 27.329 degrees Celsius
gyro x,y,z : -4933, 3934, 490,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 5580, 1760, 14720
temperature: 27.047 degrees Celsius
gyro x,y,z : -4620, 3913, 428,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 3392, -84, 16828
temperature: 26.576 degrees Celsius
gyro x,y,z : -4559, 4073, 308,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 5672, 352, 10612
temperature: 26.765 degrees Celsius
gyro x,y,z : -4640, 3980, 364,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 4392, 1068, 13724
temperature: 27.000 degrees Celsius
gyro x,y,z : -4496, 3749, 380,
(for more, see the attachment...)

Please, suggest me ASAP!

@mit2sumit
Please connect AD0 to GND as well.
And please try to run my example code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050, so I'm able to help you out.

Thanks for this info....
but, its too late and unuseful....
as, i got my sensor worked with the program u mentioned, and even completed my project...
though it was not much successful....

I still wonder, whether, why was those variations....even with connecting AD0 to gnd....
and later when i uploaded this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050 , by myself it worked.
I even mapped the value with the range of -20 to 20, so that the horizontal stationary value gives...0 output...

however, thanks for response,

Okay. I can't say what is wrong with the other code, but I know mine works.

Hey Lauszus,
What value would you give for sensitivity in the code if in the datasheet, the value is given in LSB/(o/s)?

romit26:
Hey Lauszus,
What value would you give for sensitivity in the code if in the datasheet, the value is given in LSB/(o/s)?

Use the same value as-is.
For example if the datasheet states "14.375 LSB/(º/s)" then the sensitivity will be "14.375"

@romit26
As Kashif said. Simply just divide by that value. Take a look at the following code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino. But it's even necessary to do this with the accelerometer, as you can just pass in the values directly into atan2 - see: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino#L89-L90.

I'm new to the whole arduino programming scene so please pardon my ignorance if this is a dumb question.

I've got and Arduino Uno and a MPU6050 GY-521 that I'm trying to use for a project I'm working on.

I've gotten several different sketches to "work" so far including the one from here:

http://playground.arduino.cc/Main/MPU-6050

And 2 of Jeff Rowberg's ones from here:

I have trouble tryin to figure out how to use the raw data from the playground code and I've noticed quite a bit of drift with the Rowberg "teapot sketch" so I'm trying to figure out how to use yours.

I keep getting these errors though when verifying your code from here:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino

sketch_oct20a.ino: In function 'void setup()':
sketch_oct20a:45: error: 'i2cWrite' was not declared in this scope
sketch_oct20a:46: error: 'i2cWrite' was not declared in this scope
sketch_oct20a:48: error: 'i2cRead' was not declared in this scope
sketch_oct20a:57: error: 'i2cRead' was not declared in this scope
sketch_oct20a.ino: In function 'void loop()':
sketch_oct20a:78: error: 'i2cRead' was not declared in this scope

I've placed the Kalman.h file in my library. Is there something I'm doing wrong? and why does your code seem so much shorter and simpler than the others?

@Rhett
You need to download the I2C.ino file as well: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/I2C.ino. Simply just download the entiry repository - go to the front page: GitHub - TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter: Software for "Guide to gyro and accelerometer with Arduino including Kalman filtering" and then notice the button labeled: "Download ZIP" and will download all you need.

and why does your code seem so much shorter and simpler than the others?

Because my is just a minimal code, but it should be plenty for most users. The code on the playground got all the different registers defined - I simply just hardcode them.

The code here: i2cdevlib/Arduino/MPU6050 at master · jrowberg/i2cdevlib · GitHub is quite different as it uses the built-in DMP - you can read more about it here: MPU-6050 6-axis accelerometer/gyroscope | I2C Device Library.

Hi guys, please advise how to calculate Z angle within listed code below:

I've tried to play with

arcsin(2(q0q2-q1q3))

but what is q0q1q2q3 within the current given code snippet?

freak174:

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;

unsigned long timer;

double zeroValue[5] = {950, -400, 13500, -100, -500};

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;

double compAngleX = 180;
double compAngleY = 180;

void setup() {

// join I2C bus (I2Cdev library doesn't do this automatically)
   Wire.begin();
   Serial.begin(115200);

// initialize device
   Serial.println("Initializing I2C devices...");
   accelgyro.initialize();

// verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

timer = micros();
}

void loop() {
   // read raw accel/gyro measurements from device
   //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accelgyro.getRotation(&gx, &gy, &gz);
   
   double gyroXrate = -((gx-zeroValue[3])/131); //Change the sensitivity after your sensor
   gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
   
   double gyroYrate = ((gy-zeroValue[4])/131);
   gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Without any filter
   
   
   ///////////////////////////
   //The acc X and Y angle///
   //////////////////////////
   accelgyro.getAcceleration(&ax, &ay, &az);
   
   double accXval = ax-zeroValue[0];
   double accZval = ay-zeroValue[2];    
   double accXangle = (atan2(accXval,accZval)+PI)RAD_TO_DEG;
   
   double accYval = ay-zeroValue[1];
   accZval = ay-zeroValue[2];    
   double accYangle = (atan2(accYval,accZval)+PI)RAD_TO_DEG;
   
   //////////////////////////
   //////////////////////////
   
   compAngleX = (0.93
(compAngleX+(gyroXrate
(double)(micros()-timer)/1000000)))+(0.07accXangle);
   compAngleY = (0.93
(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

timer = micros();
   
   
 Serial.print(compAngleX);Serial.print("\t");  
 Serial.print(compAngleY); Serial.print("\t");
 //Serial.print(timer); Serial.print("\t");
 Serial.print("\n");
   
  delay(10);

}

Is called quaternions. You can find more information about it here: Quaternions – x-io Technologies. Wikipedia is a good resource as well: Quaternion - Wikipedia.

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