Guide to gyro and accelerometer with Arduino including Kalman filtering

Hello
It sounds like, its not sending anything. Are you sure, that you are getting the data? Or even getting the data? Have you uncomment this line:
processing();? :slight_smile:

If you are talking about the "processing()" at the end of the IMU6DOFVer2, than yes that is uncommented. As far as I can tell it was never commented out. Thanks for the patience!

Have you connected the IMU the right way, please double check :slight_smile: It sounds like your not getting the data for some reason..

Hi,
I am getting data now! I took the razor off of the proto-shield I was using and just set it up on a breadboard. Except now my graph looks very erratic. It will react to movement but it will not react as predictably as the graph appears to in the video. Any advice?

Thanks
-builditnow

Are you sure, that you are sending the right data, you might have swopped some of the data? You have to troubleshoot it yourself, or give me some more information, as I will gladly help you out :wink:

Hi Lauszus,

First - thank you for sharing your work!

I picked up a 6DOF razor and thought I'd try your code before I dug into it...

I'm getting nothing from the serial port running IMU6DOF2. (Arduino 022, 168-based board)

I confirmed connections are correct. processing(); is not commented out. It compiles and uploads fine, then sits like a bump on a log.

So I stripped it down to the essentials. I get some data, but I'm not sure it's sensible. Here's "the essentials" and the datastream I get:

//made by Kristian Lauszus - see http://arduino.cc/forum/index.php/topic,58048.0.html for information
#define gX A0
#define gY A1
#define gZ A2

#define aX A3
#define aY A4
#define aZ A5

#define RAD_TO_DEG 57.295779513082320876798154814105

//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;

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

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


//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;

int accZeroY;//y-axis
float accYadc;
float accYval;
float accYangle;

int accZeroZ;//z-axis
float accZadc;
float accZval;
float accZangle;

//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;

float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;

void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(115200);
  delay(100);//wait for the sensor to get ready
  timer=millis();//start timing
}
void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter
  
  gyroYadc = analogRead(gY);
  gyroYrate = (gyroYadc-gyroZeroY)/1.0323;//(gyroYadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroYangle=gyroYangle+gyroYrate*dtime/1000;//Without any filter
  
  gyroZadc = analogRead(gZ);
  gyroZrate = (gyroZadc-gyroZeroZ)/1.0323;//(gyroZadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  //gyroZangle=gyroZangle+gyroZrate*dtime/1000;//Without any filter
  
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accYadc = analogRead(aY);
  accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accZadc = analogRead(aZ);
  accZval = (accZadc-accZeroZ)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  accZval++;//1g in horizontal position
  
  R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  //accZangle = acos(accZval/R)*RAD_TO_DEG;
  
  //used for debugging
  Serial.print(gyroXrate,0);Serial.print("\t");
  Serial.print(gyroYrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");

  Serial.print(gyroXangle,0);Serial.print("\t");
  Serial.print(gyroYangle,0);Serial.print("\t");
  Serial.print(gyroZangle,0);Serial.print("\t");

  Serial.print(accXval,2);Serial.print("\t");
  Serial.print(accYval,2);Serial.print("\t");
  Serial.print(accZval,2);Serial.print("\t");

  Serial.print(accXangle,0);Serial.print("\t");
  Serial.print(accYangle,0);Serial.print("\t");
  //Serial.print(accZangle,0);Serial.print("\t");

  Serial.print("\n");
  
  dtime = millis()-timer;
  timer = millis();
}

Here's a sample of the data sitting on my desk:

159	363	382	9184	9231	0	5.70	5.04	6.60	-34	-30	
157	368	378	9185	9234	0	5.72	5.01	6.59	-35	-30	
161	375	374	9186	9236	0	5.68	4.98	6.61	-34	-30	
167	384	367	9187	9239	0	5.61	5.00	6.66	-34	-30	
170	396	359	9188	9241	0	5.57	5.06	6.68	-34	-30	
170	410	354	9189	9244	0	5.58	5.08	6.61	-34	-30	
175	418	351	9190	9246	0	5.63	5.09	6.49	-34	-31	
181	422	351	9192	9249	0	5.68	5.11	6.45	-35	-31	
187	425	354	9193	9252	0	5.71	5.14	6.40	-35	-31	
192	426	358	9194	9254	0	5.75	5.16	6.32	-35	-31

I mention that I'm not sure it's sensible (and that may just be due to a lack of calibration?) because, if we look at the last line:

gyroXrate: 192
gyroYrate: 426
gyroZrate: 358
gyroXangle: 9194 [!!!]
gyroYangle: 9254
gyroZangle: 0
AccX: 5.75
AccY: 5.16
AccZ: 6.32
AccXAngle: -35
AccYAngle: -31

