Go Down

Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering (Read 326252 times) previous topic - next topic

Lauszus

To be honest I really don't know. It's pretty complex mathematics. I just found a kalman filter used by another Arduino user (he also got it from some place else). But yes it represents a matrix.

sbsscooter

here's a copy of my void loop , i had a look at my sensitivity values i think they r right now. My combo board uses 5v input.
Code: [Select]

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/5.11;        //(gyroXadc-gryoZeroX)/Sensitivity - in quids       Sensitivity = 0.025/5*1023=5.11
  gyroXangle += gyroXrate*dtime/1000;            //Without any filter
 
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/204.6;             //(accXadc-accZeroX)/Sensitivity - in quids     Sensitivity = 1.00/5*1023=204.6
  accXangle = asin(accXval)*RAD_TO_DEG;

  dtime = millis()-timer;
  timer = millis();
 
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));


  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
 
  processing();

  delay(10);
  }
[code]
[/code]


sbsscooter

yea thanks for all your help!!! , I'm pretty sure it is working correctly now i put a short video on you tube of my results in simplot if u r interested.

http://www.youtube.com/watch?v=KhWYbPgsRMg

Lauszus

Looks nice. Did not know simplot, I might use in another project :)
Can you please share the code/setup you used?

cristo829

I love your program it saved my life well, not yet but maybe it will. Your explanation was really easy to understand and the program works very well. There is just one problem with version 3 or at least for me, when you move the sensor from 0 to 90 only in one of the axis (when you move it fast) you get something bigger than 110 on the other axis although it is supposed to stay at 0. What i did is that i removed the 360 part of the program and it works perfectly from 0 to 90, after you pas +90 it goes +89 +88 and so on. I dont care if it doesnt measures the whole 360 but it would be nice to find a solution for this. I think that when the z axis turns negative it automatically shifts the degrees, maybe it is necessary to make a distintion between the x and y axis i dont know how to do it but if i think of something ill tell you. Im using the same board so it doesnt depend on i think ... @_@

Thanks again

Lauszus

Thank you for your good observation :)
Actually I know, but I simple haven't got the time to look into that :b
For most people it is not a problem, as they only need one angle, so I choose not to look into it.
But if you come up with something, please post the code, and I will update the .zip file :b

sbsscooter

#142
Sep 15, 2011, 01:16 am Last Edit: Sep 15, 2011, 01:18 am by sbsscooter Reason: 1
hey mate glad u liked it couldn't have done it without your help  ;) , maybe i can return the favour with simplot

on the main screen the only thing to add it this line

Code: [Select]
 compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));

 xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);


plot(accXangle, gyroXangle, xAngle, compAngleX);       <<<<<<<<<<<<<this line
processing();
 set_motor();
 delay(10);
 }


then add this to a new tab

Code: [Select]
void plot(int data1, int data2, int data3, int data4)
{
 int pktSize;
 int buffer[20]; //Buffer needed to store data packet for transmission
 buffer[0] = 0xCDAB;             //SimPlot packet header. Indicates start of data packet
 buffer[1] = 4*sizeof(int);      //Size of data in bytes. Does not include the header and size fields
 buffer[2] = data1;
 buffer[3] = data2;
 buffer[4] = data3;
 buffer[5] = data4;
 pktSize = 2 + 2 + (4*sizeof(int)); //Header bytes + size field bytes + data
 Serial.write((uint8_t * )buffer, pktSize);
}
void plot(int data1, int data2, int data3)
{
 int pktSize;
 int buffer[20]; //Buffer needed to store data packet for transmission
 buffer[0] = 0xCDAB;             //SimPlot packet header. Indicates start of data packet
 buffer[1] = 3*sizeof(int);      //Size of data in bytes. Does not include the header and size fields
 buffer[2] = data1;
 buffer[3] = data2;
 buffer[4] = data3;
 pktSize = 2 + 2 + (3*sizeof(int)); //Header bytes + size field bytes + data
 Serial.write((uint8_t * )buffer, pktSize);
}
void plot(int data1, int data2)
{
 int pktSize;
 int buffer[20]; //Buffer needed to store data packet for transmission
 buffer[0] = 0xCDAB;             //SimPlot packet header. Indicates start of data packet
 buffer[1] = 2*sizeof(int);      //Size of data in bytes. Does not include the header and size fields
 buffer[2] = data1;
 buffer[3] = data2;
 pktSize = 2 + 2 + (2*sizeof(int)); //Header bytes + size field bytes + data
 Serial.write((uint8_t * )buffer, pktSize);
}
void plot(int data1)
{
 int pktSize;
 int buffer[20]; //Buffer needed to store data packet for transmission
 buffer[0] = 0xCDAB;             //SimPlot packet header. Indicates start of data packet
 buffer[1] = 1*sizeof(int);      //Size of data in bytes. Does not include the header and size fields
 buffer[2] = data1;
 pktSize = 2 + 2 + (1*sizeof(int)); //Header bytes + size field bytes + data
 Serial.write((uint8_t * )buffer, pktSize);
}


