Guide to gyro and accelerometer with Arduino including Kalman filtering

Oh okay, thats right. Had to do the same thing once, but forgot that you also should adjust that as well.
You're welcome :slight_smile:

Hi All
I had a collection of problems when I started using the code with stability. It had a habit of rounding numbers to zero then dividing by zero. Initially I got around this by adding delays, but this increased the rate at which the gyroscope drifted.
Even though these were smoothed out by the complementary filter it didn't seem like the right way to go, especially when the input were so stable..
Not sure if it was my Mega causing problems.
By changing the equations around I'm finally happy with the outputs from the gyro and accelerometer...

Just need to work out how to add a PID loop and servos now...

If anyone is interested I can also post the code for the accelerometer and Gyros before they were combined...

float aX = A1;
float aY = A2;
float aZ = A3;

float gX = A5;
float gY = A6;
float gZ = A7;


float accXadc;
float accYadc;
float accZadc;

float accXval;
float accYval;
float accZval;

float accXangle;
float accYangle;
float accZangle;

float accZeroX;
float accZeroY;
float accZeroZ;

float AccSen;
float Rad;
float R;

float gyroZeroX;  
float gyroXadc;
float gyroXrate;
float gyroXangle;

float gyroZeroY;//y-axis
float gyroYadc;
float gyroYrate;
float gyroYangle;

float gyroZeroZ;//z-axis
float gyroZadc;
float gyroZrate;
float gyroZangle;

 int i;
  int Loop;
  
unsigned long timer;
unsigned long dtime;

float compAngleX;
float compAngleY;

void setup()
{
Serial.begin(115200); // initialize the serial communications
analogReference(EXTERNAL); 
 
delay (500);

Rad = 57.295779513082320876798154814105;
AccSen = 102.3;
Loop=500;


timer = micros();
  for (i = 0; i < 500; i++){
  
accZeroX = accZeroX + (analogRead(A1));
accZeroY = accZeroY + (analogRead(A2));
accZeroZ = accZeroZ + (analogRead(A3));
gyroZeroX = gyroZeroX + (analogRead(A5));
gyroZeroY = gyroZeroY + (analogRead(A6));
gyroZeroZ = gyroZeroZ + (analogRead(A7))

;

}
accZeroX = accZeroX / 500;
accZeroY = accZeroY / 500;
accZeroZ = accZeroZ / 500;

gyroZeroX = gyroZeroX / 500;
gyroZeroY = gyroZeroY / 500;
gyroZeroZ = gyroZeroZ / 500;

Rad = 57.295779513082320876798154814105;
AccSen = 102.3;

}
void loop()

