Guide to gyro and accelerometer with Arduino including Kalman filtering

I haven't got any experience myself, but it look likes what you need :slight_smile:
Or you could just write something like this:

int difference = reading - lastreading;
abs(difference);
if(difference > 10) reading = lastreading;

It will ignore the data if you for example shake the sensor.

The best solution might be combining those two approaches.

  • Lauszus

Hey Lauszus
thanks for providing this great guide!!

I've read your comments to onidaito and I'm not exactly sure of what you mean when you are saying that

the accelerometer can not measure the yaw

I'm trying to understand if for my project I need 6 or 9 degrees of freedom
So let's be straight and have a visual example:

can the sensor you are using (and the code) measure all these 6 variables?
thanks in advance
b.

The accelerometer can measure the roll and the pitch, but not the yaw. The gyro can measure both the roll, pitch, and the yaw, but all these will drift. In total that gives only the roll and the pitch. You need a magnetometer to measure the yaw too.

  • Lauszus

Ok thanks for the clarification...
one last thing: in my project there is actually one movement which I won't need to do: which is the "pitch" on the image I've posted earlier.

I'm guessing that all I need to do is position the sensor perpendicular to ground
hence transforming the yaw (which I wouldn't need) into the pitch
and your code would still work...is that right?
would it work if I shift the frame of reference in such a way or the code measures pitch and roll in reference to the force of gravity?

thanks
b.

No that is not correct. The thing is that the accelerometers can not "see" when rotate the IMU along yaw. So it will not make a difference if you just rotate the sensor. Remember that it measures acceleration, which I use to measure Earth gravitational force to calculate the angle. You can also use a accelerometer in for example a rocket to measure the acceleration of the rocket.

I am not sure that I understand your second question? :slight_smile:

  • Lauszus

ok let's check with an image
I really don't understand why one parameter can't be measured if it's said to be a 6DOF and not a 5DOF!?!?!
moreover gyroscopes measure angular velocity around themselves...not influenced by the force of gravity :~

So here goes the image

I made it very simple :slight_smile:
can you please answer the questions at the bottom simply with a Y or N?
in your setup (I'm deducing) can the following be measured???
1.1 Yaw = N?!?! right?
1.2 Roll = Y
1.3 Pitch = Y
1.4 - 1.5 - 1.6 = x y z = Y Y Y

In my setup - can the following being measured??
2.1 - old Yaw - new Pitch = Y? N?
2.2 - Roll = Y
2.3 - old Pitch - new Yaw = Y? N?
2.4 - 2.5 - 2.6 - x y z = Y Y Y

What I really need for my project is (considering the my sensor positioning pictured right)
to be able to measure
x, y, z, ROLL, and PITCH (old yaw, blue line)

Will it work with Sparkfun's 6DOF Ultra-Thin IMU
or will I need one of the 9DOF? ( 9 Degrees of Freedom - Sensor Stick (Old-School) - SEN-10321 - SparkFun Electronics & 9 Degrees of Freedom - Razor IMU - AHRS compatible - SEN-10125 - SparkFun Electronics)

thanks for the patience :slight_smile:

It is right gyros are not affected by the gravity only accelerometers!

Okay you have to understand that just because you rotate the sensor does not mean that you can measure yaw. So your answer is:
1.1: N
2.1: Y
2.3: N

No matter what you can NOT measure yaw with a accelerometer, unfortunately :frowning: If you only need a estimated angle you could just use the gyro, and then keep calibrating the zeroValue.
If you need to know the exact angle of the yaw, you have to use a 9DOF!!
Hopes that was what you needed to know or just post another comment, and I will gladly help :slight_smile:
Nice hand drawing btw :smiley:

  • Lauszus

ok thanks for your reply
my fault I should have made it clearer: when I asked if it could be measured, I meant the ANGLE...not the acceleration

It's clear (sort of :slight_smile: ): you can't measure yaw with the accelerometer...but I was thinking with the gyro...and as you said you can.
in Fact at the end of your code you are only calculating x and y angles and not Z angle...is there are reason why?

compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
  yAngle = kalmanCalculateY(accYangle, gyroYrate, dtime);

Can the same concept be extended to the Z angle?

compAngleZ = (0.98*(compAngleZ+(gyroZrate*dtime/1000)))+(0.02*(accZangle));
zAngle = kalmanCalculateZ(accZangle, gyroZrate, dtime);

Moreover...when you say "keep calibrating the zeroValue"
you mean that after the previous calculation I set the
gyroZeroZ = zAngle? so I keep calculating incremental angle variation?ù
or do you mean to take out the comments to the line

gyroZangle=gyroZangle+gyroZrate*dtime/1000;//Without any filter

why did you leave that commented? it wasn't working?

Anyhow, thanks a lot for the help :slight_smile:
if you ever need a quick hand drawn sketch (dirty and quick) just ask :wink:

cheers
b.

You can not measure the yaw like that. The problem is that accZangle is not correct. Because accZangle is not affected when rotation the sensor, as I described earlier.

When I wrote calibrating I mean that you should keep doing this:

gyroZeroZ = calibrateGyroZ();

The reason why I outcommented it, is because it drifts, and I do not use it anywhere.

  • Lauszus

Thank you Lauszus
I'll give it a try!

Although it uses one more sensor (the magnetometer) I found this guy's work pretty amazing - the FreeIMU (library and hardware)
an Arduino library for reading the 3 sensors (the accelerometer and gyroscopes are the same as yours)
and performs all the sensor fusion needed for a simple interface

http://www.varesano.net/projects/hardware/FreeIMU

inside the library you can actually see 3 separate libraries, one for each sensor

Now I'm just unsure if going with the 6DOF that you so well explained...or the 9DOF of Fabio Varesano :slight_smile:

thanks for all the help
cheers
b.

That looks pretty sweet. Does he fusion the magneton reading with the gyro and accelerometer? :slight_smile:
That totally depends on what your needs are. I just wanted to build a balancing robot, thats why I did not buy the 9DOF. I can always buy a magnetometer if I need it.

  • Lauszus

Yes he does sensor fusion with all 3 sensors using Sebastian Madgwick's algorithms

if you want to perform sensor fusion on just 2 sensors I've found this useful link :slight_smile:
http://mbed.org/cookbook/IMU

cheers
b.

Hey guys! I'm the FreeIMU guy! Thanks for your interest in my project! Let me know if you have any questions on FreeIMU or its algorithms. If you need an introductions on accelerometers, gyroscopes and magnetometers and their algorithms you may find my thesis useful.

Good luck with your projects,

Fabio

hey Fabio!
sei fortissimo! :smiley: grazie una cifra! :slight_smile:

thanks a lot for providing the freeIMU library and hardware for the community

I was about to send you an email asking the following 2 questions.

  • do you sell the freeIMU boards as well? I know sparkfun has a similar board but I was wandering how much would it cost to buy it off of you directly

  • do you think that the IMU can be done on the lower level version without magnetometers?? Solving the yaw missing piece somehow and the drifting problem? From your thesis it seems possible, but on the discussions we've been having on this forum post it seems as there are some drifting problems or some information (yaw) which cannot be obtained.

grazie ancora / thanks a lot
ciao
b.

I was about to send you an email asking the following 2 questions.

  • do you sell the freeIMU boards as well? I know sparkfun has a similar board but I was wandering how much would it cost to buy it off of you directly

Yes, that's possible some time. I may have some spare prototypes. Please discuss this privately (mail me: fvaresano at yahoo dot it) as I don't want to pollute the thread with commercial stuff.

  • do you think that the IMU can be done on the lower level version without magnetometers?? Solving the yaw missing piece somehow and the drifting problem? From your thesis it seems possible, but on the discussions we've been having on this forum post it seems as there are some drifting problems or some information (yaw) which cannot be obtained.

You can compute the orientation of an IMU (gyro+accelerometer) but you will have always small drifting on the yaw. You need a magnetometer to fix the yaw drifting.

These days, there is the very good HMC5883L magnetometer which costs about $2 raw, so, with such a small increase in price, I wouldn't buy an IMU without a magnetometer.

Thanks Fabio for a very useful thread. I've never implemented a software filter like this before and it's very interesting.

I have run into a problem in my project, though. When I move the x-axis from 180 to 0 degrees, and don't change the y-axis, the measured complimentary angle for y also changes due to the rotation about the y-axis.

This makes sense, since it rotates around the y-axis when the angle of x is changed. (See the complimentary filter formula) but obviously isn't desireable for a smooth tilt measurement.

Here's a processing screenshot. You can see the x-axis (red) spiking up and down from 90 degrees when the y-axis tilt is changed from 0-180 degrees and back.

Any ideas how to get around this?

I find that increasing the weight from 0.98 to 0.95 amplifies the problem, while decreasing from 0.98 allows noise to corrupt the measurement.

Is this just a side affect or is there a solution?

What algorithm are you using?

I'm using the complimentary filter, like yours, but I'm using I2C sensors so the code is a bit different.

void gyroRead(float vals[])
{
  //First data register
  int regAddress = 0x1D;
  
  int x, y, z;

  byte buffer[6];
  readFrom(addrGyro, regAddress, 6, buffer);

  vals[0] = (int)(((((int)buffer[0]) << 8) | buffer[1])/14.375);   
  vals[1] = (int)(((((int)buffer[2])<< 8) | buffer[3])/14.375);
  vals[2] = (int)(((((int)buffer[4]) << 8) | buffer[5])/14.375);

  vals[0] *= (-1);
  vals[2] *= (-1);

}
{
  //First data register
  int regAddress = 0x32;
  
  byte buffer[6];
  readFrom(addrAccel, regAddress, 6, buffer);
  
  vals[0] = ((((int)buffer[1]) << 8) | buffer[0]);
  vals[1] = ((((int)buffer[3])<< 8) | buffer[2]);
  vals[2] = ((((int)buffer[5]) << 8) | buffer[4]);
  
  vals[0] /= xAccSens;
  vals[1] /= yAccSens;
  vals[2] /= zAccSens;

  vals[0] *= (-1);
  vals[2] *= (-1);


}
void kalman(float accelVals[2], float gyroRate[2], float compAngle[2])
{
  //Vector for tilt from accelerometer only
  float r = sqrt((pow(accelVals[0], 2) + pow(accelVals[1], 2) + pow(accelVals[2], 2)));
  
  //Angles from x,y,z axis to R
  vectorAngle[0] = acos(accelVals[0]/r) * (180/pi);
  vectorAngle[1] = acos(accelVals[1]/r) * (180/pi);

  deltatime = millis()-time;
  time = millis();

  //Computed angle using accelerometer data and gyroscope rate of change.
  compAngle[0] = (0.98*(compAngle[0]+(gyroRate[0]*deltatime/1000)))+(0.02*(vectorAngle[0]));
  compAngle[1] = (0.98*(compAngle[1]+(gyroRate[1]*deltatime/1000)))+(0.02*(vectorAngle[1]));
}

accelVals array stores the x and y acceleration in g's
gyroRate is the x and y rotation in degrees/sec
vectorAngle is the angle between the axis and the R vector

I was thinking it may have something to do with the frequency of the loop. When I increase the frequency, the problem is not as severe. Unfortunately there is only so fast I can make it.

I've tried various values for the weighted average. There isn't really much margin for change from what I can see before bad things start happening.

Another idea I just had....I may be doing some funny things with the sign (positive/negative) of various angles and rates. I will look into this.

I'm not sure if I'm missing something here or not.

Supposing that the algorithm you have here is correct, the problem you are experiencing may be related to the axis of your sensors as not being aligned. Check your sensors datasheet for pictures of the sensors axis and check that your IMU has them aligned. If they are not, you can align them later in software by swapping X and Y and/or signs.

You were right, everything is working perfectly now :slight_smile:

Thanks for your comments.