Clearly that looks "a little funny"...

If you'd like to compare the project I'm running (entirely unchanged), I pushed the zip file here:
http://www.jlrdesigns.com/IMU6DOFVer2.zip

Thanks for any advice!

Now that I read what I wrote...

Does this really work?

accXval = (accXadc-accZeroX)/102**,**3;//(accXadc-accZeroX)/Sensitivity - in quids Sensitivity = 0.33/3.3*1023=102,3

I know that's used as a decimal outside of the US, but ANSI C would puke on that for sure... And why do you use a decimal elsewhere, but a comma decimal separator in those?

gyroZrate = (gyroZadc-gyroZeroZ)/1.0323;//(gyroZadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323

  
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accYadc = analogRead(aY);
  accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3

Thanks!

[edit] Ok - that doesn't work, as I expected.

float test1;
float test2;
float test3;

void setup()
{
 
  Serial.begin(115200);
 
}

void loop()
{
  test1=2.4;
  test2=5,75;
test3 = test1*test2;

Serial.print(test3);Serial.print("\n");
}

Output = 12.0 == 2.45.0 != 2.45.75

2.4 * 5.75 = 13.8

Am I missing something? Have you gotten the code you uploaded running? Would you decompress and verify that it does run? Thanks again!

You have to include this in the setup() function:

  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY();  
  gyroZeroZ = calibrateGyroZ();
  
  accZeroX = calibrateAccX();
  accZeroY = calibrateAccY();
  accZeroZ = calibrateAccZ();

If you look in the original code it is there!
Also it would be a good idea to change:

int gyroZeroX;//x-axis
int gyroZeroY;//y-axis
int gyroZeroZ;//z-axis
int accZeroX;//x-axis
int accZeroY;//y-axis
int accZeroZ;//z-axis

To float values, as this will give higher precision.

I do not know why the code you linked to does not work. Try downloading the original software again from my post, as this works! Then when you have it up and running modify the code for your needs :slight_smile:

I am using your original code. As linked in the zip file I linked to.

The upshot is:

Using a comma for a decimal doesn't work. Returns the wrong values.

Calling any two calibrates work fine. The instant we call a third calibrate, it goes away and never comes back.

This works:

  //calibrate all sensors in horizontal position
  Serial.print("DEBUG: Calling Calibration \n");
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY();  
//  gyroZeroZ = calibrateGyroZ();
  
//  accZeroX = calibrateAccX();
//  accZeroY = calibrateAccY();
//  accZeroZ = calibrateAccZ();
 Serial.print("DEBUG: Return from Calibrations");
  timer=millis();//start timing

This works:

  //calibrate all sensors in horizontal position
  Serial.print("DEBUG: Calling Calibration \n");
//  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY();  
  gyroZeroZ = calibrateGyroZ();
  
//  accZeroX = calibrateAccX();
//  accZeroY = calibrateAccY();
//  accZeroZ = calibrateAccZ();
 Serial.print("DEBUG: Return from Calibrations");

This never makes it to the second Debug, just goes away and never comes back.

  //calibrate all sensors in horizontal position
  Serial.print("DEBUG: Calling Calibration \n");
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY();  
  gyroZeroZ = calibrateGyroZ();
  
//  accZeroX = calibrateAccX();
//  accZeroY = calibrateAccY();
//  accZeroZ = calibrateAccZ();
 Serial.print("DEBUG: Return from Calibrations");
  timer=millis();//start timing

Doesn't matter which third one we add in, I've cycled all of them in every combination. Any two work, any three (or more) fail to ever come back.

Yup, back again using:

http://www.jlrdesigns.com/IMU6DOFVer2.zip

which is my local copy of:

https://rapidshare.com/files/1073395588/IMU6DOFVer2.zip

which is the same as:

http://www.instructables.com/files/orig/FYG/4OVJ/GN77W1DR/FYG4OVJGN77W1DR.zip

Never makes it through calibration. Just goes away forever and never comes back.

I'll try your version 1 and see if it makes it.

[edit to note:]

IMU6DOF has the same problem(s) as IMU6DOF2.

Try changing the calibration page to this:

//This is pretty simple. It takes 100 readings and calculate the average.

//gyros
long resultGyroX;//x-axis
long resultGyroY;//y-axis
long resultGyroZ;//z-axis

//accelerometers
long resultAccX;//x-axis
long resultAccY;//y-axis
long resultAccZ;//z-axis