{
timer = micros();  
for (i = 0; i < 100; i++){
  
accXadc = analogRead(aX);
accXval = accXval + (accXadc-accZeroX);//AccSen;

accYadc = analogRead(aY);
accYval = accYval + (accYadc-accZeroY);//AccSen;

accZadc = analogRead(aZ);
accZval = accZval + (accZadc-accZeroZ);//AccSen;

gyroXadc = gyroXadc + analogRead(gX); 
 gyroYadc = gyroYadc + analogRead(gY); 
 gyroZadc = gyroZadc + analogRead(gZ);  

}
 accXval =  accXval/(100 * AccSen) ;
 accYval =  accYval/(100 * AccSen);
 accZval =  accZval/(100 * AccSen)+1; 

accXangle = atan (accXval/(sqrt(pow(accYval,2)+pow(accZval,2)))) * Rad;
accYangle = atan (accYval/(sqrt(pow(accXval,2)+pow(accZval,2)))) * Rad;

gyroXadc = gyroXadc / 100;    // average
 gyroYadc = gyroYadc / 100;    // average
 gyroZadc = gyroZadc / 100;    // average
 
 gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
 gyroYrate = (gyroYadc-gyroZeroY)/1.0323;
 gyroZrate = (gyroZadc-gyroZeroZ)/1.0323;
 
 dtime = micros()-timer;
 gyroXangle=gyroXangle+(gyroXrate*dtime/1000000 )*-1;//Without any filter
 gyroYangle=gyroYangle+(gyroYrate*dtime/1000000 )*-1;
 gyroZangle=gyroZangle+(gyroZrate*dtime/1000000 )*-1;
 
 compAngleX = (0.98 *(compAngleX +(gyroXrate*dtime*-1/1000000 ))) + (0.02*accXangle);
 compAngleY = (0.98 *(compAngleY +(gyroYrate*dtime*-1/1000000 ))) + (0.02*accYangle);
 
 Serial.print(" Gyro X Angle:  ");
 Serial.print (gyroXangle);
 Serial.print("\t");

Serial.print(" accXangle:  ");
 Serial.print (  accXangle);
 Serial.print("\t");
 
 Serial.print(" Gyro Y Angle:  ");
 Serial.print (gyroYangle);
 Serial.print("\t");

Serial.print(" accYangle:  ");
 Serial.print (  accYangle);
 Serial.print("\t");
 
  Serial.print("compAngleX:  ");
  Serial.print (compAngleX);
  Serial.print("\t");
 
  Serial.print("compAngleY:  ");
  Serial.print (compAngleY);
  Serial.print("\t");
 
  Serial.println();
 gyroXadc = 0; 
 gyroYadc = 0; 
 gyroZadc = 0;  
 ;


}

Foilcontrol_7.pde (3.3 KB)

#Laurencet
Thanks for sharing :slight_smile:

It's based on your hardwork....

Thanks anyway :slight_smile:

Hi Lauszus,

I stumbled upon your article at "Instructables" and then followed the link on Arduino forum.
I have read the whole 8 pages long thread and am pretty impressed by your work.

I wanted to buy the same IMU to give it a try but Sparkfun's website calls it "Retired". So, I am thinking of buying another 6DOF digital IMU http://www.sgbotic.com/index.php?dispatch=products.view&product_id=787 and want to couple it with HMC5843 to get roll, pitch and yaw all.

What changes your code may requie in order to talk to the above IMU?

This IMU uses "ITG3200" gyro and "ADXL345" accel.

I am from Computer Science background and have just started playing with Arduino and electronics. Co-incidently, google is my only friend and some times it takes ages to figure out a tiny thing. .... well, that's part & parcel of the game though, just wanted to give you a little bacground of mine, in case my questions sound silly to you :wink:

I will greatly appreciate your reply.

I really have no experience with I2C IMU's, but it would not be that hard to implement it, as you only have to change the code, that reads the g's (accelerometer) and rad/s (gyro), and then just use the complimentary filter or the kalman filter as I provided.

I presume that you know the I2C protocol? If not, have a look at the wire library: Wire - Arduino Reference
See the datasheet for the accelerometer page 14: http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf and the gyros datasheet page 22: http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
Also check out the github for example code: GitHub - bitsmashed/6DOF-Digital: I2C interface breakout board for ADXL345 accelerometer and ITG3200 gyro .

  • Lauszus

It's scary how fast there developing new Gyros / Accelerometers... I only order mine a couple of weeks ago and that had just been updated. Depending on your knowledge of I2C, I would stick with a analogue IMU.
Based on how little i know about I2C

Can you get hold of
IMU Analog Combo Board - 5 Degrees of Freedom IDG500/ADXL335
http://www.sparkfun.com/products/9268

If you do go for the above board the gyroscope / accelerometer the sensitivity values will need changing..
You should be fine without the a gyroscope on the Yaw as it's not needed to calculate pitch or roll angles.. All partially depends what your using it for though.

Laurence

Lauszus:
I really have no experience with I2C IMU's, but it would not be that hard to implement it, as you only have to change the code, that reads the g's (accelerometer) and rad/s (gyro), and then just use the complimentary filter or the kalman filter as I provided.

