Go Down

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


The KF should be used for reducing the noise of measured value.
So IMO, the KF needs to be used on the raw data before you begin with calculating the angles.
Your XK vector has the following components. (L =lenth raw data)




but lauszus code does that way, no?

Code: [Select]
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);

kalAngleX & kalAngleY should be correct angles of the plane at all times, right?


IMO, in oder to take advantage from positive properties of gyroscope and accelerometer, you need to use a sensor fusion. These can be done by using the KF. In these case, a gyroscope and accelerometer are used together to create a more accurate measurement of overall movement and location through space.

Here you need to use the state vector with 6DOF:
X=[x, y, z, x", y", z"]

So your system matrix A is a 6x6.


I'm back made ??a mess. I thought that just that to the code lauszus ...


Apr 04, 2014, 07:18 am Last Edit: Apr 08, 2014, 10:05 am by mariocaptain Reason: 1

I still don't understand why to convert AccX (Accelerometer X raw data) into degrees we use this formula:
roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

instead of simply:
roll = (double)((accX/16384.0)*90.0); //IMU's range: 2g

I have been using the second formula on the MPU-6050(GY-521)  for its simplicity (and being less computationally expensive) and I think the angle is pretty accurate (keeping the IMU leveled on the table - checked with a level tool - returns something very close to zero). Can anyone elaborate on the accuracy of the first vs the second formula?

Should the MPU-6050 already does Y-Z-axis-dependent calculations internally and outputs an already-accurate raw value for accX?


roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

accX, accY, accZ are vectores. So you need to calculate the result-vector in accX-accZ-layer before you can calculate the Rad-Value between accY and  the result-vector of accX-accZ-layer. Later you need to convert all Rad-values into Deg.

gyX, gyY, gyZ, accX, accY, accZ needs to be Kalman-filtered.


Apr 08, 2014, 10:14 am Last Edit: Apr 08, 2014, 10:16 am by mariocaptain Reason: 1

accX, accY, accZ are vectores. So you need to calculate the result-vector in accX-accZ-layer before you can calculate the Rad-Value between accY and  the result-vector of accX-accZ-layer. Later you need to convert all Rad-values into Deg.

gyX, gyY, gyZ, accX, accY, accZ needs to be Kalman-filtered.

Lany, I am using a complemetary filter based on Lauszus' sample code (MPU6050.ino - actually this one implements both Kalman and Comp in the same file). For the complementary filter the atan equation for conversion was also used.

My question is why not just use the simpler second formula above? It works, I am just not sure if the atan conversion is more advantageous in some way?


Hi Lauszus and everyone,

I'd like to know why Lauszus added the values 2000.0, 3319.84 and 664.48 to the acceleration values read from the accelerometer registers. You can see it in this piece of code:

Code: [Select]
accX = ((i2cData[0] << 8) | i2cData[1]) + 2000.0;
  accY = ((i2cData[2] << 8) | i2cData[3]) + 3319.84;
  accZ = ((i2cData[4] << 8) | i2cData[5]) + 664.48;

Thanks very much for your answer!!



It will only help to increase the resolution if you expect to do maneuvers that exceed +-2g.
What you could do instead is try to use the built in filter inside the MPU-6050. I am using that myself for the Balanduino: https://github.com/TKJElectronics/Balanduino/blob/aec22dafe8e98f0a16df6b5031a92354bb27ec5b/Firmware/Balanduino/Balanduino.ino#L225-L226 and the MPU-6050 datasheet: http://invensense.com/mems/gyro/documents/RM-MPU-6000A-00v4.2.pdf at page 13.

Yes I know that you could do that as well, but it would be easier to for example just use a low pass filter on the raw acceleromter values.

Please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf.

These are zero-values for the accelerometer - just send set them to 0. I have added a note about it: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/commit/21c9f6fcdf781861cb461c3078f9bef25cf3d5ca.
I'm planning to make a short calibration routine as I have done for the Balanduino: https://github.com/TKJElectronics/Balanduino/blob/aec22dafe8e98f0a16df6b5031a92354bb27ec5b/Firmware/Balanduino/Tools.ino#L109-L144, but for now you have to enter them manually.


Hi guys! Now I making a robot, and i have GY-521, i found a lot of examples of code, but nowhere everywhere there is no computing of Z axis.  Nobody needs it? Can somebody help me?


Please take a look at the following code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050_HMC5883L.

You will need to buy a HMC5883L 3-axis magnetometer as well and use it together with the MPU-6050. Note that "GY-521" is just the name of the breakout board and not the sensor. The sensor on the "GY-521" is the MPU-6050.


i see that:
Code: [Select]
roll = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

the angle will change : -180 to +180 degree
whether i change:
Code: [Select]
roll = (atan2(accY, accZ)+PI) * RAD_TO_DEG;
  pitch = (atan(-accX / sqrt(accY * accY + accZ * accZ))+PI) * RAD_TO_DEG;

To the angle change: 0 to 360?



Hi Lauszus, can you post a video in which you lay the board flat and slide it in x and y directions? I wanna to see if your code is susceptible to linear accelerations. I wanna see if pitch and roll change as you accelerate the board. Thanks!


No I haven't got time to do that at the moment, as I got my finals coming up.

Go Up

Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

via Egeo 16
Torino, 10131