Guide to gyro and accelerometer with Arduino including Kalman filtering

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

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)

XK=

LX
LY
LZ
L
L
L

but lauszus code does that way, no?

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

Hi,

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?

mariocaptain:
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.

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

 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!!

:slight_smile:

@juanvivo
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: Balanduino/Balanduino.ino at aec22dafe8e98f0a16df6b5031a92354bb27ec5b · TKJElectronics/Balanduino · GitHub and the MPU-6050 datasheet: http://invensense.com/mems/gyro/documents/RM-MPU-6000A-00v4.2.pdf at page 13.

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

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

@grasshopper
These are zero-values for the accelerometer - just send set them to 0. I have added a note about it: Added note about zero values · TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter@21c9f6f · GitHub.
I'm planning to make a short calibration routine as I have done for the Balanduino: Balanduino/Tools.ino at aec22dafe8e98f0a16df6b5031a92354bb27ec5b · TKJElectronics/Balanduino · GitHub, 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?