//gyros
int calibrateGyroX()
{
  for(int i=0;i<100;i++)
  {
    resultGyroX += analogRead(gX);
    delay(1);
  }
  resultGyroX = resultGyroX/100;
  return resultGyroX;
}

int calibrateGyroY()
{
  for(int i=0;i<100;i++)
  {
    resultGyroY += analogRead(gY);
    delay(1);
  }
  resultGyroY = resultGyroY/100;
  return resultGyroY;
}

int calibrateGyroZ()
{
  for(int i=0;i<100;i++)
  {
    resultGyroZ += analogRead(gZ);
    delay(1);
  }
  resultGyroZ = resultGyroZ/100;
  return resultGyroZ;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    resultAccX += analogRead(aX);
    delay(1);
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}
 
int calibrateAccY()
{
  for(int i=0;i<100;i++)
  {
    resultAccY += analogRead(aY);
    delay(1);
  }
  resultAccY = resultAccY/100;
  return resultAccY;
}

int calibrateAccZ()
{
  for(int i=0;i<100;i++)
  {
    resultAccZ += analogRead(aZ);
    delay(1);
  }
  resultAccZ = resultAccZ/100;
  return resultAccZ;
}

And double check your connections:

Acc_Gyro Arduino
3.3V <—> 3.3V
GND <—> GND
Gx4 X <—> AN0
Gx4 Y <—> AN1
Gx4 Z <—> AN2
Acc X <—> AN3
Acc Y <—> AN4
Acc Z <—> AN5

Also connect 3.3V to the AREF pin on the Arduino for more accuracy.

Changing the Calibration as you detailed is looking much more promising!

The data is starting to look reasonable, and it's not going away and never coming back anymore.

That's serious progress! Thanks!

Please check:

accXval = (accXadc-accZeroX)/102,3;
accYval = (accYadc-accZeroY)/102,3;
accZval = (accZadc-accZeroZ)/102,3;

I suspect you may have a problem there. For me, 102,3 is not the same as 102.3... We can check this by just writing a sketch that multiplies two decimal numbers together, once with a decimal point and once with a comma. The results are not the same for me. Are they for you?

Glad I could help. I see your point. I am not home right now, but I will check it tonight :slight_smile:

Once again, thanks!

That change to Calibration did the trick.

After changing 102,3 to 102.3, I'm getting results from just compAngle plenty repeatable enough for my use. (Part of a nearspace-craft sensor array) All we're really doing is syncing the rotation and vibration readouts to the video cameras from the balloon and rocket. The ground control UI is taking data down from the craft over a radio telemetry link. Now I just need to integrate it. :wink: compAngleX, compAngleY, AccX,Y,Z values are all I need.

Thanks!

I have updated the code, so it is now ver3. Please try it out, and say if it works :slight_smile:
The accelerometer can not measure the rotation (yaw), look at some of the previous post for help. The accelerometer can only measure pitch and roll:

You need a magnetometer to measure yaw. Here is a link for one at Sparkfun: Triple Axis Magnetometer Breakout - HMC5843 - SEN-09371 - SparkFun Electronics.
Why do you want to use the complimentary filter? The kalman, is more precise :slight_smile:
Anyway the project sound really cool, please post a video when your are done :wink:

Lauszus, I made use of your already functional Kalman Filter algorithm in my project: A self stabilizing platform.

You code worked great and you saved me a lot of time :smiley: Thank you

Here are the results:

I also posted the project in the Exhibition forum, in this post.

#Rivello
Awesome job, glad I could help :wink:

  • Lauszus

Hi!!

I was hoping you could help me as I'm getting nuts with this IMU :cold_sweat:

I've tested the IMU just as in your post, same pin connections and the original code (version 2 & 3) but I'm getting some weird data. Or at least that's what it looks to me!

The xAngle and yAngle printed at the end of the processing function doesn't go further than 40 degrees. I've tested it by rotating over one of the axes until I reach 90º. I've checked the voltage output directly from the accelerometers to see if something is wrong but the readings I believe make sense. At calibration voltage for accelerometer is around 1.57v. At 90º it is around 1.9v. If I have understood correctly sensitivity is 330mV/g, so just rotating it by 90º should vary +- 0.33v; exactly as I can read. The gyros look "good". Obviously it is hard for me to make a precise reading of it, but when stable it shows around 1.22v which vary while being rotated. If done quite fast I can see values under 1v and around 2v.

I would really appreciate any help as I'm really lost on why I can only measure angles up to 40º. I've seen other replies commenting the same but no root cause. I'm quite sure I'm doing something wrong but I can't trace it :disappointed_relieved:

Thanks for the help!

Guillermo

#Elwylly
Have you remembered to connect 3.3V to the VREF pin? :slight_smile: