Guide to gyro and accelerometer with Arduino including Kalman filtering

Yeah I saw that.

I've got all 3 (accel/gyro/mag), so that's the direction I'm going.

This would be ideal:

The academic paper is HARD to read through, and the straight code isn't directly compatible since I'm doing everything in (computer) software, so it's a matter of extracting the math from the code/paper and applying it elsewhere.

For the moment I have 6 streams (accel/gyro) coming in nicely calibrated (thanks to your guide), but in the end I just watch the absolute angles.

Speaking of that, when you do sensor fusion (like your kalman code, or this marg example) what is the end result?

So with kalman filter you get 3 values (pitch/roll/yaw?) but more stable due to the fusion?
If you throw in the magnetometer do you end up with 3 values at the end also, but super stable and non drifting? Or does it produce quaternions?

You can use his C# implementation of his code: http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms. If you are not running Windows, then see: http://monodevelop.com/ which will allow you to build it on Linux and Mac OS X as well.

The problem when using euler angles is that you wouldn't get any precise measurements, when you tilt the device too much, try for instance to tilt the IMU 90 degrees for instance at pitch, you can immediately see that roll doesn't make sense anymore as it's suddenly yaw (this is called gimbal lock: Gimbal lock - Wikipedia).
So instead of using euler angles you have to use something called quaternions, I won't try to explain what it is since I have never taken the time to read through the theory, as I have never needed to know the exact position of all the axis, as I have always used the Kalman filter for my Balancing robot: TKJ Electronics » The Balancing Robot.
You can find more about quaternions in the following video: Sensor Fusion on Android Devices: A Revolution in Motion Processing - YouTube and in the first link in the post.
Also take a look at: Gimbal lock - Wikipedia and of course: Quaternion - Wikipedia

So in short, if you want something like in the video above, you shouldn't be using euler angles.

Regards
Lauszus

That google video is great!

So in watching that I think I've come to a couple of conclusions.

The first one is that it might be best to crunch my numbers back on the Arduino. My original thinking in sending out raw data is that I'm more comfortable with programming on the computer (Max/MSP) than I am in Arduino, and it's much easier to test/adapt/modify in that environment. I also thought it would be faster since my computer CPU is significantly faster than an Arduino.

I didn't account for the bottleneck of serial communication through. So all my fusion stuff will happen potentially displaced by serial latency (unless I'm incorrect in thinking that data travelling across a serial pipe is slower than data in a void loop circling around and around).

That's a big change from what I'm doing now, and I'm in the middle of a project based around this so I'm not gonna 'jump ship' just yet (and just use the 6axis data I have).

It also looks like quaternions is in fact what I'm after (and what that code does).

Now it's just a matter of how much of that code can live in Arduino land (and how much is based on their hardware/protocol).

The final thing is that I'm wondering if the way I have my sensors oriented is a problem. You can see my boards here in an old photo when I had first put them together.

I have the magnetometer directly above the MCU. It is pointing in the correct direction, but I don't know if having something directly below it (a couple millis as there's poofy double-sided tape) is a problem.

Actually the comments in the video indicate that what we're seeing is the onboard fusion (on the right) and the raw data being sent to the computer for fusion there and both look good/tight. This is a bluetooth device so I don't know what kind of data rates that has for the raw data. The fastest I've been able to run my XBees is 57600, without it crashing (115200 worked for 2 seconds then stopped).

Hi Lauzus, first of all i would like to thank you for the information you post. I think your post is just what i need for mi project. I need to make a control system for a camera roll-tilt mount, using a 5DOF(ADXL335 and IDG500) IMU and two servomotors. Here is the board i'm using IMU Analog Combo Board - 5 Degrees of Freedom IDG500/ADXL335 - SEN-09268 - SparkFun Electronics.

Here are the datasheets from the gyros and the accelerometer.

http://www.analog.com/static/imported-files/data_sheets/ADXL335.pdf