i found all this imformation from this page in the arduino forum http://arduino.cc/forum/index.php/topic,58911.0.html
all thanks going to "Brijesh" he designed the software

Lauszus

Thanks for sharing. I am sure, someone could use that for debugging if they are not familiar with processing :) Or simply doesn't want to make there own graph.

Kashif

Lauszus,

I am modifying your code so that it can talk to my I2C based digital IMU http://www.sgbotic.com/index.php?dispatch=products.view&product_id=787

Any idea how to to calculate "sensitivty" for this board? It's also 3.3V.


Lauszus

You should see the datasheets:
The gyro:
http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf (page 7 - "Sensitivity Scale Facto")
It says 14.375 LSB/(ยบ/s)

The accelerometer
http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf (page 3 - "SENSITIVITY")
It depends on your range. So for example if you use -+2g the sensitivity is 256 LSB/g.

Kashif

Thanx Lauszus,

I have another questions thought.
If I only change Analogue reading code in your code and replace it with I2C based digital reading to suite my IMU, that onwards I can use your code as-is, right?

Please have a look below, it's from the link:
http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/lijojl/docs/main_8cpp_source.html

00022 #define GYROSCOPE_GAIN (1 / 14.375)
00023 //Full scale resolution on the ADXL345 is 4mg/LSB.
00024 #define ACCELEROMETER_GAIN (0.004 * g0)
00025 //Sampling gyroscope at 200Hz.
00026 #define GYRO_RATE   0.005
00027 //Sampling accelerometer at 200Hz.
00028 #define ACC_RATE    0.005
00029 //Updating filter at 40Hz.
00030 #define FILTER_RATE 0.1

I have not seen such things like GYROSCOPE_GAIN, ACCELEROMETER_GAIN,  GYRO_RATE and ACC_RATE etc in your code.
But your code still seems to work very well. I am just wondering, what's the significance of these GAINS & RATES etc? Is it because of the nature of sensors (Analogue or Digital) or what?


Lauszus

Yes that is correct. You only have to change it, and then it will work.
What he call GAIN is just what I call sensitivity :) Just replace sensitivity in my code with the values I provided in the previous post.
RATE is just how fast he wants to read the gyro and accelerometers.

Kashif

oh so it was 2 ways of calling the same thing ;-). Thanx a lot.
I am gonna give it a try and keep you posted.

Kashif

Ok, I am using the following values for Gyro and Accel sensitivity parameters
GYRO =  4.45625  ---- >  (((14.375/1000) / 3.3) * 1023)
ACCEL = 79.36    ---- >  (((256/1000) / 3.3) * 1023)

I took datasheet values (as per your last reply) and applied the formula you described in your article. Did I do it correctly?

I don't know whether I am getting correct results or not.
When IMU is lying horizontally, both X, Y axis reads 0 (with +-5 fluctuations)
1. Shouldn't I get a steady angle with no fluctuations, bcoz I am already applying filter on it?.
2. When I tilt it 90 degree along x-axis, I expect x-axis to read 90 degree but it gives 120 (+-5).
3. When I tilt it around one axis, shouldn't the other axis remain constant? but it's changing too.

I am using Fabio's code to read the I2C sensor and then applying your algorithm to calculate the angle.
Kalman Filter or Complementary Filter, both are giving me more or less the same results.

Any advice?

Go Up