Guide to gyro and accelerometer with Arduino including Kalman filtering

Hello,

Please I have some questions about this gyro+acc

  1. my arduino (ARDUINO UNO R3) works with a SPI communication (i think so :s) , and i think the gyro+acc board works with a I2C communication protocol. Do you think my arduino can fit with it ? Or do you think it can work with a I2C communication like maybe i'm wrong ?

  2. I wanted to buy this IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU (IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics) but in the link, it is said that the IMU is replaced by another one (IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345). I think it is quite the same but i want to be sure before buying it. Do you think your different equations to translate acc and gyro data in degrees is gonna work on the new board ?

  3. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Thanks

  1. my arduino (ARDUINO UNO R3) works with a SPI communication (i think so :s) , and i think the gyro+acc board works with a I2C communication protocol. Do you think my arduino can fit with it ? Or do you think it can work with a I2C communication like maybe i'm wrong ?

The Arduino supports both SPI and I2C at the same time. By default you don't use the SPI for your Arduino, it is programmed via UART using the bootloader. See the last section at the following page: http://arduino.cc/en/Guide/Environment#uploading. You only use SPI for programming if you are using a programmer (see http://arduino.cc/en/Hacking/Programmer).
You can also use SPI to communicate with sensors etc. See SPI - Arduino Reference. A lot of sensors support both SPI and I2C. Since the name I2C is copyright protected by Phillips, the Arduino team is not allowed to call the library I2C, so instead they call it the Wire library. See: http://arduino.cc/it/Reference/Wire.

  1. I wanted to buy this IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU (IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics) but in the link, it is said that the IMU is replaced by another one (IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345). I think it is quite the same but i want to be sure before buying it. Do you think your different equations to translate acc and gyro data in degrees is gonna work on the new board ?

It will works just as well, there is even more functions when using a digital board - like tap sensing, free fall detection etc.
The reason why I started out by using analog sensors, were because I didn't need the increased functionality that a digital IMU offers, as I wanted to build a balancing robot (GitHub - TKJElectronics/BalancingRobotArduino: This is the Arduino version of the code for my balancing robot/segway) since I only wanted to calculate the angle of the robot, I just bought a analog one. But I also got a cheap digital IMU to mess around with.
It takes a bit more of work to get a digital sensor working, but there is plenty of example code for all kinds of IMU's so even a beginner could get it working in a couple of hours.

  1. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Yes, it's pretty simple, just devide the reading (minused the offset) by 14.375 to convert it to º/s.
I will upload a alternative version of the code using a digital IMU instead in a couple of hours to get you started!

Regards
Lauszus

@kevinh4c
The code for a digital IMU can now be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/Digital_IMU

Regards
Lauszus

This post really help my understanding. Thank a lot!!! Keep it up!

Thanks So much Lauszus For your HELP
I'll look after all this :slight_smile:

Lauszus:

  1. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Yes, it's pretty simple, just devide the reading (minused the offset) by 14.375 to convert it to º/s.
I will upload a alternative version of the code using a digital IMU instead in a couple of hours to get you started!

So i just got to do this operation for the digital GIRO ?

gyroRate = (gyroAdc-gryoZero)/14.375 ?

Thanks

Hello Lauszus, I greet the other Builders can someone help me cause even Lauszus unreachable!

(Translated by Google Interpreter) My English is very very weak

I'm very fascinated by your robot. Its stability captivated me. My dream is to also perform similar trinket.

Much I ask you to kinda helped me by my project.

I just finished assembling Balance Robot.
Because I do not even ACC and Gyro (Analog) I can not even begin your trial.
All Sensor Signal (GYRO and ACC) from A/D Converter set to ca. 1,65V

How do I connect the power supply is buzzer Arduino seems a 100ms tone.
Without connected sensor ACC and Gyro the outputs 11,12, 20, 21.32, and 33 are High (Is this a good sign?)
After switching the PS3 controller, Press Home Key(PS KEY) BT LED indicates the connection.

Nothing pressing the keys does not cause any reactions mentioned above, get out.
I Send my configurations for my Arduino 2560. It my configuration is okay?

(USB is PIN 9 and 10)

The signals from the sensors.
Do I understand correctly:
bending forward and gives a minus ACC and GYRO.
slope backwards gives plus ACC and Gyro
If the robot is standing upright is a sensor "Z" gives a minus

You have a way to check the accuracy of encoder connection? (Trick)

Cordial greetings
Roman