I have upload your code IMU6DOFVer3 and change the pins configuration to that 5DOF. For my project i don't need to get 360 degrees resolution, so I'm using the part from the previous version to calculate the AccXangle and Yangle ( i mean i'm calculating the force vector and then using acos function).

When i upload your code, the printed results for Xangle are the ones for the Yangle and viceversa. I think it's because the orientation of the IC's (specifically the IDG500). I looked to the 6DOF IMU you use and notices that the gyro x axis from yours is the gyro y axis of my 5DOF. A similar thing happens with the gyro y axis of your 6DOF. I've changed the pins Gx for Gy, but i'm still getting the same problem.

I've been stuck in this for 2 days. I really hope you could help me.

Hello,

First of all, thank you !

I took a look into ur code (for I2C) u provided in a reply and put it together with another code.

ZeroValues > I did not take these values from the datasheet, I just ran a script with raw values and took the average values when the sensor was lying on the floor.
GyroSensitivity > I actually dont know which one I should take from the datasheet, but Ive tried everyone (there are 4 levels).

These are the values I get when the sensor is lying on the floor

343.26 136.31
344.40 126.77
345.49 117.89
346.50 109.65
347.41 101.99
348.27 94.83
349.07 113.36
349.82 105.43
350.48 98.05
351.12 91.18
351.71 84.80
352.27 104.04
352.78 96.76
353.25 89.98
353.69 83.68
354.12 77.82
354.51 72.36
354.87 67.28
355.23 87.76
330.36 106.78
332.41 99.32
334.33 92.38
336.12 85.92
337.78 79.92
339.31 74.32
340.73 69.13
342.06 64.28
343.30 59.78
344.46 55.58
345.50 76.87
346.49 96.67
322.25 115.07
324.85 107.02
327.25 124.71
329.51 115.98
331.62 107.86
333.54 100.32

And here is the code

// 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.07*accXangle);
    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);


    
}

Edit:
Im using a sensor called MPU6050.
Datasheet can be found here:

When I move it like one mm to another direction it seems to be stable, I guess there is a point where everything gets screwd up..

2.51 1.53
2.49 1.50
2.48 1.49
2.48 1.49
2.47 1.49
2.47 1.48
2.46 1.46
2.47 1.44
2.49 1.41
2.51 1.37
2.52 1.38
2.54 1.38
2.51 1.38
2.51 1.39
2.50 1.41
2.48 1.40
2.46 1.38
2.48 1.39
2.49 1.38
2.51 1.39
2.50 1.38
2.51 1.39
2.50 1.40
2.49 1.39
2.50 1.40
2.49 1.42
2.46 1.43
2.45 1.41
2.44 1.41
2.47 1.40
2.44 1.42
2.46 1.43
2.47 1.46
2.44 1.45
2.44 1.46

@kriista
I would like to know what his loop rate is, as I don't think he can do much else than printing the values as there is a lot of calculation going on. But you can always try to use a more powerfull device like the mbed (http://mbed.org/). I have used it in the past and I really like them as well.

I don't think it's a problem that you have just stacked the boards. My only concern is that the airflow under the magnetometer is really bad so the heat from the ICs can't get away, but since a gyro or accelerometer doesn't produce much heat I really don't think it's a problem :slight_smile:

My XBees work just fine at 115200 baud. I use the normal ones with onboard antenna.

@Battousai
Take a look at the newest code at github: GitHub - TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter: Software for "Guide to gyro and accelerometer with Arduino including Kalman filtering". What I normally do is that I just calculate the unfiltered angle using both the gyro and accelerometer so I'm sure that the orientation and sign is the same and then after that I use the Kalman filter to calculate the angle.

@freak174
I don't have much experience with the MPU6050, but you should have a look at this library for Arduino: i2cdevlib/Arduino/MPU6050 at master · jrowberg/i2cdevlib · GitHub
Actually it looks like you are already using it. Try this example sketch: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino and uncomment the following line to print pitch, roll and yaw: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95

I also use the same approach as you to find the zero value. For instance take a look at my balancing robot source code: BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub

The latency in that video looks close to nil so the loop rate must be pretty low. I really don't know what I'm doing to overflow the buffer, or wherever my problem is happening.