I presume that you know the I2C protocol? If not, have a look at the wire library: Wire - Arduino Reference
See the datasheet for the accelerometer page 14: http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf and the gyros datasheet page 22: http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
Also check out the github for example code: GitHub - bitsmashed/6DOF-Digital: I2C interface breakout board for ADXL345 accelerometer and ITG3200 gyro .

  • Lauszus

Thanx a lot Lauszus for such a prompt and descriptive reply. Yup, I've some working knowledge of I2C as my HMC5843 is working fine.
Ok, I'll give it a try. But I have to find a code sample which works with Arduino, the Github code seems to be for standard AVR programming.
If anyone already have a resource, please forward me the link.

The interfacing looks simple, 3.3V, GND of IMU to respective pins of Arduino and SDA, SCL to A4, A5 of Arduino.
Please correct me, if I am wrong.

Laurencet:
It's scary how fast there developing new Gyros / Accelerometers... I only order mine a couple of weeks ago and that had just been updated. Depending on your knowledge of I2C, I would stick with a analogue IMU.
Based on how little i know about I2C

Can you get hold of
IMU Analog Combo Board - 5 Degrees of Freedom IDG500/ADXL335
IMU Analog Combo Board - 5 Degrees of Freedom IDG500/ADXL335 - SEN-09268 - SparkFun Electronics

If you do go for the above board the gyroscope / accelerometer the sensitivity values will need changing..
You should be fine without the a gyroscope on the Yaw as it's not needed to calculate pitch or roll angles.. All partially depends what your using it for though.

Laurence

Thanx to you too Laurence, may I just ask why you prefer Analogue IMU over digital? Based upon my research on internet, digital is better than analogue. Not to offend you, just asking to clarify my concepts.

Okay good, then it should not be that hard to get it working.
Have you seen this post: varesano.net - ?
Especially take a look at the bottom, as he provided his code.

Yes, that is correct. The INT1 and INT2 input, is interrupt pins for the ADXL345. For more info, see this comment: SparkFun 6 Degrees of Freedom IMU Digital Combo Board - ITG3200/ADXL345 - SEN-10121 - SparkFun Electronics

I do not know if digital is more accurate in general, but in this case it is, because the Arduino has a 10-bit analog-to-digital converter (value 0-1023), but the build in digital-to-analog converter, in both the gyro and the accelerometer, on the digital IMU has a 16-bit digital-to-analog converter (value 0-65535).

I've just never played with I2c and the code for analogue imu was readily available and looked easier to understand.
Let me know how you get on with digital. It would be great if you could post your code..
I might have to swap over at some point......

Thanx Lauszus, I have already started following Fabio's blog, it's a great source of information.
I must appreciate the way you reply and provide on-the-dot links. I am feeling truly grateful.

Laurence, I will probably receive the IMU by coming Tuesday, will give it a try and surely post my findings.
With you people around, I feel so encouraged.

Thanx once again to both of you.

Thanks for your feedback :slight_smile: Really glad I could help!
Looking forward to see your work :smiley:

What a blessed day today, I got my 6DOF Digital IMU earlier this morning (3 days ahead of time) and just a couple of hours' effort and it's up and running.

I have used Fabio's blog (the one mentioned above by Lauszus) varesano.net -

All the Arduino & Processing code is available on the link above. It took me a couple of hours to establish the whole link as it was my first ever interaction with "Processing", Otherwise it was just a matter of a few minutes. Fabio's code is quite nicely written and commented wherever necessary. The only change I had to make in the code was to change Gyro Address (from 0x69 to 0x68), that too mentioned by Fabio.

The interfacing of the sensor with Arduino is too simple (3.3V, GND, SDA and SCL) of sensor to (3.3V, GND, A4 and A5 respectively) of Arduino.

Since all is hooked up now and running like a charm, it's time to give another "good reading" to Lauszus's article :wink:
Lauszus, I am just loving the way you write.