#define leftA PIND0 // BEI ATMEGA2560 Pin 21 PD0 - pin 0

#define leftB PIND1 // BEI ATMEGA2560 Pin 20 PD1 - pin 1

#define leftPWM PINB5 // BEI ATMEGA2560 PB.5 PIN 11 PB1 - pin 9 (OC1A)

/* Right motor */

#define rightPort PORTC

#define rightPortDirection DDRC

#define rightPwmPortDirection DDRB

#define rightA PINC4 // BEI ATMEGA2560 Pin 33 PC4 - pin A4

#define rightB PINC5 // BEI ATMEGA2560 Pin 32 PC5 - pin A5

#define rightPWM PINB6 // BEI ATMEGA2560 PB.6 PIN 12 PB2 - pin 10 (OC1B)

/* Encoders */

#define leftEncoder1 2

#define leftEncoder2 4

#define rightEncoder1 3

#define rightEncoder2 5

volatile long leftCounter = 0;

volatile long rightCounter = 0;

/* IMU */

#define gyroY A0

#define accY A1

#define accZ A2

Hi,
I've been working with two different IMUs, Chr-UM6 from CH Robotics and 3space TSS-EM from Yost Engineering. I'd like to estimate the acceleration bias for x, y, and z so I can subtract it out. For example, when I move my IMU up from the bench surface and then return it to a rest position the acceleration does not go back to the same value, or at least not in a reasonable time. Most of the kalman filters I've seen implemented in code appear to focus on gyros and attitude.

Does anyone know where I might find a project that has a kalman filter for estimating bias?

Thanks

@timbopoise
It sounds a bit odd that your accelerometer value doesn't get back to the same value - all my IMU's get back to nearly perfect the same.
You could actually just rewrite the kalman filter, as it would be possible to only use the accelerometer or you could use a digital low-pass filter to filter out the noise from the accelerometer.

Regards
Lauszus

Loving this guide! My project is quite different as I'm just sending out raw data and doing the actual math/crunching/calibration in Max/MSP, but the 'plain english' description of everything on the first post has been invaluable!

Now I'm trying to calibrate my magnetometer data (I have a 9DOF setup) but I can't figure out how to get to a useful number.

I'm using an HMC5883L (3-axis magnetometer)(SparkFun Triple Axis Magnetometer Breakout - HMC5883L - SEN-10530 - SparkFun Electronics) coming in over i2c, so it's 12-bit ADC. Looking at the datasheet the sensitivity is a massive range. Here is what it says:

I'm powering the HMC5883L breakout board with the 3.3v from my Arduino board (ArduIMU v2+)(ArduIMU+ V2 (Flat) - DEV-09956 - SparkFun Electronics). On the sparkfun page it lists the resolution as "5 milli-gauss resolution". So am I supposed to get the digital resolution from the above spec?

(nevermind on that part, I figured out the math).

So with this being 12-bit data, should my first formula be this:
sensitivity/3.3* 4095

0.0035958333/3.3*4095 = 4.46209814

So following on I should have

magRate = (magAdc-magZero)/4.46209814

Looking good so far. But now I'm not sure what type of number I'm looking at now, and if it needs further conversion. Do I need to calculate it against the previous value (like a gyro)?

?

@kriista
Actually you don't have to use the sensitivity at all, you can simply use atan2 like so:

atan2(raw.YAxis, raw.XAxis)

I don't have any experience with magnetometers, but have a look at this guide: https://www.loveelectronics.co.uk/Tutorials/8/hmc5883l-tutorial-and-arduino-library

They even wrote a library for the HMC5883L: https://www.loveelectronics.co.uk/Download/Arduino%20Library%20for%20HMC5883L/aHR0cDovL2M0ODc1My5yNTMuY2YzLnJhY2tjZG4uY29tL0xvdmVFbGVjdHJvbmljc19ITUM1ODgzTF9BcmR1aW5vTGlicmFyeS56aXA%3D

Regards
Lauszus

That's a handy guide! Having a look now.

So would I do that for each pair? (X/Y, Y/Z, Z/X)

@kriista
No if you only need to know the heading while the magnetomer is in a horizontal position, that is all you need!
But if you are going to tilt it or if it's not perfectly flat, you have to "tilt compensate" it using a accelerometer (and a gyro). You can use both a accelerometer and a gyro to get better results, as you can then use the output from the Kalman filter.

See this guide for more information: https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial

Regards
Lauszus

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