Would it matter that I don't have a delay() in my Arduino code at all? I'm only polling for data (on the computer) every 10ms, so perhaps I should throw a delay(5) in there for good measure?

@kriista
You can always try to add a delay and see if it helps. Why don't you just send the data as fast as possible. I'm sure the computer can handle it? :slight_smile:

I'm doing that but sometimes it craps out, which is weird as I'm using serial call-response, so I shouldn't be asking for data until it's ready for it.

Sometimes it works fine for over an hour and other times it drops after a couple of minutes.

Stone cold bummer.

@freak174

I'm sorry .. I use the same sensor.

How did you find these values?

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

you can edit your skecth?

julio79:
@freak174

I'm sorry .. I use the same sensor.

How did you find these values?

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

you can edit your skecth?

Hello! I dont have the sketch no more, I partly gave up on this sensos hehe.
However, if you look at Lazarus sketch it is basically the same BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub

But Id recommend you to use this sketch instead as it is made for your sensor https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95

When I used the later mentioned sketch I got it to work with the yaw,pitch,roll values but when I try to combine it to work with a servo I get fifo overflow and the code gets screwd up.

cheers

This is awesome, and probably just what i need!

I'm planning to create a gyroscopic platform on which i will mount a GoPro camera or a Contour camera. I will be mounting this onto a motorbike on an actual race track, so it will go through some pretty intense forces. Then i want to compensate the camera angles using e few servos. I only need to compensate for the pitch and roll at this time.

I have experience with arduino's and the coding but im not familiar with the gyro and accelerator sensors.

I do have a few questions:

Offcourse the first question is; considering the forces applied on the sensors by putting it on a motorbike. Will this still work?

and; is using a gyro and accelero enough to fully remove the drift or will i also need the magnetometer (i need the platform to remain absolutely level to the human eye)?

and; are the resulting values usable to compensate using servo's?

i will probably be using this board with my arduino:

Thanks in advance,

Niels

@APH3X2N
It should work perfectly fine if you can find some servos that are strong enough to handle force.
You don't need to use a magnetometer an accelerometer and gyro should be fine.

I actually got some example code for that IMU: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino

Also take a look at this project a guy posted at this post a while ago: Guide to gyro and accelerometer with Arduino including Kalman filtering - #96 by Rivello - Sensors - Arduino Forum
And his post as well: http://arduino.cc/forum/index.php/topic,68755.0.html

Regards
Lauszus

@Lauszus

Thanks for the reply!

And offcourse for the samples.

I''ll be using some pretty strong servos which i'll be using inside brackets to hold the force and weight.

Thanks,

Niels

Hi, Sparkfun doesn't sell IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics? any more.
it's been replaced by SparkFun 6 Degrees of Freedom IMU Digital Combo Board - ITG3200/ADXL345 - SEN-10121 - SparkFun Electronics.

is your code compatible with the new product ? if yes what modifications should I make .
Thank you

Take a look at this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/ITG3205_ADXL345, it's for a ITG3205 and a ADXL345. I know the gyro is a bit different, but it should work just fine with your as well.

Hallo Lauszus, Thanks so much for this great blog. I need ur help with something. I'm using itg 3200 and bma 180, I applied your code and it works fine but when rotating around roll for exaple with 90 degrees, the output reading is 70 and the same for pitch although before that, i was applying a comp filter (explained by starlino) and it was giving an output of 90 when rotating with 90 degrees. Do you have any idea what might be causing that?

@totyforever
Try this code I wrote: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino and tell me if that works.

Man, you are great :slight_smile: , the problem was in this function:-
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));
accXangle = acos(accXval/R)*RAD_TO_DEG-90;
accYangle = acos(accYval/R)*RAD_TO_DEG-90;

I think it's not accurate or something as I was applying it in my old code and instead of (accXval/R), it was (Rxest/R) where Rxest is calculated using both acc and gyro readings. However, after I changed it with the one u sent me in the previous post, it worked so fine. Thank you so much.