I don't know about you guys but since I am new to Arduino/electroics, my way of accomplishing a task is usually as follows.

  1. Interfacing - Get the electronics (sensors, wires, resistors, capacitors etc) and hook them all up
  2. Interaction - Get the code, algorithm (wherever applicable) and read the data
  3. Understanding - Since by now I have had a working model of what I wanted to experiment on. At this point I start playing with the "toy" and do a lot of R&D to understand the concept and know and insight of the blackbox. And that's done by Google readings and Forum postings.

OK guys, billions of thanx to all of you. it's like a big milestone hit for me. Thanx to Google but people like you are cherry on the cake (make it '"cherries" instead :wink:

Nice to hear you got it working :slight_smile:
Haha why exactly do you "love" the way I write? Be more specific, so I can do more of that in the future.
Normally I actually read a lot about the subject, before even hooking it up, so I got a understanding of what I am doing before I hook it up.

Lauszus:
Nice to hear you got it working :slight_smile:
Haha why exactly do you "love" the way I write? Be more specific, so I can do more of that in the future.
Normally I actually read a lot about the subject, before even hooking it up, so I got a understanding of what I am doing before I hook it up.

Since you asked, here is the truth :wink:
Your are quite expressive, your "reply" is to-the-point & brief and at the same time it contains all the nuts-n-bolts it needs to have to properly convey the message. Don't know if others have felt it or not, I do feel a "spread the knowledge" passion in your writing. Specially when you reposted the same article on Instructables with just this thought that "Many others can reach this too" .... It was great spirit.

Knowing something doesn't mean that one can convey it as well. That's a skill in its own. And I also felt the kindness in your tone as if you are really keen about other people to learn from you. You are not here to make high scores or just to be popular, rather you want to help others to know as much as you know.

Well, may be I am too sensitive to such things, but ya, I really feel if someone is making the difference.

Despite of the fact that you didn't have working knowledge with I2C, you pionted me to the correct location. What I do appreciate, is the thought behind such things. It's about creating a positive atmosphere, a friendly circle and it's just gives a lot of coooooool feeling.

What I will suggest is that please continue your passion of sharing. The killer combination is "Knowledge+Passion to share+Expression" and you have it all. Keep it up.

Oh ya, the reason I "experiment" first and "read" later is that, after I have a working model of that "something", it serves as an energy booster for me, telling me that "yes I can do it". Since I am not properly trained about electronics/physical computing and I have no one in access who can supervise me, so I do require a yard stick to find out if it's do-able by a hobyist or not. It's working for me so far :wink:

Guys, I am not paid for writing it all :wink: Lauszus really helped me a lot thru this forum and his articles. All of you who contribute to the community one way or the other, please keep doing so, .... you are doing a great job ... you may not realize ... but the person on the other side does!

Thank you for your kind words :smiley:
You should check out my blog: http://blog.tkjelectronics.dk/ , if you want to see what else I am doing.

Okay, seems logic. The problem is that I live in Denmark (Scandinavia), so almost all parts comes from the US or china, so it takes about 3-4 weeks for me to receive anything, if I just pay for standard shipping. So normally I just read a lot about it before I can get my hands on it :slight_smile:

I am currently working on a Quad flying machine and I am using the IMU (ArduIMU+ V2 (Flat) - DEV-09956 - SparkFun Electronics) with the magnetometer (Triple Axis Magnetometer Breakout - HMC5843 - SEN-09371 - SparkFun Electronics).
Currently I am working on the IMU with the magnetometer to give a precise reading to control the position of the flying machine. The problem I faced is that the readings from the IMU and the magnetometer will tend to drift and this will then lead to drift in the flying machine as well. I would like to know can I use your code including the Kalman filter to reduce the amount of drift overtime ? Thanks.

Yes you can!
The reason why you experience drift, is because the gyro tends to drift over time. To compensate for that you can either use the complementary filter or the Kalman filter. Have a look at the code taps "KalmanX" and "KalmanY". And line 151-152 in the main code:

compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));

Kk, will give it a try...