Go Down

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

#### Lauszus

#135
##### Sep 13, 2011, 12:50 pm
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

#136
##### Sep 14, 2011, 12:45 am
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]

#### Lauszus

#137
##### Sep 14, 2011, 12:56 am
It all looks correct. Have you tested it yet?

#### sbsscooter

#138
##### Sep 14, 2011, 03:15 am
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.

#### Lauszus

#139
##### Sep 14, 2011, 10:32 am
Looks nice. Did not know simplot, I might use in another project
Can you please share the code/setup you used?

#### cristo829

#140
##### Sep 14, 2011, 11:40 pm
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

#141
##### Sep 15, 2011, 12:00 am
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 amLast 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

#143
##### Sep 15, 2011, 01:22 am
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

#144
##### Sep 16, 2011, 03:36 pm
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

#145
##### Sep 16, 2011, 03:59 pm
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
It depends on your range. So for example if you use -+2g the sensitivity is 256 LSB/g.

#### Kashif

#146
##### Sep 16, 2011, 06:27 pm
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?

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

#147
##### Sep 16, 2011, 09:09 pmLast Edit: Sep 16, 2011, 09:12 pm by Lauszus Reason: 1
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

#148
##### Sep 17, 2011, 05:45 am
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

#149
##### Sep 17, 2011, 09:34 am
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.

Go Up

Please enter a valid email to subscribe