Arduino Forum

Using Arduino => Sensors => Topic started by: Lauszus on Apr 09, 2011, 09:25 pm

Title: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 09, 2011, 09:25 pm
Hallo everybody
I recently bought this analog 6DOF (six degrees of freedom) IMU board (http://www.sparkfun.com/products/10010 (http://www.sparkfun.com/products/10010)) from watterott.com. It uses three gyros and three accelerometers to calculate angles in three dimensions.


I looked a while for some code online and how to connect with them. After many hours of research I succeeded of making af precise measurement of angles in two directions. I decided to write a short guide for fellow electronic enthusiasts.
The main purpose of this guide is to teach others how to get some useful data from their IMU or just a gyro or accelerometer. The code for Arduino can be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter (https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter). It should be pretty easy to implement my code to your own sensor. I will not describe all the details about the theory behind, instead you can look at the sources for more info.

Before you begin you have to connect the IMU as follows:

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.
It is VERY important that you do not connect the sensor to 5V - this will destroy the sensor.

Now your are ready for reading some data from the sensor.

To communicate with the sensor is straightforward:
The gyro measures degrees per second (0/s) while the accelerometer measures acceleration (g'a) in three dimensions. Both outputs the measurements as a analog signal.
To get these translated into degrees you have to do some coding:

The gyro
First you have to translate quids (a number from 0-1023) into something useful (this is for a ADC with a 10 bit resolution, for example this should be 4095 (2^12-1=4095) for 12 bit ADC). To do this I just use this simple equation:
gyroRate = (gyroAdc-gyroZero)/sensitivity - where gyroAdc are the readed value from our sensor, gyroZero is the value when it is stationary (this is done in the code - look in the "Setup" section) while sensitivity is the sensitivity found in the datasheet, but translated into quids.

If you look in the two gyros datasheets (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf) and http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf (http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf)) you will see that the sensitivity is 3.33mV/0/s for the 4xOUT. To translate these into quids is pretty easy: sensitivity/3.3*1023.
So in this example I get:
0.00333/3.3*1023=1.0323.

NB: to translate mV to V simple just divide by one thousand.

The final equation will look like this:
gyroRate = (gyroAdc-gryoZero)/1.0323

The result will come out as degrees per second (0/s). To translate this into degrees you have to know the exact time since the last loop. Fortunately, the Arduino got a simple command to do so: millis(). By using that, one can calculate the time difference (delta time) and thereby calculate the angle of the gyro. The final equation will look like this:
gyroAngle += gyroRate*dtime/1000

Unfortunately, the gyro drifts over time. That means it can not be trusted for a longer timespan, but it is very precise for a short time. This is when the accelerometer comes in handy. It does not have any drift, but it is too unstable for shorter timespan. I will describe how to combine these measurements in a while, but first I will describe how to translate the readings from the accelerometer into something useful.

The accelerometer
The accelerometer measures the acceleration (g's) in three dimensions. To translate the analog readings into degrees you simply need to read the axis and to subtract the zero offset like so:
accVal = accAdc-accZero

Where accAdc is the analog reading and accZero is the value when it reads 0g - this is calculated in the start of the code, look in the "Setup" section. The zero value can also be found in the datasheet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf (http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf). You will see that the zero voltage at 0g is approximately 1.5V, to translate this into quids, you again have to use this equation: zeroVoltage/3.3*1023.
So in this example I get:
1.5/3.3*1023=465.

You can then calculate the pitch and roll using the following equations:
pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI

Atan2 has a output range from -? to ? (see http://en.wikipedia.org/wiki/Atan2), I simply add ?, so the range it converted to 0 to 2?.
To convert it from radians to degrees we simply multiply the result by 57.295779513082320876798154814105 - this is predefined in the Arduino IDE as RAD_TO_DEG.

Kalman filter
As I explained earlier the gyro is very precise, but tend to drift. The accelerometer is a bit unstable, but does not drift. You can calculate the precise angle by using something called a Kalman filter. A detailed guide on how it's implemented can be found at my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.

If you want to use something a bit more simple, you can use what's called a Complementary Filter. It is pretty easy to understand and the math is much simpler, because it only works in one step.
For example the equation could look like this:
angle = 0.98 *(angle+gyro*dt) + 0.02*acc - you can fine tune the numbers to whatever you like. Just remember that the sum must be 1.
For me the result from the Complementary Filter was very close (or almost the same) as the one calculated by the Kalman filter.

You have now learned (hopefully) how to get analog data from IMU and translate it to something useful. I have attached my own code for my 6DOF IMU (http://www.sparkfun.com/products/10010 (http://www.sparkfun.com/products/10010)), but with some slightly modification, I am pretty sure that it is possible to use it with any analog gyro/accelerometer.

If you have any question, fell free to post a comment below.

Sources:
http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/ (http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/)
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418)
http://www.x-firm.com/?page_id=148 (http://www.x-firm.com/?page_id=148)
http://web.mit.edu/first/segway/ (http://web.mit.edu/first/segway/)

Update
I have just finished a Processing code which prints out data from the Arduino on a nice graph. As you can see in the video below the filtering is quit effective. The light blue line is the accelerometer, the purple line is the gyro, the black line is the angle calculated by the Complementary Filter, and the red line is the angle calculated by the Kalman filter. As you might see the Kalman filter is just a bit more precise (i know it is difficult to see in the video) than the Complementary Filter, especially when I shake it.
The code can be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/Graph (https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/Graph).
It is also possible to see the data from the y-axis. Just uncomment drawAxisY(); in the code.

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

My Balancing robot
Below is a video of my balancing robot. It uses the same IMU and algorithm as described in the post above.
The source code can be found at the following link: https://github.com/TKJElectronics/BalancingRobotArduino
http://www.youtube.com/watch?v=N28C_JqVhGU

Kickstarter
I have just released my balancing robot on Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit (http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit).
Please consider backing the project.
http://www.youtube.com/watch?v=_kQniPbg9zc

Update
I have also provided code for several other IMUs including a 9DOF which allows you to estimate yaw as well.
All can be found in the Github repository: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter (https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter).
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: wyojustin on Apr 10, 2011, 07:04 am
Thanks for posting this guide.  I will give it a go!

Justin
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 11, 2011, 06:19 pm
I have updated the code, so it now measures 3600 instead of 1800. Just add this after you calculate the angle measured by the accelerometers:
Code: [Select]
if(accZval < 0)//360 degrees
  {
    if(accXangle < 0)
    {
      accXangle = -180-accXangle;
    }else
    {
      accXangle = 180-accXangle;
    }
    if(accYangle < 0)
    {
      accYangle = -180-accYangle;
    }else
    {
      accYangle = 180-accYangle;
    }
  }
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aerosam on Apr 13, 2011, 12:58 pm
Awesome work

I am making a 3 axis rate table to measure how accurate the imu is.
I want to answer the following questions
1)For a  given set of sensors, how can i get the best possible performance from my Kalman filter in estimating angles.
2)Now that the "optimal" Kalman filter code is identified, can i achieve better performance by choosing better gyros and accelerometers.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 13, 2011, 01:49 pm
Thank you. Are you using the same IMU as me or a different one?
If so will you please tell me what you come up as the best values for Q_angleX, Q_gyroX, and R_angleX?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aerosam on Apr 13, 2011, 03:37 pm
I am using the Ardu-Imu
http://www.sparkfun.com/products/9956

R is the co-variance matrix. It should typically be the square of the standard deviation of the Gyro.
I think it should be there in the datasheet, or you could just keep things stationary and log the data for about 5 mins.
Calculate the mean and std deviation.
R=std_deviation^2


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: brijesh on Apr 20, 2011, 03:02 am
Hello,

This is a great post about IMU's. Lots of good information. I am also working with Gyro's and accelerometers in my Quadrotor project. I needed a simple tool to visualize data and ended up writing one. Although there are ways to plot data using Processing, I wanted a stand alone tool.

Here is link to relevant thread. Hope you guys find the tool useful.

http://arduino.cc/forum/index.php/topic,58911.0.html (http://arduino.cc/forum/index.php/topic,58911.0.html)

I will probably change the tools to accept float data in the next version.

Cheers
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 24, 2011, 11:52 pm
Aerosam:
What unit do you measure the standard deviation and mean in? For example is it %/C or what?

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Fons1302 on Apr 27, 2011, 10:33 am
This is a great for IMU-starters like me. I will give it a try.
Thank you very much.

Fons
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Brettj on May 06, 2011, 09:32 am
This isn't working for me. My angle is still drifting at a degree per second.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 06, 2011, 10:05 am
Are u using the same IMU as me or a different one?
If not, try to change the value of R_angleX and R_angleY and see if it helps.

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Brettj on May 06, 2011, 12:13 pm
I'm using a different one, the digital combo board. http://www.sparkfun.com/products/10121. I had to mod the code to suit the digital version, but i'm confident it works.
I've tried changing the values but they don't seem to have any benificial effects :(
Sometimes the result jump about 20 degrees, quite rarely. Does this ever occur for you?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 06, 2011, 02:19 pm
Okay.
It sounds like you are getting data from the accelerometer. I you sure you are using both sensors? :)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Brettj on May 06, 2011, 02:57 pm
Haha, yeh definitely getting all the data :P For some reason i get more accurate data from the non filtering. The calculated accelerometer angle doesn't go through 360degrees. Gets about a max of 50. Is this supposed to happen?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 06, 2011, 03:07 pm
Try to use the complementary filter instead and see what happens.

Post your code and I will  have a look at it - you must have done something wrong :)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Brettj on May 06, 2011, 03:14 pm
I modded the code into java, as i'm doing the postprocessing on the PC.
the only thing i think i modded is this...
xAngle += kalmanCalculateX(accXangle, gyroXrate, dtime);
I made the xAngle += so I just print total xAngle directly. Is that wrong?
Apart from that, using nano seconds and slightly different calibration position it is the same.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 06, 2011, 03:27 pm
Yes that it totally wrong. If you do like that, it will plus the angle with the last one. It should be:
Code: [Select]
Angle = kalmanCalculateX(accXangle, gyroXrate, dtime);

It is only when you calculate the gyros angle that you have to plus the last angle with the new one!
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Brettj on May 06, 2011, 04:45 pm
ok fixed that. But it only works for about 50 degrees +-. Similar to the other kalman filter i tried :(
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 06, 2011, 05:10 pm
Maybe you forgot that gyroXrate should be in degrees per second and not degrees in this line:

Code: [Select]
Angle = kalmanCalculateX(accXangle, gyroXrate, dtime);

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fino1990 on May 09, 2011, 10:45 am
Hi Lauszus, I bought the same gyro(http://www.sparkfun.com/products/9165) used by IMU on your project. I just want to read angle from the gyro but having a little confuse about the HP, PD and ST pin. Should I just left them all unconnect or do you have any suggestion?

~Thanks  :)
~Fino
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 09, 2011, 10:57 am
You can just leave them all unconnected :)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 13, 2011, 11:33 pm
Hey Lauszus!

First off thanks for posting, this looks great and could be exactly what i need!

basically i am creating a ball full of different sensors which will then act as a synthasizer.this is my final year project and i am stuck here with only a few weeks to go. I have other sensors but what i really want is to be able to read if the ball is being tilted forward or back,left and right and by how much so i can modulate sound accordingly.( i am not trying to measure the acceleration,just the amount it is tilted)

i have been searching all the different possibilities with gyroscopes and accelerometers but can't seem to find clear cut examples of what i want. It looks like your code here is what i need? If i was to buy the 6dof is it as simple as using the code you linked? and then i would be able to get proper tilt values?

I am only a beginner with arduino and just know the basics of reading sensors but i'm finding the task of measuring tilt a bit deep for me.

your help or advice would be greatly appreciated!

(a final year college student freaking out lol) Billy.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 14, 2011, 09:28 pm
Hallo bilbo-bongo

First of all: it sounds like it is exactly what you need :)
You really don't need a 6DOF for your project. You only need 3 accelerometers and 2 gyros. But if you don't want to get your hands dirty, you can just buy the same IMU as me and just copy the code.
The only thing you need is some way of picking up the serial commands from the Arduino, exactly as I did with Processing.
Hope that gave you some answers :)

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 14, 2011, 10:17 pm
cool!

Yeah i already have Pd reading the serial and picking up values from my other sensor (pressure) and modulating sound. It's just sensing tilt i couldn't figure out.

you have saved me!

I'm going to order one of those. i'm in ireland,hope it arrives in time!

thanks again!

billy.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 14, 2011, 10:57 pm
Just order it from Watterott.com, it only took 3 days to arrive here in Denmark ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 15, 2011, 12:24 am
perfect. even better!

Do you know what is the difference between the 70 euro one and the 50 euro one??? specs look the same and the name is the same??


thanks again  :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 15, 2011, 02:02 am
looks like the 70 euro one is sold out anyway
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 15, 2011, 07:32 am
The one that cost 70 euro, is the old one (look at the product number). The one that cost 50 euro is in fact the updated one. The only difference is that it "removes the high-pass filters which were creating some issues with the output." :)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: jaysettle on May 16, 2011, 08:07 pm
where's the code?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 16, 2011, 08:34 pm
zip files under the youtube video. have to be logged in to see them i think
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: onidaito on May 18, 2011, 12:20 pm
Hi chaps. I've been playing around with this board and the code and I must say, thanks for this. Its definitely been a big help.

Im porting this to C++ and Cinder for use in an experiment. I need to get all 3 axes represented though. Is there any particular reason that the Z Axis is not represented?

I notice that altering the Y rotation also alters the Z rotation on the Gyros but not on the Accelerometers. I believe I need to compute the Z somehow but I'm really not too sure on the math.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 18, 2011, 01:32 pm
It is not possible to calculate the z-axis with the accelerometer. Remember that it measures earth gravitational force, and it does not change when you rotate the IMU horizontal. You need something like a magnetometer to calculate the last angle :)
If you only need to see if it changes you can just use the gyro and then compensate for drift in the code.
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: onidaito on May 19, 2011, 12:23 am
Ok that can't be right surely? I get that its not a compass but its billed as 6 Degrees of Freedom which suggests Z. Indeed, its even marked on the board so surely, if can measure the other 2, this 3rd is nothing special. I mean, I own a pair of these vuzix glasses things and they only have a set of gyros and accelerometers. Im not after a direct heading, thats not a problem and I know we cant get that but we most definitely should be able to get the 3rd.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 19, 2011, 12:27 am
Not as far as I know. You can easily get the z-axis from the gyro, but it will drift. It is not possible to get the z-axis using the accelerometer.
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: onidaito on May 19, 2011, 12:42 am
Interesting. I need to look over this compensation for drift then. However, Im still not sure why one cant get acceleration on the Z axis (Im British though, so Z axis means depth to me but in this case, I believe we mean yaw). There is a yaw gyro indeed but it is claimed there is a yaw accelerometer as well. Why is it there if it can't be used? It seems confusing. Clearly I must be missing something.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 19, 2011, 12:50 am
Yes it is yaw (the rotation around the z-axis). The reason why you can not measure yaw with a accelerometer is because the gravitational force from earth does not change when you rotate the IMU horizontal. In theory you will not measure any difference at all, in pratical however you see the acceleration from the movement. I use the z-value from the accelerometer together with pitch and roll to calculate the force vector (this is theoretical 1, but in practice there is a small difference). I also use the z-value from the accelerometer to know when the IMU is tilted more than 90 degrees to both side, so I can get 360 degree resolution instead of only 180 :)  
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: pito on May 19, 2011, 02:48 am
.."while the accelerometer measures Earth gravitational acceleration (g) in three dimensions"..
not only, it measures the acceleration of the unit as well..P. :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: onidaito on May 19, 2011, 10:38 am
Yes, I understand in the case of the accelerometer now. That does make sense. So, do you use the gyro at all for the Yaw calculation?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 19, 2011, 10:59 am
Pito: exactly! That is why you can not count on the accelerometer on short terms, because it is to unstable. For example it will jump all over the place if you shake it. That is why you have to combine it with a gyro, that do not change when you for example shake it.

Onidaito: I do not use it right now, but if you look in my code I actualy calculate the yaw. Just write
Code: [Select]
Serial.println(gyroZangle); and you will see the yaw, but remember it will drift!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Senso on May 19, 2011, 12:04 pm
If it is an ITG a simple offset calibration is enought to cancel the drift, or almost all of it.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 19, 2011, 12:22 pm
What do you mean? Will you just keep calibratrating all the time, to compensate for drift?
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 20, 2011, 12:31 pm
hey Lauszus,

I got my sensor (nice and fast too,thanks for the germany tip!)

I have it all connected and it seems to be working. One thing i notice though is that tilting forward on the y-axis gives me values up to 60 but backwards on the y-axis i only get as high as 20. on the x-axis then i an even 40 on each side.

I notice you wrote to add in a piece of code to make it read full 360, is that why mine isn't reading right?

i tried putting in that code after
"  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;"

But didnt change anything.

also i'm not connecting the z-axis because i don't need it. I assumed it wouldnt make a difference.

Just wanted to say thanks again for the code. Looks like my project will become what i dreamed  :) I'll post a video up here when its done
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 20, 2011, 01:07 pm
Glad I could help. Have you connected the accelerometers z-value? If not that is why you data is getting weird. Remember that the code uses the z-value from the accelerometer to calculate the force vector.
Looking forward to see your project in action :)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 20, 2011, 01:29 pm
no i didn't plug it in because i didnt need to measure the x-axis.

I am using a pressure sensor also, and using an arduino Uno so only have 6 inputs  :~

so it needs to be used for calculation even if you dont plan on printing the value?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 20, 2011, 01:34 pm
Don't you mean the z-axis (yaw)? You need it for my code, but yes it it possible to calculate the angle with just one two values I think, haven't really researched on that, because it hasn't been a problem to me.

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on May 20, 2011, 01:39 pm
yes sorry i meant z-axis.

ok, i'll try research into that. Or else i might be able to get an arduino mega.

Worst case scenario i can still work with the values i am getting
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 20, 2011, 01:42 pm
Okay. Please upload the code for calculating the angle with only two values from the accelerometer when you get it working :)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 26, 2011, 01:04 pm
I thought a bit about it, and actually you can calculate the axis with just one value.

In the code it says:
Code: [Select]

accXangle = acos(accXval/R)*RAD_TO_DEG-90;
accYangle = acos(accYval/R)*RAD_TO_DEG-90;


Instead you could write:
Code: [Select]

accXangle = asin(accXval)*RAD_TO_DEG;
accYangle = asin(accYval)*RAD_TO_DEG;

Then you should get an angle very close to the first one!! It is not as precise as the first one, but that kind of precision is only important if you are building a balancing robot or something like that.
The resolution will also only be 180 degrees.

That should fix your problem :)

- Lauszus  
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: onidaito on May 29, 2011, 07:27 pm
Guys, I really think I should be getting better yaw values from the Gyro. I found this:

http://code.google.com/p/imumargalgorithm30042010sohm/ (http://code.google.com/p/imumargalgorithm30042010sohm/)

This shows some pretty decent output which leads me to believe that my unit could be broken as the Yaw value doesn't change at all when I rotate around that axis. I noticed that on the sparkfun page for this item, someone else had a similar problem. I've emailed the chap from the link above in the hopes that I can figure this out. At the moment, the lack of decent/any yaw (no need to use a magnometer just yet) is halting my project :( Im wondering if there is something else im missing here. I've tried the kalman filter approach as well and that seems worse than just using the raw gyro values :S
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 30, 2011, 02:46 pm
You can not use the kalman filter for the yaw value, because the accelerometer can not measure the yaw :)
Hope you get it working!
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: onidaito on May 31, 2011, 02:55 pm
Yeah, I've done a little more research and I see what you mean. I guess i just need to integrate over time for yaw off the gyro and then reset when the drift gets too much... until maybe I get a magnetometer as well!

Thanks though! Its been a big help!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: corez on Jun 09, 2011, 12:13 pm
Hey guys,

Is it possible to also use the kalman filter for only an accelerometer? It will only use one axis (of the three available). I just need a smooth movement of the servo I'm using
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 09, 2011, 01:05 pm
You can not use the kalman filter with only one input. Sorry but you need a gyro too, that is why it called "sensor fusion" :)
You could perhaps use some other kind of smoothing algorithm.
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on Jun 09, 2011, 01:20 pm
Hey Lauszus, I finished my final year project in college. I thought I would show it on here so people can see what can be done with the code and sensor. Thanks again for you help!



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

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 09, 2011, 01:46 pm
It look and sounds very cool. Thanks for sharing. So which academic degree do have now?
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: bilbo-bongo on Jun 09, 2011, 01:59 pm
multimedia  :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: corez on Jun 09, 2011, 10:59 pm

You can not use the kalman filter with only one input. Sorry but you need a gyro too, that is why it called "sensor fusion" :)
You could perhaps use some other kind of smoothing algorithm.
- Lauszus


Any recommendations? I found these...

http://arduino.cc/en/Tutorial/Smoothing

http://www.arduino.cc/playground/Main/Smooth

Don't know how smooth they work, didn't get time to test them yet
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 10, 2011, 10:18 am
I haven't got any experience myself, but it look likes what you need :)
Or you could just write something like this:

Code: [Select]

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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 12, 2011, 07:10 pm
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
Quote
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 13, 2011, 01:45 pm
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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 13, 2011, 03:57 pm
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 13, 2011, 04:15 pm
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? :)

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 14, 2011, 05:48 pm
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 :)
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? ( http://www.sparkfun.com/products/10321 & http://www.sparkfun.com/products/10125)

thanks for the patience :)





Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 14, 2011, 08:54 pm
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 :( 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 :)
Nice hand drawing btw :D
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 15, 2011, 08:20 pm
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 :) ): 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?

Code: [Select]
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?
Code: [Select]
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 :)
if you ever need a quick hand drawn sketch (dirty and quick) just ask ;)

cheers
b.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 16, 2011, 10:52 am
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:
Code: [Select]
gyroZeroZ = calibrateGyroZ();

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

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 16, 2011, 07:50 pm
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 (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 :)

thanks for all the help
cheers
b.




Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 17, 2011, 01:21 am
That looks pretty sweet. Does he fusion the magneton reading with the gyro and accelerometer? :)
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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 17, 2011, 10:52 am
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 :)
http://mbed.org/cookbook/IMU

cheers
b.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fax8 on Jun 19, 2011, 10:38 pm
Hey guys! I'm the FreeIMU (http://www.varesano.net/projects/hardware/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 (http://www.varesano.net/blog/fabio/my-mos-thesis-using-arduino-tangible-human-computer-interaction) useful.

Good luck with your projects,

Fabio
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: beltran on Jun 20, 2011, 05:28 pm
hey Fabio!
sei fortissimo! :D grazie una cifra! :)

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.





Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fax8 on Jun 21, 2011, 02:36 pm
Quote

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.

Quote

- 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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Nex2k12 on Jul 19, 2011, 01:17 am
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?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fax8 on Jul 19, 2011, 03:04 pm
What algorithm are you using?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Nex2k12 on Jul 19, 2011, 06:32 pm
I'm using the complimentary filter, like yours, but I'm using I2C sensors so the code is a bit different.
Code: [Select]
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);

}

Code: [Select]
{
  //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);


}

Code: [Select]
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fax8 on Jul 19, 2011, 07:14 pm
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Nex2k12 on Jul 20, 2011, 09:56 pm
You were right, everything is working perfectly now :)

Thanks for your comments.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fax8 on Jul 20, 2011, 10:08 pm
Awesome! Plasure to be helpful.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: builditnow on Jul 28, 2011, 11:10 pm
Hi,
I am trying to use this code/setup to look at the forces acting on snowboarders while going through a turn. I have setup the 6DOF chip from Sparkfun, just as the tutorial describes. But when I load the code and open up processing the graph only shows a flat red line. It does not react to moving the Razor. I have also tried looking at the serial output of the Arduino code and that shows simply all 0's. I am pretty experienced with Arduinos but I have not played much with accelerometers and gyros before so I may be over looking something simple.

Thanks in advance for any help! And for all of the great info!
-Builditnow
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 29, 2011, 01:17 am
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:
Code: [Select]
processing();? :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: builditnow on Jul 29, 2011, 02:48 pm
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!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 29, 2011, 02:56 pm
Have you connected the IMU the right way, please double check :) It sounds like your not getting the data for some reason..
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: builditnow on Jul 29, 2011, 03:57 pm
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  
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 29, 2011, 04:10 pm
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 ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 07:59 am
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:

Code: [Select]

//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:

Code: [Select]

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!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 08:03 am
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?

Code: [Select]

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.

Code: [Select]

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.4*5.0 != 2.4*5.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!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 30, 2011, 10:47 am
You have to include this in the setup() function:
Code: [Select]

  //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:
Code: [Select]

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 :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 11:30 am
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 11:38 am
This works:

Code: [Select]

  //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:

Code: [Select]

  //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.

Code: [Select]

  //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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 11:48 am
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. 
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 30, 2011, 12:07 pm
Try changing the calibration page to this:
Code: [Select]

//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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 12:14 pm
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?

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 30, 2011, 12:25 pm
Glad I could help. I see your point. I am not home right now, but I will check it tonight :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lara on Jul 30, 2011, 12:49 pm
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. ;)  compAngleX, compAngleY, AccX,Y,Z values are all I need.

Thanks!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 31, 2011, 03:48 pm
I have updated the code, so it is now ver3. Please try it out, and say if it works :)
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: http://www.sparkfun.com/products/9371.
Why do you want to use the complimentary filter? The kalman, is more precise :)
Anyway the project sound really cool, please post a video when your are done ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Rivello on Aug 09, 2011, 09:32 am
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 :D   Thank you

Here are the results:
http://www.youtube.com/watch?v=T0SFAdPUUYs

I also posted the project in the Exhibition forum, in this post. (http://arduino.cc/forum/index.php/topic,68755.0.html)

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 09, 2011, 10:14 am
#Rivello
Awesome job, glad I could help ;)
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: elwylly on Aug 10, 2011, 03:55 pm
Hi!!

I was hoping you could help me as I'm getting nuts with this IMU  :smiley-roll-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  :smiley-sad-blue:

Thanks for the help!

Guillermo
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 10, 2011, 04:00 pm
#Elwylly
Have you remembered to connect 3.3V to the VREF pin? :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: elwylly on Aug 10, 2011, 04:28 pm
Hi Lauszus,

Yes, I've connected AREF to 3.3v. Also the rest of the pins are connected as you described to make sure I was not messing up with the values  XD

I'm wondering whether the gyros are working correctly. I think the acc is working as expected due to the voltage readings, but I'm trying to figure out the self-test option the gyros have. To be honest, that's the only explanation I seem to find. :~

Cheers,
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 10, 2011, 04:34 pm
#Elwylly
Are you sure that you have connected the it to the right way:
Gx4 X      <-->   AN0
Gx4 Y      <-->   AN1
Gx4 Z      <-->   AN2
Acc X      <-->    AN3 
Acc Y      <-->    AN4 
Acc Z      <-->    AN5
? :)
It is pretty hard for me, to help when I do not know more :)
It should not be necessary to use the self-test.
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: elwylly on Aug 10, 2011, 07:13 pm
Yep. Everything connected as you mentioned. I've checked hundreds of times as it is a stupid mistake that I would love to avoid  XD

I've been checking just the accelerometer value, accYangle, and it is not even close to be correct. It is showing a value of 30º when rotated 90º. At 90º, considering that Gx4 sensitivity is 0.33v/g, the voltage reading is 1.9v (1.57v when flat). That seems to be correct as if I have understood correctly accelerometer in that position should be reflecting gravitational force only for that axis. I think the problem may be there. I'll try to see what's going on when calculating the accelerometer values as they seem to be the source of error. Maybe as somebody mention is a environment config problem that is messing with calculations.

Cheers,
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: elwylly on Aug 10, 2011, 07:58 pm
Hi Lauszus,

I think I found what's disturbing my readings. The problem seems to be the Z axis. I've got the IMU with the ICs facing down, and I think the problem is there. I need to calculate correctly the Z axis...

Cheers,
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 10, 2011, 10:12 pm
Okay, it is simple then.
In line 97, is says:
Code: [Select]
 accZval++;//1g in horizontal position
That should just be changed to:
Code: [Select]
 accZval--;//-1g when facing down
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: elwylly on Aug 11, 2011, 08:34 pm
I had to adjust the 360 degrees part as well but finally it is working as expected  ]:D

Thanks for your post Lauszus, it was really helpful.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 11, 2011, 08:45 pm
Oh okay, thats right. Had to do the same thing once, but forgot that you also should adjust that as well.
You're welcome :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Laurencet on Sep 01, 2011, 11:01 am
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...

Code: [Select]



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; 
;


}
[/size]
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 01, 2011, 12:41 pm
#Laurencet
Thanks for sharing :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Laurencet on Sep 01, 2011, 02:08 pm
It's based on your hardwork....
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 01, 2011, 02:15 pm
Thanks anyway :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 06, 2011, 06:56 pm
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 ;-)

I will greatly appreciate your reply.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 06, 2011, 08:22 pm
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: http://www.arduino.cc/en/Reference/Wire
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: https://github.com/a1ronzo/6DOF-Digital .
- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Laurencet on Sep 07, 2011, 11:25 am
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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 07, 2011, 05:20 pm

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: http://www.arduino.cc/en/Reference/Wire
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: https://github.com/a1ronzo/6DOF-Digital .
- 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.


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


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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 07, 2011, 05:42 pm
Okay good, then it should not be that hard to get it working.
Have you seen this post: http://www.varesano.net/blog/fabio/my-first-6-dof-imu-sensors-fusion-implementation-adxl345-itg3200-arduino-and-processing ?
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: http://www.sparkfun.com/products/10121#comment_20331

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).
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Laurencet on Sep 08, 2011, 12:20 pm
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...... 


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 08, 2011, 04:33 pm
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 08, 2011, 07:52 pm
Thanks for your feedback :) Really glad I could help!
Looking forward to see your work :D
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 10, 2011, 06:20 am
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)  http://www.varesano.net/blog/fabio/my-first-6-dof-imu-sensors-fusion-implementation-adxl345-itg3200-arduino-and-processing

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 ;-)
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 ;-)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 10, 2011, 11:33 am
Nice to hear you got it working :)
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 10, 2011, 02:26 pm

Nice to hear you got it working :)
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 ;-)
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 ;-)

Guys, I am not paid for writing it all ;-) 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!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 11, 2011, 05:24 pm
Thank you for your kind words :D
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 :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kuxz2008 on Sep 11, 2011, 05:25 pm
I am currently working on a Quad flying machine and I am using the IMU (http://www.sparkfun.com/products/9956 (http://www.sparkfun.com/products/9956)) with the magnetometer (http://www.sparkfun.com/products/9371 (http://www.sparkfun.com/products/9371)).
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 11, 2011, 05:32 pm
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:
Code: [Select]

compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kuxz2008 on Sep 12, 2011, 03:19 am
Kk, will give it a try...
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aine on Sep 12, 2011, 11:33 am
hello guys..im new here :)
im currently doing my final year project about measuring tilt angle with gyro and accelerometer sensor..
im using IMU5DOF,kalman filter and arduino duemillanove..did u have the source code for connecting all this?
i hope that u guys can guide me :)tq
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 12, 2011, 12:52 pm
Yeah. Take a look at the bottom of the guide. Or click at this link: http://arduino.cc/forum/index.php?action=dlattach;topic=58048.0;attach=5095
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kuxz2008 on Sep 12, 2011, 02:31 pm
I would like to know how can I read raw ADC value of the accelerometer and the gyro from this IMU(http://www.sparkfun.com/products/9956 (http://www.sparkfun.com/products/9956))?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 12, 2011, 02:41 pm
You should have a look at the code provided by Sparkfun: http://code.google.com/p/ardu-imu/downloads/list :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sbsscooter on Sep 13, 2011, 06:16 am
hi , before i get into my problem i would just like to thank "Lauszus " for this thread it has been a huge help. Im relatively new to Arduino and very new to posting my problems , so far i have been lucky and worked through all my problems by browsing through the forum but this time im stuck.

Im using a different gyro/acc combo board to the example (http://www.sparkfun.com/products/9249) so ive had to change the sample code a fair bit and been debugging everything as i go. im only interested in solving in the X direction for my project and ive narrowed down my problem to this part of the program

#
  R = sqrt(accXval);//the force vector
   accXangle = (acos(accXval/R))*RAD_TO_DEG-90;

    if(accXangle < 0)
    {
      accXangle = -180-accXangle;
    }
    else
    {
      accXangle = 180-accXangle;
    }   
#

When i debug accXangle with the code like this it seams  to give me the number i expect to see , however when i run the debug part of the code further down

#
compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);

  Serial.print("comp_angle");
  Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print ("kalman");
  Serial.println(xAngle,0);Serial.print("\t");
#
it returns only the value of "0" for both the comp_angle and xAngle i know my problem lies with accXangle  because when i substitute in accXval the equations work (not giving anywhere the right answers but they do give me a reading )

any help you could all lend me would be tops, im sure there is a better way to post snipets of my code also but thats another thing im  not yet sure how to do.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sbsscooter on Sep 13, 2011, 08:05 am
scrap that i figured it out because im only interested in one angle i dont need to include the force vector (R) or cos-1 (acos) now my code reads like this

# accXadc = analogRead(aX);
 accXval = (accXadc-accZeroX)/204.6;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102.3....204.6
 accXangle = accXval*RAD_TO_DEG;
#  
the program would reads zero when level but now for some reason my range reads

100........20..0.....20...100  (always giving a positive reading but zero when level), and when i debug accXangle it also only gives me positive readings

back to scratching my head lol
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 13, 2011, 08:57 am
Are you sure your gyro reading are correct? You can not use a complimentary filter og kalman filter on only one of them, you need both to have a gyro reading (degrees/s) and a accelerometer reading (degrees).

You can not calculate the force vector using your IMU, as it only uses a dual axis accelerometer - you need three to calculate the force vector.

Instead you should calculate the angle using:
Code: [Select]

accXangle = asin(accXval)*RAD_TO_DEG;

If you then got the gyro reading (degrees/s), you can use the complimentary and the kalman filter provided :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sbsscooter on Sep 13, 2011, 09:35 am
hey , yea im definitely reading the gyro correctly and i substituted in the line of code u suggested everything is working including kalman complementary filter gyro and accelerometer!!! :) ....except im still getting only positive value's from accXangle , however accXval is working perfectly with both negative and positive values . ive tried changing a few things but i don't seam to be getting anywhere

Code: [Select]

  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/102.3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102.3
  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);
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 13, 2011, 10:17 am
First of all, you are using the wrong sensitivity. If you take a look at the datasheet: http://www.sparkfun.com/datasheets/Accelerometers/ADXL203.pdf you you will see that the sensitivity is 1000mV/g - translated into quids: 310 (1.000/3.3*1023=310).
Please post your code for the gyro readings as well and I will have a look at it :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kuxz2008 on Sep 13, 2011, 12:42 pm
I would like to know what does PX_00, KX_0, KX_1 under the Kalman section code represent ? Is it a representing as a form of matrix ?

Code: [Select]

float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
float KX_0, KX_1;
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on 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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sbsscooter on 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]
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 14, 2011, 12:56 am
It all looks correct. Have you tested it yet? :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sbsscooter on 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.

http://www.youtube.com/watch?v=KhWYbPgsRMg
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on 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?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on 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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on 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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sbsscooter on Sep 15, 2011, 01:16 am
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
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on 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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on 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.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on 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
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on 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?

Please have a look below, it's from the link:
http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/lijojl/docs/main_8cpp_source.html (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?

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 16, 2011, 09:09 pm
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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on 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.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on 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.

Any advice?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 17, 2011, 11:34 am
It is a bit wrong. Your code should look something like this:
Code: [Select]

gyroYadc = readGyroX; //Read with I2C
gyroYrate = (gyroYadc-gyroZeroY)/14.375;

accYadc = readAccX; //Read with I2C
accYval = (accYadc-accZeroY)/256;


Then just calculate the angles as usual :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 17, 2011, 12:19 pm
Thank you so much Lauszuz,

Finally my angles have started looking like "angles" ;-)

Though I noticed one thing, it takes it a while to get to the correct angle.
E.g. if I tilt the device slowly, all stays good but if i do it slightly faster, the change in values takes place gradually? (roughly 2+ seconds for getting down to 0 degree from 90 degree or vice versa) ... is that normal?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 17, 2011, 12:25 pm
Is that both with the Kalman- and Complementary filter? Mine does that as well, but definitely not that slow.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Sep 17, 2011, 10:26 pm
Ok, so I've read nearly all of this thread and have a couple of questions.

In my understanding, you need 6dof (3accel, 3gyro) to be able to produce two absolute angles. The third angle is not possible to calculate at all? Or reliably due to gyro drift?

So if you add a magnetometer you can get 3 absolute angles?

If I have an ArduiMU (http://www.sparkfun.com/products/9956) (arduino + 3axis gyro + 3axis accel) can I add a manegometer that uses i2c and be able to spit out all the data to a computer? (without needing more analog ins).

Does it matter how many axis' the magnetometer has? (in order to calculate a third absolute angle)


Lastly, if the data is going to end up in a computer (running Max/MSP) is it better to do the math/calculations there rather than on the Arduino? Any benefit to doing the calculations locally on the Arduino?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 18, 2011, 12:58 pm

Is that both with the Kalman- and Complementary filter?


yup Kalman filter is giving much better results, so I have switched to that now.
One more thing I noticed, the moment X-axis angle starts to approach 90 degree and onwards, Y-axis angle suddenly goes crazy. And same is the case with X-axis angle when Y-axis starts to get near to 90 degree.

Is that due to my IMU or you are experiencing the simillar results with yours?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 18, 2011, 02:15 pm
#kriista
You actually only need a 5DOF to calculate pitch and roll. You can even calculate it with a 4DOF, as you can just calculate the angle using sinus. No the third angle (yaw) is not precise due to gyro drift - but it can be calculate with some kind of filter. You need a magnetometer to calculate the last angle (yaw). Yes you can just use a I2C magnetomer with your board (http://www.sparkfun.com/products/9956). I am pretty sure that you need a 3-axis magnetometer to calculate the last angle - haven't tried it yet, as I do not own a magnetometer.
I your need is very time-critical, then it is better to do the calculation on the computer, as it got a lot more processing power than the Arduino. The good thing about doing it on the Arduino, is that you can easily hook it up to any program, with supports serial communication.

#Kashif
It is because you use 360 resolution. Just uncomment it if you need both angles :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Sep 18, 2011, 02:43 pm
It'd be for a musical performance setting, so latency is definitely undesirable.
I'm gonna try going with just the 6DOF and approximating the 3rd angle to see if that works, and if it doesn't I can add the magnetometer with i2c. I was initially worried that I had used up all my analog ins, so I didn't know this was a way around that.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 18, 2011, 02:47 pm
Okay, please post your work when you are finished :) It always nice to see what people come up with!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Sep 18, 2011, 03:05 pm
Will do, though it will be a while as I've not even sorted out the wireless and hardware assembly part....not to mention figuring out the math on the computer side, and then applying it to something meaningful etc...

But yeah, I'll post it.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 18, 2011, 05:37 pm

#Kashif
It is because you use 360 resolution. Just uncomment it if you need both angles :)


uncomment what? I don't have anything commented out ;-) using ur code as-is, the only change i made was to read the I2C
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 18, 2011, 05:39 pm
This part: :D
Code: [Select]

 if(accZval < 0)//360 degrees
 {
   if(accXangle < 0)
   {
     accXangle = -180-accXangle;
   }
   else
   {
     accXangle = 180-accXangle;
   }
   
   if(accYangle < 0)
   {
     accYangle = -180-accYangle;
   }
   else
   {
     accYangle = 180-accYangle;
   }    
 }
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 19, 2011, 12:48 am
That's what I meant, this part is already uncommented. ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Sep 19, 2011, 04:40 am
@Kashif  You have to comment (or delete) that part in order to use both axis without any problems, but it will only measure correctly from 90 to -90
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: AntR on Sep 19, 2011, 12:58 pm
If your filter is laggy, it might be because your gyro and accel signs do not match. Try inverting the gyro value passed to the filter.  The filter should be fast - I use a 10ms update rate for my quadcopter and the filter is fast and smooth for pitch and roll.

With a mag, you will have to calculate the tilt compensated angle using either the raw pitch, roll data from the accel or pass in the filtered data to the mag filter with the gyro Z angle.  However, depending on your mag, the update rate will be quite slow (10hz maybe).  I haven't had much success Kalman filtering mag data.  You might try using FreeIMU which uses a MARG algorithm to combine 9DOF data.  It's faster than the kalman, but far more suseptable to vibration.  Also, external magnetic influences may cause it to drift.

Make sure you calibrate your mag correctly so that you apply offsets to the mag outputs - otherwise you will never get reliable heading angles.


Ok, so I've read nearly all of this thread and have a couple of questions.

In my understanding, you need 6dof (3accel, 3gyro) to be able to produce two absolute angles. The third angle is not possible to calculate at all? Or reliably due to gyro drift?

So if you add a magnetometer you can get 3 absolute angles?

If I have an ArduiMU (http://www.sparkfun.com/products/9956) (arduino + 3axis gyro + 3axis accel) can I add a manegometer that uses i2c and be able to spit out all the data to a computer? (without needing more analog ins).

Does it matter how many axis' the magnetometer has? (in order to calculate a third absolute angle)


Lastly, if the data is going to end up in a computer (running Max/MSP) is it better to do the math/calculations there rather than on the Arduino? Any benefit to doing the calculations locally on the Arduino?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 19, 2011, 03:38 pm

@Kashif  You have to comment (or delete) that part in order to use both axis without any problems, but it will only measure correctly from 90 to -90


Thanx Cristo829, I think Lauszus by mistake said "comment out", while he meant to "uncomment" ;-)
ya the angles seem ok now but only measuring +90 to -90

So, there is no way I can get a full 360 swing without compromising the other angle?

Lauszus, your code has helped me a lot reaching here, thats why I am sticking to this thread. Please be patient while I fire up some silly questions ;-) Once done, I will pass you my code which you can place along with the thread. It's all your code anyways just with the variation of I2C IMU, hope it will add more flavor to your article ;-)

OK guys, I am now starting my trial-n-error method once again to acheive the following
1. be able to calculate 360 degree (or at least 180) accurately on both axis without making the other one go crazy.
2. Remove (or at least minimize) the lag.


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 23, 2011, 02:40 pm
I really haven't got the time to play around with it :( But if you succeed in getting 360 degrees resolution, please post the code, and I will update it :)
I am thinking about buying a magnetometer, and update the guide, so one can calculate yaw as well. But that might be in the future, as I have so many things going on right now! :(

I think you could minimize the lag, by doing something like this:
Code: [Select]

int STD_LOOP_TIME = 10;           
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

setup()
{
//some code
}
loop()
{
//Calculate the angles

lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME)         
    delay(STD_LOOP_TIME-lastLoopUsefulTime);
loopStartTime = millis();
}


Then it will only read the IMU every 10 ms - that might do the trick :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: d0nut on Sep 23, 2011, 03:38 pm
Hello,

i´ve got a question about the accelero angle calculation. i´m trying myself to calculate the accelero angles with a 3-axis accelerometer.

in my opinion your calcultion is wrong, because you use all three components fromthe accelero. because you want to calculate the angle on the y-axis for example you dont need data of the x-axis (it may cause a certain error).

Here:

R = sqrt(accXval2+accYval2+accZval2)

accYangle = acos(accYval/R)

So the accYangle contains information about the xAxis (accXval2) but you only need the vectorlength of the YZ-plane to compute the angle. or am i wrong ? i´m trying do get 360° angle calculaton, but i don't get results that arent jumping in some situations, where accZval2 is about zero .. :/ tried tan/cos/scalarcos ... grrml ...

oh and thank you Lauszus for the tutorial :)

greets

d0nut
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 23, 2011, 03:58 pm
The reason why I use all three, is to compensate for tiny errors. In theory the force vector should be 1, but if you debug the data, you will see that it's not. If you don't need high precision you could just do the following:
Code: [Select]

accXangle = asin(accXval)*RAD_TO_DEG;
accYangle = asin(accYval)*RAD_TO_DEG;


Also to calculate the force vector you should write instead - as in the original code:
Code: [Select]
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector

The reason why it jumps around, is because if the roll (x-axis) is more than 90 degrees, it will also affect the pitch (y-axis) - I think you might need a magnetometer for full 360 degrees resolution :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mearsy on Sep 25, 2011, 09:50 am
Hi,

I finally got my accelerometer giving sensible angles but I am totally stuck on understanding the Gyro, I am using the following code with a IMU 5DOF Sparkfun SEN-09268.

int gyroPin = 4;               //Gyro is connected to analog pin 4
float gyroVoltage = 3.3;         //Gyro is running at 3.3V
float gyroZeroVoltage = 1.36;   //Gyro is zeroed at 1.36V
float gyroSensitivity = .002;  //Our example gyro is 2mV/deg/sec
float rotationThreshold = 5;   //Minimum deg/sec to keep track of - helps with gyro drifting

float currentAngle = 0;          //Keep track of our current angle

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

void loop() {
  float gyroRate = (analogRead(gyroPin) * gyroVoltage) / 1023;

  //This line finds the voltage offset from sitting still
  gyroRate -= gyroZeroVoltage;

  //This line divides the voltage we found by the gyro's sensitivity
  gyroRate /= gyroSensitivity;

  //Ignore the gyro if our angular velocity does not meet our threshold
  if (gyroRate >= rotationThreshold || gyroRate <= -rotationThreshold) {
    //This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)
   // gyroRate /= 100;
    currentAngle += gyroRate;
  }

  //Keep our angle between 0-359 degrees
  if (currentAngle < 0)
    currentAngle += 90;
  else if (currentAngle > 89)
    currentAngle -= 90;

  //DEBUG
  Serial.println(gyroRate);

  delay(10);
}

When the IMU is level the numbers are stable, when I tip it the numbers increase. When I tip the other way the numbers decrease. Trouble is these numbers dont seem to make any sense in terms of degrees. All I am after is a clean -90 to +90 degree sweep. Please can you help me to achieve this with my code

Kind Regards
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 25, 2011, 11:32 am
You have made a couple of mistakes. Remember that the output is in deg/s, so you have to know the time since your last reading (delta time). Have a look at my code, and change the gyro reading to the following:

Code: [Select]

#define gX A4
int gyroXadc;
float gyroZeroX = 421.6;//1.36/3.3*1023 - you could also calculate this - have a look in my code

gyroXadc = analogRead(gX);
gyroXrate = (gyroXadc-gyroZeroX)/.62;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.002/3.3*1023=0.62
gyroXangle += gyroXrate*dtime/1000;//Without any filter - dtime is in millis, so we divide it by 1000, to get in into secounds (use 1000000 if using micros())




Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mearsy on Sep 25, 2011, 04:50 pm
Thanks but I am still having problems. When I use your code the gyro constantly drifts toward the negative. I know they drift but this is the part I dont understand. All the complementry filter is doing it trusting the Gyro a lot and the accelerometer a little.

So as my gyro figures constantly drift. My final angle also will constantly drift. I must be doing something really obviously wrong.

Any takers?

Code below

Code: [Select]
#include <math.h>

// Accelerometer variables
int xAcc_ADC;
int zAcc_ADC;
int xOffset=506; 
int zOffset=610;
float xAngle;
float xAtan;
float vRef = 3.3;
float IMU_sensitivity = 0.329;
float finalangle;

// Gyro variables
float gyroZeroX = 421.6;                                                                  //1.36 volts /9which is Gyro zero / 3.3 which is AREF *1023 which is the ADC
int gyroXadc;                                                                             
float gyroXrate;
float gyroXangle;
unsigned long timer=0;
unsigned long dtime=0;

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

void loop()
{
  xAcc_ADC=analogRead(0);
  zAcc_ADC=analogRead(2);
  xAcc_ADC= xAcc_ADC - xOffset;
  zAcc_ADC= zAcc_ADC - zOffset;

  xAtan=atan2(xAcc_ADC*vRef/(1024*IMU_sensitivity),(zAcc_ADC*vRef/(1024*IMU_sensitivity)+1))*180/PI;    // Final X value from acelerometer

  gyroXadc = analogRead(4);
  gyroXrate = (gyroXadc-gyroZeroX)/.62;                                                                  //(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.002/3.3*1023=0.62
  gyroXangle += gyroXrate*dtime/1000;                                                                    //Without any filter - dtime is in millis, so we divide it by 1000, to get in into secounds (use 1000000 if using micros())

  finalangle = (float)(0.98 * gyroXangle) + (0.02 * xAtan); 

  Serial.print("Acc X: ");
  Serial.print(xAtan);
  Serial.print(" Gyro X: ");
  Serial.print(gyroXangle);
  Serial.print(" Filtered Angle ");
  Serial.print(finalangle);

  dtime = millis()-timer;
  timer = millis();
 
  Serial.print(" Millis:  ");
  Serial.println(dtime);
}
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 25, 2011, 04:54 pm
No that is normal. The gyro will always drift! The complementary filter or Kalman filter will take care of the rest :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mearsy on Sep 25, 2011, 05:31 pm
Unfortunately with my code, its not! the drift accumulates very quickly, within seconds.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 25, 2011, 05:33 pm
Try add the following in setup():
Code: [Select]

timer=millis();//start timing

And this in loop():
Code: [Select]

finalangle = ((0.98 * gyroXangle) + (0.02 * xAtan)); 



Or try adapting my code, and see if there still is a problem :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mearsy on Sep 25, 2011, 06:31 pm
I cant see any difference between your complementry filter and mine.. Still getting the same problem

Heres the serial output from when its started. You can see the gyro decreasing at a fast rate

Anyone got any idea please?

Acc X: 1.11 Gyro X: -0.03 Filtered Angle -0.01 Millis:  5
Acc X: 1.11 Gyro X: -0.07 Filtered Angle -0.04 Millis:  6
Acc X: 0.56 Gyro X: -0.11 Filtered Angle -0.10 Millis:  5
Acc X: 0.56 Gyro X: -0.15 Filtered Angle -0.13 Millis:  6
Acc X: 1.12 Gyro X: -0.19 Filtered Angle -0.17 Millis:  5
Acc X: 0.56 Gyro X: -0.23 Filtered Angle -0.21 Millis:  6
Acc X: 1.12 Gyro X: -0.27 Filtered Angle -0.25 Millis:  6
Acc X: 1.12 Gyro X: -0.32 Filtered Angle -0.29 Millis:  6
Acc X: 1.12 Gyro X: -0.36 Filtered Angle -0.33 Millis:  5
Acc X: 1.12 Gyro X: -0.40 Filtered Angle -0.37 Millis:  6
Acc X: 1.12 Gyro X: -0.45 Filtered Angle -0.41 Millis:  5
Acc X: 1.11 Gyro X: -0.48 Filtered Angle -0.45 Millis:  6
Acc X: 0.56 Gyro X: -0.53 Filtered Angle -0.51 Millis:  5
Acc X: 0.56 Gyro X: -0.56 Filtered Angle -0.54 Millis:  6
Acc X: 1.12 Gyro X: -0.61 Filtered Angle -0.57 Millis:  6
Acc X: 1.12 Gyro X: -0.65 Filtered Angle -0.62 Millis:  6
Acc X: 1.12 Gyro X: -0.70 Filtered Angle -0.66 Millis:  5
Acc X: 0.56 Gyro X: -0.73 Filtered Angle -0.71 Millis:  6
Acc X: 1.12 Gyro X: -0.78 Filtered Angle -0.74 Millis:  5
Acc X: 0.56 Gyro X: -0.82 Filtered Angle -0.79 Millis:  6
Acc X: 0.56 Gyro X: -0.86 Filtered Angle -0.83 Millis:  5
Acc X: 1.11 Gyro X: -0.90 Filtered Angle -0.86 Millis:  7
Acc X: 0.56 Gyro X: -0.95 Filtered Angle -0.92 Millis:  5
Acc X: 1.11 Gyro X: -0.99 Filtered Angle -0.94 Millis:  6
Acc X: 0.56 Gyro X: -1.03 Filtered Angle -1.00 Millis:  5
Acc X: 0.56 Gyro X: -1.07 Filtered Angle -1.04 Millis:  6
Acc X: 1.12 Gyro X: -1.11 Filtered Angle -1.07 Millis:  5
Acc X: 1.12 Gyro X: -1.15 Filtered Angle -1.10 Millis:  6
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 25, 2011, 06:45 pm
Maybe your have to multiply the gyro angle with -1. That might be the problem...
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mearsy on Sep 26, 2011, 09:00 am
Thanks for the suggestion Lauszus but I don't see how multiplying the gyroangle with minus 1 is going to solve the ever increasing negative drift. eg if the drift is -10, -11, -12 etc all thats going to happen by multiplying by -1 is it will become 10,11,12 etc.

I really need to understand why my code is not working as you suggested.

To recap. The board is a sparkfun 5dof IMU (SEN-09268) and the Gyro is an IDC 500. I am powering it of 3.3v and the AREF is also on 3.3v.

All I am trying to get from the gyro is a stable angle in the range -90 to +90 degrees. I need to understand the logic of where my code is going wrong. Can someone please help?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 26, 2011, 11:35 am
The only suggestions, I can give is to try using my code instead, as it is confirmed to work! :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: d0nut on Sep 29, 2011, 10:18 am
I found some interesting functions for the calculation of angles between 0 and 360°. They work almost perfect. perhaps you can help me to improve them ;)

alpha = atan( Ax /(sqrt( Ay^2 + Az^2 ) ) )
beta = atan( Ay  /(sqrt( AX^2 + Az^2 ) ) )

Ai = Accelero data

alpha = roll
beta = pitch

i´m doing stuff in scilab, so i cant give you c-code  ]:D

a short pdf about this stuff:

http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/APPLICATION_NOTE/CD00268887.pdf (http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/APPLICATION_NOTE/CD00268887.pdf)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 29, 2011, 05:35 pm
It's the same as the original one. It's only 180 degrees of resolution too :/
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: d0nut on Oct 04, 2011, 09:28 am
did you use atan2 insteadt of atan? in scilab atan = atan2 ... sry to confuse you
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 04, 2011, 10:15 am
Yes I used atan (or arctan) as the document says at page 13 :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Oct 17, 2011, 01:04 am
@ Lauszus Im trying to use your code with an I2c gyro and accelerometer but the sensitivity is given in lowest significant bit, would you know how to apply this?
Im using
gyro  ITG 3200 with sensitivity 14.375 LSB/(º/s)
accelerometer BMA180 the sensitivity varies with the G but at 1 g is 8192 LSB/g
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 17, 2011, 01:20 am
@cristo829
Hi
You should have a look at the dialog I had with "Kashif" starting at this link: http://arduino.cc/forum/index.php/topic,58048.msg534263.html#msg534263 (http://arduino.cc/forum/index.php/topic,58048.msg534263.html#msg534263). He is using the same gyro as you (ITG3200), but not the same accelerometer though (ADXL345). This is the IMU: http://www.sparkfun.com/products/10121 (http://www.sparkfun.com/products/10121).

If you follow the dialog, I think you will get it working. Just remember to change the values for the accelerometer!

Alternative send him a PM and ask for his code..

If you need more help, just send another reply and I will try to help you as best I can :D
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Oct 17, 2011, 04:31 am
Great thanks :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: AntR on Oct 17, 2011, 02:10 pm
I'm using this sensor combination (FreeIMU 0.3.5MS) also - you can just feed the raw values from the BMA accelerometer into the filter.  There's no need to scale the sensitivity, although it makes no difference if you do.  Also, if you're using up to 4G sensitivity, there's no need to calibrate (ie subtracting the offsets).  The BMA seems to have a fairly accurate calibration I've found.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Oct 19, 2011, 04:43 pm
So you changed this:
gyroX = analogRead(gX);
  gyroXrate = (gyroX-gyroZeroX)/1.0323;//(gyroX-gryoZeroX)/Sensitivity  Sensitivity = 0.00333/3.3*1023=1.0323   
  gyroXangle += gyroXrate*dtime/1000;

to this?

(read x value on i2c gyro)
gyroXrate = (gyroX-gyroZeroX)
gyroXangle += gyroXrate*dtime/1000;

Im building a qudcopter, how many Gs do you think is a good idea to use on the BMA? Also, have you programmed the Magnetometer on your IMU im having some trouble with the tilt compensation, my measurement varies within 10 degrees when i tilt it which is finw for me, except when i tilt it to one specific side when i tilt it that way it varies something like 40 degrees is this normal?
Thanks for your help
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: AntR on Oct 19, 2011, 04:57 pm
I'm using 3G for my quad, this seems to be a good compromise between pitch/roll accuracy and sensitivity to noise.  If you try 1.5G, you might find it too sensitive to vibrations.

I have used the mag,  I have to calibrate it by working out the offsets.  The best accuracy I've got has been a calibration with the sensor in place and all four motors running (without props on obviously).  I get pretty good stable results with the MARG algorithm as part as the FreeIMU library, but I haven't recently tried tilt compensation calculations with my current offsets so can't really comment on how well this works.

Your gyro code looks ok to me.


So you changed this:
gyroX = analogRead(gX);
  gyroXrate = (gyroX-gyroZeroX)/1.0323;//(gyroX-gryoZeroX)/Sensitivity  Sensitivity = 0.00333/3.3*1023=1.0323   
  gyroXangle += gyroXrate*dtime/1000;

to this?

(read x value on i2c gyro)
gyroXrate = (gyroX-gyroZeroX)
gyroXangle += gyroXrate*dtime/1000;

Im building a qudcopter, how many Gs do you think is a good idea to use on the BMA? Also, have you programmed the Magnetometer on your IMU im having some trouble with the tilt compensation, my measurement varies within 10 degrees when i tilt it which is finw for me, except when i tilt it to one specific side when i tilt it that way it varies something like 40 degrees is this normal?
Thanks for your help
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Oct 21, 2011, 05:01 am
Thanks AntR ill try that
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Almost_Scientific on Oct 27, 2011, 10:00 am
Hello everyone --

I'm really glad to find this thread so active.  I'm currently working on a project that involved tracking the orientation and position of an object.

I've got an ADXL345 accelerometer, ITG-3200 gyro and an HMC5883L magnetometer. 

I've gotten Fabio Varesano's awesome code working which is giving me great position information.

But I'm hoping someone on this thread can point me to some code that I can use to get a good measurement of velocity so I can calculate position.

Thanks in advance.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 27, 2011, 10:10 pm
I would be possible using the accelerometers and then multiply it with one g and the delta time. But you have to do some math to compensate for rotation. Please post the code if you get it working :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 06, 2011, 10:40 am
Hey,

I'm using this LISY300AL Gyro: http://www.sparkfun.com/datasheets/Sensors/LISY300AL.pdf (http://www.sparkfun.com/datasheets/Sensors/LISY300AL.pdf)
But i cant get a precise data..

Here is my calculation:
Code: [Select]

   sensitivity = 0.716454545;
   gyroAdc = analogRead(0);
   if(abs(gyroAdc - gyroZero) <= Threshold)
     gyroAdc = gyroZero;
   gyroRate = (gyroAdc-gyroZero)/sensitivity;
   gyroRotation=gyroRotation+gyroRate*dtime1/1000;
   gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360
   dtime = millis() - stime;
   stime = millis();


I dont get a precise angle, i move my object 90 degrees but the gyro drifts too much so it showes something else...
Maybe i'm doing something wrong ? or i should just add an Accelerometer to make it more precise ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 06, 2011, 04:16 pm
First of all, how did you calculate your sensitivity?
If you use either 3.3 or 5 volt as reference, it's either:
0.0033/3.3*1023=1.023
or
0.0033/5*1023=0.67518
As the sensor has a sensitivity of 3.3 mV/ °/s
Or do you use a different reference? As i can calculate you use 4.711952801 volt as reference (0.0033/4.711952801*1023=0.716454545).

Why do you need this line:
Code: [Select]
gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360

What you call "gyroRotation", is actually the angle :)

For more precision use integration.
Something like this would work:

Code: [Select]

#define OFFSET 0.0005

gyroRaw = analogRead(0);
gOffset = OFFSET * gyroRaw + (1-OFFSET) * gOffset;
gyroSpeed = (gyroRaw - gOffset)/sensitivity;

gyroAngle += gyroSpeed*dtime/1000;


See this page for more information: http://www.hitechnic.com/blog/gyro-sensor/htway/

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 06, 2011, 05:30 pm

First of all, how did you calculate your sensitivity?
If you use either 3.3 or 5 volt as reference, it's either:
0.0033/3.3*1023=1.023
or
0.0033/5*1023=0.67518
As the sensor has a sensitivity of 3.3 mV/ °/s
Or do you use a different reference? As i can calculate you use 4.711952801 volt as reference (0.0033/4.711952801*1023=0.716454545).

Why do you need this line:
Code: [Select]
gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360

What you call "gyroRotation", is actually the angle :)

For more precision use integration.
Something like this would work:

Code: [Select]

#define OFFSET 0.0005

gyroRaw = analogRead(0);
gOffset = OFFSET * gyroRaw + (1-OFFSET) * gOffset;
gyroSpeed = (gyroRaw - gOffset)/sensitivity;

gyroAngle += gyroSpeed*dtime/1000;


See this page for more information: http://www.hitechnic.com/blog/gyro-sensor/htway/

Regards
Lauszus



Hey, thanks for the replay.

I'm using 3.3v so i changed the sensitivity to 1.023, now the numbers are even less precise.
the gyroZero after calibration is around 325, and the gyro goes about -+300.

I removed the
Code: [Select]
gyroAngle = abs((gyroRotation - 360 * ceil(gyroRotation / 360))); // Round up between 0-360

When i use this:
Code: [Select]

#define OFFSET 0.0005

gyroRaw = analogRead(0);
gOffset = OFFSET * gyroRaw + (1-OFFSET) * gOffset;
gyroSpeed = (gyroRaw - gOffset)/sensitivity;

gyroAngle += gyroSpeed*dtime/1000;


The numbers goes crazy and keep going up even when the gyro doesn't move.

Bottom line:
Sensitivity = 1.023 - When i move my object 90 Degrees, the gyro showes 50-55 Degrees.
Sensitivity = 0.716454545 - When i move my object 90 Degrees, the gyro showes 83-88 Degrees.

Those drifts just getting worse every move so after few movements the gyro isn't helpfull.

Any ideas ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 06, 2011, 07:00 pm
Have you remembered to connect 3.3 Volt to the VREF pin on the Arduino? :)

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 07, 2011, 08:43 am

Have you remembered to connect 3.3 Volt to the VREF pin on the Arduino? :)

Regards
Lauszus


I used this guide: http://wiring.org.co/learning/basics/gyrolisy300al.html (http://wiring.org.co/learning/basics/gyrolisy300al.html)
I connected the VCC on the gyro to the Arduino 3.3v pin.

Is that what you ment ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 07, 2011, 10:29 am
No you have to connect 3.3V to the VREF/AREF pin on the Arduino.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 08, 2011, 11:42 am
Hey, thanks alot for you replays i learn alot of new stuff and you are being very helpfull :)

So in my Gyro i have VCC & GND pin, i connected the VCC to the Arduino 3.3v pin and the GND to one of the GND pins on the Arduino.
And i connected the OUT pin of the gyro to the Analog0 pin on the Arduino.

I use this external Power Supply


As you can see i'm giving 12v to the Arduino, and the Fixed volts are set to 3.3v and connected stright to the AREF pin and one of the GND pins on the arduino.

Now i changed my sensitivity back to 1.023, and added
Code: [Select]
analogReference(EXTERNAL); to the
Code: [Select]
setup() in my code.



You can see the purple black and red goes to the gyro, the big connector that connected to the 7,8,9 digitals is a Servo controller, and the  2 pins from the external supply to the AREF & GND.

Here is the code, every one milliseconds i call this:
Code: [Select]
void gyroTest()
{
   if(gyroInit) {
     finalGyroAngle = calculateGyroAngle(0);
     Serial.println(finalGyroAngle);
   }
}


Code: [Select]
float calculateGyroAngle(int pin)
{
   gyroAdc = calibrateGyro(0);
   if(abs(gyroAdc - gyroZero) <= Threshold)
     gyroAdc = gyroZero;
   gyroRate = (gyroAdc-gyroZero)/sensitivity;
   gyroRotation+=gyroRate*dtime/1000;
   unsigned long t = millis();
   dtime = t - stime;
   stime = t;
   return gyroRotation;
}

void InitGyro(int pin)
{
 Serial.println("Calibrating Gyro...");
 sensitivity = 1.023; //0.692454545; //0.716454545;
 gyroAdc = 0;
 gyroRate = 0;
 gyroRotation = 0;
 gyroAngle = 0;
 gyroZero = calibrateGyro(pin);
 Serial.print("Gyro Calibrated: ");
 Serial.println(gyroZero);
}

int calibrateGyro(int pin)
{
 long resultGyro = 0;
 for(int i=0;i<100;i++)
 {
   resultGyro += analogRead(pin);
   delay(0.5);
 }
 resultGyro = resultGyro/100;
 return resultGyro;
}


Am i doing everything ok ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 08, 2011, 11:50 am
Glad to see you figured it out :)
What are you going to use the setup for? Controlling the servo or something like that?

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 08, 2011, 05:22 pm
Hey, i still didn't solve it :P

I'll explain why the setup for and what is my problem.
As you see i have a WiFi shield connected, i send him commands to spin the servo that is connected to a arm that the gyro is on so the gyro spins with it.

I want to tell the servo to spin 90 degrees, and then go back to 0 degrees, and do that in a loop.
That's why i need the Gyro for, to get the degrees.

My problem is that after 2-3 time it goes to 90 then 0 the drifting goes crazy and it goes to 110 then -20...
So my problam still isn't solved.

I posted all my setup so you can see if i'm doing everything right, i hope you can help me figure it out.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 08, 2011, 09:26 pm
Sorry, I didn't see the last line in your post :)

Hmm, it seams like you do everything correct.

But hey, why do you need a gyro to tell the degrees? You don't need that for a servo. Just send it the correct pulse, and it will go to the requested position!!

See this page for more information: http://www.arduino.cc/en/Reference/Servo
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 09, 2011, 02:44 pm
Hey :)

I have a continues Servo... and after doing all of this, my gyro is still like 95% accurate ...
i need 99% atleast..

I just think i'm gonna get an Absolut Angle sensor and connect it to the servo..

Thanks for the help anyway, you were very nice and helpfull :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 09, 2011, 05:08 pm
Why not just use a ordinary Servo then?
Thank you for the kinds words :)

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: stefanelsener on Nov 14, 2011, 06:10 am
Hello,

I am using an Arduino UNO and a Sparkfun 6 DOF Razor IMU.  I am hoping to adapt it to control roll and pitch for a quadrotor stabilization system.  I used your post as a guide to setup the wiring and used the version 3 code to get it going.  My problem is I get about 2-3 lines of actual data and then it just scrolls jibberish.  I double checked the wiring and I have not changed anything in the code.  Could it be that I am using the http://www.sparkfun.com/products/10010 IMU with the high pass filters removed.

I would like to apologize in advance, I have a lot of RC experience and a mechanical engineering background but programming and debug has never been my forte.

All I want to do is read in the roll and pitch degrees and then convert that to a servo command to send to a brushless motor as an increase in thrust to compensate.

Thanks for any help you can provide.

Stefan
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 14, 2011, 11:06 am
It is exactly the same IMU as mine. So it should be almost plug n' play. Triple check your wires, or try resetting the high pass filter, by pulling the HP pin high (see the datasheet page 7 http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf). Not sure if it would work, as the description says that it it removed, as you said.

You do not have to apologize :) We have all been noobs once :D

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Nov 15, 2011, 03:13 am
@StefanElsener
I also have the board with sensors removed and the program works great, if you say that you can read a few lines and then it just scrolls jibberish it might be the baud rate. 98% of the time that i see just jibberish on the Serial monitor is because the baud rate in it is not the same as the one on the program check that the baud rate on the script is the same as in the serial monitor (bottom right). If that doesnt work then do as Lauszus says check your wires, maybe download the program again and make sure not to erase anything important on it. If that doesnt work then im sorry to say that your board might be damaged (its painful i know :( ...) good luck
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: stefanelsener on Nov 15, 2011, 04:40 am
Thank You Cristo!  I had the baud rate set at 9600 baud (default setting I assume as this is the first time I have used the program) and when I changed it to 115,200 baud it worked!!

Thank you Lauszus for the quick reply.  Now for my next task, creating the control function to provide stabilization for a quadcopter.... does anybody have a good starting point now that my IMU is working?

Thanks again!

Stefan
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Nov 16, 2011, 04:39 pm
In fact im doing the exact same thing, although i gotta warn you this sensor might not be the best choise it is too sensitive to vibrations and its not the fastest one there is but if you wana try it join the club lol, also i think is better to use DCM rather than Kalman unless you use expanded Kalman or something like that so you can go the 360. I hope you are taking someone elses program Aeroquad or Multiwiii as your program creating your own takes a long long time and it will never work as good as their programs. I had to do it since it is a project for school but if you like programing and that sort of stuff try using one of those programs which are already working and just add some crazy stuff to make it automated like a GPS where you can set the route or stuff like that.

If you still feel like doing this then i can send you as many links as you want the first one is a DCM code for the same board we are using although i dont know why but i havent been able to make it work maybe you have more luck with it: http://voidbot.net/razor-6dof.html if you have any luck let me know.

Also if you want to learn the basics on all the stuff you need to create the program take a look at my posts:

http://arduino.cc/forum/index.php/topic,74234.0.html

http://diydrones.com/forum/topics/quadcopter-sos

The DIY is better since there is a guy that answered all my questions.

Good luck and i hope you are making this with someone else otherwise is a lot of work  If you are doing this as a hobby dont try it just use the programs there are you will enjoy it much more and save a lot of money building structures...
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: stefanelsener on Nov 16, 2011, 05:18 pm
No, I would love to use existing program. I am not a good enough programmer to make one myself.  I looked at the Aeroquad but it doesn't work for my board (Ardunio Uno).  I know this won't be the best setup but right now I am just looking to make a basic IMU with Roll and Pitch stabilization.  I can add better functionality later.  Just want to play with this in my backyard maybe with an on board camera or something.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Nov 17, 2011, 12:15 am
I remember those days when i thought it was that simple, i have 3 weeks to make this quadcopter fly and right know it only hovers i have a good idea of what might help it though. I hope ill get it this weekend.

The Aeroquad can use the UNO it just uses less sensors (accelerometer and  gyroscope) just buy whatever they say and you will have it working
http://aeroquad.com/showwiki.php?title=Parts+List
In there you see that there are 3 different configurations one with the mega one with the uno and the other with the mini just follow the instructions and be careful.

If you dont want to use the aeroquad code just use the multiwii thats what almost everyone in RCgroups use also they have a nice one where you can use the wii control sensors could make this really cheap.

But if you want to be certain that it will work i would recomend aeroquad

PS i havent tried any of those programs personally, it would brake my heart to see my quad fly with another program.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 18, 2011, 09:48 am

Why not just use a ordinary Servo then?
Thank you for the kinds words :)

Regards
Lauszus


Hey Lauszuz,

Sorry for the long time till i replay i was away.

An ordinary Servo wont give me the Power i need, and i need 360 degrees and not 180, and i have size capabilities.

I think i will get the IMU that you use, will it give me 100% accuuracy ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 18, 2011, 06:01 pm
Okay :) It is actually possible to get a servo that can rotate 360 degrees. I found this after some googling: http://www.pololu.com/catalog/product/522 , that might do the job.

The IMU I used can only get 360 degrees resolution, if you only use either pitch or roll, but it can not be used at the same time...
But if you only need one of them, the IMU will work just fine :D

Regards Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: danpe on Nov 19, 2011, 05:39 pm

Okay :) It is actually possible to get a servo that can rotate 360 degrees. I found this after some googling: http://www.pololu.com/catalog/product/522 , that might do the job.

The IMU I used can only get 360 degrees resolution, if you only use either pitch or roll, but it can not be used at the same time...
But if you only need one of them, the IMU will work just fine :D

Regards Lauszus


Hey, Thanks !

I actually need a really strong Servo, i need something like 70kg/cm :)
I found this http://www.hobbyking.com/hobbyking/store/uh_viewItem.asp?idProduct=18742 (http://www.hobbyking.com/hobbyking/store/uh_viewItem.asp?idProduct=18742) and i will add him another gear to make it 100gk/cm :)

If i get the IMU that you use in your example and use the Kalman Filtering, will i get 100% accuracy on 360 rotation ?

Thanks,
Dan
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cristo829 on Nov 19, 2011, 09:58 pm
If you have a servo you dont need the sensor to know the angle, you just tell the servo which angle you want with the servo library. If you need an even bigger motor then you could buy a motor decoder some of them give you the exact position of the motor i saw one once and it wasperfect i cant seem to find a link sorry.

@ Lauszus sorry i always interfere in your post it was of so much help and want to help as much as i can xD

I have a question im using different sensors the i2c ones Im able to get all the angles now X Y Z but when i go past 90 it starts going down like 87 88 89 89 87 or -87 -89 -89 -87 how could i get the full 180... Pythagoras was never my friend maybe you have some good ideas.

PS the Z axis is working fine it goes 187 188 189 -189 -188 -187 if i could get the same result on the other 2 angles it would be perfect

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: arduinoobo on Nov 24, 2011, 07:10 am
Hi

what do i have to change to the program to use it with a 5 d-o-f imu?

I'm using a idg500/adxl335 IMU and an arduino uno
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 24, 2011, 07:59 am
You only need to change the sensitivity for the gyro and accelerometer, since the gyro for the yaw axis is not used, you can still get the same result as shown in the demonstration video with a 5DOF :)

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: arduinoobo on Nov 24, 2011, 08:46 am
Thank you for your reply, Lauszus.

we changed the values of the sensitivity as you recommended, but it doesn't seem to be working.
I'm not sure what does
    float Q_angleY  =  0.3;
    float Q_gyroY   =  0.002; 
    float R_angleY  =  0.03;
refer to..

do they mean
gyro
accelerometer
force vector?

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

#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;

//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;

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

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;

int inputGyroY[100];//y-axis
long resultGyroY;

//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

int inputAccY[100];//y-axis
long resultAccY;

int inputAccZ[100];//z-axis
long resultAccZ;

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

int calibrateGyroY()
{
  for(int i=0;i<100;i++)
  {
    inputGyroY[i] = analogRead(gY);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroY += inputGyroY[i];
  }
  resultGyroY = resultGyroY/100;
  return resultGyroY;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}

int calibrateAccY()
{
  for(int i=0;i<100;i++)
  {
    inputAccY[i] = analogRead(aY);
  }
  for(int i=0;i<100;i++)
  {
    resultAccY += inputAccY[i];
  }
  resultAccY = resultAccY/100;
  return resultAccY;
}

int calibrateAccZ()
{
  for(int i=0;i<100;i++)
  {
    inputAccZ[i] = analogRead(aZ);
  }
  for(int i=0;i<100;i++)
  {
    resultAccZ += inputAccZ[i];
  }
  resultAccZ = resultAccZ/100;
  return resultAccZ;
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.3;
    float Q_gyroX   =  0.002; 
    float R_angleX  =  0.03; 

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
   
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
   
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
   
    return x_angle;
  }

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.3;
    float Q_gyroY   =  0.002; 
    float R_angleY  =  0.03; 

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
   
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
   
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
   
    return y_angle;
  }

//print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
  Serial.print(gyroYangle);Serial.print("\t");
 
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t"); 
 
  Serial.print(compAngleX);Serial.print("\t"); 
  Serial.print(compAngleY); Serial.print("\t");
 
  Serial.print(xAngle);Serial.print("\t");
  Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
}



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
 
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY(); 
 
  accZeroX = calibrateAccX();
  accZeroY = calibrateAccY();
  accZeroZ = calibrateAccZ();
 
  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
 
  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(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,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");
*/
 
  dtime = millis()-timer;
  timer = millis();
 
  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);

  /*Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print(compAngleY,0);Serial.print("\t");
  Serial.print(xAngle,0);Serial.print("\t");
  Serial.print(yAngle,0);Serial.print("\t");
  Serial.println("");*/
 
  processing(); 
}


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 24, 2011, 11:28 am
That's the process and measurement noise constant used in the Kalman Filter. Do not change it!! At least until you got everything working. I recommend changing them back to there originals values:
Code: [Select]

float Q_angleX  =  0.001;
float Q_gyroX   =  0.003; 
float R_angleX  =  0.03; 

And then wait until you got it working.
See these links for more information: http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf.

You have to change the set the sensitivity in the code. Right now you are not setting the constant. For example in this line:
Code: [Select]

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


Send me a link to the datasheets and I will figure the sensitivity out for you :)

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 24, 2011, 01:25 pm
Hi,
I will respond to your personal messages here, so everyone else can learn by your mistakes :)
First you wrote:
Quote

Thank you for your reply.

How should i program it so? How do i integrate both the gyro and accelerometer into one program? i'm relatively new to arduino and have just started this project, i have totally no idea how to do it. I tried the gyro program in the arduino playground but the values do not seem to stop dropping..

your reply is very appreciated!  sorry for my bad english

Just grab my program and change the sensitivity (see the next respond). The reason why you experience a drop, it's what called a drift. All gyros do that!! But the Kalman filter will take care of this :)

The second message:
Quote

Hi,

I just saw your reply to my question in the post, here you go
idg500 http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
adxl335 http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf

your help is really much appreciated :)


If your are using 3.3 volt, then your sensitivity would be:

gyroSensitivity: 0.0091/3.3*1023=2.821 (4.5 times output) else 0.002/3.3*1023=0.62 (normal output)
accelerometerSensitivity: 0.300/3.3*1023=93

Hope it make more sense to you now :)

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: arduinoobo on Nov 25, 2011, 07:08 am
Thank you Lauszus for your prompt reply,

Code: [Select]
#define gX A0

#define aX A3

#define RAD_TO_DEG 57.295779513082320876798154814105

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

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

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

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

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

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;

//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

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

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.001;
    float Q_gyroX   =  0.003; 
    float R_angleX  =  0.03; 

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
   
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
   
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
   
    return x_angle;
  }

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.001;
    float Q_gyroY   =  0.003; 
    float R_angleY  =  0.03; 

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
   
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
   
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
   
    return y_angle;
  }

//print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
   
  Serial.print(accXangle);Serial.print("\t");
 
  //Serial.print(compAngleX);Serial.print("\t"); 
  //Serial.print(compAngleY); Serial.print("\t");
 
  Serial.print(xAngle);Serial.print("\t");
  //Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
}



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
 
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
 
  accZeroX = calibrateAccX();
 
  timer=millis();//start timing
}

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/0.62;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter

 
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/93;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
 
 
  //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(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,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");
*/
 
  dtime = millis()-timer;
  timer = millis();
 
  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);

  /*Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print(compAngleY,0);Serial.print("\t");
  Serial.print(xAngle,0);Serial.print("\t");
  Serial.print(yAngle,0);Serial.print("\t");
  Serial.println("");*/
 
  processing(); 
}


we require vertical tilt detection, does it matter which accelerometer we use? we are currently using the x gyro and x accelerometer...
and after we turn the gyro 360 degrees it only shows 180 degrees rotation, i think we are doing something wrong, could you please advice :D

your reply is very much appreciated!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 25, 2011, 12:10 pm
First of all. You can't calculate the force vector with only 1 accelerometer axis. You need 3! You are dividing with a unset variable.
Instead your should write:
Code: [Select]

accXangle = asin(accXval)*RAD_TO_DEG;

But if you use all 3-axis, then you will get better precision, so I recommended that!!
So connect all three axis and calculate all the values as so:
Code: [Select]

accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/93;

accYadc = analogRead(aY);
accYval = (accYadc-accZeroY)/93;

accZadc = analogRead(aZ);
accZval = (accZadc-accZeroZ)/93;
 
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
accXangle = acos(accXval/R)*RAD_TO_DEG-90;


You can only use the x and y axis.

You can get 360 resolution by using the z-axis. Take a look in the version 3 of the code. This is what I did:
Code: [Select]

  if(accZval < 0)//360 degrees resolution
  {
    if(accXangle < 0)
    {
      accXangle = -180-accXangle;
    }
    else
    {
      accXangle = 180-accXangle;
    }       
  }


- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: arduinoobo on Nov 28, 2011, 06:44 am
Lauszus thank you for your help, i've got stable readings for the gyro, it still drifts but not as much as before. thanks again!  :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Dec 27, 2011, 09:32 am
Hy guys, i would use this IMU http://www.robot-italy.com/product_info.php?cPath=15_138&products_id=1688. Can  I use your code? you think the same? how to connect Arduino? I would also  connect to arduino 2 servant http://www.robot-italy.com/product_info.php?cPath=85_28&products_id=1826 , according to you is enough for me an Arduino? I do not know if enough analog inputs
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Dec 29, 2011, 04:06 am
#batista1987
Hi,
Actually I already helped another person with the exact requirements as you. Please see the following reply and onward:
http://arduino.cc/forum/index.php/topic,58048.msg543286.html#msg543286
Feel free to write again if you need more help or something you don't understand :)

Regarding the servo, it's very easy to use it with a Arduino. Look at the playground: http://www.arduino.cc/playground/ComponentLib/Servo or just use google as there is a ton of information on how to hook up a servo to a Arduino.

As with the analog inputs, you only need two (4 and 5) for the I2C communication - if you don't use a Arduino Mega. The servo(s) can use any pwm compatible pin. See the hardware page for the Arduino Uno: http://www.arduino.cc/en/Main/arduinoBoardUno at "Input and Output - PWM".

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Jan 02, 2012, 04:23 pm
#Lauszus
Hi Lauszus, the output of your code is not correctly. the program output are all 0
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 02, 2012, 04:43 pm
#batista1987
Try to send a PM to Kashif and ask him for his code :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Jan 02, 2012, 04:45 pm
ok:-) tks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 04, 2012, 09:03 pm
Hey lauszus, first off thanks for writing this great guide- I'm also using kas's approach and both have helped me get much farther than I would have thought- however I have hit a roadblock and through you might be able to help. btw- I'm using a 2-axis XY gyro (http://www.pololu.com/catalog/product/1266/specs (http://www.pololu.com/catalog/product/1266/specs)) and 3-axis accelerometer (http://shop.moderndevice.com/products/mma7260qt-3-axis-accelerometer (http://shop.moderndevice.com/products/mma7260qt-3-axis-accelerometer)) in tandem. If it's needed I can also connect a standalone single-axis gyro for z-axis measurement, but I don't think it will be needed for angle measurement now.

Like yours, my bot also has the requirement of incline-sensing in both X and Y axes- therefore, I used the force-vector method as you did. My raw acc-angles look great, showing incline measurements independent of one another (essential for my bot to properly balance, as it's a biped). Here's a shot of my acc-calculate angles :

[img=http://img43.imageshack.us/img43/9821/scrn1c.th.jpg] (http://imageshack.us/photo/my-images/43/scrn1c.jpg/)

red line = y-axis inclination, black line = x-axis inclination. As you can see, the angle measurements are clearly independent of one another when the imu is tilted in a cardinal direction. (imu board tilted 90deg right with no y-axis inclination)

However, after applying kalman filtering, I get this:

[img=http://img85.imageshack.us/img85/8039/scrn2n.th.jpg] (http://imageshack.us/photo/my-images/85/scrn2n.jpg/)

Here, the board is in the same orientation as in the previous screenshot, but it is clear that the two measurements are not independent of each other after applying the kalman filter. What's more, the angle estimation jumps and then settles at the true angle. Finally, a screenshot with both acc and kalman data shown:

[img=http://img689.imageshack.us/img689/6681/scrn3m.th.jpg] (http://imageshack.us/photo/my-images/689/scrn3m.jpg/)

The highlighted box shows how the x-axis measurement jumps and settles in response to the y-axis tilt, while the accelerometer reading is relatively unchanged.

Here is the relevant code:

source of gyro data
Code: [Select]

int getGyroRateX() {                                             // ARef=3.3V, Gyro sensitivity=2.5mV/(deg/sec)
 return int(sensorValue[GYR_X] / 0.775);                 // sensitivity = 0.0025 / 3.3 * 1023
}

int getGyroRateY() {                                             // ARef=3.3V, Gyro sensitivity=2.5mV/(deg/sec)
 return int(sensorValue[GYR_Y] / 0.775);                 // i'm guessing this value is in degrees
}


force vector calcs
Code: [Select]

 R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));
 xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;  //unless I'm not mistaken, this angle is in rads
 xACC_angleDeg = xACC_angle * RAD_DEG;
 xACC_angleQuid = xACC_angle * RAD_DEG * DEG_QUID;
 yACC_angle = (acos(sensorValue[ACC_Y]/R)-pi/2)*-1;
 yACC_angleDeg = yACC_angle * RAD_DEG;
 yACC_angleQuid = yACC_angle * RAD_DEG * DEG_QUID;


kalman calculations
Code: [Select]

 xGYRO_rate = getGyroRateX();
 xGYRO_angle = xGYRO_angle + xGYRO_rate*loopTime/1000;
 yGYRO_rate = getGyroRateY();
 yGYRO_angle = yGYRO_angle + yGYRO_rate*loopTime/1000;

 xfinAngleDeg = kalmanCalculateX(xACC_angleDeg, xGYRO_rate, loopTime);
 yfinAngleDeg = kalmanCalculateY(yACC_angleDeg, yGYRO_rate, loopTime);


These angle measurements will be feeding a PID that drives an IK calculator to get joint angles for the bot, so it is a must that these angles are nearly dead-on  ;)
Could you help me figure this out? Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: elwylly on Jan 04, 2012, 10:01 pm
Hi Lauszus,

Quick question. What's the other gyro in this IMU for? You use the Gx4 but, what's the Gx1? I just don't get why you have 2 gyros but only use this one.

Thanks!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 04, 2012, 11:32 pm
#asrinivasan
You are doing the conversion from radians to degrees wrong:
Code: [Select]

xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;  //unless I'm not mistaken, this angle is in rads
xACC_angleDeg = xACC_angle * RAD_DEG;


Instead it should be:

Code: [Select]

xACC_angleRad = (acos(sensorValue[ACC_X]/R))*-1;
xACC_angleDeg = xACC_angleRad * RAD_DEG;


I guess you are multiplying by -1, to transform the angle, so it has the same sign as the gyro :)

#elwylly
The Gx4 output the Gx1 output multiplied by 4. It just allows the user to get either higher or lower sensitivity depending on what you need it for.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 05, 2012, 03:23 am
@lauszus

Thanks for the quick reply! However, I don't think that changed my problem  :P The pi/2 I subtracted only represents a translation of the accelerometer's angle value, which was already an accurate reading- it was the kalman filtering that produced the erroneous results.

The reason I convert it like this
Code: [Select]

 R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));
 xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;


is because in your original code, you have it like this.
Code: [Select]

 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;


instead of subtracting 90 from the converted angle, I simply subtracted pi/2 from the angle in rad. My problem is not with the accelerometer data- the angle estimate from my accelerometers seem to be accurate; x-axis inclination does not affect y-axis data, and so on- my problem arises when I perform the kalman filtering.
As I showed in my previous post, the last picture [img=http://img689.imageshack.us/img689/6681/scrn3m.th.jpg] (http://imageshack.us/photo/my-images/689/scrn3m.jpg/)  shows how the distinct difference between the kalman filtering and the accelerometer data. This was not a problem when I was measuring a single axis using atan2 method, so I am wondering why, if the accelerometer data is accurate, my results are any different. (Also there is a definite lag in the kalman-filtering but I will approach that once this problem is fixed  :) ).

If need be I can give you my source code
Thanks


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Jan 05, 2012, 09:16 am
#Lauszus

i find this code  http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/lijojl/docs/main_8cpp_source.html. How convert cpp files in pde? tks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: iamnarendra on Jan 05, 2012, 08:56 pm
hi....i am using imu 5dof... for buildin my self balancin robot..... i used ur program..... bt,in imu reading dre is a drift within seconds.....
and also the its measurin upto -90 to +90..... cn u help me pls.....  xpct a quick reply.....
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 05, 2012, 09:08 pm
#asrinivasan
Sorry my bad :)
Do your gyro data seem correct? Do it start out correct and then start to drift?
Also try adjusting the constants in the Kalman filter (the process and measurements noise constants), as you use a different accelerometer and gyro than me.
Do you get better results using the complimentary filer - if so, then it's something with the Kalman filter.
Try posting your code and I will have a look at it :)

#batista1987
That might take a bit of work, as he uses a "library" for the mbed. Have a look at the following files and try to port them:
"IMUfilter.h"
"ADXL345.h"
"ITG3200.h"

#iamnarendra
Have you tried both the Kalman filter and the complimentary filter approach? Try both of them and see if it helps.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 12, 2012, 01:21 pm
@lauszus

Sorry for the late reply, we've had a lot of work in school these days :)
As far as I know, the gyro and acc have the same scale, oriented  in the same direction, reflect accurate measurements, etc. but nonetheless kalman filtering still produces false (not true to original signal) results. Adjusting the constants of the kalman filtering seem to make the random jumps only more prominent - so I'll just post the code here, and hopefully someone can see my mistake :)

I'll just post the relevant code (leaving out PID, etc.)
Here is the main file (with setup/loop):
Code: [Select]

#include <math.h>

#define GYR_X          0
#define GYR_Y          1

#define ACC_X          2
#define ACC_Y          3
#define ACC_Z          4

const float RAD_DEG = 57.2957795;
const float pi = 3.1415926;
const float QUID_DEG = 0.3515625;
const float DEG_QUID = 2.8444444;

int sensorValue[5] = {0,0,0,0,0};
int sensorZero[5] = {0,0,0,0,0};

float xACC_angle = 0;
float xACC_angleDeg = 0;
float xACC_angleQuid = 0;
int xGYRO_rate = 0;
float xGYRO_angle = 0;

float yACC_angle = 0;
float yACC_angleDeg = 0;
float yACC_angleQuid = 0;
int yGYRO_rate = 0;
float yGYRO_angle = 0;

float xfinAngleQuid = 0;
float xfinAngleDeg = 0;

float yfinAngleQuid = 0;
float yfinAngleDeg = 0;

int PIDcorrect = 0;
int error = 0;

float R = 0;

//alternate timing mechanism...
int STD_LOOP_TIME = 10;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

unsigned long timer=0;
unsigned long dtime=0;

void setup() {
  analogReference(EXTERNAL);
  Serial.begin(115200);
  delay(100);
  calibrateSensors();
}

void loop() {
  updateSensors();

//atan2 angle calculation method (single-axis inclination only)
  /*xACC_angle = getAccAngle();
  xACC_angleDeg = int (xACC_angle/2.84444);
  xGYRO_rate = getGyroRate()*-1;*/

//vector angle calculation method
  R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));
  xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;
  xACC_angleDeg = xACC_angle * RAD_DEG;
  xACC_angleQuid = xACC_angle * RAD_DEG * DEG_QUID;
  yACC_angle = (acos(sensorValue[ACC_Y]/R)-pi/2)*-1;
  yACC_angleDeg = yACC_angle * RAD_DEG;
  yACC_angleQuid = yACC_angle * RAD_DEG * DEG_QUID;
 
  xGYRO_rate = getGyroRateX();
  xGYRO_angle = xGYRO_angle + xGYRO_rate*lastLoopTime/1000;
  yGYRO_rate = getGyroRateY();
  yGYRO_angle = yGYRO_angle + yGYRO_rate*lastLoopTime/1000;
 
  dtime = millis()-timer;
  timer = millis();
 
//not working-- fix
  xfinAngleDeg = kalmanCalculateX(xACC_angleDeg, xGYRO_rate, dTime);
  yfinAngleDeg = kalmanCalculateY(yACC_angleDeg, yGYRO_rate, dTime);
 
  //PIDcorrect = updatePid(0, xfinAngleDeg, dtime);
 
  //serialOut_sensor();
  serialOut_timing();
  //processing();
 
  /*lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();*/
}


and the sensors/calibrate/read code:

Code: [Select]

void calibrateSensors() {
   long v;
   for(int n=0; n<5; n++) {
     v=0;
     for(int i=0; i<50; i++)    v += readSensor(n);
       sensorZero[n] = v/50;
   }
  sensorZero[ACC_Z] -= 166;
}
 
  void updateSensors() {
  long v;
  for(int n=0; n<5; n++) {
    v=0;
    v = readSensor(n)+2*readSensor(n)+3*readSensor(n)+4*readSensor(n)+3*readSensor(n)+2*readSensor(n)+readSensor(n); //tri. avg
    sensorValue[n] = v/16 - sensorZero[n];
  }
}
 
int readSensor(int channel) {
  return (analogRead(channel));
}

//******************************* angle calculations **************************************
int getAccAngle() {
   return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256;    // in Quid: 1024/(2*PI))
}

int getGyroRateX() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int(sensorValue[GYR_X] / 0.775);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

int getGyroRateY() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int(sensorValue[GYR_Y] / 0.775);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

int arctan2(int y, int x) {                                    // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
   int coeff_1 = 128;                                          // angle in Quids (1024 Quids=360°)
   int coeff_2 = 3*coeff_1;
   float abs_y = abs(y)+1e-10;
   float r, angle;

   if (x >= 0) {
     r = (x - abs_y) / (x + abs_y);
     angle = coeff_1 - coeff_1 * r;
   }  else {
     r = (x + abs_y) / (abs_y - x);
     angle = coeff_2 - coeff_1 * r;
   }
   if (y < 0)      return int(-angle);
   else            return int(angle);
}


Finally, the kalman module (unchanged constants):

Code: [Select]

   float Q_angleX  =  0.001;
    float Q_gyroX   =  0.003; 
    float R_angleX  =  0.03; 

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
   
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
   
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
   
    return x_angle;
  }
 
  float Q_angleY  =  0.001;
    float Q_gyroY   =  0.003; 
    float R_angleY  =  0.03; 

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
   
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
   
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
   
    return y_angle;
  }


and just for reference, this is how I'm graphing the data (basically using your processing sketch to graph custom inputs):

Code: [Select]

void processing()
{
  Serial.print(xACC_angleDeg);Serial.print("\t");
  Serial.print(0);Serial.print("\t");
 
  Serial.print(yACC_angleDeg);Serial.print("\t");
  Serial.print(0);Serial.print("\t"); 
 
  Serial.print(xfinAngleDeg);Serial.print("\t"); 
  Serial.print(0); Serial.print("\t");
 
  Serial.print(yfinAngleDeg);Serial.print("\t");
  Serial.print(0);Serial.print("\t");
   
  Serial.print("\n");
}


Thanks for your help!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 12, 2012, 01:44 pm
#asrinivasan
Hmm, It looks alright. Have you tried to just debug the accelerometer and gyro angles? Just to see if they seem correct?
You could just do something like this:
Code: [Select]

Serial.print(xACC_angle, DEC);
Serial.print("\t");
Serial.println(xGYRO_angle, DEC);


Regards
Lauszus


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 19, 2012, 01:12 am
@Lauszus

Thanks for your help! I finally figured it out- I'm not surprised I didn't catch this, as it was not syntax or logic error but in fact simply due to the speed of the Arduino system. I'll post my solution for others who experience this problem. Here is the offending code:

Code: [Select]

 dtime = millis()-timer;
 timer = millis();

//not working-- fix
 xfinAngleQuid = kalmanCalculateX(xACC_angleQuid, yGYRO_rate, dtime); //note the use of y-gyro for x-axis measurement
 xfinAngleDeg = xfinAngleQuid * QUID_DEG;
 yfinAngleQuid = kalmanCalculateX(yACC_angleQuid, xGYRO_rate, dtime); //here too
 yfinAngleDeg = yfinAngleQuid * QUID_DEG;


This produces unexpectedly inaccurate results- the kalman-filtered reading jumps then settles, resulting in very inaccurate inclination measurements. Here is my problem signal:

[img=http://img96.imageshack.us/img96/5797/scrn5.th.jpg] (http://imageshack.us/photo/my-images/96/scrn5.jpg/)

As you can see in the problem code, kalmanCalculate is called twice- but as it happens, this module is very resource-intensive, involving matrix calculations and such. Therefore, it takes a significant amount of time return a result (about ~3.5 ms per call according to my experimentation). Therefore, because the timing module measures not only the time of a single loop but also part of the last loop (as you can see it is in the middle of the code, after sensor calibration but before kalman calculation) and hence returns a "dtime" that is much larger than the actual time taken to traverse a single loop. Because of this, depending on the complexity of operations occurring after the 'dtime' measurement, the kalman calculations (and potentially PID loops) tend to behave strangely. The solution to this is simple:

Code: [Select]

 dtime = millis()-timer-7; // to compensate for extra kalman calls
 timer = millis();


... and produces this result:

[img=http://img32.imageshack.us/img32/4615/scrn6a.th.jpg] (http://imageshack.us/photo/my-images/32/scrn6a.jpg/)

Alternately, you could simply place the 'timer' variable after kalman calculations:

Code: [Select]

  dtime = millis()-timer;

  xfinAngleQuid = kalmanCalculateX(xACC_angleQuid, xGYRO_rate, dtime);
  xfinAngleDeg = xfinAngleQuid * QUID_DEG;
  yfinAngleQuid = kalmanCalculateY(yACC_angleQuid, yGYRO_rate, dtime);
  yfinAngleDeg = yfinAngleQuid * QUID_DEG;

  timer = millis();//place after kalman calculations
  processing();


This way, you can call kalman as many times as you want without affecting timing.


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 19, 2012, 04:13 pm
Nice tip. I have never thought about that :)
So if you already wrote it, but what are you going to use it for?

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 20, 2012, 10:19 pm
I'm actually using this for a much larger project- it's a lower-body robotic orthosis (think exoskeleton) that is supposed to be able to dynamically self-balance and require minimal input from the user to operate- in this respect, it is almost autonomous in the way it behaves. It basically facilitates movement of the legs to aid in walking for those with congenital diseases (or simply due to senility/osteoperosis)- as far as I know, this is the first biped orthosis able to self-balance. I know it is a complicated project, but those who know me know that I am prone to such things  :P (last year's project was a mind-controlled robotic arm http://sites.google.com/site/eegprosthetics/home (http://sites.google.com/site/eegprosthetics/home) http://news.cnet.com/2300-11386_3-10008553-8.html (http://news.cnet.com/2300-11386_3-10008553-8.html)). I am entering it for this year's Science Fair (I am a high school sophomore).

It uses several different controlling procedures to facilitate the gait, including an ANN (using back-propagation learning) to control step movements, an IK calculator to determine joint angles, calculation of ZMP (rzero-moment-point), etc. So you can see how frustrated I was when this little part of the project wasn't working properly!  :) It uses incline sensing as well as body velocity, joint torques, operator inertia, etc. to determine optimal foot placement- since we as humans can balance in place without moving the feet.
Wish me luck! It's only a few weeks away...  :smiley-eek:
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 20, 2012, 11:31 pm
Nice, thats sounds really interesting. Just saw the video of your mindcontrolling arm - man you speak fast. Did you speed it up or something? :) I would really like to see a video of the arm in action. Do you got it on youtube or somewere else :O
How long are you from finishing your project, because it really sounds complicated and how long did you have in total from start to finish?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 20, 2012, 11:58 pm
No, I haven't uploaded it on youtube yet- that was last year's project, but I have yet to make a video :). But you can see a picture of my exhibition at the Google science fair last year (I was one of the 15 finalists) here: http://news.cnet.com/2300-11386_3-10008553-8.html (http://news.cnet.com/2300-11386_3-10008553-8.html). With regard to the video speed , as part of the submission process, we were supposed to limit the length to two minutes- doesn't exactly do the project justice, but it went through nevertheless. 

As far as timing goes, I have as long as I want between fairs, in this case I have spent the last 9 months or so researching, designing, and building the exoskeleton. I have less the two weeks to complete it- programming is nearly finished but there is still extensive work to be done on the physical design.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sirbow2 on Jan 21, 2012, 03:22 am
im trying to simply get the y axis angle for a blancing bot. but i cant get any good values. im using a digital IMU so i dont need to do quids etc, they are already in g(gravity) format. coudl someone look here and see if they could help: http://arduino.cc/forum/index.php/topic,87850.msg660981.html#msg660981
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: asrinivasan on Jan 22, 2012, 05:13 pm
I took a look at your thread- it seems you are having the same "jumping and settling" problem as I had. Look a couple of posts earlier for my solution. There is nothing wrong with your kalman or atan2 functions, but with the loop timing.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: sirbow2 on Jan 22, 2012, 06:17 pm
but i read the time delay before anything in the loop starts and after the last line of IMU code in the loop--including the kalman functions--so im not sure what you mean.

PA: kalman gyro and acc data-> final platform angle
Correc: combined platform angle goes into PID, this is the output
aY: calculated yaxis value frof g's(gravity) to angle in deg
aYr: raw y axis data from acc in mp/digit, an AHRS for my IMU subtracts 256 from the acc axis measurements to turn into g's(gravity)

without subtracting 256 from the acc axis', the angle value make no sense. (the PA and Correc values make no sense anytime)

held still
Code: [Select]
PA -42
Correc 3143
aY 13
aYr -258
 
PA -50
Correc 4669
aY 13
aYr -257
 
PA -57
Correc 6433
aY 13
aYr -258
 
PA -64
Correc 8442
aY 14
aYr -262
 
PA -71
Correc 10696
aY 13
aYr -266
 
PA -79
Correc 13237
aY 13
aYr -255
 
PA -86
Correc 16016
aY 14
aYr -252
....a few seconds
PA 23
Correc -32663
aY 13
aYr -255
 
PA 23
Correc 32278
aY 14
aYr -257
 
PA 23
Correc 31683
aY 13
aYr -254


changing pitch of imu
Code: [Select]
PA -3621
Correc -2948
aY 27
aYr 219
 
PA -3582
Correc -9208
aY 38
aYr 325
 
PA -3047
Correc 27876
aY 39
aYr 276
 
PA -2139
Correc 30549
aY 10
aYr 161
 
PA -1192
Correc -236
aY 16
aYr 143
 
PA -322
Correc 4579
aY 22
aYr -107
 
PA 732
Correc -28739
aY 9
aYr -146
 
PA 1309
Correc -13477
aY 8
aYr -239
 
PA 2259
Correc 31585
aY 19
aYr -450
 
PA 3174
Correc -20729
aY 3
aYr -581
 
PA 3273
Correc -5375
aY 19
aYr -664
 
PA 3254
Correc 11580
aY 27
aYr -674
 
PA 3264
Correc 27992
aY 4
aYr -637
 
PA 2774
Correc -552
aY 10
aYr -678
 
PA 1986
Correc 600
aY 15
aYr -653
 
PA 1159
Correc 31005
aY 8
aYr -561
 
PA 335
Correc 24698
aY 15
aYr -379


code:
Code: [Select]
//-----Libraries------------------------------------------------//
#include <Wire.h>
#include <LSM303.h> //ACC, MAG(not used)
#include <L3G4200D.h> //gyro
#include <IRremote.h> //IR library
#include <Servo.h> //Servo support

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0; //stores motor speed value(0-180)
int RmtrSpd=0;
#define LServoMtrPin 2  
#define RServoMtrPin 3
Servo LServoMtr;
Servo RServoMtr;

//Loop refresh rate------------------------------------------------
int TimeTwo = 0;
int TimeOne = 0;
int LoopDelay = 0;  //later is modified to equal additional delay needed to make loop 30ms long
#define SetDelay 35 //how long loop should be

//IMU------------------------------------------------------------
//http://www.pololu.com/picture/view/0J3680
LSM303 compass;
L3G4200D gyro;

#define Gravity 0
int AccYAngle = 0;
int CalibSensor[4] = {
 0,0,0,0}; //for storing IMU centering values during setup, we only need gyro Y, ACC X Yand Z
int SensorValues[4] ={
 0,0,0,0}; //stores gyro y, ACC x y z
int PlatformAngle = 0; //stores final angle in degree of the platform, used as input for PID
int CenterVal = 0; //balancing value for IMU

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193
#define offset 90 //motor center

//change for stability
#define Kp 7   //proportional
#define Ki 1   //Integral
#define Kd -5   //derivative

//for storing calculated values later on
int Error = 0; //Error
int Proportion = 0; //value to store the Proportional value from PID equation
int Integral = 0; //value to store the ... value from PID equation
int Derivative = 0; //value to store the ... value from PID equation
int PrevError = 0; //stores error for next loop + PID
int Output = 0; //PID final calculation value
int Correction = 0; //final PID value

// --- Kalman filter module  ----------------------------------------------------------
float Q_angleY  =  0.001;
float Q_gyroY   =  0.003;  
float R_angleY  =  0.03;  

float y_angle = 0;
float y_bias = 0;
float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
float dtY, yY, SY;
float KY_0, KY_1;

//---Setup---------------------------------------------------------//
void setup()
{
 Serial.begin(115200);

 //LM303 acc setup
 Wire.begin();
 compass.init();
 compass.enableDefault(); //enables acc LSM303 readings
 gyro.enableDefault(); //enables gyro L3G44200D readings

 LServoMtr.attach(LServoMtrPin); //attach then set servos to center
 LServoMtr.write(90);
 RServoMtr.attach(RServoMtrPin);
 RServoMtr.write(90);

 IMUCenter(); //center IMU at current position

 Serial.print("Calibration values: ");
 for(int x=0; x<=3; x++)
 {
   Serial.print(x);
   Serial.print(":");
   Serial.print(CalibSensor[x]);
   Serial.print(" ");
 }
 CenterVal = (int)compass.a.x-CalibSensor[1];
 delay(2000);

}

//--Loop------------------------------------------------------------
void loop()
{
 TimeOne = millis(); //recordes time at beginning of loop

 IMURead();

 //calculates acc angle
 AccYAngle = acos(SensorValues[2]/(sqrt(pow(SensorValues[1],2)+pow(SensorValues[2],2)+pow(SensorValues[3],2))))*RAD_TO_DEG-90;  
 //gyro rate is already correct in SensorValues[0]
 /*
 if(SensorValues[3] < 0)//360 degrees
  {    
  if(AccYAngle < 0)
  {
  AccYAngle = -180-AccYAngle;
  }
  else
  {
  AccYAngle = 180-AccYAngle;
  }
  } */

 PlatformAngle = KalmanCalc(AccYAngle, SensorValues[0], SetDelay); //combines gyro and acc

 Correction = PIDCalc(); //calculates change in PlatformAngle and smoothes out for servo output.

 Serial.println("  ");
 Serial.print("PA ");
 Serial.println(PlatformAngle);
 Serial.print("Correc ");
 Serial.println(Correction);
 Serial.print("aY ");
 Serial.println(AccYAngle);
 Serial.print("aYr ");
 Serial.println(SensorValues[1]);


 LServoMtr.write(Correction + offset);
 RServoMtr.write(Correction + offset);

 TimeTwo = millis(); //recordes time at end of loop
 LoopDelay = SetDelay - (TimeTwo - TimeOne); //calculates delay needed to maintain SetDelay
 delay(LoopDelay); //delays needed time
}

void IMURead()
{
 gyro.read(); //reads gyro
 compass.readAcc(); //reads compass and acc(only acc used)

 SensorValues[0]=(int)gyro.g.y-Gravity-CalibSensor[0]; //256(gravity) is for the gravity of the ACC
 SensorValues[1]=(int)compass.a.x-Gravity-CalibSensor[1];
 SensorValues[2]=(int)compass.a.y-Gravity-CalibSensor[2];
 SensorValues[3]=(int)compass.a.z-Gravity-CalibSensor[3];
}

void IMUCenter()
{
 gyro.read(); //reads gyro
 compass.readAcc(); //reads compass and acc(only acc used)

 for(int x=0; x<=50; x++)
 {
   gyro.read(); //reads gyro
   compass.read(); //reads compass and acc(only acc used)
   CalibSensor[0] += (int)gyro.g.y;
   CalibSensor[1] += (int)compass.a.x;
   CalibSensor[2] += (int)compass.a.y;
   CalibSensor[3] += (int)compass.a.z;
 }
 //store values for centering after finding average(/20)
 CalibSensor[0] = CalibSensor[0]/50;
 CalibSensor[1] = CalibSensor[1]/50;
 CalibSensor[2] = CalibSensor[2]/50;
 CalibSensor[3] = CalibSensor[3]/50;
}

float KalmanCalc(float newAngle, float newRate,int looptime)
{
 dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
 y_angle += dtY * (newRate - y_bias);
 PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
 PY_01 +=  - dtY * PY_11;
 PY_10 +=  - dtY * PY_11;
 PY_11 +=  + Q_gyroY * dtY;

 yY = newAngle - y_angle;
 SY = PY_00 + R_angleY;
 KY_0 = PY_00 / SY;
 KY_1 = PY_10 / SY;

 y_angle +=  KY_0 * yY;
 y_bias  +=  KY_1 * yY;
 PY_00 -= KY_0 * PY_00;
 PY_01 -= KY_0 * PY_01;
 PY_10 -= KY_1 * PY_00;
 PY_11 -= KY_1 * PY_01;

 return y_angle;
}

int PIDCalc()
{
 //PID-------------------------------------------------------------
 Error = CenterVal - PlatformAngle; //finds error between the wanted value and what it currently is.
 Proportion = Error; //"makes a change to the output that is proportional to the current error value"
 Integral = Integral + (Error*SetDelay); //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
 Derivative = (Error - PrevError)/SetDelay; //"how fast a change in angle occurs"

 /*
Integral = Integral + (Error*SetDelay); //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
  Derivative = (Error - PrevError)/SetDelay; //"how fast a change in angle occurs"
 
  w/o time:
  Integral += Error; //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
  Derivative = (Error - PrevError); //"how fast a change in angle occurs"
  */

 //calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
 //the program and the resulting values from Proportion, Integral, and Derivative calculations
 Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
 PrevError = Error; //puts current error in another value for next loop to use.
 return Output;
}
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Jan 24, 2012, 01:23 am
Hello everybody.

I am making a 2 wheel balancing robot.

http://www.youtube.com/watch?v=51nDmkFh3wM

I already convert the accelerometer X reading to g's.
Is it possible to use only one axis of the accelerometer to get the angle in radians?


Thanks!

George.



Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 24, 2012, 07:25 am
#georpo
It looks really nice. Yes it's actually pretty simple. You can calculate the angle in radians using sinus:

Code: [Select]

accXangleRad = asin(accXval);
accYangleRad = asin(accYval);


To convert it to degrees, you multiply it by 180/PI like so:

Code: [Select]

accXangleDeg = asin(accXval)*RAD_TO_DEG;
accYangleDeg = asin(accYval)*RAD_TO_DEG;


Where RAD_TO_DEG is aprox. 57.2957795

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Jan 24, 2012, 07:59 am
Hello and thanks for the reply.

I use this

Code: [Select]

accVal = (accAdc-accZero)/sensitivity


and the result is a float in the range of -1 to 1 which indicates the g's value of the accelerometer.

By using

Code: [Select]

accXangleRad = asin(accXval);


I should get the value in radians. Right?

Thanks again.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 24, 2012, 08:00 am
#georpo
Yeah, that's correct ;)

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Jan 24, 2012, 09:22 am
and if I finally convert to degrees I should have 90 degrees for 1g and -90 for -1g.
Right?

I will try this later today. Now I am supposed to be working...

Thanks.


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 24, 2012, 04:08 pm
#georpo
Yes that is correct :)

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Jan 24, 2012, 08:49 pm
Well I tried the code and I have -1.57 radians for -1g and about 1.3 radians for 1g.
The gyroscope gives me an output of  +-50 radians when tilted back and forth by hand at a normal speed.

The point is that before feeding the values to the Kalman filter they should have the same range. Right?

I also noticed from previous experiments that I need to multiply the accelerometer output by a value of about 30
to make it have a wider range of values and have a tighter response from my robot.

Is there something wrong here?

Thanks.

George.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 24, 2012, 09:09 pm
How do you determine the zero values? Did you calibrate it like I did or use the zero values in the datasheet? ;)
You shouldn't have to multiple the output - you should adjust the PID values instead.

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Jan 24, 2012, 09:28 pm
The zero values are calculated by taking 50 ADC samples and averaging.
As I told you the output of the accelerometer is -1 g and +1g when tilted from -90 to +90 degrees
which means that zero values etc are correct.

I'm just saying that the gyro and accelerometer values are in different scale.

This is how I update the sensor values. The value is the average of 5 readings for each sensor.
Code: [Select]

  v = 0;
  SET_ADC_CHANNEL(GYRO);
  delay_us(20);
  for(i=0; i<5; i++)   v+=read_adc();
  GyroValue=(v/5)- GyroZero;
  GyroRate=GyroValue/4.63595;          //sensitivity/3.3*1023.     sensitivity is 0.015V/d/s

  v = 0;
  SET_ADC_CHANNEL(ACCX);
  delay_us(20);
  for(i=0; i<5; i++)   v+=read_adc();
  AccelX = (v/5)- AccelXZero;
  AccAngle=asin(AccelX/148.35); //sensitivity/3.3*1023.    sensitivity is 0.48V/g




Any ideas?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 24, 2012, 09:55 pm
I'm not sure what you mean by they are in a different scale? You should convert both of them to an angle, as I did in the original code. If you use the Kalman filter the input should be the angle (deg) calculated using the accelerometer, the gyro rate (deg/s), and the delta time (time since last calculation).

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Jan 24, 2012, 10:07 pm
The code above gives me the values from the two sensors in radians.
No further conversion is done. The two values are fed in the kalman filter.

By saying different scale I mean that the accelerometer is -1.57 to 1.3 radians and the gyro is much wider range of about +-50 radians.

Do I need to convert the values to degrees before feeding in the kalman filter?
And how can I convert my "GyroRate" value to degrees/sec as you say?

Thanks.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 24, 2012, 10:47 pm
You should have a look in the code, it's pretty simply. To convert from radians to degrees you multiply it by 180/PI (approximately 57.2957795), normally the gyro output will already be in deg/s, so you don't have to change that. What IMU are you using? Post the datasheet and I will have a look at it :)
You should the feed the Kalman filter with the accelerometer angle in degrees and the gyro rate in degrees per seconds as mentioned in the orignal post.
To see if your gyro data is correct try multiplying with the delta time and see if the accelerometer angle and gyro angle are close - it's only close in the beginning, as the gyro will drift over time.

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Jan 28, 2012, 09:01 am
I've a questions.

In this code i think  the angle measured dependent  from the initial instant. That is, depending on how it is at the beginning. If I wanted to know the absolute angle instead of (or an angle that does not depend from the initial instant ) how should I do? Why kalman angles are scaled with respect to real angles?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 28, 2012, 12:25 pm
I'm not totally sure that I understand your question.
You should just use the zerovoltage constant in the datasheet, instead of "calibrating" the device at startup.

What do you mean by scaled angles? :)

- Lauszus

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Jan 28, 2012, 12:35 pm
tks for the first solutions, i will try this solution and will update.

For "scaled angles" i mean that  betwen x_angle (kalmanCalculateX output) and gyroXangle there is a big difference, and this difference i think is costant.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 28, 2012, 01:00 pm
In the beginning they will be almost the same, but as the gyro drift the difference will get bigger and bigger :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: batista1987 on Jan 28, 2012, 01:01 pm
ok ok :-) tks :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: iamnarendra on Feb 01, 2012, 07:37 pm
m gettin error like this while running the program.....
wat does it mean..... pls help me......

avrdude: stk500_getsync(): not in sync: resp=0x00
avrdude: stk500_disable(): protocol error, expect=0x14, resp=0x51
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 01, 2012, 11:20 pm
Are you using a programmer? :) That error is related to programmers.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Feb 05, 2012, 07:43 pm
Hello Everybody!

Lauszus, I was stuck on page 17 and thought you never replied! I am so silly!

Anyway, thanks for the support. Look at what I have till now : http://www.youtube.com/watch?v=3rn2kOoLw6E (http://www.youtube.com/watch?v=3rn2kOoLw6E)

I am using complementary filter.

Code: [Select]

x_angle=(0.993)*(x_angle+GyroRate*dt)+(0.007)*(AccAngle);


I modified the values to take more of the gyro output because if I use the defaults
the robot gets lazy and needs big tilt before trying to react.

Code: [Select]

x_angle=(0.98)*(x_angle+GyroRate*dt)+(0.02)*(AccAngle);



How can I also modify the kalman filter to take more out of the gyro?

I think the secret is here... Right?

Code: [Select]

   float Q_angle  =  0.001; //0.001
    float Q_gyro   =  0.003; //0.003
    float R_angle  =  0.03;  //0.03


But which is which???


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 05, 2012, 10:08 pm
#georpo
Q_angle is the process noise covariance for the accelerometer, Q_gyro is the process noise covariance for the gyro and R_angle is the measurment noise covariance.
You should try to lower the Q_gyro value and raise the Q_angle value :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Feb 12, 2012, 12:50 pm
complementary behaves better for me...

http://youtu.be/Youro5J2RrA

still needs some pid tuning....
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 12, 2012, 02:33 pm
#georpo
Awesome job!! Where did you buy the belt, gears and the shaft? :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Feb 12, 2012, 03:48 pm
Thanks!

Belts and pulleys are from http://www.econobelt.com/e-store/Pulleys.htm (http://www.econobelt.com/e-store/Pulleys.htm)
They are the cheapest I could find after much searching...

Shafts, bearings and bearing housing are all from class 500 RC helicopter ( I also fly RC models helicopters, airplanes etc)
You can find many things at hobbyking

http://www.hobbyking.com/hobbyking/store/__10199__HK_500GT_Ball_Bearing_16_x_8_x_5mm_Align_part_H50067_.html (http://www.hobbyking.com/hobbyking/store/__10199__HK_500GT_Ball_Bearing_16_x_8_x_5mm_Align_part_H50067_.html)
http://www.hobbyking.com/hobbyking/store/__10158__HK_500GT_Main_Shaft_Align_part_H50011_.html (http://www.hobbyking.com/hobbyking/store/__10158__HK_500GT_Main_Shaft_Align_part_H50011_.html)
http://www.hobbyking.com/hobbyking/store/__9795__HK_500GT_Metal_Main_Shaft_Holder_Align_part_H50075_.html (http://www.hobbyking.com/hobbyking/store/__9795__HK_500GT_Metal_Main_Shaft_Holder_Align_part_H50075_.html)

Motor base is made of FR4 (the material we use to make printed circuit boards )  cut with a CNC router that I have at where I work.

Of course the shaft needs cutting and you also need a 8mm adapter for the wheels.

Until now complementary filter works better for me. Do you think that kalman will make a big difference?
Is it worth to spend some more time for it?

For now it seems to balance well enough but it needs more clever way to control the attitude, for example if I push it hard enough
it will start chasing itself and finally fall.. I think that I should add another K for the gyro rate. If we are falling too fast, go faster!

This robot is just a lesson for me because I need all the balance knowledge to make a self flying quadcopter. I hope....

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 12, 2012, 03:56 pm
Thanks for the links, might need it some day :) Or useful to others.

I don't think you will need a Kalman filter, for instance these guys: http://web.mit.edu/first/segway/ used a complimentary filter just like you.

You can prevent it from falling when pushed, if you add encoders or add body velocity (read gyro) into the PID loop.
What constants did you end up with for the IMU for the complementary filter? I haven't really experimented on that, as I just used some I found on the web.

Nice. A loot of people are trying to do just that, so there's a ton of info out there :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Feb 12, 2012, 04:08 pm
Code: [Select]
x_angle=(0.993)*(x_angle+GyroRate*dt)+(0.007)*(AccAngle);

My gyro is IDG500 and my accelerometer is MMA6271 both soldered on a custom made pcb.
I am using only 1 axis of the accelerometer and calculate the angle with asin as you suggested.



Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 12, 2012, 04:11 pm
Okay thanks for the info :) Might try to fine-tune the complimentary filter for my IMU some day and see how it perform compared to the Kalman filter.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: georpo on Feb 12, 2012, 09:24 pm
I tried adding angular velocity and introduced a new K (kv) to the PID.
My dt is fixed at 10mS so it is omitted from the calculation.

Code: [Select]

    pid_v = Kv *(angle-LastAngle);
    pid_value = (pid_p + pid_i + pid_d + pid_v);

   LastAngle=angle;


This did not work :(

I also tried the same with just the gyro value but no luck.

I just want to get the robot move faster when pushed faster so it will overshoot the balance point and brake.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 12, 2012, 09:37 pm
Hmm, actually I'm building a segway too.
This is an old video:
http://www.youtube.com/watch?v=pOJy0YtPuF4

It balance a bit better now, but I also got problems, when it's being pushed. I would like it to do that before I add remote control with a PS3 Controller (see my github: https://github.com/TKJElectronics/USB_Host_Shield_2.0). You might have a look at this robot:
http://www.youtube.com/watch?v=Kd2kJxBkPmk

He replied this, when a user ask how he made it push back:
"the key was to get it to balance just using the angle, only then did I add in body and wheel velocity (and lastly position error) , with just angle error it should stand upright but drift/stop with a? push. the body velocity was calc from 2h*sin(angle change) /dt . so my equation looks just like yours. This feeds balance PID which feeds motor PID with wheel vel also."
But I don't know that "h" stands for?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ars on Feb 23, 2012, 07:28 am
Hello Lauszus,
I am currently working on a quadrotor, for this im using a 6DOF digital imu(i2c), so i used your code for the kalman filter for it and modified the sensitivity to 14.375 and 256 , i am getting the values in the kalman from -90 to 0 to +90, however the time taken by the kalman filter to reach the final angle is very high, if i tilt the quad in one direction , while tilting it the values are like in the 100 to 200 range but when i rest the quad and after about 2 seconds the correct kalman angle is obtained. As this response is very slow for the quadrotor, how should i modify your code?Please help me Lauszus..

-Arvind Sanjeev
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 23, 2012, 05:41 pm
It sounds like it "trust" your gyro to much. Try increasing Q_gyroX. Also try changing Q_angle and R_angleX.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ars on Feb 28, 2012, 03:41 pm
On changing rangle to a small value, the filtering is fast however on switching on the motors the angle values are not at all stable.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ars on Feb 28, 2012, 06:50 pm
how can I increase the speed of your kalman filter code??...please help
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 28, 2012, 09:44 pm
What do you mean by "the angle values are not at all stable"? Do you mean like they are all over the place? That might be because that you are "trusting" the accelerometer too much, as it's really sensitive too acceleration - that's why it's called a accelerometer :) So if the motor make the IMU start to vibrate the accelerometer will pick up these vibrations and the angle calculated by the accelerometer will be incorrect.

You should try to experiment with the three noise covariances:
Q_angle // Process noise covariance for the accelerometer - Sw
Q_gyro // Process noise covariance for the gyro - Sw
R_angle // Measurement noise covariance - Sv

By lowering "R_angle" you trust the new angle more. So you should find just the right value, where it's fast enough, but at the same time doesn't get affected by vibrations. Alternatively try raising "Q_angle" as this will "trust" the accelerometer less!

For more info see the following pages: http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

These three noise covariances are the key to make it faster and more correct :)

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: redvincenzo on Mar 01, 2012, 01:01 am
Hi all,

I am using the ADXL345 and itg3200 IMU board from sparkfun.

i have read this thread and it has been very helpful.

i have communicated to both, set them and receiving there output, but my gyros raw X values are always roughly 65533,  Y is 65500 +/- 1 for both and Z is roughly 8.

are these values correct? the board is steady on the table. arnt they sapposed to output close to 0 +/- the offset?

can someone show me there raw values from the gyro so i could see if maybe my gyro is not working well.

I am completely comfused with these outputs.

thank you all in advance.

Vince
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ars on Mar 01, 2012, 08:32 pm
Also, I am experiencing some discontinuity when i reach a particular angle as so:
18   171.00   153.00   
18   170.00   153.00   
18   171.00   153.00   
19   171.00   153.00   
19   171.00   153.00   
19   172.00   153.00   
27   190.00   153.00   
31   189.00   153.00   
29   181.00   153.00   
28   180.00   153.00   
35   197.00   153.00   
40   200.00   153.00   
45   200.00   153.00   
49   200.00   153.00   
52   200.00   153.00   
57   200.00   153.00   
58   200.00   153.00   
59   200.00   153.00   
60   200.00   153.00   
58   200.00   153.00   
59   200.00   153.00   
59   200.00   153.00   
59   200.00   153.00   

How can i prevent this?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 01, 2012, 10:30 pm
#redvincenzo
How to you connect to the IMU, is it via I2C or is it simply a analog sensor? If it's I2C then you properly did something wrong in the I2C commands somewhere.

#ars
How are you calibrating the accelerometer? Remember that you can only get 360 degrees resolution at only one of the axis. If you need both pitch and roll you can get more than 180 degrees resolution without a magnetometer :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ars on Mar 02, 2012, 03:05 am
I;m using I2C, and it's a 6DOF without magnetometer, I haven't used that "if statement (360)"......I am just getting discontinuous angle values as shown previously, my quadrotor cannot respond effectively to it.Please help......
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: iamnarendra on Mar 02, 2012, 05:13 pm
thanks 4ur code... i have been using it with sligt change for my self balancing robot project....
iam using only x axis for readings....
the output i got using ur pgm ....


-36   -2   -13   -0   
-37   -2   -14   -0   
-38   -2   -15   -0   
-39   -2   -16   -0   
-40   -2   -17   -0   
-40   -2   -18   -0   
-41   -2   -19   -0   
-42   -1   -21   -0   
-43   -1   -22   -0   
-44   -1   -23   -0   
-45   -1   -24   -0   
-46   -1   -25   -0   
-47   -1   -26   -0   
-47   -1   -27   -0   
-48   -1   -28   -0   
-49   -1   -29   -0   
-50   -1   -30   -0   
-50   -1   -32   -0   
-51   -1   -33   -0   
-52   -1   -34   -0   
-52   -1   -35   -0   
-53   -1   -36   -0   
-54   -1   -37   -0   
-54   -1   -38   -0   
-55   -1   -39   -0   
-55   -1   -40   -0   
-56   -1   -42   -0   
-57   -1   -43   -0   
-57   -1   -44   -0   
-58   -1   -45   -0   
-58   -1   -46   -0   
-59   -1   -47   -0   
-59   -1   -48   -0   
-60   -1   -49   -0   
-60   -1   -50   -0   
-61   -1   -51   -0   
-61   -1   -52   -0   
-62   -1   -53   -0   
-62   -1   -54   -0   
-63   -1   -55   -0   
-63   -1   -56   -0   
-64   -2   -57   -0   
-64   -2   -58   -0   
-65   -2   -59   -1   
-65   -2   -60   -1   
-65   -2   -60   -1   
-66   -2   -61   -1   
-66   -2   -62   -1   
-67   -2   -63   -1   
-67   -2   -64   -1   
-68   -2   -65   -1   
-68   -1   -66   -1   
-68   -1   -67   -0   
-69   -1   -68   -0   
-69   -1   -68   -0   
-70   -1   -69   -0   
-70   -1   -70   -0   
-70   -1   -71   -0   
-71   -1   -72   -0   
-71   -1   -72   -0   
-71   -1   -73   -0   
-72   -1   -74   -0   
-72   -1   -74   -0   
-72   -1   -75   -0   
-73   -1   -76   -0   
-73   -1   -76   -0   
-73   -1   -77   -0   
-74   -1   -77   -1   
-74   -1   -78   -1   
-74   -1   -78   -1   
-74   -1   -79   -1   
-75   -1   -80   -1   
-75   -1   -80   -1   
-75   -1   -81   -1   
-75   -1   -81   -1   
-76   -1   -82   -1   
-76   -1   -82   -1   
-76   -1   -83   -1   
-76   -1   -83   -1   
-77   -1   -83   -1   
-77   -1   -84   -1   
-77   -1   -84   -1   
-77   -1   -85   -1   
-77   -1   -85   -1   
-78   -1   -86   -1   
-78   -1   -86   -1   
-78   -1   -86   -1   
-78   -2   -87   -1   
-78   -2   -87   -1   
-78   -2   -87   -1   
-79   -2   -87   -1   
-79   -2   -88   -1   
-79   -2   -88   -1   
-79   -2   -88   -1   
-79   -2   -88   -1   
-79   -2   -89   -1   
-79   -2   -89   -1   
-80   -2   -89   -1   
-80   -2   -89   -1   
-80   -2   -89   -1   
-80   -2   -90   -1   
-80   -2   -90   -1   
-80   -2   -90   -1   
-80   -2   -90   -1   
-80   -2   -90   -1   
-81   -2   -90   -1   
-81   -2   -91   -1   
-81   -2   -91   -1   
-81   -2   -91   -1   
-81   -2   -91   -1   
-81   -2   -91   -1   
-81   -2   -91   -1   
-81   -2   -91   -1   
-81   -2   -92   -1   
-82   -1   -92   -1   


gettin values in the range -30 to -90... not gettin +values.... eventho tilt is in +axis.... help me pls.....
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 04, 2012, 02:48 am
#ars
It's pretty weird that the angles simply just stops. The output your wrote is that the output from the Kalman filter or what? Could you please post the angle calculated by the gyro, accelerometer and Kalman filter, so I can have a look at it? :)

#iamnarendra
Hi,
What are the different columns?

Best Regards
Kristian Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: noob on Mar 06, 2012, 10:21 am
this is a long guide and I am not sure if someone has posted this before, nevertheless I would like to point out some ways to optimise the Kalman filter code so kindly provided.

1) for the file KalmanX
the variables KX_0 and KX_1 will converge to a constant value over time. In fact you should find that the variables PX_00, PX_01, PX_10, PX_11 and Sx will also converge to a constant value. This is because the system is modelling a stationary process as indicated by the noise, as represented by Q_angleX, Q_gyroX and R_angleX being constant. Therefore it is possible to run the kalman filter offline in say matlab on some offline data and find the constant K gains. The final K values can then be used in the Kalman filter reducing it to

float Kalman_Compute(float y, float u, float dt)
{
       // y - measured angle from the accelerometer
       //u - measured value from the gyroscope
       //dt - time step
   x_angle = x_angle - dt*x_bias + dt*u + K_0*(y - x_angle);
   x_bias = x_bias + K_1*(y - x_angle);
   return (x_angle);
}

2) for bonus marks
floating point arithmatics generally takes more time to execute on a microprocessor than fixed point arithmatic. Therefore you should get some performance increases by avoiding floating point where you can. In this instance you can scale the variable by say 100, which would give you accuracy to 2 decimal places. But you should remember this scaling factor when you subsequently operate on those variables

I can provide a more thorough explanation for all this when I have a bit more time.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 06, 2012, 10:34 am
#noob
Thank you a lot for such useful information!!
Are you familiar with Matlab or something else equivalent, so you could run the code and then find the constants values, so I could test it? :)

- Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: noob on Mar 07, 2012, 09:37 am
I have outlined 3 kalman filtering equations in an attached document. These 3 equations are calculated on each iteration of your loop. You will notice that the K is dependant on the matrices A, P, C and Sz which are all constant. Note that the P matrix will become a constant because it is itself calculated with respect to matrices that do not vary.
I used the adxl335 accelerometer and idg500 gyroscope.
To this post I have attached
1) two sets of raw data from my IMU. These have NOT been scaled. I used one set of data to obtain the K1 and K2 values, and the second dataset to validate the K values.
2) matlab scripts that I used to calculate the K1 and K2 constants. Please note that when determining the K constants the sampling time step WILL AFFECT THE VALUES K CONVERGES TO. Also be very careful about the direction or rotation of the gyroscope and in particular the positive and negative direction. The derivative of the angle calculated from the accelerometer (i.e the angular velocity as determined by the angular position measured by the accelerometer) must rotate in the same direction as the gyroscope. Finally I used the modal number to determine the final K constants.

I should also disclaim I found the idea of calculating the constant K values from this website http://home.earthlink.net/~schultdw/bbot/bbot.html. Furthermore I also agree with that website when it argues that the state space equation used by the kalman filter code provided is technically wrong. But considering the kalman filter code works (well enough to keep bots balanced) I dont see why whether the state space equation is technically correct or not correct is that relevant. After all if it aighnt broke, why fix it? (or you may disagree)

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 07, 2012, 05:21 pm
Hi,
Thank for your contribution. I see one mistake though, you have set the Sz covariance matrix to 0.3, but really it's 0.03 in the code :)

Yeah, I have actually wrote a paper in my high school about the Kalman filter (it's in danish or I would have posted it - have not got the time to translate it) after I wrote the post and I also came to the same conclusion, that it wasn't 100% correct - but I agree, if it works, why fix it?
I actually created a balancing robot based on the algorithm:
http://youtu.be/N28C_JqVhGU

But as I used a mbed running 96MHz, the calculation didn't take that long - only a few milliseconds. But I might try to rewrite the Kalman filter some day.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ppaing126 on Apr 21, 2012, 08:57 am
Hello Lauzus,

This is my first post. I want to thank you for your Great and Excellent guide to IMU. I was wondering to understand Kalman for about 3 years but never had the enough time to dig into the required math, moreover kalman is hard to understand for me. Your guide in fact is the one I'm using for my first try. I built my 6DOF instead of bough it. I used 3  XV-8000CB gyros (+/-60 Deg/s) and a 3 Axis ADXL330 accelerometer.

I use a ArduinoMega 2560 R3 to hookup the sensors. I transalated your IMU code to Delphi as I'm doing all the processing in the computer, I'm just sending Adc values from channels thru serial port. I love to code in Delphi and is easier to try code changes in Pc instead of reflashing the Arduino.

At first try I tested the Complemetary filter, and noticed the reuired time to stabilize after severe deviation or shaking. Testing Kalman provided  funny results until I reversed the gyro values (*-1 trick).

I had a quesion about how to obtain Yaw from the Z axis but once I read the 20 forum pages I understanded I need to include a magnetometer to get 9DOF. I was thinking to use the Gps heading as an aid to the Gyro Yaw angle, instead of magetometer. Maybe the bads is when Gps is not moving to any direction the heading is lost, but the magnetometer works all the time.

I started to learn Gyro, accel and kalman to see if I can eliminate roll and pitch ofsset when measuring vehicle driving forces from my car. I'm using Accelerometers only to measure and log the driving forces but many of the times the data seems to be 'shifted' from zero a little bit, however the Accel calibration and zero was Ok. this is due mainly due to the track inclinations.

I mean if the accelerometers are calibrated with the vehicle is flat leveled I expect the logged data to populate to zero or near zero when not doing turns or accelerating/breaking the vehicle. Altough there always is inherent noise from mechanic suspension, bumpy road, interference, etc I expect this to be centered on chart. I guess most of the time is due to track inclination.

So real acceleration + earth gravity + vehicle inclination due to centrifugal is what the sensor see.

In your oppinion, would you think if is possible to separate one or the three components using the computed angles from the IMU..?

Thanks in Advance!!!




Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 23, 2012, 09:02 pm
#ppaing126
You can calculate the yaw bias (the amount the gyro has drifted), using DCM or a extended Kalman Filter.

You could in fact also use a complimentary filter, but it's not as precise. Which is just a digital high and low pass filter to calculate the bias, as they did at this page: http://www.hitechnic.com/blog/gyro-sensor/htway/

You should check out this video - isn't that what you are trying to do?
http://www.youtube.com/watch?v=Mt0MOr7rDPU

You could also try to use the standard zero values in the datasheet.

What do you mean by the following line:
"In your oppinion, would yuo think if is possible to separate one or the three components using the computed angles from the IMU..?"?

Regards
Lauszus

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ppaing126 on Apr 24, 2012, 01:05 am
Hello Lauzus,

I have your Kalman code working. Filtering looks great. I'm new to this and wonder if using this IMU I could someway calculate more 'real' driving forces from the vehicle (adding the compass to get Yaw filtered too) using the computed angles.

For 'real' I mean eliminating the vehicle body roll and gravity component..
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 25, 2012, 12:51 am
@ppaing126
To calculate the yaw, try googling DCM or a extended kalman filter - there a lot of example code on how to do it. Try quadrocopter forums.

I'm not sure what you mean by "vehicle body roll"? You can just calculate the pitch only if you don't care about the roll.

But really, if you want to use more sensors to get better readings, try DCM or extended kalman filter.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ppaing126 on Apr 26, 2012, 02:54 pm
Kristian,

After doing some short runs, I'm starting to believe that Kalman will not benefit at all the terrestrial vehicle application. In my charts the dominant signals are the Accelerometer forces as I can see. Kalman X and Y angles follows nicely the Accel angles, there are a little disturbs where the Kalman helped filtering. This maybe tells me the for the vehicle application, Accelerometer is stable and must be thrusted?

The Gyro benefits for the vehicle appear to be in the Yaw but at the moment I do not have the magnetometer to do the filtering fusion.

And yes, the video you indicated is likely what I do with my car, but I just log the data in SD card.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 26, 2012, 03:03 pm
It depends on what you are trying to measure. If you are trying to measure true pitch, roll and raw then you will need other sensors as well, but if you only want to know acceleration then you can just use a accelerometer.
But you can still use a accelerometer to measure the pitch and roll, but it will be affected by road bump etc.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ppaing126 on Apr 26, 2012, 04:46 pm
I was interested in acceleration forces only, but start looking in to Gyro and Imu in the hope to obtain:

1. Better filtered forces (minimize road bumps). Maybe a 8 pole filter works better? 
2. Eliminate the 'false' G buildup caused if the vehicle inclinates because road leveling.

Say I want to drive in conditions of +0.30g to -0.30g  but road inclination shifts the average zero value so finally I end with +0.45 to -0.15g.

Actually I postprocess and shift back the data getting an average and then substracting this value from all the data. But sometimes I'm affraid of substract too much or too few.

That's the point to where I was going.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 26, 2012, 04:54 pm
Okay. I not sure exactly how to do that.
What exactly do you need it for? :)

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: dhbromley on Apr 26, 2012, 07:29 pm
This is great!  I'm building a self-balancing robot using the Arduino Uno, the Sparkfun IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 (http://www.sparkfun.com/products/10121 (http://www.sparkfun.com/products/10121), and the Nubotics WW-12 - WheelWatcher Optical Encoder (http://www.nubotics.com/products/ww12/index.html (http://www.nubotics.com/products/ww12/index.html)).

I've had trouble finding any code examples for the wheel encoder.

I'll let you know how it goes.

Thanks,

Don Bromley
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 26, 2012, 10:16 pm
@ dhbromley
Take a look at how I implemented my encoders:
https://github.com/TKJElectronics/BalancingRobotArduino

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ppaing126 on May 01, 2012, 06:58 am
:D well, I'm interested in to build my Segway too, I think it's time to start looking for motors. About the vehicle, I need to take time to figure another plan.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: jasmine_hossam on May 26, 2012, 04:17 pm
i need a help plz i dont know how to get use of the reading of the gyro L3G4200D to control a stepper motor
i need to make a stabilization platform using this gyro L3G4200D
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 27, 2012, 04:34 pm
@jasmine_hossam
Check out Pololu's library for Arduino:
https://github.com/pololu/L3G4200D

And also see Sparkfun's example code for the device: http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Gyros/3-Axis/L3G4200D_Example.zip (http://www.sparkfun.com/products/10612).

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ea123 on May 28, 2012, 12:18 pm
Hi,
here is my implementation of Kalman filter and a comparison with raw sensor and complementary filter
in roll/pitch angle estimation:

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



Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 28, 2012, 01:46 pm
@ea123
Looks really nice :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Jase_MK on May 30, 2012, 04:45 pm
Hi all. Been lurking for a while and reading this thread with interest.

I'm putting together a data logger for my car and have managed to find a reasonably priced 9DOF unit. From what I can see, using the Kalman filter (or similar alternative) only works to calculate a angular position if you're not accelerating. Surely if a car is accelerating, braking or cornering hard then accelerometer readings used as part of the algorithm for calculating pitch and roll is going to generate duff results?

Similarly, I'm not sure how to measure something simple such as g-forces generated due to cornering. Simply taking an accelerometer reading on the x-axis will give an incorrect result due to the fact that the car will be experiencing some roll and therefore the sensor won't be flat. If I knew the angle of roll, I could correct this reading using some simple trig, but this takes us back to the original problem of not being able to calculate pitch/roll whilst the IMU is experiencing significant acceleration (other than gravity).

Hmmmm
/strokes chin...

Apologies if I've misunderstood things or missed this coming up earlier in the thread.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 30, 2012, 08:58 pm
@Jase_MK
Sorry, but you misunderstand the whole point of a Kalman filter or similar. The whole point of a Kalman filter or similar is to filter out any noise i.e. acceleration from a car etc. So if you tune your filter properly it will filter out all the noise from the accelerometer! To get the total amount of g-force, you just have to calculate the length of the force vector, as I did in the code.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Jase_MK on May 31, 2012, 12:02 am
OK, thanks. So using the Kalman filter, or similar, I can use my IMU readings to calculate angular position (pitch, roll), even if the car is accelerating about all over the place. I think I need to read the thread again from the start and try out some code and see what happens.

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 31, 2012, 12:06 am
@Jase_MK
Yes exactly :)
Okay, do that - I think the best way to "learn" how it works is to see the effect of the filter yourself.
Try to upload the Arduino code provided and use the Processing program I created to see a visual representation of the data.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Jase_MK on May 31, 2012, 12:15 am
Will do. My IMU is in the post so I shall get playing asap.

Thanks again
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 31, 2012, 12:23 am
@Jase_MK
Okay. Your welcome :)
Just write me again if you need further assistance!

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: George64 on May 31, 2012, 12:09 pm
Hello,

Pretty new to the forum and to using the Arduino Uno with the SLM303.

I'm trying to build a tilt compensated compass with an nmea string as out. This string is going to be input for my boat network so my tiller pilot will be able to better steer a course. Quite a project....

The code I have so far:
Code: [Select]
#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
 
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -397; compass.m_min.y = -284; compass.m_min.z = -464;
  compass.m_max.x = 150; compass.m_max.y = 287; compass.m_max.z = 227;
 
  /* Output van Calibrate
  M min X: -397 Y: -284 Z: -464
  M max X: 150 Y: 287 Z: 227
  */
}

/* Oorspronkelijke code
void loop() {
  compass.read();
  int heading = compass.heading((LSM303::vector){0,-1,0});
  Serial.println(heading);
  delay(500);
}
*/

/*----------------------------------------------------------------------------
        1   2   3 4   5 6
        |   |   | |   | |
$--HDG,x.x,x.x,a,x.x,a*hh<CR><LF>
------------------------------------------------------------------------------

Field Number:
1. Magnetic Sensor heading in degrees
2. Magnetic Deviation, degrees
3. Magnetic Deviation direction, E = Easterly, W = Westerly
4. Magnetic Variation degrees
5. Magnetic Variation direction, E = Easterly, W = Westerly
6. Checksum */

char nmeastr[17]; // HCHDG,000.00,,,,*
int checksum;
char hs[6]; // 000.00

void loop() {
  strcpy(nmeastr,"HCHDG,");
 
  //dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER);
  compass.read();
  dtostrf(compass.heading(), 5, 0, hs);
  //Serial.print("hs ");
  //Serial.print(hs);
  //Serial.println(); 
 
  strcat(nmeastr,hs);
  strcat(nmeastr,",,,,");
 
  //add a checksum
  checksum=0;
  for (int n=0; n < strlen(nmeastr); n++) {
    checksum ^= nmeastr[n];
  }
 
  Serial.print("$");
  Serial.print(nmeastr);
  Serial.print("*");
  Serial.print(checksum, HEX);
  Serial.println();
 
  /* 5hz == 200 */
  delay(500);
}


When laying flat on the table, it produces:
Quote
$HCHDG,  207,,,,*59
$HCHDG,  211,,,,*5E
$HCHDG,  213,,,,*5C
$HCHDG,  209,,,,*57
$HCHDG,  213,,,,*5C
$HCHDG,  211,,,,*5E
$HCHDG,  210,,,,*5F
$HCHDG,  215,,,,*5A
$HCHDG,  212,,,,*5D
$HCHDG,  207,,,,*59
$HCHDG,  208,,,,*56
$HCHDG,  208,,,,*56
$HCHDG,  208,,,,*56
$HCHDG,  211,,,,*5E
$HCHDG,  210,,,,*5F
$HCHDG,  210,,,,*5F
$HCHDG,  207,,,,*59
$HCHDG,  217,,,,*58
$HCHDG,  215,,,,*5A
$HCHDG,  211,,,,*5E
$HCHDG,  211,,,,*5E
$HCHDG,  209,,,,*57
$HCHDG,  212,,,,*5D
$HCHDG,  210,,,,*5F
$HCHDG,  209,,,,*57
$HCHDG,  210,,,,*5F
$HCHDG,  211,,,,*5E
$HCHDG,  212,,,,*5D
$HCHDG,  211,,,,*5E
$HCHDG,  210,,,,*5F


As you can see, not a stable course to steer ;) The video shows the issue I'm facing when it comes to stable ocurse
http://youtu.be/3XHxAKu6n7E (http://youtu.be/3XHxAKu6n7E)

I know I should do some filtering, and this is where I got to this topic and also got stuck on how to get the kalman filter to this code. I'd like some help on how I can put the filter in place so the course gets more stable.

Many thanks in advance for your reply!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 31, 2012, 05:46 pm
Hi,
I don't have any experience with magnetometers - I know that Sparkfun called it a compass sensor, but it's really a magnetometer.

Have you check out this application note:
http://www.sparkfun.com/datasheets/Sensors/Magneto/Tilt%20Compensated%20Compass.pdf

Also check out this library I found:
https://github.com/pololu/LSM303

I don't know if that is the library you are already using? But it look like it? Have you tried to run the example sketch (https://github.com/pololu/LSM303/blob/master/LSM303/examples/Heading/Heading.ino) and see how that went?

Remember that any magnetic field will affect your reading, so keep away all electronics - my friend had a big problem with his until he realized that his laptop's harddrive affected his magnetometer a lot!

Sorry that I can't help you no more - I have no experience with magnetometers. If it were me would properly try the example sketch provided in the library and then read the application note to understand what was going on :)
I don't think you can use the Kalman filter I provided. You need another type of filter - try google for "ahrs magnetometer" and see what you can find.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: George64 on May 31, 2012, 06:31 pm
Thanks for your explaination Lauszus.

The code from https://github.com/pololu/LSM303 is the code I use for the compass function. The output shown is the result of a minor addition I did to create nmea output.

I'll google "ahrs magnetometer" to see if it can help. Part of the learning curve ;)

George
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 31, 2012, 11:16 pm
Okay. I will try to talk with my teammate tomorrow as he is currently building a quadrocopter, so he has some experience with magnetometers, and then ask him for good links and so on.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: George64 on Jun 01, 2012, 07:24 am
Thanks Lauszus, that woud be great!

Been searching using the keywords you suggest, however I did not find a solution that would fit my needs. Sites I found suggest to use some sort of Kalman filter. Will also do more digging.

Cheers, George
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: tkj on Jun 02, 2012, 09:41 am
Dear George.

This is Thomas, Lauszus' teammate. As Lauszus sure enough introduced me I am building a Quadrocopter. To do this you need a 9DOF sensor, which means a 3-angled accelerometer, gyro and magnetometer.
To combine all the sensor data I use a DCM algorithm - Direct Cosine Matrix - which then gives me the orientation of the board in x, y and z rotational degrees.

With the orientation I am then also able to get the tilt compensated heading by combining the magnetometer data and the orientation values.

On the Arduino, which I am not using for this project though, it can be done with the following code: https://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs/wiki/Tutorial
If that doesn't work out for you because of it being to advanced then have a look at the smaller version of the code here: http://code.google.com/p/sf9domahrs/

Let me know if you can get it working.

Best Regards
Thomas Jespersen
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: George64 on Jun 03, 2012, 10:46 am
Dear Thomas, Lauszus,

Thanks for the time you took to reply.

I read the article and searched for a calibration programm related to the LSM303. I found this program:
Code: [Select]
#include <Wire.h>
#include <LSM303.h>

LSM303 compass;
LSM303::vector running_min = {2047, 2047, 2047}, running_max = {-2048, -2048, -2048};

// temp
float x_min; float x_max; float x_cur;
float y_min; float y_max; float y_cur;
float z_min; float z_max; float z_cur;

void setup() {
 
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();

}

void loop() { 
  compass.read();
 
  running_min.x = min(running_min.x, compass.m.x);
  running_min.y = min(running_min.y, compass.m.y);
  running_min.z = min(running_min.z, compass.m.z);

  running_max.x = max(running_max.x, compass.m.x);
  running_max.y = max(running_max.y, compass.m.y);
  running_max.z = max(running_max.z, compass.m.z);
 
  Serial.print("M min ");
  Serial.print("X: ");
  Serial.print((int)running_min.x);
  Serial.print(" Y: ");
  Serial.print((int)running_min.y);
  Serial.print(" Z: ");
  Serial.print((int)running_min.z);

  Serial.print(" M max "); 
  Serial.print("X: ");
  Serial.print((int)running_max.x);
  Serial.print(" Y: ");
  Serial.print((int)running_max.y);
  Serial.print(" Z: ");
  Serial.println((int)running_max.z);
 
  delay(100);
}


After running the programm and looking at the serial output, I started moving the board in all sorts of direction, and after some time I got a stable output:
Quote
/* Output of Calibrate.ino
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  */


These values I entered in the programm "Heading.ino"
Code: [Select]
#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
 
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -548; compass.m_min.y = -378; compass.m_min.z = -547;
  compass.m_max.x = 248; compass.m_max.y = 413; compass.m_max.z = 314;
 
  /* Output van Calibrate
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  */
}

/* Oorspronkelijke code
void loop() {
  compass.read();
  int heading = compass.heading((LSM303::vector){0,-1,0});
  Serial.println(heading);
  delay(500);
}
*/

/*----------------------------------------------------------------------------
        1   2   3 4   5 6
        |   |   | |   | |
$--HDG,x.x,x.x,a,x.x,a*hh<CR><LF>
------------------------------------------------------------------------------

Field Number:
1. Magnetic Sensor heading in degrees
2. Magnetic Deviation, degrees
3. Magnetic Deviation direction, E = Easterly, W = Westerly
4. Magnetic Variation degrees
5. Magnetic Variation direction, E = Easterly, W = Westerly
6. Checksum */

char nmeastr[17]; // HCHDG,000.00,,,,*
int checksum;
char hs[6]; // 000.00

void loop() {
  strcpy(nmeastr,"HCHDG,");
 
  //dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER);
  compass.read();
  dtostrf(compass.heading((LSM303::vector){0,-1,0}), 5, 0, hs);
  //Serial.print("hs ");
  //Serial.print(hs);
  //Serial.println(); 
 
  strcat(nmeastr,hs);
  strcat(nmeastr,",,,,");
 
  //add a checksum
  checksum=0;
  for (int n=0; n < strlen(nmeastr); n++) {
    checksum ^= nmeastr[n];
  }
 
  Serial.print("$");
  Serial.print(nmeastr);
  Serial.print("*");
  Serial.print(checksum, HEX);
  Serial.println();
 
  /* 5hz == 200 */
  delay(500);
}


While running this programm, serial output showed the heading as NMEA-string. Having it away from all sorts of electroncs and put a normal (old fashion ;) ) compass pointing in the same directoin, and laying flat on the floor,, the output showed:
Quote
$HCHDG,    0,,,,*5C
$HCHDG,    2,,,,*5E
$HCHDG,    5,,,,*59
$HCHDG,    1,,,,*5D
$HCHDG,    3,,,,*5F
$HCHDG,    1,,,,*5D
$HCHDG,    2,,,,*5E
$HCHDG,    4,,,,*58
$HCHDG,    1,,,,*5D
$HCHDG,    2,,,,*5E
$HCHDG,    2,,,,*5E
$HCHDG,    0,,,,*5C
$HCHDG,    3,,,,*5F
$HCHDG,    1,,,,*5D
$HCHDG,    2,,,,*5E
$HCHDG,    3,,,,*5F
$HCHDG,    3,,,,*5F
$HCHDG,    3,,,,*5F
$HCHDG,    3,,,,*5F
$HCHDG,    2,,,,*5E
$HCHDG,    4,,,,*58
$HCHDG,    4,,,,*58
$HCHDG,    5,,,,*59


Allthough not very accurated, it points north with some variance. Not too bad, but room for improvement, I think.

As soon as I start turning the lsm303 up and down , still pointing in the same direction, I would expect to see the same heading however the output shows:
Quote
$HCHDG,   14,,,,*49
$HCHDG,   14,,,,*49
$HCHDG,   16,,,,*4B
$HCHDG,   12,,,,*4F
$HCHDG,   10,,,,*4D
$HCHDG,    6,,,,*5A
$HCHDG,    5,,,,*59
$HCHDG,    6,,,,*5A
$HCHDG,   10,,,,*4D
$HCHDG,    7,,,,*5B
$HCHDG,    9,,,,*55
$HCHDG,    8,,,,*54
$HCHDG,    9,,,,*55
$HCHDG,   12,,,,*4F
$HCHDG,   13,,,,*4E
$HCHDG,   11,,,,*4C
$HCHDG,   10,,,,*4D
$HCHDG,   11,,,,*4C


IMHO, this heading info is too inaccurate to use as a compass. The autopilot will move the boat from left to right, like a drunken sailor ;)

I'm not sure wether this is part of the lsm303 (in that case it is not a sufficiënt solution for a compass for an auto pilot) or filtering or something else is required to get a more stable output.

Many thanks, George
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: zencuke on Jun 04, 2012, 03:17 am

You can not use the kalman filter with only one input. Sorry but you need a gyro too, that is why it called "sensor fusion" :)
You could perhaps use some other kind of smoothing algorithm.
- Lauszus


This is not a true statement. I'm surprised no one picked up on it. A Kalman filter is a useful tool often used to to do sensor fusion. It is not a synonym for sensor fusion. It was originally invented for single measurement systems. The value of a Kalman filter over a regular filter is that it is focused on predicting a future state rather than just  estimating the current state.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 04, 2012, 04:17 pm
@zencuke
You are totally right, sorry if it has caused anybody problems!
You could actually try to use the Kalman code provided in the sketch and the just set the gyrorate at 0. I havn't tried it, but in theory it could be done, but you will probably have tune the noise covariances for it to work properly.

Thanks for your feedback!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kevinh4c on Jun 05, 2012, 12:57 am
Hello,

Please I have some questions about this gyro+acc

1. my arduino (ARDUINO UNO R3) works with a SPI communication (i think so :s) , and i think the gyro+acc board works with a I2C communication protocol. Do you think my arduino can fit with it ? Or do you think it can work with a I2C communication like maybe i'm wrong ?

2. I wanted to buy this IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU (http://www.sparkfun.com/products/10010) but in the link, it is said that the IMU is replaced by another one (IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345). I think it is quite the same but i want to be sure before buying it. Do you think your different equations to translate acc and gyro data in degrees is gonna work on the new board ?

3. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 05, 2012, 03:29 pm
Quote

1. my arduino (ARDUINO UNO R3) works with a SPI communication (i think so :s) , and i think the gyro+acc board works with a I2C communication protocol. Do you think my arduino can fit with it ? Or do you think it can work with a I2C communication like maybe i'm wrong ?

The Arduino supports both SPI and I2C at the same time. By default you don't use the SPI for your Arduino, it is programmed via UART using the bootloader. See the last section at the following page: http://arduino.cc/en/Guide/Environment#uploading. You only use SPI for programming if you are using a programmer (see http://arduino.cc/en/Hacking/Programmer).
You can also use SPI to communicate with sensors etc. See http://arduino.cc/en/Reference/SPI. A lot of sensors support both SPI and I2C. Since the name I2C is copyright protected by Phillips, the Arduino team is not allowed to call the library I2C, so instead they call it the Wire library. See: http://arduino.cc/it/Reference/Wire.

Quote

2. I wanted to buy this IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU (http://www.sparkfun.com/products/10010) but in the link, it is said that the IMU is replaced by another one (IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345). I think it is quite the same but i want to be sure before buying it. Do you think your different equations to translate acc and gyro data in degrees is gonna work on the new board ?

It will works just as well, there is even more functions when using a digital board - like tap sensing, free fall detection etc.
The reason why I started out by using analog sensors, were because I didn't need the increased functionality that a digital IMU offers, as I wanted to build a balancing robot (https://github.com/TKJElectronics/BalancingRobotArduino) since I only wanted to calculate the angle of the robot, I just bought a analog one. But I also got a cheap digital IMU to mess around with.
It takes a bit more of work to get a digital sensor working, but there is plenty of example code for all kinds of IMU's so even a beginner could get it working in a couple of hours.

Quote

3. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Yes, it's pretty simple, just devide the reading (minused the offset) by 14.375 to convert it to º/s.
I will upload a alternative version of the code using a digital IMU instead in a couple of hours to get you started!

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 05, 2012, 04:04 pm
@kevinh4c
The code for a digital IMU can now be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/Digital_IMU

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aizzat on Jun 18, 2012, 06:43 pm
This post really help my understanding. Thank a lot!!! Keep it up!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kevinh4c on Jun 22, 2012, 05:11 am
Thanks So much Lauszus For your HELP
I'll look after all this :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kevinh4c on Jun 25, 2012, 08:11 pm


Quote

3. I Also saw in the sensor's datasheet of the new board that the sensivity is 14.375 LSB/º/s... It's not the same unit as the sensivity of the old one (3.33mV/º/s)... Do you have an idea of what i got to do ?

Yes, it's pretty simple, just devide the reading (minused the offset) by 14.375 to convert it to º/s.
I will upload a alternative version of the code using a digital IMU instead in a couple of hours to get you started!



So i just got to do this operation for the digital GIRO ?

gyroRate = (gyroAdc-gryoZero)/14.375 ?

Thanks

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: RomanF on Jun 29, 2012, 11:13 pm
Hello Lauszus, I greet the other Builders can someone help me cause even Lauszus unreachable!

(Translated by  Google Interpreter) My English is very very weak


I'm very fascinated by your robot. Its stability captivated me. My dream is to also perform similar trinket.

Much I ask you to kinda helped me by my project.


I just finished assembling Balance Robot.
Because I do not even ACC and Gyro (Analog) I can not even begin your trial.
All Sensor Signal (GYRO and ACC) from A/D Converter set to ca. 1,65V

How do I connect the power supply is buzzer Arduino seems a 100ms tone.
Without connected sensor ACC and Gyro the outputs 11,12, 20, 21.32, and 33 are High (Is this a good sign?)
After switching the PS3 controller,  Press Home Key(PS KEY) BT LED indicates the connection.

Nothing pressing the keys does not cause any reactions mentioned above, get out.
I Send my configurations for my Arduino 2560. It  my configuration is okay?

(USB is PIN 9 and 10)



The signals from the sensors.
Do I understand correctly:
bending forward and gives a minus ACC and GYRO.
slope backwards gives plus ACC and Gyro
If the robot is standing upright is a sensor "Z" gives a minus

You have a way to check the accuracy of encoder connection? (Trick)





Cordial greetings
Roman




#define leftA PIND0 //   BEI ATMEGA2560 Pin 21            PD0 - pin 0

#define leftB PIND1 //   BEI ATMEGA2560 Pin 20            PD1 - pin 1

#define leftPWM PINB5 // BEI ATMEGA2560 PB.5 PIN 11       PB1 - pin 9 (OC1A)



/* Right motor */

#define rightPort PORTC

#define rightPortDirection DDRC

#define rightPwmPortDirection DDRB



#define rightA PINC4 //    BEI ATMEGA2560 Pin 33         PC4 - pin A4

#define rightB PINC5 //    BEI ATMEGA2560 Pin 32         PC5 - pin A5

#define rightPWM PINB6 //  BEI ATMEGA2560 PB.6  PIN 12   PB2 - pin 10 (OC1B)



/* Encoders */

#define leftEncoder1 2

#define leftEncoder2 4

#define rightEncoder1 3

#define rightEncoder2 5



volatile long leftCounter = 0;

volatile long rightCounter = 0;



/* IMU */

#define gyroY A0

#define accY A1

#define accZ A2

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: timbopoise on Jul 16, 2012, 09:06 am
Hi,
      I've been working with two different IMUs, Chr-UM6 from CH Robotics and 3space TSS-EM from Yost Engineering.  I'd like to estimate the acceleration bias for x, y, and z so I can subtract it out.  For example, when I move my IMU up from the bench surface and then return it to a rest position the acceleration does not go back to the same value, or at least not in a reasonable time.  Most of the  kalman filters I've seen implemented in code appear to focus on gyros and attitude.

     Does anyone know where I might find a project that has a kalman filter for estimating bias?

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 16, 2012, 01:05 pm
@timbopoise
It sounds a bit odd that your accelerometer value doesn't get back to the same value - all my IMU's get back to nearly perfect the same.
You could actually just rewrite the kalman filter, as it would be possible to only use the accelerometer or you could use a digital low-pass filter to filter out the noise from the accelerometer.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 13, 2012, 12:33 am
Loving this guide! My project is quite different as I'm just sending out raw data and doing the actual math/crunching/calibration in Max/MSP, but the 'plain english' description of everything on the first post has been invaluable!

Now I'm trying to calibrate my magnetometer data (I have a 9DOF setup) but I can't figure out how to get to a useful number.

I'm using an HMC5883L (3-axis magnetometer)(https://www.sparkfun.com/products/10530) coming in over i2c, so it's 12-bit ADC. Looking at the datasheet the sensitivity is a massive range. Here is what it says:



I'm powering the HMC5883L breakout board with the 3.3v from my Arduino board (ArduIMU v2+)(https://www.sparkfun.com/products/9956). On the sparkfun page it lists the resolution as "5 milli-gauss resolution". So am I supposed to get the digital resolution from the above spec?

(nevermind on that part, I figured out the math).

So with this being 12-bit data, should my first formula be this:
sensitivity/3.3* 4095

0.0035958333/3.3*4095 = 4.46209814


So following on I should have

magRate = (magAdc-magZero)/4.46209814


Looking good so far. But now I'm not sure what type of number I'm looking at now, and if it needs further conversion. Do I need to calculate it against the previous value (like a gyro)?

?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 14, 2012, 02:35 am
@kriista
Actually you don't have to use the sensitivity at all, you can simply use atan2 like so:
Code: [Select]

atan2(raw.YAxis, raw.XAxis)


I don't have any experience with magnetometers, but have a look at this guide: https://www.loveelectronics.co.uk/Tutorials/8/hmc5883l-tutorial-and-arduino-library

They even wrote a library for the HMC5883L: https://www.loveelectronics.co.uk/Download/Arduino%20Library%20for%20HMC5883L/aHR0cDovL2M0ODc1My5yNTMuY2YzLnJhY2tjZG4uY29tL0xvdmVFbGVjdHJvbmljc19ITUM1ODgzTF9BcmR1aW5vTGlicmFyeS56aXA%3d

Regards
Lauszus

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 14, 2012, 03:03 am
That's a handy guide! Having a look now.

So would I do that for each pair? (X/Y, Y/Z, Z/X)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 14, 2012, 03:11 am
@kriista
No if you only need to know the heading while the magnetomer is in a horizontal position, that is all you need!
But if you are going to tilt it or if it's not perfectly flat, you have to "tilt compensate" it using a accelerometer (and a gyro). You can use both a accelerometer and a gyro to get better results, as you can then use the output from the Kalman filter.

See this guide for more information: https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 14, 2012, 03:21 am
Yeah I saw that.

I've got all 3 (accel/gyro/mag), so that's the direction I'm going.

This would be ideal:
http://www.youtube.com/watch?v=fOSTOnQzZCI

The academic paper is HARD to read through, and the straight code isn't directly compatible since I'm doing everything in (computer) software, so it's a matter of extracting the math from the code/paper and applying it elsewhere.

For the moment I have 6 streams (accel/gyro) coming in nicely calibrated (thanks to your guide), but in the end I just watch the absolute angles.

Speaking of that, when you do sensor fusion (like your kalman code, or this marg example) what is the end result?

So with kalman filter you get 3 values (pitch/roll/yaw?) but more stable due to the fusion?
If you throw in the magnetometer do you end up with 3 values at the end also, but super stable and non drifting? Or does it produce quaternions?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 14, 2012, 03:37 am
You can use his C# implementation of his code: http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms. If you are not running Windows, then see: http://monodevelop.com/ which will allow you to build it on Linux and Mac OS X as well.

The problem when using euler angles is that you wouldn't get any precise measurements, when you tilt the device too much, try for instance to tilt the IMU 90 degrees for instance at pitch, you can immediately see that roll doesn't make sense anymore as it's suddenly yaw (this is called gimbal lock: http://en.wikipedia.org/wiki/Gimbal_lock).
So instead of using euler angles you have to use something called quaternions, I won't try to explain what it is since I have never taken the time to read through the theory, as I have never needed to know the exact position of all the axis, as I have always used the Kalman filter for my Balancing robot: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/.
You can find more about quaternions in the following video: http://youtu.be/C7JQ7Rpwn2k and in the first link in the post.
Also take a look at: http://en.wikipedia.org/wiki/Gimbal_lock#The_quaternion_solution and of course: http://en.wikipedia.org/wiki/Quaternion

So in short, if you want something like in the video above, you shouldn't be using euler angles.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 14, 2012, 04:50 am
That google video is great!

So in watching that I think I've come to a couple of conclusions.

The first one is that it might be best to crunch my numbers back on the Arduino. My original thinking in sending out raw data is that I'm more comfortable with programming on the computer (Max/MSP) than I am in Arduino, and it's much easier to test/adapt/modify in that environment. I also thought it would be faster since my computer CPU is significantly faster than an Arduino.

I didn't account for the bottleneck of serial communication through. So all my fusion stuff will happen potentially displaced by serial latency (unless I'm incorrect in thinking that data travelling across a serial pipe is slower than data in a void loop circling around and around).

That's a big change from what I'm doing now, and I'm in the middle of a project based around this so I'm not gonna 'jump ship' just yet (and just use the 6axis data I have).

It also looks like quaternions is in fact what I'm after (and what that code does).

Now it's just a matter of how much of that code can live in Arduino land (and how much is based on their hardware/protocol).

The final thing is that I'm wondering if the way I have my sensors oriented is a problem. You can see my boards here in an old photo when I had first put them together.



I have the magnetometer directly above the MCU. It is pointing in the correct direction, but I don't know if having something directly below it (a couple millis as there's poofy double-sided tape) is a problem.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 14, 2012, 05:03 am
Actually the comments in the video indicate that what we're seeing is the onboard fusion (on the right) and the raw data being sent to the computer for fusion there and both look good/tight. This is a bluetooth device so I don't know what kind of data rates that has for the raw data. The fastest I've been able to run my XBees is 57600, without it crashing (115200 worked for 2 seconds then stopped).
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Battousai on Aug 16, 2012, 08:07 am
Hi Lauzus, first of all i would like to thank you for the information you post. I think your post is just what i need for mi project. I need to make a control system for a camera roll-tilt mount, using a 5DOF(ADXL335 and IDG500) IMU and two servomotors. Here is the board i'm using https://www.sparkfun.com/products/9268 (https://www.sparkfun.com/products/9268).

Here are the datasheets from the gyros and the accelerometer.
http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf (http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf)
http://www.analog.com/static/imported-files/data_sheets/ADXL335.pdf (http://www.analog.com/static/imported-files/data_sheets/ADXL335.pdf)


I have upload your code IMU6DOFVer3 and change the pins configuration to that 5DOF. For my project i don't need to get 360 degrees resolution, so I'm using the part from the previous version to calculate the AccXangle and Yangle ( i mean i'm calculating the force vector and then using acos function).

When i upload your code, the printed results for Xangle are the ones for the Yangle and viceversa. I think it's because the orientation of the IC's (specifically the IDG500). I looked to the 6DOF IMU you use and notices that the gyro x axis from yours  is the gyro y axis of my 5DOF. A similar thing happens with the gyro y axis of your 6DOF. I've changed the pins  Gx for Gy, but i'm still getting the same problem.

I've been stuck in this for 2 days. I really hope you could help me.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: freak174 on Aug 16, 2012, 11:00 pm
Hello,

First of all, thank you !

I took a look into ur  code (for I2C) u provided in a reply and put it together with another code.

ZeroValues > I did not take these values from the datasheet, I just ran a script with raw values and took the average values when the sensor was lying on the floor.
GyroSensitivity > I actually dont know which one I should take from the datasheet, but Ive tried everyone (there are 4 levels).


These are the values I get when the sensor is lying on the floor
Quote
343.26   136.31   
344.40   126.77   
345.49   117.89   
346.50   109.65   
347.41   101.99   
348.27   94.83   
349.07   113.36   
349.82   105.43   
350.48   98.05   
351.12   91.18   
351.71   84.80   
352.27   104.04   
352.78   96.76   
353.25   89.98   
353.69   83.68   
354.12   77.82   
354.51   72.36   
354.87   67.28   
355.23   87.76   
330.36   106.78   
332.41   99.32   
334.33   92.38   
336.12   85.92   
337.78   79.92   
339.31   74.32   
340.73   69.13   
342.06   64.28   
343.30   59.78   
344.46   55.58   
345.50   76.87   
346.49   96.67   
322.25   115.07   
324.85   107.02   
327.25   124.71   
329.51   115.98   
331.62   107.86   
333.54   100.32      


And here is the code

Code: [Select]
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;

unsigned long timer;

double zeroValue[5] = {950, -400, 13500, -100, -500};

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;

double compAngleX = 180;
double compAngleY = 180;


void setup() {

   // join I2C bus (I2Cdev library doesn't do this automatically)
   Wire.begin();
   Serial.begin(115200);

   // initialize device
   Serial.println("Initializing I2C devices...");
   accelgyro.initialize();

   // verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");


  timer = micros();
}

void loop() {
   // read raw accel/gyro measurements from device
   //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

   accelgyro.getRotation(&gx, &gy, &gz);
   
   double gyroXrate = -((gx-zeroValue[3])/131); //Change the sensitivity after your sensor
   gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
   
   double gyroYrate = ((gy-zeroValue[4])/131);
   gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Without any filter
   
   
   ///////////////////////////
   //The acc X and Y angle///
   //////////////////////////
   accelgyro.getAcceleration(&ax, &ay, &az);
   
   double accXval = ax-zeroValue[0];
   double accZval = ay-zeroValue[2];    
   double accXangle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
   
   double accYval = ay-zeroValue[1];
   accZval = ay-zeroValue[2];    
   double accYangle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
   
   //////////////////////////
   //////////////////////////
   
   compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle);
   compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

   timer = micros();
   
   
 Serial.print(compAngleX);Serial.print("\t");  
 Serial.print(compAngleY); Serial.print("\t");
 //Serial.print(timer); Serial.print("\t");
 Serial.print("\n");
   
  delay(10);


   
}



Edit:
Im using a sensor called MPU6050.
Datasheet can be found here:
http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Components/General%20IC/PS-MPU-6000A.pdf (http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Components/General%20IC/PS-MPU-6000A.pdf)

When I move it like one mm to another direction it seems to be stable, I guess there is a point where everything gets screwd up..
Quote
2.51   1.53   
2.49   1.50   
2.48   1.49   
2.48   1.49   
2.47   1.49   
2.47   1.48   
2.46   1.46   
2.47   1.44   
2.49   1.41   
2.51   1.37   
2.52   1.38   
2.54   1.38   
2.51   1.38   
2.51   1.39   
2.50   1.41   
2.48   1.40   
2.46   1.38   
2.48   1.39   
2.49   1.38   
2.51   1.39   
2.50   1.38   
2.51   1.39   
2.50   1.40   
2.49   1.39   
2.50   1.40   
2.49   1.42   
2.46   1.43   
2.45   1.41   
2.44   1.41   
2.47   1.40   
2.44   1.42   
2.46   1.43   
2.47   1.46   
2.44   1.45   
2.44   1.46
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 22, 2012, 04:30 pm
@kriista
I would like to know what his loop rate is, as I don't think he can do much else than printing the values as there is a lot of calculation going on. But you can always try to use a more powerfull device like the mbed (http://mbed.org/). I have used it in the past and I really like them as well.

I don't think it's a problem that you have just stacked the boards. My only concern is that the airflow under the magnetometer is really bad so the heat from the ICs can't get away, but since a gyro or accelerometer doesn't produce much heat I really don't think it's a problem :)

My XBees work just fine at 115200 baud. I use the normal ones with onboard antenna.

@Battousai
Take a look at the newest code at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. What I normally do is that I just calculate the unfiltered angle using both the gyro and accelerometer so I'm sure that the orientation and sign is the same and then after that I use the Kalman filter to calculate the angle.

@freak174
I don't have much experience with the MPU6050, but you should have a look at this library for Arduino: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
Actually it looks like you are already using it. Try this example sketch: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino and uncomment the following line to print pitch, roll and yaw: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95

I also use the same approach as you to find the zero value. For instance take a look at my balancing robot source code: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L317-334
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 22, 2012, 05:56 pm
The latency in that video looks close to nil so the loop rate must be pretty low. I really don't know what I'm doing to overflow the buffer, or wherever my problem is happening.

Would it matter that I don't have a delay() in my Arduino code at all? I'm only polling for data (on the computer) every 10ms, so perhaps I should throw a delay(5) in there for good measure?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 24, 2012, 12:45 am
@kriista
You can always try to add a delay and see if it helps. Why don't you just send the data as fast as possible. I'm sure the computer can handle it? :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kriista on Aug 24, 2012, 04:47 am
I'm doing that but sometimes it craps out, which is weird as I'm using serial call-response, so I shouldn't be asking for data until it's ready for it.

Sometimes it works fine for over an hour and other times it drops after a couple of minutes.

Stone cold bummer.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: julio79 on Sep 14, 2012, 10:55 am
@freak174

I'm sorry .. I use the same sensor.

How did you find these values?

double zeroValue[5] = {950, -400, 13500, -100, -500};

you can edit your skecth?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: freak174 on Sep 14, 2012, 08:40 pm

@freak174

I'm sorry .. I use the same sensor.

How did you find these values?

double zeroValue[5] = {950, -400, 13500, -100, -500};

you can edit your skecth?


Hello! I dont have the sketch no more, I partly gave up on this sensos hehe.
However, if you look at Lazarus sketch it is basically the same https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L317-334 (https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L317-334)

But Id recommend you to use this sketch instead as it is made for your sensor https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95 (https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino#L95)

When I used the later mentioned sketch I got it to work with the yaw,pitch,roll values but when I try to combine it to work with a servo I get fifo overflow and the code gets screwd up.

cheers
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: APH3X2N on Sep 26, 2012, 06:05 pm
This is awesome, and probably just what i need!

I'm planning to create a gyroscopic platform on which i will mount a GoPro camera or a Contour camera. I will be mounting this onto a motorbike on an actual race track, so it will go through some pretty intense forces. Then i want to compensate the camera angles using e few servos. I only need to compensate for the pitch and roll at this time.

I have experience with arduino's and the coding but im not familiar with the gyro and accelerator sensors.

I do have a few questions:

Offcourse the first question is; considering the forces applied on the sensors by putting it on a motorbike. Will this still work?

and; is using a gyro and accelero enough to fully remove the drift or will i also need the magnetometer (i need the platform to remain absolutely level to the human eye)?

and; are the resulting values usable to compensate using servo's?

i will probably be using this board with my arduino:

https://www.sparkfun.com/products/10121

Thanks in advance,

Niels
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 28, 2012, 12:19 am
@APH3X2N
It should work perfectly fine if you can find some servos that are strong enough to handle force.
You don't need to use a magnetometer an accelerometer and gyro should be fine.

I actually got some example code for that IMU: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino

Also take a look at this project a guy posted at this post a while ago: http://arduino.cc/forum/index.php/topic,58048.msg508589.html#msg508589
And his post as well: http://arduino.cc/forum/index.php/topic,68755.0.html

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: APH3X2N on Sep 29, 2012, 11:21 pm
@Lauszus

Thanks for the reply!

And offcourse for the samples.

I''ll be using some pretty strong servos which i'll be using inside brackets to hold the force and weight.

Thanks,

Niels
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: nathan_kia on Oct 20, 2012, 06:55 pm
Hi, Sparkfun doesn't sell https://www.sparkfun.com/products/10010? any more.
it's been replaced by https://www.sparkfun.com/products/10121?.

is your code compatible with the new product ? if yes what modifications should I make .
Thank you
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 21, 2012, 01:44 pm
Take a look at this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/ITG3205_ADXL345, it's for a ITG3205 and a ADXL345. I know the gyro is a bit different, but it should work just fine with your as well.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: totyforever on Oct 23, 2012, 04:25 pm
Hallo Lauszus, Thanks so much for this great blog. I need ur help with something. I'm using itg 3200 and bma 180, I applied your code and it works fine but when rotating around roll for exaple with 90 degrees, the output reading is 70 and the same for pitch although before that, i was applying a comp filter (explained by starlino) and it was giving an output of 90 when rotating with 90 degrees. Do you have any idea what might be causing that?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 23, 2012, 07:09 pm
@totyforever
Try this code I wrote: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino and tell me if that works.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: totyforever on Oct 24, 2012, 03:15 pm
Man, you are great :) , the problem was in this function:-
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));
accXangle = acos(accXval/R)*RAD_TO_DEG-90;
accYangle = acos(accYval/R)*RAD_TO_DEG-90;

I think it's not accurate or something as I was applying it in my old code and instead of (accXval/R), it was (Rxest/R) where Rxest is calculated using both acc and gyro readings. However, after I changed it with the one u sent me in the previous post, it worked so fine. Thank you so much.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 24, 2012, 03:37 pm
@totyforever
Nice. You welcome :)
atan2 will also give you 360 degrees resolution!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: totyforever on Oct 24, 2012, 03:53 pm
Yup I noticed that :), but shouldn't I use the acc sensitivity in this function and for the Y angle as well :
double getXangle() {
double accXval = Accx-zeroValue[0];
double accZval = Accz-zeroValue[2];
double angle = (atan2(accXval,accZval))*RAD_TO_DEG;
return angle;

I mean would be like this: (4095 = sens in 2g range)

double getXangle() {
  double accXval = (Accx-zeroValue[0])/4095;
  double accZval = (Accz-zeroValue[2])/4095;
  double angle = (atan2(accXval,accZval))*RAD_TO_DEG;
  return angle;
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 24, 2012, 07:13 pm
It does not make any difference.

For instance take a look at this picture: http://upload.wikimedia.org/wikipedia/commons/7/7e/Trigonometry_triangle.svg
You can get the angle using atan(a/b).

For instance if a=100 and b=500, then the angle would be:
atan(100/500)=11.31deg
Then if you divide both of them with the same constant (this could be the sensitivity) lets say it's 100. Then a=100/100=1 and b=500/100=5, so:
atan(1/5)=11.31deg

atan2 is used to get 360 resolution and is dealt within the function. See this wiki page: http://en.wikipedia.org/wiki/Atan2#Definition
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: totyforever on Oct 24, 2012, 09:16 pm
yes got it :) Thanks Lauszus a lot for ur efforts, wish u all the best
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 24, 2012, 09:32 pm
@totyforever
Your welcome and you too :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: lparisi on Nov 09, 2012, 10:26 am
Hi mate, I've seen you videos on Youtube and thats how I've got here!

I need a 2 axis gyro and 2 axis accelerometer for what I need, wich board do you suggest?

Thanks!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Nov 09, 2012, 07:26 pm
@lparisi

One of the most popular at the moment is the MPU-6050 from InvenSense: http://www.invensense.com/mems/gyro/mpu6050.html. I got a one as well and the performance is pretty good and you can also do a lot of advances stuff with it.
There is a breakout board available form Sparkfun: https://www.sparkfun.com/products/11028 and a lot of other places like ebay.

Here is a basic example sketch I wrote: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050

If you need to work with the onboard DMP, I recommend the following library: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050




Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Swoop132 on Dec 24, 2012, 04:45 am
What is the Arduino that you used for this project and do you think that there would any problems with coding the gyro/accelerometer module that you used for a Nano v3.0?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Dec 24, 2012, 12:16 pm
@Swoop132
I originally used a Duemilanove, but I have since used the same code for Arduino Uno, Mega etc.
You can use it with a Nano just fine.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: futureM on Jan 10, 2013, 07:08 am
Hi Lauszus,

Like the many others, I got my IMU working by studying at lot of your hard work, just wanted you to know that I really appreciate it!
I have a question, with the project I'm working on - I need to filter out the accel data that is affecting the
Kalman output such as when the IMU is under accel in any of the 3 axis. For example while placing the IMU
on a rotating platform, the addition of of the X or Y accel vectors start to affect the roll
and pitch angles. What do you recommend to eliminate these bias errors for when the IMU is not stationary and under accel/decel?

-futureM
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 10, 2013, 08:33 pm
@futureM
That's the whole purpose of the Kalman filter. If you are not satisfied with the performance as it is right now, then you have to tune the values.
See my blog for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: triplenox on Jan 15, 2013, 11:53 am
Hi Lauszus,

I'm working with the Quaternion Values getting from the DMP of the MPU6050. i've some problems to get a full 360° Range.
Maybe you have a hint to convert the DMP Quaternion Values to Degrees. Normally the calculation is based on the Sensitivity Scale Factor for both Gyro and Accel. I'm correct?
But i'm getting one fusion Value of the Gyro and Accel. So i don't now how to convert this to a 360° Range ....

Why do you not use the DMP Values like Quaternion?
Is it better to work with raw Vaules and Kalman Filter?

With atan2 i should get the 360°:
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print((atan2(q.y,q.z)+PI)*RAD_TO_DEG);

but if i move the MPU6050 90° i get on the Serial only 70°

many Thanks!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 15, 2013, 05:50 pm
@triplenox
I have never used the DMP on the MPU-6050 before as I thought it were a bit overkill as I usually only need the angle in one of the axis.

You can't use atan2 or anything like that on Quaternions!

To use Quaternions see the following recourses:
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aziz-qessate_arduino on Jan 19, 2013, 01:30 am
hello , i'm working on the same subject , and it's my first time i use the ARDUINO uno AND the IMU 6050s ,
i'm a beginner, now i try to finish the mechanical par ,
it'is possible to give my the full code, because it's a subject that i must give to the school , and the mechanical part it took a lot of time to built,
please i wayit for somewone to help my.
and thank you four your solidarity
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 20, 2013, 02:33 pm
@aziz-qessate_arduino
Hi,

I can't write all the code for you, but for now take a look at this example code I wrote: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ezikiel12 on Jan 28, 2013, 01:42 am
So here's the issue I'm running into. I'm using Lauszus' code to try and set up a self stabilizing camera system. Although what I am finding is that the kalAngleX value is greatly effected by any acceleration in the z axis. If I shake the MPU6050 up and down the servo just goes crazy. When I hook it up to the bike the bumpiness of the road basically ruins it. I though that was the whole point of kalman filtering was to minimize this?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: michinyon on Jan 28, 2013, 01:27 pm
You have run into the fundamental problem,  that no scheme based on accelerometers can
distinguish between movement acceleration and gravity acceleration.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 28, 2013, 02:12 pm
@ezikiel12
You have to tune the Kalman filter. See my blog post on how to do so: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
The problem is that you are trusting the measurements from the accelerometer too much.

Or you could try to tune a complimentary filter to start with.

@michinyon
That's the whole point of the filter ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mamette on Jan 28, 2013, 02:33 pm
How if i connect IMU to my Arduino Board using I2C ? How to read and filter it?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 28, 2013, 02:37 pm
See this code I wrote a while back: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino for the MPU6050

And here is some code for the ITG3205/ITG3200 gyroscope and ADXL345 accelerometer: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/ITG3205_ADXL345/ITG3205_ADXL345.ino
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: michinyon on Jan 28, 2013, 05:59 pm
Quote
That's the whole point of the filter


Well no.  The whole point of the filter is dealing with noise.  The problem of distinguishing true acceleration from the effect of gravity on the sensor remains.  I've yet to see a convincing implementation of the Kalman filter approach which considers more than one dimension.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: jjspierx on Jan 28, 2013, 06:16 pm

Quote
That's the whole point of the filter


Well no.  The whole point of the filter is dealing with noise.  The problem of distinguishing true acceleration from the effect of gravity on the sensor remains.  I've yet to see a convincing implementation of the Kalman filter approach which considers more than one dimension.


You should look at the FreeIMU's implentation of MARG orientation fusion algorithm.  I am using a modified version of this and it seems to work well.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: longlongvn on Jan 28, 2013, 09:50 pm
@Lauszus
Thanks for your guide.
I'm working with an MPU6050 for my balancing robot project. But the model required yaw measurement. I wonder if the MPU6050 is capable of measure the yaw accurate enough or it needs additional magnetometer. Please help :D
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: jjspierx on Jan 28, 2013, 10:17 pm

@Lauszus
Thanks for your guide.
I'm working with an MPU6050 for my balancing robot project. But the model required yaw measurement. I wonder if the MPU6050 is capable of measure the yaw accurate enough or it needs additional magnetometer. Please help :D


It can do short term yaw measurements just fine, but it drifts over time.  So if you just need to know incremental yaw measurements during yaw events it will work fine. However if you are using to it maintain a specific orientation for extended periods of time, it will not work well unless you integrate a magnetometer.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: longlongvn on Jan 29, 2013, 03:16 pm
thank you very much. I'm gonna try integrating an magnetometer to my IMU.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: jjspierx on Jan 29, 2013, 03:51 pm
The MPU-6050 supports auxillary SCL and SDA.  My GY-521 breakout board broke the pins out and called them XCL and XDA.  If the magnetometer you choose uses I2C, you can plug it directly into the MPU-6050. 

I used a modified FreeIMU library to get mine all working. 
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: wturn on Jan 29, 2013, 09:28 pm
@Lauszus Sir I have a L3g4200d. It just a  3 axis Gyro sensor. My problem is to how to use kalman filtering to Arduino Uno. And I want to get the reading values to a degree(x,y,z). To able to control my Servo. Thanks in Advance.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aziz-qessate_arduino on Jan 30, 2013, 12:30 am
thank you for your help ,that's kind of you
the code that you give me, I'll bring in the card and it will arduino controller actuators? is that the code is complete?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 31, 2013, 03:16 pm
@michinyon
If true acceleration is not noise in the system, what is it then?

@longlongvn
As jjspierx pointed out you will need a magnetometer as well if you need a precise yaw measurement.

@wturn
To translate the gyro rate in to an angle simply multiply the reading by the delta time as I described in the blog post, but it will properly drift a lot if you use a gyro only.

@aziz-qessate_arduino
What do you mean? Please describe it in more detail.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ezikiel12 on Feb 01, 2013, 04:05 am
Lauszus do you know what specifically I could change within your code to further minimize the noise from shaking the sensor in the z axis. There has to be something that can be done. Iv'e read through all your blogs on calibration and such but nothing is jumping out at me. You say I'm relying on the accelerometer values too much, but me being bad at math it doesn't jump out at me very quick exactly where I can adjust its reliance on the accelerometer or the gyro. It seems that the Q_angle, Q_bias variables are what I adjust and those correspond to the variance and the bias respectively from your blog. (http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/) The filter will filter out relatively small vibration, but not when the road gets really bumpy. If I shake the MPU6050 relatively strong up and down it just messes it slowly drifts the camera to one side or the other. It seems that no matter what I tune these variables too, it still doesn't work properly. Is there possibly something else that can be done with this chip, or are there other sensors that would be better suited for what I'm trying to accomplish. My gopro camera mount is soooo close to working properly.. Any extra help would be greatly appreciated. I'll continue working through your blog in the meantime.

It seems there is also a way to change the sensitivity of the gyro and accelerometer but I can't find any information on how to do that for a newbie like myself... =/
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ezikiel12 on Feb 02, 2013, 07:27 am
So basically I have limited the issue with the motorcycle gyro cam down to the accelerometers z value. What I did to help reduce issues with vibrations was filter the incoming values by just loading them into an array and averaging the values. By increaing the amount of values loaded into the array it helps smooth it more, but also slows down responsiveness. But for the application, the responsiveness isn't too important. Seems to be working pretty well so far.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 02, 2013, 12:19 pm
@ezikiel12
That exactly what I would have recommended or you could try to look at the difference between the current angle and the new angle from the accelerometer, if the difference is larger that a certain value, then decrease the value or even just drop it.

I'm actually working on a project with the MPU-6050 right now, but unfortunatly I can't show the full source code, but I will just copy paste the relevant sections:

This goes into setup:
Code: [Select]

Wire.begin();
i2cWrite(0x1B,0x00); // Set Full Scale Range to ±250deg/s
i2cWrite(0x1C,0x00); // Set Full Scale Range to ±2g
i2cWrite(0x19,0x27); // Set the sample rate to 200Hz
i2cWrite(0x6B,0x01); // PLL with X axis gyroscope reference and disable sleep mode

uint8_t buf;
i2cRead(0x75,1,&buf);
if(buf != 0x68) { // Read "WHO_AM_I" register
 Serial.println(F("Error reading sensor"));
 while(1);
}

delay(100); // Wait for sensor to get ready
uint8_t data[4];
i2cRead(0x3D,4,data);  
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;

kalman.setAngle(accAngle); // Set starting angle


And put this in your loop:
Code: [Select]

uint8_t data[8];
while (!i2cRead(0x3D,8,data));
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
gyroX = ((data[6] << 8) | data[7]);

accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
gyroRate = (double)gyroX/131.0;

pitch = kalman.getAngle(accAngle, gyroRate, (double)(micros()-timer)/1000000.0); // Calculate the angle using a Kalman filter
timer = micros();


And these are the i2cRead and i2cWrite functions:
Code: [Select]

void i2cWrite(uint8_t registerAddress, uint8_t data) {
 Wire.beginTransmission(IMUAddress);
 Wire.write(registerAddress);
 Wire.write(data);
 Wire.endTransmission(); // Send stop
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t nbytes, uint8_t * data) {
 unsigned long timeOutTime = micros();
 Wire.beginTransmission(IMUAddress);
 Wire.write(registerAddress);
 Wire.endTransmission(false); // Don't release the bus
 Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
 for(uint8_t i = 0; i < nbytes; i++) {
   if (Wire.available())
     data[i] = Wire.read();
   else {
     timeOutTime = micros();
     while (((micros() - timeOutTime) < I2C_TIMEOUT) && !Wire.available());
     if (Wire.available())
       data[i] = Wire.read();
     else
       return 0; // Error in communication
   }
 }
 return 1;
}
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: michinyon on Feb 03, 2013, 03:01 pm
Quote
If true acceleration is not noise in the system, what is it then?


The accelerometer sensor reading is part of the measurement of the state of your system,  just
as the gyroscope sensor reading is.

So it should be part of the measurement vector,   not the input vector

If you have a system model like this

x(k) = F.x(k-1) + B.u(k) + noise
z(k)=  C.x(k) + noise

then any quantities you are actually able to measure to enable identification of the best estimate
of the state of your system,  x,   go into the z vector  not the u vector.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aziz-qessate_arduino on Feb 03, 2013, 11:05 pm
first i will thank you for your help , my project is the same one like the project of RIVELLO; so i need the all code please ,
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 05, 2013, 12:26 am
@michinyon
What do you mean? z is the measurement of the state - in this case the measurement is the angle measured using the accelerometer. The x in your equation has nothing to do with the estimate of the state. It is the state of the system which is not observable.

@aziz-qessate_arduino
As I have already wrote to you in my email. I don't have the code for the self stabilizing platform, as I didn't write it. But you could try to contact him here at the forum or youtube and ask for the code: http://arduino.cc/forum/index.php?action=profile;u=48118
You can find some of his code here: http://arduino.cc/forum/index.php/topic,68755.msg507679.html#msg507679
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ezikiel12 on Feb 05, 2013, 04:36 am
Thanks laz again. Very helpful, taught me quite a bit about I2C that I didnt understand in just a little snippet of code. Thanks to your help though I have successfully got it working extremely well. The biggest issue was not so much the bumpyness of the road, I was able to tune it out and use a band pass filter + some averaging to get that taken care of. It was the high frequency engine vibration that was killing it. The quadcopter guys have the same issues with there motors and sensors. I was able to completely fix this by mounting the system on the bike with some padding and velcro.

Check it out!!

http://youtu.be/G61yXgXGFyo

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 05, 2013, 11:44 pm
@ezikiel12
It looks really nice. Glad I could help :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Crossuuuuu on Feb 08, 2013, 10:55 pm
Hi,

someone can share the file with code for self positioning platform on e-mail ?

I would be gratefull for help
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 09, 2013, 05:16 pm
He simple added the following to my code: http://arduino.cc/forum/index.php/topic,68755.msg507679.html#msg507679

You can also send him a personal message if you like: http://arduino.cc/forum/index.php?action=profile;u=48118
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: richardtheboffin on Feb 11, 2013, 08:41 pm
Hi Lauszus,

I've been learning a lot from your code, thanks very much for putting it on here.

I have a board with single axis gyro with 3 axis accelerometer, analogue outputs.

I'm looking to find the Roll angle, so the single gyro board will need to be orientated up on its end so that the Roll axis goes through the middle of the gyro. The accelerometers with then be: X=up/down, Y=left/right, Z=forward/backward.

In this configuration which combination of the X,Y and Z accelerometers should I be using in the code?

I am guessing that it's the X and Y as the Z axis accelerometer is not effected by roll when the board is mounted on it's end

Your code uses X and Z and Y and Z in the arctan function, so I should use X and Y, correct?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 11, 2013, 11:38 pm
@richardtheboffin
It depends on your gyro - some have to be orientated up others don't.
Regarding which axis you should use on your accelerometer: I can't say for sure. It depends on how it's orientated. Either it's X and Z or Y and Z.
Try to modify this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/LPR530AL_%20LY530ALH_%20ADXL335/LPR530AL_%20LY530ALH_%20ADXL335.ino, so it's uses the correct sensitivity values for your setup.
Then afterwards you can easily see which angles should be fused together.
You might also need to invert some of the readings, as I have done here: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/LPR530AL_%20LY530ALH_%20ADXL335/LPR530AL_%20LY530ALH_%20ADXL335.ino#L51
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: richardtheboffin on Feb 12, 2013, 12:29 am
Hi Lauszus,

I have your code running ok with the single gyro and x,y,z accelerometers.

I'll dump the intermediate angle calculation values out and see if I can figure which pair I need to combine with the gyro.

Cheers,

Richard.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: richardtheboffin on Feb 12, 2013, 10:32 am
Hi Lauszus,

All sorted out now. I had everything coded correctly but missed the initial offset from zero for the down axis accelerometer due to gravity.

It works very well and rejects linear movements really well.

Just need to feed the angle into my motor controller now.

Thanks for everything!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aziz-qessate_arduino on Feb 12, 2013, 09:34 pm
please , can you share with us all the program?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: tensa on Feb 18, 2013, 05:03 pm
i was able to implement the filter thanks for this guide it was  really helpful...thank you very much
but i dont know how to relate these values to motor's rpm of a quadcopter so i will stabilize, can anybody post links to guides like this?

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: AntR on Feb 18, 2013, 05:41 pm

i was able to implement the filter thanks for this guide it was  really helpful...thank you very much
but i dont know how to relate these values to motor's rpm of a quadcopter so i will stabilize, can anybody post links to guides like this?




You will need to use a PID controller to give roll,pitch,yaw outputs when the orientation deviates from 0,0,0.  However,  I have used these filters on a quadcopter and they suffer from the problem of linear acceleration affecting the pitch and roll.  The filters are fast enough, smooth and very stable (calculation time was 80micros for twin Kalmans on an Arduino Due) but you'd probably be better off using Mahonys DCM or Madgwicks filter - they're less sensitive to linear acceleration but a bit more prone to noise (vibration from the motors).

You'll find plenty of links via Google for any of this stuff.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: dheerajp on Feb 18, 2013, 06:12 pm
i want to work with MPU-6050-Triple Axis Accelerometer Gyro can it be done
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: dheerajp on Feb 18, 2013, 06:30 pm
i want the same guide for MPU-6050-Triple Axis Accelerometer Gyro
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aziz-qessate_arduino on Feb 20, 2013, 01:52 pm
the same think , so my ,i'd like this idea and i would like  someone to share us the GUIDE of IMU 6050 and arduino
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 20, 2013, 02:44 pm
Hi,

The setup is very similar just note that if you are using an Arduino running at 5V you might damage the IMU as it's only running at 3.3V, but some breakout boards have a logic level circuit built-in. If not use have to use a logic level converter like this one: https://www.sparkfun.com/products/8745.

To connect the Arduino with the MPU-6050, you need to connect it as it says in the following comment: https://www.sparkfun.com/products/11028#comment-50b3c165ce395f484d000000.

Hopes this helps you out for now, as I havn't got time to write a guide at the moment.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: stunito0o0 on Mar 13, 2013, 06:48 pm
Hi Lauszus,
I make a little customisation of your source for digital IMUs so it can work with  Minimu-9 (http://www.pololu.com/catalog/product/1265) board , hope you don't mind.But there is something wrong here is what i get:
Code: [Select]

....(the board is on the ground and i don't touch it the whole time)
X: 180.00 Y: 180.01
X: 180.05 Y: 180.06
X: 180.22 Y: 180.14
X: 180.51 Y: 180.16
X: 180.82 Y: 180.11
X: 181.07 Y: 180.04
X: 181.42 Y: 179.92
X: 181.79 Y: 179.80
X: 182.22 Y: 179.65
X: 182.72 Y: 179.46
X: 183.23 Y: 179.28
X: 183.85 Y: 179.08
X: 184.54 Y: 178.80
X: 185.26 Y: 178.48
X: 185.99 Y: 178.02
X: 186.60 Y: 177.45
X: 187.54 Y: 176.95
X: 188.54 Y: 176.43
X: 189.64 Y: 175.72
X: 190.82 Y: 175.03
X: 192.13 Y: 174.37
......
X: 280.48 Y: 86.86
X: 280.43 Y: 86.66
X: 280.32 Y: 86.91
X: 280.19 Y: 87.02
X: 280.09 Y: 87.10
X: 279.97 Y: 87.32
X: 279.81 Y: 87.84
X: 279.71 Y: 87.88
X: 279.60 Y: 88.26
X: 279.51 Y: 88.09
X: 279.38 Y: 88.20
X: 279.27 Y: 88.09
X: 279.10 Y: 88.24
X: 278.96 Y: 88.26
X: 278.80 Y: 88.56
...(finally the X stopped growing and the Y stopped getting lower)
X: 278.61 Y: 89.11
X: 278.44 Y: 89.23
X: 278.29 Y: 89.41
X: 278.25 Y: 88.81
X: 278.07 Y: 89.24
X: 277.85 Y: 89.62
X: 277.68 Y: 89.88
X: 277.53 Y: 90.03
X: 277.36 Y: 90.22
X: 277.24 Y: 90.48
X: 277.12 Y: 90.17
X: 276.99 Y: 90.08
X: 276.90 Y: 89.87
X: 276.76 Y: 89.84
X: 276.61 Y: 89.97
X: 276.44 Y: 90.35
X: 276.26 Y: 90.62
X: 276.12 Y: 90.84
X: 276.02 Y: 90.76
X: 275.78 Y: 91.38
X: 275.67 Y: 91.39
X: 275.47 Y: 91.74
X: 275.32 Y: 91.92
X: 275.16 Y: 92.18
X: 275.07 Y: 92.02
X: 274.95 Y: 92.17
X: 274.83 Y: 92.30
X: 274.68 Y: 92.31
X: 274.52 Y: 92.53
X: 274.43 Y: 92.53
X: 274.27 Y: 92.90
X: 274.18 Y: 92.90
X: 274.03 Y: 92.95
..(and all over again)

and here is my source

Code: [Select]
#include <Wire.h>
#include <L3G.h>
#include <LSM303.h>

L3G gyro;
LSM303 compass;
double zeroValue[5] = {-200, 44, 660, -18.5, 52.3}; // Found by experimenting

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;

double compAngleX = 180;
double compAngleY = 180;

unsigned long timer;
/* Kalman filter variables and constants */
const double Q_angleX = 0.001; // Process noise covariance for the accelerometer - Sw
const double Q_gyroX = 0.003; // Process noise covariance for the gyro - Sw
const double R_angleX = 0.03; // Measurement noise covariance - Sv

double angleX = 180; // The angle output from the Kalman filter
double biasX = 0; // The gyro bias calculated by the Kalman filter
double PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
double dtX, yX, SX;
double KX_0, KX_1;

double kalmanX(double newAngle, double newRate, double dtime) {
 // KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
 // See also http://www.x-firm.com/?page_id=145
 // with slightly modifications by Kristian Lauszus
 // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and
 // http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
 dtX = dtime / 1000000; // Convert from microseconds to seconds

 // Discrete Kalman filter time update equations - Time Update ("Predict")
 // Update xhat - Project the state ahead
 angleX += dtX * (newRate - biasX);

 // Update estimation error covariance - Project the error covariance ahead
 PX_00 += -dtX * (PX_10 + PX_01) + Q_angleX * dtX;
 PX_01 += -dtX * PX_11;
 PX_10 += -dtX * PX_11;
 PX_11 += +Q_gyroX * dtX;

 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
 // Calculate Kalman gain - Compute the Kalman gain
 SX = PX_00 + R_angleX;
 KX_0 = PX_00 / SX;
 KX_1 = PX_10 / SX;

 // Calculate angle and resting rate - Update estimate with measurement zk
 yX = newAngle - angleX;
 angleX += KX_0 * yX;
 biasX += KX_1 * yX;

 // Calculate estimation error covariance - Update the error covariance
 PX_00 -= KX_0 * PX_00;
 PX_01 -= KX_0 * PX_01;
 PX_10 -= KX_1 * PX_00;
 PX_11 -= KX_1 * PX_01;

 return angleX;
}
/* Kalman filter variables and constants */
const double Q_angleY = 0.001; // Process noise covariance for the accelerometer - Sw
const double Q_gyroY = 0.003; // Process noise covariance for the gyro - Sw
const double R_angleY = 0.03; // Measurement noise covariance - Sv

double angleY = 180; // The angle output from the Kalman filter
double biasY = 0; // The gyro bias calculated by the Kalman filter
double PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;
double dtY, yY, SY;
double KY_0, KY_1;

double kalmanY(double newAngle, double newRate, double dtime) {
 // KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
 // See also http://www.x-firm.com/?page_id=145
 // with slightly modifications by Kristian Lauszus
 // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and
 // http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
 dtY = dtime / 1000000; // Convert from microseconds to seconds

 // Discrete Kalman filter time update equations - Time Update ("Predict")
 // Update xhat - Project the state ahead
 angleY += dtY * (newRate - biasY);

 // Update estimation error covariance - Project the error covariance ahead
 PY_00 += -dtY * (PY_10 + PY_01) + Q_angleY * dtY;
 PY_01 += -dtY * PY_11;
 PY_10 += -dtY * PY_11;
 PY_11 += +Q_gyroY * dtY;

 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
 // Calculate Kalman gain - Compute the Kalman gain
 SY = PY_00 + R_angleY;
 KY_0 = PY_00 / SY;
 KY_1 = PY_10 / SY;

 // Calculate angle and resting rate - Update estimate with measurement zk
 yY = newAngle - angleY;
 angleY += KY_0 * yY;
 biasY += KY_1 * yY;

 // Calculate estimation error covariance - Update the error covariance
 PY_00 -= KY_0 * PY_00;
 PY_01 -= KY_0 * PY_01;
 PY_10 -= KY_1 * PY_00;
 PY_11 -= KY_1 * PY_01;

 return angleY;
}
int readGyroX(void) {
 int data;
 data =gyro.g.x;

 return data;
}
int readGyroY(void) {
 int data;
 data = gyro.g.y;

 return data;
}
double getXangle() {
 double accXval = (double)readAccX()-zeroValue[0];
 double accZval = (double)readAccZ()-zeroValue[2];
 double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
 return angle;
}
double getYangle() {
 double accYval = (double)readAccY()-zeroValue[1];
 double accZval = (double)readAccZ()-zeroValue[2];
 double angle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
 return angle;
}
int readAccX(void) {
 int data;
 data = compass.a.x;
 return data;
}
int readAccY(void) {
 int data;
 data = compass.a.y;
 return data;
}
int readAccZ(void) {
 int data;
 data =compass.a.z;
 return data;

}


void setup() {
 Serial.begin(9600);
 Wire.begin();
 
 if (!gyro.init())
 {
   Serial.println("Failed to autodetect gyro type!");
   while (1);
 }

 gyro.enableDefault();
  compass.init();
 compass.enableDefault();
   timer = micros();
}

void loop() {
 gyro.read();
compass.read();

 double gyroXrate = (((double)readGyroX()-zeroValue[3])/14.375);
 gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
 
 double gyroYrate = -(((double)readGyroY()-zeroValue[4])/14.375);
 gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Without any filter
 
 double accXangle = getXangle();
 double accYangle = getYangle();


 compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle);
 compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
 
 double xAngle = kalmanX(accXangle, gyroXrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter
 double yAngle = kalmanY(accYangle, gyroYrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter
 
 timer = micros();
 

 Serial.print("X: ");Serial.print(xAngle);Serial.print("\t");
 Serial.print("Y: ");Serial.print(yAngle);Serial.print("\t");
 
 Serial.print("\n");
 
 delay(10);


}

Could you please take a look of my source and tell me what I am doing wrong.I want to thank you in advance for your time.
Stanislav.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 14, 2013, 12:42 am
@stunito0o0
It looks like you are reading the magnetometer (compass) and not the accelerometer.
Btw the newest version of the code can be found here: https://github.com/TKJElectronics/KalmanFilter.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: stunito0o0 on Mar 14, 2013, 10:31 am

@stunito0o0
It looks like you are reading the magnetometer (compass) and not the accelerometer.
Btw the newest version of the code can be found here: https://github.com/TKJElectronics/KalmanFilter.

Hi again, :)
I thought too that it might be the compass data I am reading so i opened the example for the "LSM303.h" libraly and here is the source:
Code: [Select]
#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
 Serial.begin(9600);
 Wire.begin();
 compass.init();
 compass.enableDefault();
}

void loop() {
 compass.read();

 Serial.print("A ");
 Serial.print("X: ");
 Serial.print((int)compass.a.x);
 Serial.print(" Y: ");
 Serial.print((int)compass.a.y);
 Serial.print(" Z: ");
 Serial.print((int)compass.a.z);

 Serial.print(" M ");  
 Serial.print("X: ");
 Serial.print((int)compass.m.x);
 Serial.print(" Y: ");
 Serial.print((int)compass.m.y);
 Serial.print(" Z: ");
 Serial.println((int)compass.m.z);

 
 delay(100);
}

According to the example  "compass.a.x","compass.a.y" and "compass.a.z" are for reading the acc data.I will really appreciate if you take another look of my code.I think that when i was customising your source i might have deleted something important.Thank you again for your time. :)
P.S. Now i rememberd that i deleted part of the source for getting the data of acc and gyro.For example "readAccX" function  was something like this:
Code: [Select]
int readAccX(void) {
 int data;
 data = i2cRead(adxlAddress, 0x32);
 data |= i2cRead(adxlAddress,0x33)<<8;
 return data;
}

and i didn't understand what  "data |= i2cRead(adxlAddress,0x33)<<8;" is for so i deleted it.
now it looks like this:
Code: [Select]
int readAccX(void) {
 int data;
 data = compass.a.x;
 return data;
}
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 16, 2013, 12:19 am
Sorry I was in a hurry when I wrote the first reply.
First of all I think it will be easier for you to if you modify this example: https://github.com/pololu/LSM303/blob/master/LSM303/examples/Serial/Serial.ino to your needs.

First of all you should NOT subtract the zeroValues, as they are specific for the IMU used in the example.
I think you can use your values directly, so you don't even have to subtract any zeroValue.
Take a look at this example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino

Also you assume that the gyro has the same sensitivity as the one used in the example which is most likely not the case (take a look at your datasheet)!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: stunito0o0 on Mar 16, 2013, 12:39 am
Hi again Lauzurus , :)
Thank you verry much for your answer ,but i am afraid that I won't need your source anymore.I just found out this link (https://github.com/pololu/MinIMU-9-Arduino-AHRS) it is a source for Arduino and MinIMU-9 board wich is exactly what i have.Once again I am really sorry for wasting your time. :) I hope you that you won't get angry at me. :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 16, 2013, 12:44 am
No problem. I'm just glad you got it working :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: psyoptica on Mar 16, 2013, 09:10 pm
Hey Lauszus!

I was hoping to learn something about using IMU effectively with the help of Kalman filtering as I came across this wonderful tutorial. The only problem I have is I have 2 separate gyro and accelerometer modules (L3G4200D and ADXL 335). How do I combine and use them according to your pin config.

gyro has pins GND, VIN, SCL, SDA, SDD, INT1, INT2 and accelerometer pins are VCC, GND,X,Y,Z,ST. Where do these pins go?



Thanks!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 16, 2013, 09:19 pm
@psyoptica
The gyroscope is a digital gyroscope so you need to connect it to the SDA and SCL on the Arduino. See the following page for more information: http://arduino.cc/en/Reference/Wire. Then connect VIN to 3.3V and GND to GND on the Arduino.

Your accelerometer is a analog one, so you need to connect X, Y, and Z to your analog inputs. And yet again connect VIN to 3.3V and GND to GND on the Arduino.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: psyoptica on Mar 16, 2013, 09:31 pm
Thanks! this really helps but aren't A4 and A5 analog input pins?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 16, 2013, 09:35 pm
@psyoptica
Yes they are, but pins can have several capabilities.

For instance you can also use any analog pin as a digital pin like so:
Code: [Select]

pinMode(A0,OUTPUT);
digitalWrite(A0,HIGH);


You can read more about it in the datasheet: http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet.pdf.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: psyoptica on Mar 16, 2013, 09:44 pm
Tell me if I have configured it correctly

Acc_Gyro          Arduino
3.3V       <-->   3.3V   
GND       <-->   GND
X           <-->   AN0
Y           <-->   AN1
Z           <-->   AN2
SDA        <-->   AN4
SCL        <-->    AN5 
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 16, 2013, 09:48 pm
@psyoptica
Yes that looks correct. But beware that if there is no logic level circuit on your breakout board, you might damage the IC. So get a logic level converter if you need to use if on the long run: https://www.sparkfun.com/products/8745.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: psyoptica on Mar 16, 2013, 09:53 pm
I don't think there is a need for any logic level converter device since the arduino I'm using already gives a 3.3V signal.

Thanks for the quick responses. Really hope I learn something from this tutorial.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 16, 2013, 09:54 pm
@psyoptica
You are using an Arduino Uno, right?
Then there is need for a logic level converter! Since the ATmega328P is running at 5V.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: psyoptica on Mar 16, 2013, 09:57 pm
Thanks for clearing it up. Will definitely have to get a converter circuit then..
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: brianb00 on Mar 26, 2013, 05:06 pm
I see a lot of experience embedded in this thread, so I have a question about any noticed issues with the LSM303DLM or DLH regarding bad readings in abrupt changes in orientation. Abrupt is defined as peak accelerations of about 3 g's in all directions.  I am experiencing abrupt / large heading changes in a tilt compensated compass implemented with this device.  In relatively "flat" conditions I don't see failures, but in severe conditions, especially sharp pitch (dx/dt) and sharp roll (dy/dt) I seem to get non linear readings. I say I seem, because as of yet I have not captured the case live as trials are expensive to duplicate in my moving platform. 

My question is two fold:

1) Has anyone experienced big non linear results in the accelerometers ?
2) Has anyone noticed big non linear results in the magnetometer that are somehow induced with strong acceleration ?

My application is using a 9DOF IMU and I have wrapped the sensors in filters that limit rate of change of data as well as do a variety of selectable filter algorithms.

Regards,
Brian
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: doantrungnghia05 on Mar 28, 2013, 03:30 pm
Hi Lauszus,

I have read your codes about the gyro sensor and it's very helpfull. So thank you about it. Now i try to modify your code to my quadrotor project but it stills have some problem about "gyro drift". I know that i have to use the Kalmal filter but my board just only has 3 angular rate sensors (the gyro sensor as i think) and have no accelerometer, so i don't know how to use the Kamal filter. Could you take a look at my problem in here:
http://arduino.cc/forum/index.php/topic,156912.0.html (http://arduino.cc/forum/index.php/topic,156912.0.html)

And could you show me some simple equation like the Complementary Filter which can aplly to my board.

Regards,
Nghia
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 28, 2013, 06:08 pm
@brianb00
I haven't got much experience with magnetometers, sorry.

@doantrungnghia05
The Kalman I filter I wrote can't be used without an accelerometer. The easiest thing would be to use a digital high-pass filter.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: doantrungnghia05 on Mar 29, 2013, 02:13 am

@brianb00
I haven't got much experience with magnetometers, sorry.

@doantrungnghia05
The Kalman I filter I wrote can't be used without an accelerometer. The easiest thing would be to use a digital high-pass filter.


Thank Lauszus, here is my board, i bought it because in the internet many people use this board and they also buid their quad fly, but they just assembly these things like: motors, rf-controller, ...together without writing any code for the uController.

1.Kk_board datasheet: http://www.kkmulticopter.com/downloads/resources/KKmulticontroller%20v.5.5.pdf (http://www.kkmulticopter.com/downloads/resources/KKmulticontroller%20v.5.5.pdf)

2.The angular rate sensor use in kk board: (ENC series) http://www.murata.com/products/catalog/pdf/s42e.pdf (http://www.murata.com/products/catalog/pdf/s42e.pdf)

http://www.youtube.com/watch?v=MRSKs7iZbRQ (http://www.youtube.com/watch?v=MRSKs7iZbRQ)

My code is C++ for atmega168pa and i know this forum is about adruino, but i think my code is very cleary and easy to understand. Here is it:

Code: [Select]
/************************************************************************/
/*  MAIN FUNTION                                                       */
/************************************************************************/
int main(void)
{
CLKPR=0x80;
CLKPR=0x00;

//----------- system init --------------------
sei();
usart_init();
adc_sensor_init();
led_init();
motor_control_init();
motor_start();
DDRB|=(1<<PINB3);// measure loop time for cheking
//-----------------------------------------------------------
//---------- update the zero value ---------
adc_sensor_get_value();
roll_zero=roll_adc;//~=824
yaw_zero=yaw_adc;//~=816
pitch_zero=pitch_adc;//~=846
roll_angle=360;
yaw_angle=360;
pitch_angle=360;
//----------------------------------------------------------
timer2_start();//start measure system time by timer2
_delay_ms(2000);
led_on();
while(1)
   {
//--------------   get value in 3 sensors -----------------------
PORTB^=(1<<PINB3);// toogle this pin per main loop
adc_sensor_get_value();//

//-------- just process roll gyro--------------------------------------------------------------
if(roll_adc<roll_zero)
{
roll_rate=(roll_zero-roll_adc)/sensitivity;//tinh toc do goc theo phuong roll [degree/s]
roll_angle-=roll_rate*dtime/1000;//tra ve gia tri degree theo phuong roll [degree]
}

if(roll_adc>roll_zero)
{
roll_rate=(roll_adc-roll_zero)/sensitivity;//tinh toc do goc theo phuong roll [degree/s]
roll_angle+=roll_rate*dtime/1000;//tra ve gia tri degree theo phuong roll [degree]
}
//---------------------------------------------------------------------------------------------

usart_clr();
usart_print_nb(roll_angle);//print roll_angle to computer screen

//----------------------- creat fixed_loop_time= 25ms per loop---------------------------------
loop_usefull_time=sys_time-last_time;
if(loop_usefull_time<fixed_loop_time)
{
for(i=0;i<(fixed_loop_time-loop_usefull_time);i++)
{
_delay_ms(1);
}
}
dtime=sys_time-last_time;//dtime=time per main loop (ms)
last_time=sys_time;//update the last_time value for the next loop
//------------------------------------------------------------------------------------------
   }
}

ISR(TIMER2_OVF_vect)//when the timer2 over flow we have: sys_time=sys_time +1(ms)
{ // so we can caculate the dtime( time per main loop)
sys_time+=1;  
TCNT2=130;
}

In my board, the three gyro sensors also have the high-pass filter as you say, but it still drifs.
Could you take a look at my "gyro drift" problem and show me how to fix it ?

Regrard,
Nghia
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 30, 2013, 07:56 pm
@doantrungnghia05
I'm pretty sure the boards which only got a gyro on them don't even convert the gyro readings into degrees, but uses the rate as the input to there PID loop.

You can find plenty of firmware for you board. There is no need to write your own. For instance I found this using Google: https://github.com/DaneGardner/KKMulticontroller

You should ask at a quadrocopter forum or something like that instead.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: garwalt on Apr 04, 2013, 05:35 pm
Hi Lauszus
when i try to compile your sketch for the mpu6050  i get the error Kalman does not name a type and line 4 is flashing , i am new to Arduino and not shure how to correct it,

thanks Garwalt
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 04, 2013, 07:25 pm
@garwalt
Remember to download this file as well: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/Kalman.h
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: garwalt on Apr 04, 2013, 09:03 pm
Thanks Lauszus  i had thet file but it didnt work, so i downloaded it again and all is well.
I want to add servo controll to this sketch could you give advice on how i might do this?.

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 04, 2013, 09:04 pm
@garwalt
Simply use the output from the Kalman filter as you input to your servo. See: http://arduino.cc/en/Reference/ServoWrite
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: garwalt on Apr 04, 2013, 10:23 pm
Thanks Lauszus ,  sorry to be a pain but this is very new to me, the only thing i have a problem with is, is where in the sketch, is the best place to put the servo write command, at the moment i am using this line from another post.

servo1.writeMicroseconds(1560+kalAngleX*9.55);

i have tried it in various positions within the loop function, and all it does is send the servo full one way , i am getting data on the serial moniter.

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 04, 2013, 10:36 pm
@garwalt
Simply add:
Code: [Select]

servo1.write(kalAngleX/2.0);
servo2.write(kalAngleY/2.0);


Just below:
Code: [Select]
timer = micros();
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: garwalt on Apr 05, 2013, 11:02 am
Thanks Lauszus, all is working as it should.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: garwalt on Apr 09, 2013, 07:18 pm
Hi Laszus the mpu6050 is working fine with your sketch, is there any way to match the angle of the servo to the angle of the gyro, at the moment if the gyro tilts to say 45 degrees the servo only moves to approx 30 degrees.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 10, 2013, 01:52 am
@garwalt
Just multiply the angle by a constant, as all Servos are a little different.
If you want more control use the writeMicroseconds function: http://arduino.cc/en/Reference/ServoWriteMicroseconds.

For instance you could fine the maximum and minimum values and then use map (http://arduino.cc/en/Reference/map) to map the angle to these values.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: madalam on Apr 24, 2013, 10:33 am
The post is very informative. Thank you so much.
I am working on something similar to it.

I am working on a self-balancing bicycle which is my final year project.The main idea is to run the bicycle,powered by a motor, in balanced position by measuring the tilt when ever the cycle tend to fall and using steering control to balance it. In my project, I am using two sensors gyro meter
http://www.nex-robotics.com/products/sensors/l3g4200d-3-axis-digital-gyroscope-with-voltage-regulator.html (http://www.nex-robotics.com/products/sensors/l3g4200d-3-axis-digital-gyroscope-with-voltage-regulator.html)

and accelerometer
http://robokits.co.in/shop/index.php?main_page=product_info&cPath=11&products_id=39 (http://robokits.co.in/shop/index.php?main_page=product_info&cPath=11&products_id=39)

to measure the tilt angle.

roll angle of the gyro along the front wheel and 'x-axis' of the accelerometer perpendicular to roll angle,towards the right side of the cycle,parallel to the ground so the roll angle will give the degree/sec and accelerometer will give the displaced angle or tilt angle when the bicycle tends to fall. 'y-axis' of the accelero also comes along the roll angle(I m not using it).


so, firstly I want to know whether the program you used to measure the tilt angle (in your case front and back) is applicable to sensors to accurately measure the tilt angle? (I m not using a imu). It would be of great help if you can make me figure out working of my sensors properly.

nowhere i could get the information about how you placed the axis of gyro and accelero....can you please tell me

please help me
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: io on May 01, 2013, 11:19 am
Hi
very interesting post, a bit offtopic but I d need to build a 3 axis angle sensor, which should pass me the pitch and yaw of the sensor in real time. I understand that part of this project iwll do exactly that but I am concerned about multi-turn sensing, so to say. I see the sensor will report easily the angles o a "floating" platform where angles go positive and negative starting for a zero/center, but how will it react when going over 360 degrees? Is i tpossible to sense this kind of rotations? Any hint is much appreciated.
Simone
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 05, 2013, 01:14 am
@madalam
I'm not totally sure what you question is? Are you talking about the physical placement of the gyro and accelerometer or something else?
Btw an IMU is simply a device that reports a crafts velocity, angle etc. In this context consist of the accelerometer and gyro that is used to calculate the angle of the device. See wiki for more information: http://en.wikipedia.org/wiki/Inertial_measurement_unit.

@io
I had the exact same problem with my Balancing robot. This is how I solved it: https://github.com/TKJElectronics/Balanduino/blob/711ef4484173694815e27a78f7cbf0dec4911c90/Firmware/Balanduino/Balanduino.ino#L201-L207.
Also check out the Kickstarter for more information about the project: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit/posts.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: tamayaytam on May 05, 2013, 10:32 am
Hello guys. I have been following this post for quite a time. Now that i had the gears , i have begun to play with mpu6050 . And i have recently  run the code successfully given in this post. Thanks for sharing.
Eventually what i am trying to do with mpu6050 is to measure sea wave data. An make a wave buoy. Since i am only a newbie in electronics and coding. I made some research on the web and found a scientific article about it. In this article they use an accelerometer to measure wave height and period. Main problem is that while buoy drifts with the wave , wave changes its position relative to the ground. So that acc data can not be used to measure wave accurately. They managed to find a way to compensate directional forces applied to buoy and measure z axis . But off course neither the code  nor how to do info are given. But equations in this project are shared fortunately. I am adding the image to the post. I am now trying to integrate these equations to the mpu6050 's acc readings. First equation is interesting i think i can measure the acc data with it  regardless of tilt or yaw. But i am getting 16xxx something number instead of g. According to mpu 's sensitivity level i am supposed to divide the number to get the g number. If you are interested. Your help would be much appreciated. Thanks in advance.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 07, 2013, 02:23 pm
@tamayaytam
The MPU-6050 will output it's values as a 16-bit signed number (-32768 to 32767). Depending on the which scaling you use 1g is a certain value. The default is +/-2g. Then 1g would be equal to 16384. Simply divide your readings by 16384 if you want to convert the reading into g's and are using the default setting.
For more information see the datasheet: http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf at page 13.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: juanv on May 17, 2013, 01:38 am
Hi!, I'm using IMUs for my final project, and I've been studying Kalman filters implementations for a while.

Studying Lauszus code, I see that Kalman filter gain (K0 and K1) is obtained from the error covariance matrix (P00, P01, P10 and P11) multiplied by the time between readings (dt) and added some constants.

If I see how Kalman gains evolve in time, I see that K0 approachs to 1 and, if time between readings remain constant, K1 approachs to -1/dt. If this is OK, where does Kalman filter improve the Complementary Filter?

Thanks and greetings from Argentina
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 17, 2013, 01:42 am
@juanv
You are totally right, see this comment on my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-1/#comment-54328 down to this one: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-1/#comment-55104.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Agasso on May 27, 2013, 06:02 pm
Hi, I'm a newbie, but i would like try with this IMU http://www.pololu.com/catalog/product/1269.
What's is your opinion?. Can I implement your algorithm with this IMU?

Albert
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 29, 2013, 11:10 pm
@AlbertG
You can use any IMU you like :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 05, 2013, 02:15 pm
Hello, i`m doing my little Self Balancing robot project, got 4 days left to show it to the pubblic, i`ve calculated the PID for the motors (using DC motors) and this forum was the GOD for me, better said Lazarus is God! So i wondered if this forum is thread is still opened and you still answer newbie quesitons. Anways i`ll just ask away, i`ve obtained the data from my sensor, i`ll try to attach it, all the action is on the X axis, do i need the kalman Y calculus to be done? or how does it work? From what i`ve read, i only have to pass to my motors the kalman X value through PWM, that`s analogWrite(kalmanX, pin number); ... wait... this is where i have the problem... where does my PID loop go? i should pass the kalman calculus value to the PID and then pass the PID calculus to the motors... no? I`m confused, i`ve read so many to get this thing working, now my sensors are accurate have accurate readings thanks to you, i`ve calculated the pid loop, now i have to put it all together and i`mm a little confused.
I`m`a try attaching that .txt sensor reading, i hope you`re still here and still have some time to post a reply  :)
Thanks in advance for the answer!
Best regards,
Cata


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 05, 2013, 02:19 pm
@LillSlick
You should check out my robot at Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit.
The source code is available at Github: https://github.com/TKJElectronics/Balanduino/tree/master/Firmware/Balanduino. It will pretty much answer all your questions ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 05, 2013, 08:19 pm
WOW thanks for the reply on such a short notice! Your code is very elaborate, you`re very thorough. I don`t need complicated stuff, only want to balance the damn thing :) no time for other stuff, i have to present my work in 4 days or else i`m screwed, it is my first robotics project, i`m an automation engineer and this project was proposed by my boss and was stupid enough to tell a deadline without even having the hardware, they gave me all the hardware 3-4 days ago and they said 1week ... so now after i`ve done the phisical part i would like to get the code done, i`ll use your data acquisition and kalman filtering and i have to get the motors to balance the robot. I had a look on your code and saw you have much control over the motors, i`m not using the encoders, i don`t care if one of the wheels will have a different caracteristic, plus i`m using two PmodHB5 h-bridges, the pins: Dir/En/Vcc/GND i`ve merged them and i thoat i`ll send the same command for them all, if dir is 1, only 1 pin is high and bolth h-bridges should behave in the same manner, and the PWM is the same for both i`ll send 1 pwm to 1 pin which runs both h-bridges at the same time... that`s what i thoat, and wanna get this sorted as soon as possible so i don`t get buttfucked by my bosses :)). It`s kinda hard putting up with 10 hours of work and after that, concentrate on the "other project" but they don`t care. Can u have some pointers, how should i implement a simplified version of ur code so that the robot balances itsself based on the kalman calculus u`ve done in the mpu6050 file and that`s about it... is it complicated? jason`s done some cool stuff with the motor control, he has an if decision there which does two types of commands for the motors, one aggressive command and one conservative command, that`s kinda nice but i won`t have the time to experiment with it. So, i only need the basics working, after that, i`ll build on it as you built on your own project which frankly is a super-project! I`m very interested in this project, i love it, i want to take it to a whole new level, but for my own, not for some fool who thinks he`s gonna unemploy me. Sorry for misspelling i`ve done so far, hope you can give me some pointers :). Thanks!
Best regards,
Cata  
PS: i`ll post the code i was talking about :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 05, 2013, 10:52 pm
@LillSlick
You should take a look at the old version of the code here: https://github.com/TKJElectronics/BalancingRobotArduino, but basically you can just comment out these lines: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Balanduino.ino#L13-L17 and then remove all the code related to the encoders. I would recommend that.

But you really need to work hard all weekend to make it in 4 days! It should be possible to make it balance if your hardware is alright.
This was my first attempt to make a balancing robot: http://blog.tkjelectronics.dk/2011/12/sneak-peak-segway-guide-code/. It didn't use the encoders - you might want to take a look at the source code below the video.

I just looked at Jason's robot: http://www.transistor.io/balancing-robot.html, but he using a servo. I'm using a brushed DC motor.

How long are you? Have you build the entire hardware and checked that all hardware is working as intended? It's important that you get the basics like the IMU, motor and all the other stuff working before you start writing the software to make it balance.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 06, 2013, 07:36 am
I got all the hardware done, the H-Bridges and motors are working correctly, the mechanical part is balanced(almost standing on its own) wiring is done, the only thing i need to do is code. i`ll attach some pictures i took while building it, they`re not the final product as it looks now, but it should give you an idea :). Thanks for the pointers, i`ll get right on it!

The pictures are a little to big for the forum so i`ll share them like this:
https://dl.dropboxusercontent.com/u/40769383/20130623_190515.png
https://dl.dropboxusercontent.com/u/40769383/20130627_005027.png
https://dl.dropboxusercontent.com/u/40769383/20130702_120216.jpg
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 07, 2013, 06:45 pm
Looks alright. Maybe a little to wide and what's up with that big cable box on the middle level? Is there anything inside it? :)

Any progress with the code?
I also noticed that you actually got encoders on those motors. If you have time I would recommend implementing them as well :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 07, 2013, 07:03 pm
yes that`s my power box, i got a battery in there 9V one, i don`t know if it`s going to be enough... i hope so!
with the code, i`ve tried some stuff, but i think i messed it up when I choose to command both Enable pins and Direction pins through the same pin, for the Enable the pin number 3 and for the Direction - pin number 1 on the arduino UNO. I`m using PmodHB5 drivers, up to 2A. I was using this code, your example for collecting and filtering data and my stupid coding :)) :
Code: [Select]
Kristian Lauszus, TKJ Electronics
Web      :  http://www.tkjelectronics.com
e-mail   :  kristianl@tkjelectronics.com
*/

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define APWM 3          // left motor PWM
//#define RPWM 10         // right motor PWM
#define ADIR 1         // left motor direction
//#define RDIR 12         // right motor direction

#define KP 0.5          // proportional controller gain [LSB/deg/loop]
#define KD 0.5          // derivative controller gain [LSB/deg/loop]

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
float output = 0.0;

void setup() { 
    // Make sure all motor controller pins start low.
  digitalWrite(APWM, LOW);
// digitalWrite(RPWM, LOW);
  digitalWrite(ADIR, LOW);
// digitalWrite(RDIR, LOW);
 
  // Set all motor control pins to outputs.
  pinMode(APWM, OUTPUT);
// pinMode(RPWM, OUTPUT);
  pinMode(ADIR, OUTPUT);
// pinMode(RDIR, OUTPUT);
  pinMode(13, OUTPUT);
 
  Serial.begin(115200);
  Wire.begin();
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
 
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
 
  delay(100); // Wait for sensor to stabilize
 
  /* Set kalman and gyro starting angle */
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2π and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
 
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
 
  timer = micros();
}

void loop() {
  /* Update all the values */ 
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]); 
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
 
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2π and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
 
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter 
// gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
 
  //compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  //compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
 
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  //kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
   
   //temp = ((double)tempRaw + 12412.0) / 340.0;
   
    // PD controller.
  output += kalAngleX * KP + gyroXrate * KD;
   
   // Clip as float (to prevent wind-up).
  if(output < -255.0) { output = -255.0; }
  if(output > 255.0) { output = 255.0; }
  if(kalAngleX > 3) {
  digitalWrite(ADIR, HIGH);
  analogWrite(APWM,output);
  }
  if(kalAngleX > 300) {
    digitalWrite(ADIR, LOW);
    analogWrite(APWM,output);
  }



  /* Print Data */
  /*
  Serial.print(accX);Serial.print("\t");
  Serial.print(accY);Serial.print("\t");
  Serial.print(accZ);Serial.print("\t");
 
  Serial.print(gyroX);Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ);Serial.print("\t");
  */
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(gyroXangle);Serial.print("\t");
  //Serial.print(compAngleX);Serial.print("\t");
  Serial.print(kalAngleX);Serial.print("\t");
  Serial.print(output);Serial.print("\t"); 
  Serial.print("\t");
 
// Serial.print(accYangle);Serial.print("\t");
// Serial.print(gyroYangle);Serial.print("\t");
// Serial.print(compAngleY); Serial.print("\t");
// Serial.print(kalAngleY);Serial.print("\t");
 
  //Serial.print(temp);Serial.print("\t");
   
  Serial.print("\r\n");
  delay(1);
}


Based on your mpu6050 example for data filtering and i added some lines to make something move :)
The wheels go in separate directions... it doesn`t balance its self.
I`m going through your code now, i`m commenting the parts with the encoders and EEPROM readings and all the stuff that stands for the BT, PS3, WII so on...
In the baladuino library i`ve modified the ports, i`m using pin 3 for the pwm and pin 1 for the enable signal that means portD,
Code: [Select]
/* Left motor */
#define leftPort PORTD // a fost #define leftPort PORTC
#define leftPortDirection DDRD

#define leftA PINC6 // PC6 - M1A - pin 23
#define leftB PINC7 // PC7 - M1B - pin 24
#define leftPWM PIND2  // a fost #define leftPWM PIND5 // PD5 - PWM1A (OC1A) - pin 18

/* Right motor */
#define rightPort PORTD
#define rightPortDirection DDRD

#define rightA PINB0 // PB0 - M2A - pin 25
#define rightB PINB1 // PB1 - M2B - pin 26
#define rightPWM PIND4 // same as my pin 5 --> PD4 - PWM1B (OC1B) - pin 17

/* Pins connected to the motor drivers enable pins */
const uint8_t leftEnable = 1; // was  const uint8_t leftEnable = 21;
//const uint8_t leftEnable = 4; // was const uint8_t rightEnable = 22;



One question until now : targetAngle - you said " the resting angle of the robot"  <=== where do i find that? gyro reading? and when the robot is balancing or?

So now i`m going through your code and making something much more simpler, only to balance the robot.
I agree with you on the Encoders, I will implement them, but not now... first i want to show them a simple thing, then i can work on it to get the complicated stuff :) make a REAL balancing robot.

I still need to figure out how this works
Code: [Select]

  /* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/doc8272.pdf page 128-135 */
  // Set up PWM, Phase and Frequency Correct on pin 18 (OC1A) & pin 17 (OC1B) with ICR1 as TOP using Timer1
  TCCR1B = _BV(WGM13) | _BV(CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling
  ICR1H = (PWMVALUE >> 8); // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz
  ICR1L = (PWMVALUE & 0xFF);

  /* Enable PWM on pin 18 (OC1A) & pin 17 (OC1B) */
  // Clear OC1A/OC1B on compare match when up-counting
  // Set OC1A/OC1B on compare match when downcounting
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);
  setPWM(leftPWM,0); // Turn off pwm on both pins
  setPWM(rightPWM,0);


and how to do it for arduino uno and for my pins :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 08, 2013, 10:38 am
getting this strange error.. dunno wtf to do about it :(
Code: [Select]

In file included from Schita001.ino:2:
Balanduino.h:59: error: redefinition of 'const uint8_t leftEnable'
Balanduino.h:58: error: 'const uint8_t leftEnable' previously defined here
Schita001:11: error: 'Kalman' does not name a type
Schita001.ino: In function 'void setup()':
Schita001:18: error: 'rightEnable' was not declared in this scope
Schita001:75: error: 'kalman' was not declared in this scope
Schita001.ino: In function 'void loop()':
Schita001:118: error: 'kalman' was not declared in this scope
Schita001:125: error: 'kalman' was not declared in this scope
Motor.ino: In function 'void moveMotor(Command, Command, double)':
Motor:9: error: 'PINC7' was not declared in this scope
Motor:13: error: 'PINC7' was not declared in this scope
Motor.ino: In function 'void stopMotor(Command)':
Motor:32: error: 'PINC7' was not declared in this scope


with the code based on the data acquisition and something to get the motors moving i get some results but the Output of the PD controller is not very good... i`ll attach a file so you can take a look.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 08, 2013, 04:57 pm
You are using an Uno (ATmega328P), right?

Then pin 3 is located on PIND3 and not PIND2.
See this page for more information: http://arduino.cc/en/Hacking/PinMapping168.

Target angle is the balancing point of the robot - this is usually 180, but you might have to change it if you CG is offset to either side. I used that when I mounted a GoPro on mine: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit/posts/450808.

Regarding the PWM registers. You should read the datasheet: http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet.pdf at page 128 to 138.

This error is caused because you are defining the same varaible twice:
Code: [Select]

Balanduino.h:59: error: redefinition of 'const uint8_t leftEnable'
Balanduino.h:58: error: 'const uint8_t leftEnable' previously defined here


Try renaming one of them two "rightEnable":
Code: [Select]

Schita001:18: error: 'rightEnable' was not declared in this scope


You need to download the Kalman library I wrote: https://github.com/TKJElectronics/KalmanFilter.
Code: [Select]

Schita001:11: error: 'Kalman' does not name a type


PINC7 is not available is on the ATmega328P:
Code: [Select]

Motor:9: error: 'PINC7' was not declared in this scope
Motor:13: error: 'PINC7' was not declared in this scope
Motor:32: error: 'PINC7' was not declared in this scope

Simply uncomment it for now.

Regarding the "Sensors.txt". Please tell me what is the output from each column or I will not be able to help you :)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 08, 2013, 05:51 pm
Yes 328 it is, my CG is as much as possible on the middle part, it's pretty well balanced, i wrote the columns but as i recon they didnt save...  so its like this: accX/gyroX/KalmanXangle/PD output

i'll try the mods u've suggested, the pwm values are the same for mine to, right?
and one more thing the PmodHB5 doesn't give information on the max frequency, how do i find it? i saw ur's is 20kHz... i dunno my max...
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 09, 2013, 04:02 pm
Hmm that doesn't look right. Are you using the example code I wrote?

The PWM value is the same, it will automatically scale the value from 0-100 to 0-PWMVALUE: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Motor.ino#L4.

The PWMVALUE is set automatically here: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Balanduino.h#L16-L17.

You will need to look in the datasheet. After that change this value: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Balanduino.h#L16 to the desired frequency.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 09, 2013, 08:04 pm
hey, i made a stupid version starting from the data acquisition code you provided :)  they wanted to see my progress so ... now i have until Thursday to finish, and one of them was so impressed he`s gonna send me tomorrow to do some work 210 KM away from home, just to be sure i don`t finish on time. This said, i`ll show u my code, i`ll have to implement that motor control of yours, i hope i`ll succeed.
Lemme show you how it works until now :
http://www.youtube.com/watch?v=hyUH676I7zA (http://www.youtube.com/watch?v=hyUH676I7zA)

And my stupid code is:
Code: [Select]


#include <Wire.h>
#include <PID_v1.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define LPWM 3          // left motor PWM
#define RPWM 5         // right motor PWM
#define LDIR 4         // left motor direction
#define RDIR 7         // right motor direction

#define KP 5          // proportional controller gain [LSB/deg/loop]
#define KD 2.5          // derivative controller gain [LSB/deg/loop]

#define W_radius 0.0035  // meters

#define minOutput 0
#define maxOutput 250


Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
double velocity;

double Setpoint = 0, Input, Output;
double aggKp=8, aggKi=0.5, aggKd=2;
double consKp=4, consKi=0.02, consKd=1; 
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
float output = 0.0;

void setup() { 
    // Make sure all motor controller pins start low.
  digitalWrite(LPWM, LOW);
  digitalWrite(RPWM, LOW);
  digitalWrite(LDIR, LOW);
  digitalWrite(RDIR, LOW);
 
  // Set all motor control pins to outputs.
  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LDIR, OUTPUT);
  pinMode(RDIR, OUTPUT);
  pinMode(13, OUTPUT);
 
  // PID library
    myPID.SetOutputLimits(minOutput,maxOutput);
    myPID.SetSampleTime(1);
    myPID.SetMode(AUTOMATIC);
 
// Serial.begin(115200);
  Wire.begin();
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
 
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
 
  delay(100); // Wait for sensor to stabilize
 
  /* Set kalman and gyro starting angle */
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
 
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
 
  timer = micros();
}

void loop() {
  /* Update all the values */ 
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3])/1000;
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]); 
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
 
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
 
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter 
// gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
 
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
 
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
   
   //temp = ((double)tempRaw + 12412.0) / 340.0;
   
   velocity = accY/W_radius;
   Input = velocity;

 
  double sens = Setpoint-Input;
        if(sens>1)
        {
          myPID.SetControllerDirection(DIRECT);
        }
        else if(sens<-1)
        {
          myPID.SetControllerDirection(REVERSE);
        }
       
        double gap = abs(Setpoint-Input);
       

          if(gap<10)
          { 
            myPID.SetTunings(consKp, consKi, consKd);
          }
          else
          {
             myPID.SetTunings(aggKp, aggKi, aggKd);
          }
       
          myPID.Compute();
        //  Output = map(Output, 0, 1000, 0, 255);
          if(Output<minOutput+20){ Output=0; }
   
     if(sens>=0)
           {
             digitalWrite(LDIR, LOW);
             digitalWrite(RDIR, HIGH);
             analogWrite(LPWM,Output);
             analogWrite(RPWM,Output);
             
           }
           else
           {
             digitalWrite(LDIR, HIGH);
             digitalWrite(RDIR, LOW);             
             analogWrite(LPWM,Output);
             analogWrite(RPWM,Output);           
             
           }
delay(1);
}

It`s not good, but ... it does SOMETHING, as you can observe i`m not using the kalman angle, i loaded that library but the code does not work it always gives me error at that single line :
Code: [Select]

#include <Kalman.h> // Kalman filter library - see:...

So, i`m making progress, but i really need to set that speed, and controller you recommended. I`ll get right on it, i have 2 hours until i have to go to sleep because i`m awfully tired. Keep in touch, what do you say about my code? stupid huh? :))
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 09, 2013, 10:38 pm
You should use either the complimentary or Kalman filter or you won't be able to make it balance!
Sounds like you are not that happy about your co-workers? ;) What is your profession?

See the following example on how to include the Kalman library: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050.
The simply is to simply add it next to the .ino file and then add it like so: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino#L19.

Hope you will get it working before your deadline!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: LillSlick on Jul 10, 2013, 09:26 am
i'm in the automation department, but the people are envious... i'm still getting that error when i create the Kalman instance at line 43,not at the include line, i'll be working on the robot later... now i'm 200km away, if you would be kind to keep an eye on the forum i will  appreciate your effort! or if you have an instant messaging possibility that would be great :)  i'll catch up with you later, when I get home. thanks a lot for doing this, i would be nothing without your help!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 10, 2013, 10:31 am
Okay. That's because you haven't included it properly for some reason. Either put it next to the ino file and include it like so:
Code: [Select]

#include "Kalman.h"


Or put it in your libraries folder as any other Arduino library and include it like so:
Code: [Select]

#include <Kalman.h>


You can find me on Google Plus. Simply just search for Kristian Lauszus ;)
Glad you appreciate my help!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Ben91 on Aug 07, 2013, 07:14 am
Hi Kristian ! First of all thank you a lot for your guide ! It is really helpful.

I am a french student in an engeneering school and I am using the crius aiop v2 on my robot. On this board there is an MPU6050 and I am communicating with it thanks to your code. I tested a lot of codes like the one on arduino playground but yours is definetely the best.

However I am still having some issues with this sensor...

Correct me if I am wrong but when I am reading the raw data for the accelerometer I am supposed to get for x, y and z : 0 , 0 and 0.98 right ?
However when I am just reading the right registers (my sensor is put still on my desk) I am getting those values instead :
  accX          accY          accZ
8324.00    4768.00   14496.00      
8208.00    4768.00   14536.00      
8216.00    4704.00   14296.00      
8288.00    4684.00   14608.00      
8356.00    4724.00   14464.00      
8372.00   4784.00   14588.00      
8280.00   4736.00   14408.00      
8332.00   4748.00   14356.00

Since I am using your code, the sensor is set on a scale range of +/-2g which means that the sensitivity of the accelerometer is of 16384 LSB/g. I know that to understand the raw datas I need to divide them with the sensitivity and get something near 0, 0, 0.98 even if there is an error.
However with the values I am having it is no where near from what it should be...
I tried the code from arduino playground and I am having the same raw datas... Your guide is really clear but I must be missing something... I am just reading the right registers but those raw datas doesn't make any sense at all...

So I would like to know if you think those datas are strange and if you happened to find something similar, I happened to find some talk on the internet which says that there is a version of the MPU6050 that doesn't have the same scale ranges and not the same sensitvities... I don't think I have that much of bad luck...

Thank you for your help,
Ben.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: hnk1412 on Aug 12, 2013, 06:05 pm
Hi everybody!
I'm using Lsm303.it include accelerometer and magnetometer.And i want to use accelerometer to estimate velocity but this is very noise.Can i combine 
accelerometer and magnetometer into kalman filter to filter the signal of accelerometer.Thank!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 16, 2013, 10:01 pm
@Ben91
No the raw data is not in g's, but a 16-bit signed value. See the datasheet: http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf page 13.
So if you are using a range of +-2 it means that you must divide the value by 16384 to convert it into g's.

@hnk1412
No I don't believe it's possible to estimate the velocity like that without a lot of noise. You might be better of with using a GPS sensor.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: gerry87 on Aug 25, 2013, 09:31 pm
Hey, great guide! I've been following it at the very early stage of a quadcopter build.

The accelerometer seems to be working fine but I'm having a bit of trouble with the gyroscope values. They don't seem to make sense, they don't go below 0 when spun counterclockwise.

Would someone mind looking through my code and seeing if anything doesn't look right?

Code: [Select]


// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include <Wire.h>
#include <Servo.h>


// I2Cdev and L3G4200D must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include <I2Cdev.h>
#include <L3G4200D.h>
#include <HMC5883L.h>
#include <ADXL345.h>


// default address is 105
// specific I2C address may be passed here
L3G4200D gyro;
ADXL345 accel;
HMC5883L mag;

int16_t magX, magY, magZ;

int16_t accValX, accValY, accValZ;
float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
float R;

int16_t gyroX, gyroY, gyroZ;
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroBias_oldX, gyroBias_oldY, gyroBias_oldZ;


float pitchComp, rollComp;

double t_0, t_1;

const float alpha = 0.5;
Servo magYservo;  // create servo object to control a servo
int val = 0;    // variable to read the value from the analog pin
int throttle = 0;
double gyro_sensitivity = 70; //From datasheet, depends on Scale, 2000DPS = 70, 500DPS = 17.5, 250DPS = 8.75.


void setSpeed(int speed, int throttle){
  int angle = map(speed, 0, 80, 59, 133);
  magYservo.write(angle + throttle);
  //Serial.println(angle);
}


void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();

  // initialize serial communication
  // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
  // it's really up to you depending on your project)
  Serial.begin(14400);
  magYservo.attach(9);
  delay(1000);
  setSpeed(0, 0);
  delay(1000);

  // initialize device
  //Serial.println("Initializing I2C devices...");
  gyro.initialize();
  accel.initialize();
  mag.initialize();

  // verify connection
  // Serial.println("Testing device connections..."); 
//   Serial.println(accel.testConnection() ? "ADXL345 (accelerometer) connection successful" : "ADXL345 (accelerometer) connection failed");
//   Serial.println(mag.testConnection() ? "HMC5883L (magnetometer) connection successful" : "HMC5883L (magnetometer) connection failed");
//   Serial.println(gyro.testConnection() ? "L3G4200D (gyro) connection successful" : "L3G4200D connection failed");

   
  // data seems to be best when full scale is 2000
  gyro.setFullScale(2000);

  // Calculate bias for the Gyro i.e. the values it gives when it's not moving
  for(int i=1; i < 100; i++){
    gyroBiasX += gyro.getAngularVelocityX();
    gyroBiasY += gyro.getAngularVelocityY();
    gyroBiasZ += gyro.getAngularVelocityZ(); 

    accBiasX += accel.getAccelerationX();
    accBiasY += accel.getAccelerationY();
    accBiasZ += accel.getAccelerationZ();
   
    delay(1);
  }
  gyroBiasX = gyroBiasX / 100;
  gyroBiasY = gyroBiasY / 100;
  gyroBiasZ = gyroBiasZ / 100;

  accBiasX = accBiasX / 100;
  accBiasY = accBiasY / 100;
  accBiasZ = accBiasZ / 100;

  // Calculate true sensitivity
  gyro_sensitivity = ((gyro_sensitivity/1000)/3.3)*1023; //16-bit resolution gyro. 2^16-1 = 65535

  t_0 = millis();
}


void loop() {

  //////////////////////
  //  Accelerometer   //
  //////////////////////
  double accPitch, accRoll;

  accel.getAcceleration(&accValX, &accValY, &accValZ);
 
   R = sqrt(sq(accValX)+sq(accValY)+sq(accValZ));
   accPitch = acos(accValX/R)*RAD_TO_DEG-90;
   accRoll= -acos(accValY/R)*RAD_TO_DEG-90;
   
  // Seems to do the same as above
  //  accPitch = -(atan2(accValX, sqrt(accValY*accValY + accValZ*accValZ))*180)/M_PI;
  //  accRoll = (atan2(-accValY, accValZ)*180)/M_PI;

  if(accValZ < 0)//360 degrees
  {
    if(accPitch < 0)
    {
      accPitch = -180-accPitch;
    }
    else
    {
      accPitch = 180-accPitch;
    }

    if(accRoll < 0)
    {
      accRoll = -180-accRoll;
    }
    else
    {
      accRoll = 180-accRoll;
    }
  }


  //////////////////////
  //      GYRO        //
  //////////////////////
 
  double gyroPitch, gyroRoll;
  // read raw angular velocity measurements from device
  gyro.getAngularVelocity(&gyroX, &gyroY, &gyroZ);

  gyroRateX =  (gyroX - gyroBiasX)/gyro_sensitivity;
  gyroRateY =  (gyroY - gyroBiasY)/gyro_sensitivity;

  t_1 = millis();
  gyroPitch += gyroRateX * (t_1 - t_0)/1000;
  gyroRoll += gyroRateY * (t_1 - t_0)/1000;
  t_0 = millis();


  //////////////////////
  //    MAGNOMETER    //
  //////////////////////

  mag.getHeading(&magX, &magY, &magZ);
 

  //Heading Estimation
  float magYaw = atan2(magY, magX);
  if(magYaw < 0)
    magYaw += 2 * M_PI;
  magYaw = (magYaw*180)/M_PI;
 
 
  //Complementary Filter
 
  pitchComp = 0.98 *(pitchComp+gyroPitch) + 0.02 *accPitch;
  rollComp = 0.98 *(rollComp+gyroRoll) + 0.02 * accRoll;


  Serial.print(accPitch);
  Serial.print(",");
  Serial.print(accRoll); 
  Serial.print(",");
  Serial.print(magYaw);
  Serial.print("\n");

  if (Serial.available()) {
    throttle = Serial.parseInt();
  }

  setSpeed(accPitch, throttle);

  delay(50);

}



Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 26, 2013, 04:48 pm
@gerry87
Please send me a link to the library you are using, so I can see the code.

Also please see this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino. It's shows a better way to calculate the angle using the accelerometers.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: gerry87 on Aug 26, 2013, 05:26 pm

@gerry87
Please send me a link to the library you are using, so I can see the code.

Also please see this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino. It's shows a better way to calculate the angle using the accelerometers.


https://github.com/jrowberg/i2cdevlib/tree/master/Arduino here is the library, and here's the specific one for the L3G4200D https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/L3G4200D

The better way you mention to calculate the angle is it:
Code: [Select]

accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;


Thanks, I'll try it when I get back home. Does this account for the 360 degrees (inverted)?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 26, 2013, 05:31 pm
It looks alright. Try to print the raw gyro values (gyroX, gyroY and gyroZ) and see if they look okay. Also try to run the provided example: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if that works.

Quote

Thanks, I'll try it when I get back home. Does this account for the 360 degrees (inverted)?

Yes ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: gerry87 on Aug 26, 2013, 08:27 pm
When i run this, here's a sample of the output from leaving it sitting, picking it up, spinning clockwise (x-axis) and then anti-clockwise (x-axis). What I'm wondering is should the values go negative when being spun anti-clockwise? They all seem to be between 0 and 255. Is my sensitivity correct (gyro_sensitivity = ((gyro_sensitivity/1000)/3.3)*1023) ? :

Code: [Select]

Initializing I2C devices...
Testing device connections...
L3G4200D connection successful
183 213 32
8 253 19
9 255 23
11 0 26
8 252 17
11 255 24
11 253 16
7 252 20
8 253 17
12 254 17
8 1 18
10 255 17
12 250 19
5 251 19
13 0 22
10 254 19
13 254 18
10 254 18
13 254 15
13 1 25
10 254 20
14 255 17
13 250 20
18 255 21
11 0 19
14 1 14
14 255 16
12 0 17
7 255 19
10 252 19
13 254 18
10 248 14
7 254 15
10 252 20
8 253 19
10 251 14
11 254 18
11 3 19
10 255 12
9 252 18
13 3 20
9 255 18
9 4 24
13 1 18
8 4 17
16 0 16
10 0 22
7 1 12
15 255 16
10 1 21
11 255 23
6 0 19
9 252 23
11 4 15
7 252 20
8 1 20
5 1 20
11 254 15
7 252 16
15 255 15
11 254 18
14 250 20
14 254 20
6 255 25
8 250 21
7 255 22
12 253 23
11 0 14
11 252 20
8 253 24
10 251 16
5 251 18
12 254 20
18 254 18
15 254 24
3 255 22
5 251 15
9 0 18
7 249 20
12 255 22
7 252 21
12 3 19
11 253 22
13 253 17
13 1 15
13 1 21
8 251 16
7 255 16
13 250 23
8 255 16
10 252 18
9 1 20
10 255 17
14 252 18
9 251 13
11 254 19
12 253 22
9 1 15
13 251 19
9 255 20
6 249 16
8 253 17
10 1 24
8 253 18
9 253 16
10 248 22
7 251 21
8 3 21
10 253 19
10 0 23
10 250 19
5 255 19
11 250 20
11 252 26
10 250 19
11 251 12
10 254 15
12 0 18
10 255 23
7 0 21
9 250 22
13 251 16
8 248 22
9 0 18
9 255 24
15 254 17
7 255 25
10 255 19
14 253 19
13 252 25
11 2 17
9 255 18
12 5 21
13 254 20
13 254 17
13 0 20
12 0 20
11 250 25
6 255 16
16 1 17
14 253 19
10 255 19
11 255 19
9 254 24
10 1 17
12 250 18
7 5 13
9 255 21
4 249 20
15 255 15
9 245 16
7 248 18
9 1 17
9 252 21
7 0 16
14 252 17
9 255 18
10 1 20
12 251 17
14 253 19
10 1 17
12 255 13
10 252 14
10 254 16
11 255 17
7 255 21
10 0 21
10 254 21
9 0 20
9 254 16
10 0 15
12 254 13
8 250 16
14 0 11
12 255 16
12 3 19
7 251 25
11 255 18
10 255 22
9 255 15
11 254 19
12 251 17
13 255 18
8 253 19
14 0 17
9 254 19
16 250 19
7 1 20
11 1 17
8 255 20
7 1 16
11 0 15
17 1 19
11 0 22
12 254 20
13 253 17
12 255 17
12 254 28
11 252 21
13 252 20
14 254 18
12 251 18
11 251 22
10 253 18
9 0 23
15 248 20
13 0 15
10 254 20
10 255 19
10 254 18
7 249 21
10 251 21
7 1 23
7 252 15
6 253 19
9 254 18
13 253 18
8 246 21
11 255 14
12 0 13
7 254 22
12 252 20
7 254 20
13 251 18
12 254 13
9 249 17
8 253 20
13 255 20
11 253 17
14 254 21
16 2 19
12 255 15
10 255 17
11 0 20
9 253 17
12 0 21
11 247 16
11 0 22
8 0 15
8 254 17
13 253 20
9 1 14
13 253 17
8 252 19
13 255 18
11 252 23
9 252 18
14 253 16
9 4 24
18 253 25
9 251 18
6 0 18
12 1 21
18 254 17
10 2 19
14 254 17
10 255 24
9 251 18
10 0 16
11 3 18
7 254 24
8 1 16
7 255 18
10 1 16
9 254 20
15 249 20
6 0 14
13 252 23
10 253 18
7 254 20
10 254 19
10 1 19
11 255 20
13 250 17
17 253 15
13 251 18
6 255 19
7 249 15
11 253 16
12 252 16
9 254 18
10 2 19
7 3 23
10 252 19
8 2 12
6 255 17
14 254 15
11 251 16
14 2 14
7 1 22
9 253 17
14 0 17
15 252 16
11 250 21
5 255 16
8 253 18
8 253 25
11 253 18
10 253 13
9 252 15
7 0 20
11 255 19
11 253 21
5 255 16
13 251 15
13 254 19
10 5 9
9 1 17
4 254 15
12 254 11
9 1 17
10 254 19
11 254 22
13 254 16
12 5 20
11 1 20
13 252 21
13 252 18
8 254 22
16 253 20
10 253 22
8 252 17
9 253 15
14 255 17
11 0 14
8 252 14
11 0 17
8 249 20
8 247 17
12 1 20
9 255 21
11 0 26
10 255 22
10 3 15
9 0 18
8 253 17
10 252 14
6 255 15
11 248 21
10 255 25
5 254 20
8 254 17
13 252 21
13 254 17
9 254 17
7 1 23
4 253 20
13 254 20
9 254 18
5 251 21
10 2 17
9 2 21
16 252 16
15 253 16
6 255 15
12 254 20
11 0 19
10 254 17
13 1 19
11 255 17
11 251 21
10 0 19
9 2 21
10 0 21
14 249 18
14 250 16
14 250 18
12 1 14
13 254 16
9 254 16
10 252 19
9 250 13
6 255 17
7 251 16
16 2 23
13 253 20
8 2 20
9 3 22
12 0 27
16 255 94
11 1 120
13 3 104
238 88 101
0 226 240
52 142 52
242 158 234
66 128 179
137 235 17
234 154 7
117 1 152
25 76 231
189 157 44
1 121 104
122 198 127
102 126 232
216 201 119
95 174 108
255 122 49
115 129 78
108 56 218
140 135 143
121 206 161
61 187 0
13 216 107
80 45 51
165 187 158
69 135 29
44 97 142
9 171 119
6 49 157
81 109 232
41 185 148
5 232 242
146 242 217
20 252 146
103 211 101
27 135 253
235 202 30
173 252 118
41 127 59
203 43 63
102 45 105
189 218 142
121 158 213
15 190 44
39 82 34
173 69 128
239 221 38
13 123 193
110 170 216
114 32 34
144 115 36
17 76 104
251 224 5
1 240 197
74 61 36
166 236 64
3 59 236
163 120 138
123 107 236
192 27 0
147 51 196
118 125 191
9 227 224
220 170 24
61 189 61
13 249 96
143 116 131
21 32 56
15 3 44
18 10 40
25 3 52
23 251 148
18 249 88
18 250 64
136 168 78
9 250 170
19 238 162
12 247 52
6 220 15
10 15 19
12 17 16
7 1 20
8 247 20
8 248 23
15 253 16
12 12 18
10 255 17
9 254 18
13 2 19
14 249 17
9 0 18
11 252 16
10 4 18
13 251 14
11 0 18
6 0 20
12 0 24
8 255 17
14 252 22
12 252 20
11 0 21
8 255 19
11 252 19
8 0 14
6 0 19
5 6 19
11 254 16
4 250 207
13 0 25
9 250 26
13 252 25
9 254 20
17 7 40
10 0 246
15 2 245
12 248 18
5 249 25
10 0 20
11 3 35
10 6 251
8 255 62
11 0 75
10 245 95
9 238 47
11 249 18
14 245 23
40 225 47
128 229 66
224 64 96
196 252 72
41 230 164
162 125 48
227 241 27
226 245 249
237 230 232
120 96 120
162 100 67
7 9 241
112 172 13
30 224 140
102 162 244
63 53 56
89 228 166
248 173 152
14 94 190
153 6 96
137 227 199
124 199 77
56 146 228
68 250 158
3 5 135
165 210 67
13 13 78
32 17 225
150 208 168
129 112 69
184 240 152
236 231 29
42 129 193
207 8 32
70 206 85
57 223 14
111 228 242
176 6 141
121 130 167
55 103 163
245 62 71
133 246 243
69 210 46
51 182 4
118 126 223
14 60 213
223 71 224
237 16 252
10 13 11
29 66 20


Quote

Thanks, I'll try it when I get back home. Does this account for the 360 degrees (inverted)?

Yes ;)
[/quote]
Tried it there, nice and elegant!

I noticed another error in my code, I changed

Code: [Select]

//Complementary Filter
 
 pitchComp = 0.98 *(pitchComp+gyroPitch) + 0.02 *accPitch;
 rollComp = 0.98 *(rollComp+gyroRoll) + 0.02 * accRoll;


to

Code: [Select]


 pitchComp = 0.5 *(pitchComp + gyroRateX * (t_1 - t_0)/1000) + 0.5 *accPitch;
 rollComp = 0.5 *(rollComp + gyroRateY * (t_1 - t_0)/1000) + 0.5 * accRoll;



since gyroPitch and gyroRoll were already the accumulated values.

Is IMU6DOFVer3 on the front page the latest version you have?

Thanks for the help here.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 26, 2013, 11:58 pm
I don't understand why you want to calculate the sensitivity like that. Simply just divide the output by the one given in the datasheet: http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf page 10.

Also please try to run the example code: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if it works.

The newest version can be found at Github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter, as I explain in the guide.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: gerry87 on Aug 27, 2013, 10:18 am

I don't understand why you want to calculate the sensitivity like that. Simply just divide the output by the one given in the datasheet: http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf page 10.

Also please try to run the example code: https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino and see if it works.

The newest version can be found at Github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter, as I explain in the guide.


Sorry, I wasn't very clear. When I run https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino, it gets values from the gyro fine, but I get the raw output below (with the words 'angular velocity:' removed because the post was too big...):

The values coming from the X-axis are in the first column. When it's at rest they hover around 15 (gyroXZero), when I pick it up and spin it clockwise they go up to >200, when I spin anti-clockwise they also go up to >200 (Min is 0 and Max is 255). Is this what is expected from the raw output?

To convert these values to Degrees Per Second, it seems that I take (gyroXRaw - gyroXZero)/70 (sensitivity from the datasheet).

Sorry for all the questions, I've found a lot of information on google, but I can't seem to find anything to clear this up.

Thanks again.

Code: [Select]

Initializing I2C devices...
Testing device connections...
L3G4200D connection successful
183 213 32
8 253 19
9 255 23
11 0 26
8 252 17
11 255 24
11 253 16
7 252 20
8 253 17
12 254 17
8 1 18
10 255 17
12 250 19
5 251 19
13 0 22
10 254 19
13 254 18
10 254 18
13 254 15
13 1 25
10 254 20
14 255 17
13 250 20
18 255 21
11 0 19
14 1 14
14 255 16
12 0 17
7 255 19
10 252 19
13 254 18
10 248 14
7 254 15
10 252 20
8 253 19
10 251 14
11 254 18
11 3 19
10 255 12
9 252 18
13 3 20
9 255 18
9 4 24
13 1 18
8 4 17
16 0 16
10 0 22
7 1 12
15 255 16
10 1 21
11 255 23
6 0 19
9 252 23
11 4 15
7 252 20
8 1 20
5 1 20
11 254 15
7 252 16
15 255 15
11 254 18
14 250 20
14 254 20
6 255 25
8 250 21
7 255 22
12 253 23
11 0 14
11 252 20
8 253 24
10 251 16
5 251 18
12 254 20
18 254 18
15 254 24
3 255 22
5 251 15
9 0 18
7 249 20
12 255 22
7 252 21
12 3 19
11 253 22
13 253 17
13 1 15
13 1 21
8 251 16
7 255 16
13 250 23
8 255 16
10 252 18
9 1 20
10 255 17
14 252 18
9 251 13
11 254 19
12 253 22
9 1 15
13 251 19
9 255 20
6 249 16
8 253 17
10 1 24
8 253 18
9 253 16
10 248 22
7 251 21
8 3 21
10 253 19
10 0 23
10 250 19
5 255 19
11 250 20
11 252 26
10 250 19
11 251 12
10 254 15
12 0 18
10 255 23
7 0 21
9 250 22
13 251 16
8 248 22
9 0 18
9 255 24
15 254 17
7 255 25
10 255 19
14 253 19
13 252 25
11 2 17
9 255 18
12 5 21
13 254 20
13 254 17
13 0 20
12 0 20
11 250 25
6 255 16
16 1 17
14 253 19
10 255 19
11 255 19
9 254 24
10 1 17
12 250 18
7 5 13
9 255 21
4 249 20
15 255 15
9 245 16
7 248 18
9 1 17
9 252 21
7 0 16
14 252 17
9 255 18
10 1 20
12 251 17
14 253 19
10 1 17
12 255 13
10 252 14
10 254 16
11 255 17
7 255 21
10 0 21
10 254 21
9 0 20
9 254 16
10 0 15
12 254 13
8 250 16
14 0 11
12 255 16
12 3 19
7 251 25
11 255 18
10 255 22
9 255 15
11 254 19
12 251 17
13 255 18
8 253 19
14 0 17
9 254 19
16 250 19
7 1 20
11 1 17
8 255 20
7 1 16
11 0 15
17 1 19
11 0 22
12 254 20
13 253 17
12 255 17
12 254 28
11 252 21
13 252 20
14 254 18
12 251 18
11 251 22
10 253 18
9 0 23
15 248 20
13 0 15
10 254 20
10 255 19
10 254 18
7 249 21
10 251 21
7 1 23
7 252 15
6 253 19
9 254 18
13 253 18
8 246 21
11 255 14
12 0 13
7 254 22
12 252 20
7 254 20
13 251 18
12 254 13
9 249 17
8 253 20
13 255 20
11 253 17
14 254 21
16 2 19
12 255 15
10 255 17
11 0 20
9 253 17
12 0 21
11 247 16
11 0 22
8 0 15
8 254 17
13 253 20
9 1 14
13 253 17
8 252 19
13 255 18
11 252 23
9 252 18
14 253 16
9 4 24
18 253 25
9 251 18
6 0 18
12 1 21
18 254 17
10 2 19
14 254 17
10 255 24
9 251 18
10 0 16
11 3 18
7 254 24
8 1 16
7 255 18
10 1 16
9 254 20
15 249 20
6 0 14
13 252 23
10 253 18
7 254 20
10 254 19
10 1 19
11 255 20
13 250 17
17 253 15
13 251 18
6 255 19
7 249 15
11 253 16
12 252 16
9 254 18
10 2 19
7 3 23
10 252 19
8 2 12
6 255 17
14 254 15
11 251 16
14 2 14
7 1 22
9 253 17
14 0 17
15 252 16
11 250 21
5 255 16
8 253 18
8 253 25
11 253 18
10 253 13
9 252 15
7 0 20
11 255 19
11 253 21
5 255 16
13 251 15
13 254 19
10 5 9
9 1 17
4 254 15
12 254 11
9 1 17
10 254 19
11 254 22
13 254 16
12 5 20
11 1 20
13 252 21
13 252 18
8 254 22
16 253 20
10 253 22
8 252 17
9 253 15
14 255 17
11 0 14
8 252 14
11 0 17
8 249 20
8 247 17
12 1 20
9 255 21
11 0 26
10 255 22
10 3 15
9 0 18
8 253 17
10 252 14
6 255 15
11 248 21
10 255 25
5 254 20
8 254 17
13 252 21
13 254 17
9 254 17
7 1 23
4 253 20
13 254 20
9 254 18
5 251 21
10 2 17
9 2 21
16 252 16
15 253 16
6 255 15
12 254 20
11 0 19
10 254 17
13 1 19
11 255 17
11 251 21
10 0 19
9 2 21
10 0 21
14 249 18
14 250 16
14 250 18
12 1 14
13 254 16
9 254 16
10 252 19
9 250 13
6 255 17
7 251 16
16 2 23
13 253 20
8 2 20
9 3 22
12 0 27
16 255 94
11 1 120
13 3 104
238 88 101
0 226 240
52 142 52
242 158 234
66 128 179
137 235 17
234 154 7
117 1 152
25 76 231
189 157 44
1 121 104
122 198 127
102 126 232
216 201 119
95 174 108
255 122 49
115 129 78
108 56 218
140 135 143
121 206 161
61 187 0
13 216 107
80 45 51
165 187 158
69 135 29
44 97 142
9 171 119
6 49 157
81 109 232
41 185 148
5 232 242
146 242 217
20 252 146
103 211 101
27 135 253
235 202 30
173 252 118
41 127 59
203 43 63
102 45 105
189 218 142
121 158 213
15 190 44
39 82 34
173 69 128
239 221 38
13 123 193
110 170 216
114 32 34
144 115 36
17 76 104
251 224 5
1 240 197
74 61 36
166 236 64
3 59 236
163 120 138
123 107 236
192 27 0
147 51 196
118 125 191
9 227 224
220 170 24
61 189 61
13 249 96
143 116 131
21 32 56
15 3 44
18 10 40
25 3 52
23 251 148
18 249 88
18 250 64
136 168 78
9 250 170
19 238 162
12 247 52
6 220 15
10 15 19
12 17 16
7 1 20
8 247 20
8 248 23
15 253 16
12 12 18
10 255 17
9 254 18
13 2 19
14 249 17
9 0 18
11 252 16
10 4 18
13 251 14
11 0 18
6 0 20
12 0 24
8 255 17
14 252 22
12 252 20
11 0 21
8 255 19
11 252 19
8 0 14
6 0 19
5 6 19
11 254 16
4 250 207
13 0 25
9 250 26
13 252 25
9 254 20
17 7 40
10 0 246
15 2 245
12 248 18
5 249 25
10 0 20
11 3 35
10 6 251
8 255 62
11 0 75
10 245 95
9 238 47
11 249 18
14 245 23
40 225 47
128 229 66
224 64 96
196 252 72
41 230 164
162 125 48
227 241 27
226 245 249
237 230 232
120 96 120
162 100 67
7 9 241
112 172 13
30 224 140
102 162 244
63 53 56
89 228 166
248 173 152
14 94 190
153 6 96
137 227 199
124 199 77
56 146 228
68 250 158
3 5 135
165 210 67
13 13 78
32 17 225
150 208 168
129 112 69
184 240 152
236 231 29
42 129 193
207 8 32
70 206 85
57 223 14
111 228 242
176 6 141
121 130 167
55 103 163
245 62 71
133 246 243
69 210 46
51 182 4
118 126 223
14 60 213
223 71 224
237 16 252
10 13 11
29 66 20

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: gerry87 on Aug 28, 2013, 01:14 pm
Just updating this for anyone with the same issue, I tried out the https://github.com/pololu/L3G/tree/master/L3G library instead of the i2cdev library and I'm now getting values between -65,535 and 65,536 with negative values for anti-clockwise rotations instead of 0 - 255 with no difference between anti-clockwise and clockwise.

There's either an issue in the i2cdev library for the L3G4200D or I was just using it incorrectly.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 28, 2013, 01:16 pm
Okay good you got it working then ;)
Maybe it would be a good idea to open to fill in an issue: https://github.com/jrowberg/i2cdevlib/issues, so he can see there is an issue.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mit2sumit on Aug 28, 2013, 04:22 pm
Can u suggest me with these data, that I initially received with my GY-521 module of MPU6050 IMU...
I'm not able to distinguish, whether why there's so much of drift...
The connections made are:
vcc=5v
GND=GND
SDA=SDA  //dedicated to arduino
SCL=SCL  //dedicated to arduino

These are the values, which I'm getting...

InvenSense MPU-6050
June 2012
WHO_AM_I : 68, error = 0
PWR_MGMT_2 : 0, error = 0

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 1804, 1256, 15184
temperature: 27.329 degrees Celsius
gyro x,y,z : -4933, 3934, 490,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 5580, 1760, 14720
temperature: 27.047 degrees Celsius
gyro x,y,z : -4620, 3913, 428,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 3392, -84, 16828
temperature: 26.576 degrees Celsius
gyro x,y,z : -4559, 4073, 308,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 5672, 352, 10612
temperature: 26.765 degrees Celsius
gyro x,y,z : -4640, 3980, 364,

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 4392, 1068, 13724
temperature: 27.000 degrees Celsius
gyro x,y,z : -4496, 3749, 380,
(for more, see the attachment...)

Please, suggest me ASAP!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Aug 30, 2013, 07:47 pm
@mit2sumit
Please connect AD0 to GND as well.
And please try to run my example code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050, so I'm able to help you out.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mit2sumit on Sep 09, 2013, 12:46 pm
Thanks for this info....
but, its too late and unuseful....
as, i got my sensor worked with the program u mentioned, and even completed my project...
though it was not much successful....

I still wonder, whether, why was those variations....even with connecting AD0 to gnd....
and later when i uploaded this code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050 , by myself it worked.
I even mapped the value with the range of -20 to 20, so that the horizontal stationary value gives...0 output...

however, thanks for response,
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 09, 2013, 04:31 pm
Okay. I can't say what is wrong with the other code, but I know mine works.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: romit26 on Sep 16, 2013, 01:26 pm
Hey Lauszus,
What value would you give for sensitivity in the code if in the datasheet, the value is given in LSB/(o/s)?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kashif on Sep 16, 2013, 05:46 pm

Hey Lauszus,
What value would you give for sensitivity in the code if in the datasheet, the value is given in LSB/(o/s)?


Use the same value as-is.
For example if the datasheet states "14.375 LSB/(º/s)" then the sensitivity will be "14.375"

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Sep 20, 2013, 12:57 pm
@romit26
As Kashif said. Simply just divide by that value. Take a look at the following code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino. But it's even necessary to do this with the accelerometer, as you can just pass in the values directly into atan2 - see: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino#L89-L90.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Rhett on Oct 20, 2013, 08:47 pm
I'm new to the whole arduino programming scene so please pardon my ignorance if this is a dumb question.

I've got and Arduino Uno and a MPU6050 GY-521 that I'm trying to use for a project I'm working on.

I've gotten several different sketches to "work" so far including the one from here:

http://playground.arduino.cc/Main/MPU-6050

And 2 of Jeff Rowberg's ones from here:

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

I have trouble tryin to figure out how to use the raw data from the playground code and I've noticed quite a bit of drift with the Rowberg "teapot sketch" so I'm trying to figure out how to use yours.

I keep getting these errors though when verifying your code from here:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino

sketch_oct20a.ino: In function 'void setup()':
sketch_oct20a:45: error: 'i2cWrite' was not declared in this scope
sketch_oct20a:46: error: 'i2cWrite' was not declared in this scope
sketch_oct20a:48: error: 'i2cRead' was not declared in this scope
sketch_oct20a:57: error: 'i2cRead' was not declared in this scope
sketch_oct20a.ino: In function 'void loop()':
sketch_oct20a:78: error: 'i2cRead' was not declared in this scope


I've placed the Kalman.h file in my library. Is there something I'm doing wrong? and why does your code seem so much shorter and simpler than the others?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 22, 2013, 01:21 am
@Rhett
You need to download the I2C.ino file as well: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/I2C.ino. Simply just download the entiry repository - go to the front page: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter and then notice the button labeled: "Download ZIP" and will download all you need.

Quote

and why does your code seem so much shorter and simpler than the others?

Because my is just a minimal code, but it should be plenty for most users. The code on the playground got all the different registers defined - I simply just hardcode them.

The code here: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 is quite different as it uses the built-in DMP - you can read more about it here: http://www.i2cdevlib.com/devices/mpu6050#source.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ectar on Oct 27, 2013, 07:28 am
Hi guys, please advise how to calculate Z angle within listed code below:

I've tried to play with

arcsin(2(q0q2-q1q3))

but what is q0q1q2q3 within the current given code snippet?



Code: [Select]
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;

unsigned long timer;

double zeroValue[5] = {950, -400, 13500, -100, -500};

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;

double compAngleX = 180;
double compAngleY = 180;


void setup() {

   // join I2C bus (I2Cdev library doesn't do this automatically)
   Wire.begin();
   Serial.begin(115200);

   // initialize device
   Serial.println("Initializing I2C devices...");
   accelgyro.initialize();

   // verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");


  timer = micros();
}

void loop() {
   // read raw accel/gyro measurements from device
   //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

   accelgyro.getRotation(&gx, &gy, &gz);
   
   double gyroXrate = -((gx-zeroValue[3])/131); //Change the sensitivity after your sensor
   gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
   
   double gyroYrate = ((gy-zeroValue[4])/131);
   gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Without any filter
   
   
   ///////////////////////////
   //The acc X and Y angle///
   //////////////////////////
   accelgyro.getAcceleration(&ax, &ay, &az);
   
   double accXval = ax-zeroValue[0];
   double accZval = ay-zeroValue[2];    
   double accXangle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
   
   double accYval = ay-zeroValue[1];
   accZval = ay-zeroValue[2];    
   double accYangle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
   
   //////////////////////////
   //////////////////////////
   
   compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle);
   compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

   timer = micros();
   
   
 Serial.print(compAngleX);Serial.print("\t");  
 Serial.print(compAngleY); Serial.print("\t");
 //Serial.print(timer); Serial.print("\t");
 Serial.print("\n");
   
  delay(10);


   
}

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 29, 2013, 07:22 pm
Is called quaternions. You can find more information about it here: http://www.x-io.co.uk/quaternions/. Wikipedia is a good resource as well: http://en.wikipedia.org/wiki/Quaternion.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: michinyon on Oct 29, 2013, 07:36 pm
What application are you trying to implement ? 

The segway type scheme is a one dimensional implementation.

General flight control is a three dimensional implementation.    Quaternions is the best way to go for this.

You program seems to be a two dimensional implementation,   what is that ?     I am not sure how quaternions would be useful there.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ectar on Oct 29, 2013, 07:58 pm

You program seems to be a two dimensional implementation,   what is that ?     I am not sure how quaternions would be useful there.

This is piece of code from original post, this is not my program, I've tried it, and yes, it works for 2D, and I using it for 2D stabilization. I want to have 3D stabilization, so I want to handle z axis as well, just want to add Z axis to listed code.
Lets forget about arcsin(2(q0q2-q1q3)) , this was something I did not know what I was talking about....

Question is simple: So what would be formula to convert Z values from accelerometer output to get angle, similar to
Code: [Select]
double accYangle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
...in this particular piece of code??

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Oct 29, 2013, 08:02 pm
It is not possible to calculate the rotation along the z-axis (yaw) using the accelerometer. More information can be found at the following comment on my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-3/#comment-431999.

Instead you should use the code that you used snippets of, which I believe is this one: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050, right?

But to get a precise yaw estimate you should use a magnetometer as well.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Pendax on Nov 29, 2013, 02:37 pm
Hi, thanks for your explanation. i still have one question, does the complementary filtering solve drift issues "forever"? I mean lets say i let the device run for 2 days straight and move it up and down and turn it all the time and then i put it down on my table. Will i measure the same values as i did when i first turned it on two days ago and put it in the exact same space? or is there a remaining integration error that got bigger over time and now i read 2° offset on my x-axis angle where there should have been 0° or something like that? i hope you understand my question.

thanks in advance for your replies.

Pendax
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Dec 25, 2013, 04:39 pm
@Pendax
I actually do not know. I have never tried to let it run for that long, but I believe it is able to do it.
Sorry for the delayed reply!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Pendax on Jan 13, 2014, 12:09 pm
Hi Lauszus,
thank you for your answer. I tried it, it works.
I have another question:
Is it possible, with the MPU6050 or with any other sensor/sensor-fusion, to measure an exact angle of an object relative to the earth's coordinate system when the object is always affected by a changing acceleration.
For example: a bicycle that drives up a hill. The acceleration of this system is never just the gravity, but always (gravity) + (acceleration produced by the rider). So there would alwys be an incorrect calculated angle (by the accelerationsensors-signals) -> i cant correct the integrated gyro signal -> i always have a false inclination angle. It has to work somehow...
Please share your thoughts and ideas

best regards

Pendax
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Ked85 on Jan 15, 2014, 08:19 pm
Great code, it is very helpful for our project.

The print data I get for the Complimentary filter and the Kalman Filter are both angles.  Is it possible to get linear data as well?
We are trying to build a tracking system with GPS.

All help will be appreciated!
Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jan 18, 2014, 12:19 pm
@Pendax
That is the point of the sensor fusion. This example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050 shows how to do it.

@Ked85
It is very difficult to get linear data, as you will need to integrate the accelerometer, so the error will also be integrated.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Pendax on Jan 22, 2014, 10:08 am
Hello Lauszus,
i tried to get an accurate angle based on a sensor fusion of accelerometer and gyroscopes using a complementary filter which should, to my understanding, output a signal similiar to a kalman filter output. the formulas im using to calculate the x angle is also in the image, gyro_y is the present gyro-rate.
i rode a bicycle on a completely flat surface (which should return 0 degree relative to earths coordinate system on the x axis) with the MPU6050 attached to it. my calculated angles look horrible, as you can see in the attached image. while i was accelerating the bicycle, of course the acceleromtere readings were biased by that (yellow) so the calculated accelerometer angle gets wrong (up to 10 degrees). Problem is, the filtered angle follows, so i dont get a "true" angle, but my sensor sees a hill where everything is flat. At 13 seconds i hit the brakes and the angle goes negative (again like 10 degrees). after that i accelerate pretty hard and the signals get even worse as you can see. Do you have any idea what i am doing wrong or why the x-angle doesnt stay at 0 degrees, as it is in reality? maybe there is something you can read out of these signals and help me with.

thanks in advance

Pendax


Title: (Relevantly) simple implementation
Post by: ninor on Jan 31, 2014, 06:41 pm
Hi Lauszus,

I downloaded your code and I'm able to get raw data which to be honest means very little to me. Also tried to run Graph but no data shown, I am not sure where to find instructions about relevant .ino and/or other configuration.

As mentioned, my application is quite simple, I would like to know the displacement in X and Y in 1000/mm, Z would be a nice bonus. The application is a servo platform which I am trying to follow it's location very precisely for a long period of work, say hours.

As a new Arduinoist with no math/physics background I am having difficulties establishing the starting point.

Thanks in advance for your (other?) help.

Nino
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ceciliazy13 on Feb 01, 2014, 07:52 pm
Hi Lauszus,

Thank you for all the demo and explanation. It is exactly something I'm looking for right now. However the Github site seems to be down from my place. Would you mind send me the codes you wrote for collecting gyro data and balance the robot through email?

My email is : ceciliazy13@gmail.com

Thank you in advance!

Yi
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ninor on Feb 02, 2014, 09:29 am
I will email you the code.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: siddhu99199 on Feb 07, 2014, 06:02 am






we are doing our project (self balancing robot ) using  arduino uno and MPU6050 . we got struck with the programming of i2c interfacing [ http://www.bajdi.com/page/3/ ]   , we are encountering the following errors in our  programme . so we request you to help us solve the errors or to provide us with a new complete programme for interfacing .. we would be glad if u can help us  =( =( :smiley-red: :smiley-slim:



sketch_feb07a.ino: In function 'void setup()':
sketch_feb07a:55: error: 'i2cWrite' was not declared in this scope
sketch_feb07a:56: error: 'i2cWrite' was not declared in this scope
sketch_feb07a:58: error: 'i2cRead' was not declared in this scope
sketch_feb07a:67: error: 'i2cRead' was not declared in this scope
sketch_feb07a.ino: In function 'void dof()':
sketch_feb07a:140: error: 'i2cRead' was not declared in this scope
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 07, 2014, 02:50 pm
@Pendax
Try to use the Kalman filter I have provided and see if that helps. You could also try to implement a low-pass filter of the accelerometer reading, to get rid of some of the noise.

@ninor
This graph code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/Graph is for these codes: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/ITG3205_ADXL345 and https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/LPR530AL_LY530ALH_ADXL335.

While this one: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph is for the MPU-6050.

@ceciliazy13
You can find the code for my balancing robot here: https://github.com/TKJElectronics/Balanduino.

@siddhu99199
You need to download the I2C.ino and Kalman.h files in this directory: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050 as well. Then simply click on the file name MPU6050.ino.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ninor on Feb 08, 2014, 06:19 am
Thanks for the graph details.
Is there a way to get the axis displacement in mm?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 08, 2014, 01:11 pm
@ninor
What do you mean by displacement?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ninor on Feb 08, 2014, 09:27 pm
Once started, the sensor is located in X,Y,Z = 0,0,0 and as it moves in space, the serial output shows relative physical location in space measured in millimeters.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 09, 2014, 09:15 pm
@ninor
You will need some other sensor like a GPS sensor to get the position. See my previous reply to Ked85: http://forum.arduino.cc/index.php?topic=58048.msg1551502#msg1551502.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kirky on Feb 10, 2014, 10:52 pm
Hi there,

I'm working on a position tracking device and I'm using the Kalman library for smoothing my values. So far it all works fine, the only problem I have is the "wrap-around" when the angle jumps from 360 to 0 (or the other way around).
For example if I just look at the X axis, I get these values around the jump (red are the numbers which I can't explain):

355.4
357.3
359.4
195.2
140.3
80.1
35.3
10.6
-8.3
-4.0
-1.5

0.5
1.5
3.9
6.4

etc.

So basically it jumps from 360 to a value near 180, then sinks to about 10 degrees, then goes to negative values of about -10 and then increases until it reaches 0 and then starts to behave normal again… The strange numbers are there for about 500 ms until it starts to behave normal again. This problem only appears in my values, that are filtered with the kalman "method". The angles given by my accelerometer are wrapping fine (from 360 directly to 0).
Any idea why this happens and how I can avoid it?

Thanks a lot in advance!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 11, 2014, 05:28 pm
@kirky
This is how I deal with it in the firmware for my balancing robot: https://github.com/TKJElectronics/Balanduino/blob/cc8beb5f3a78cfd615a00c490b6656bdcc1a9d8b/Firmware/Balanduino/Balanduino.ino#L290-L304.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kirky on Feb 11, 2014, 11:30 pm
@Lauszus

You're a god! Works great, thank you for the quick response!

Now I have just one last problem with which I struggle:
EDIT: Since its difficult to describe, I recorded a few numbers, which explain my problem.
The left column are my roll values, the left column are my pitch values. They are the result of this calculation:
roll = (atan2(ay, az)+PI)*RAD_TO_DEG;
pitch = (atan2(ax, az)+PI)*RAD_TO_DEG;

I rotated my device around the X-axis (should only change my roll value). I did not rotate around the y-axis, so my pitch should theoretically stay at ca. 180 degrees all the time… but it doesn't!
Red is the part where it gets strange….

110.037712      177.94928;
108.725166     177.268387;
107.690384    178.456924;
105.609833    179.55899;
103.349213     181.768768;
102.630997    178.129913;
100.206619    180.742752;
97.836052      181.004349;
95.612274       180.824249;
94.362167       181.906158;
92.943687      180.99469;
91.328262       165.373825;
89.467445      57.259464;
88.114891        22.104139;
86.327652      15.940088;
85.284119       15.84662;
84.20356        12.40211;
82.488235      13.507222;
81.106842       7.860759;
80.041031      5.403023;
78.477669      7.589334;
77.705246      7.589335;
75.788643      7.558187;
75.097031      6.980302;
73.839981      5.246093;
73.038017      4.990366;

I hope you understand what I mean… Any ideas why this happens?
Again, I really appreciate your help!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: kirky on Feb 14, 2014, 02:28 am
This Video shows my problem:

https://www.youtube.com/watch?v=7WJHeJ8LGKk
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: PeterCheng on Feb 16, 2014, 05:33 pm


This graph code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/Graph is for these codes:
While this one: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph is for the MPU-6050.


Can you tell me more detail how to use graph code. I copied the folder of MPU 6050 and Graph to Libraries of Arduino, but the problem is when I verified the code of Graph, it annonced there are errors with the code, some function isn't defined. Morever, when I clicked on Graph.exe, it opened for the first time, bút the second time, nothing showed up.
Looking  forward to ur reponse.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Feb 19, 2014, 04:44 pm
@kirky
I am aware of the problem and have actually solved it in my local repo, but I need to clean it a bit before I push it. You should take a look at this app-note: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf for now ;)

@PeterCheng
You should not open the Graph code in the Arduino IDE, but in Processing: http://processing.org/.

You can find more information in the readme: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph#developed-by-kristian-lauszus-tkj-electronics-2012.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: PeterCheng on Feb 22, 2014, 06:42 pm

@PeterCheng
You should not open the Graph code in the Arduino IDE, but in Processing: http://processing.org/.

You can find more information in the readme: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050/Graph#developed-by-kristian-lauszus-tkj-electronics-2012.

It works perfectly thanks to ur help. And I'm also doing control motor DC (with encoder attached) by PID, so I'm looking for a program/graph apps which can show my signal response from my motor like the graph used for Mpu6050, can u suggest one ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mbedfordm on Feb 27, 2014, 03:24 am
Hi Lausuz,

Thanks for this amazing guide. I'm new to IMUs and have learned so much from your tutorials and your active replies to questions! I made it through 15 pages of the thread before I thought I'd ask my question (maybe you could point me in the right direction if it's been asked?)

I'm looking to send a signal when a certain rate is detected by the IMU. I've gotten the program to work and it is exactly what I hoped for. Now I'd like to utilize it to send a signal when "x" degrees/sec is reached. Is this something that would be possible?

Also, could the signal be sent when either the x or the y axis hit that rate? as of right now, when I tilt the x axis to 90 degrees, the y goes crazy and bounces the whole length of the graph very quickly; which would send false positives for the angle/s occurring.

Thanks for your help and look forward to your reply ! :D

Mark
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 02, 2014, 12:19 pm
@mbedfordm
You can get the unbiased rate using the following function: https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.h#L81.

I know what you are talking about and will upload a fix very soon. Please read this for now: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 02, 2014, 02:37 pm
@mbedfordm
I just updated the MPU-6050 code. The changes newest code can be found in the Github repository: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: juanvivo on Mar 06, 2014, 05:20 pm
HI Lauszus,

I finally finished reading the 34 pages of this post! VEry valuable information, but I have not managed to find anyone with the same problems that me.

I have written some doubts in your  Kalman filter article comments . I've thought about moving them to this thread so that if someone encounters the same problem, here you can find the information to solve it (if I can ever fix it ... ) :)

First of all, I use your code LPR530AL_LY530ALH_ADXL335. I only modified the data sensitivity to suit my gyroscopes, which are the IDG650 and ISZ650. Here are their datasheets: http://www.roboard.com/Files/G145/Datasheet_IDG650.pdf and http://www.roboard.com/Files/G145/Datasheet_ISZ650.pdf . Instead of the sensitivity data your code, I used 2.27, which I think are the sensitivity data of my gyros ... ((0.00227*1023)/3.3 = 0.7037)

This is your code:
Code: [Select]
void loop() {
  double gyroXrate = -((analogRead(gX) - zeroValue[0]) / 1.0323); // (gyroXadc-gryoZeroX)/Sensitivity - in quids - Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = -((analogRead(gY) - zeroValue[1]) / 1.0323);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);


This is mine:
Code: [Select]
void loop() {
  double gyroXrate = -((analogRead(gX) - zeroValue[0]) /0.7037); // (gyroXadc-gryoZeroX)/Sensitivity - in quids - Sensitivity = (0.00227*1023)/3.3 = 0.7037
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = -((analogRead(gY) - zeroValue[1]) /0.7037);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);


Is correct?

Everything works correctly when the  imu is static. IF I move the imu, I get the correct values in xAngle and yAngle variables. The problem occurs when the imu is in the airplane for which I am doing the tests. When the airplane takes off and starts flying, readings are totally erroneous.

IN the comments, you say me that :

Quote
It is most likely happening because your accelerometer values gets screwed up since I tuned the Kalman filter for my balancing robot and not for something like an airplane which is exposed to much larger g-forces than a balancing robot.


I went back to read the section "The measurement" one more time, and I understood there that  is need to adjust the values ??of variance. it Refers to Q_angle, Q_bias and R_measure variables? But, these variables do not affect what you told me to tune the data to adjust the filter for the small g forces that supports your robot or the bigs one that is supporting my airplane… or affects?

I'm not sure these are the values ??I need to change to adjust the kalman filter to the strongest forces g the imu has to endure in the plane.

And, in the comments, you say:

Quote
Yes they do. For instance try to increase R_measure and it should trust new measurements less.


I have only tried with the values ??of kalman filter. i´ll try to modify the R_measure.  Now, I'll try with the values ??of the complementary filter, to see if the same error occurs also.

Any help will be most welcome.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: andremorais on Mar 13, 2014, 07:14 pm
Hi Lauszus, first of all congrats for the great work that you have been done around here. I'm currently using your code for MPU6050 with the kalman filter and it works perfectly but now i have an issue because in your code you restrict the pitch angle to be from -90º to 90º. But I want that the both pitch and roll to be from -180º to 180º, i have already tried to change the code and use atan2 in both angles and use some extra restrictions but i end up with the same problem. Can you give me some tips??

PS: In the video I show the problem that i have, the roll is perfect(-180 to 180) but the pitch when hits the 90º or -90º flips the block...

https://www.youtube.com/watch?v=BPuT8g5actw&feature=youtu.be

Thanks a lot :D
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Mar 15, 2014, 12:18 am
@juanvivo
I have answered your question at my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-4/#comment-486873.

@andremorais
I believe you can't use the angles directly, as the one of the angles is being restricted to +-90 degrees, you should instead modify your Processing code, so it shows the block correctly.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: juanvivo on Mar 17, 2014, 07:46 pm
Tks lauszus,

I tried, but I got no good result. Also try the complementary filter (even values ??ranging from 0.98 to 0.02 and 0.02 to 0.98 in a formula). The result is always the same. Works fine on the ground, if the imu is static, but when starts to fly, the imu does not mark well.

I have purchased a mpu 6050to see if I had better results with the updated code and the first test with the kalman has not gone well. I think I have a lot of vibration. I'll try to isolate the 6050 and I'll try again.

its correct this code to change the gyro and acc range to 2000 deg/s and +-8g ?

Code: [Select]
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x03; // Set Gyro Full Scale Range to ±2000deg/s
  i2cData[3] = 0x02; // Set Accelerometer Full Scale Range to ±8g
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cuongpnv2t on Mar 18, 2014, 10:37 am
very clearn, thanks for sharing .
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lany on Mar 27, 2014, 05:55 am
The KF should be used for reducing the noise of measured value.
So IMO, the KF needs to be used on the raw data before you begin with calculating the angles.
Your XK vector has the following components. (L =lenth raw data)

XK=

LX
LY
LZ
LX¨
LY¨
LZ¨





Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: juanvivo on Mar 31, 2014, 04:54 pm
but lauszus code does that way, no?

Code: [Select]
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);


kalAngleX & kalAngleY should be correct angles of the plane at all times, right?

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lany on Apr 01, 2014, 06:27 am
IMO, in oder to take advantage from positive properties of gyroscope and accelerometer, you need to use a sensor fusion. These can be done by using the KF. In these case, a gyroscope and accelerometer are used together to create a more accurate measurement of overall movement and location through space.

Here you need to use the state vector with 6DOF:
X=[x, y, z, x", y", z"]

So your system matrix A is a 6x6.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: juanvivo on Apr 02, 2014, 01:37 pm
I'm back made ??a mess. I thought that just that to the code lauszus ...
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mariocaptain on Apr 04, 2014, 07:18 am
Hi,

I still don't understand why to convert AccX (Accelerometer X raw data) into degrees we use this formula:
roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;

instead of simply:
roll = (double)((accX/16384.0)*90.0); //IMU's range: 2g

I have been using the second formula on the MPU-6050(GY-521)  for its simplicity (and being less computationally expensive) and I think the angle is pretty accurate (keeping the IMU leveled on the table - checked with a level tool - returns something very close to zero). Can anyone elaborate on the accuracy of the first vs the second formula?

Should the MPU-6050 already does Y-Z-axis-dependent calculations internally and outputs an already-accurate raw value for accX?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lany on Apr 07, 2014, 12:41 pm

roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;


accX, accY, accZ are vectores. So you need to calculate the result-vector in accX-accZ-layer before you can calculate the Rad-Value between accY and  the result-vector of accX-accZ-layer. Later you need to convert all Rad-values into Deg.

gyX, gyY, gyZ, accX, accY, accZ needs to be Kalman-filtered.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mariocaptain on Apr 08, 2014, 10:14 am

accX, accY, accZ are vectores. So you need to calculate the result-vector in accX-accZ-layer before you can calculate the Rad-Value between accY and  the result-vector of accX-accZ-layer. Later you need to convert all Rad-values into Deg.

gyX, gyY, gyZ, accX, accY, accZ needs to be Kalman-filtered.


Lany, I am using a complemetary filter based on Lauszus' sample code (MPU6050.ino - actually this one implements both Kalman and Comp in the same file). For the complementary filter the atan equation for conversion was also used.

My question is why not just use the simpler second formula above? It works, I am just not sure if the atan conversion is more advantageous in some way?

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: grasshopper on Apr 11, 2014, 03:49 pm
Hi Lauszus and everyone,

I'd like to know why Lauszus added the values 2000.0, 3319.84 and 664.48 to the acceleration values read from the accelerometer registers. You can see it in this piece of code:

Code: [Select]
accX = ((i2cData[0] << 8) | i2cData[1]) + 2000.0;
  accY = ((i2cData[2] << 8) | i2cData[3]) + 3319.84;
  accZ = ((i2cData[4] << 8) | i2cData[5]) + 664.48;


Thanks very much for your answer!!

:)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 11, 2014, 09:13 pm
@juanvivo
It will only help to increase the resolution if you expect to do maneuvers that exceed +-2g.
What you could do instead is try to use the built in filter inside the MPU-6050. I am using that myself for the Balanduino: https://github.com/TKJElectronics/Balanduino/blob/aec22dafe8e98f0a16df6b5031a92354bb27ec5b/Firmware/Balanduino/Balanduino.ino#L225-L226 and the MPU-6050 datasheet: http://invensense.com/mems/gyro/documents/RM-MPU-6000A-00v4.2.pdf at page 13.

@Lany
Yes I know that you could do that as well, but it would be easier to for example just use a low pass filter on the raw acceleromter values.

@mariocaptain
Please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf.

@grasshopper
These are zero-values for the accelerometer - just send set them to 0. I have added a note about it: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/commit/21c9f6fcdf781861cb461c3078f9bef25cf3d5ca.
I'm planning to make a short calibration routine as I have done for the Balanduino: https://github.com/TKJElectronics/Balanduino/blob/aec22dafe8e98f0a16df6b5031a92354bb27ec5b/Firmware/Balanduino/Tools.ino#L109-L144, but for now you have to enter them manually.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: AlexTeos on Apr 12, 2014, 03:17 pm
Hi guys! Now I making a robot, and i have GY-521, i found a lot of examples of code, but nowhere everywhere there is no computing of Z axis.  Nobody needs it? Can somebody help me?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 17, 2014, 04:28 pm
@AlexTeos
Please take a look at the following code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050_HMC5883L.

You will need to buy a HMC5883L 3-axis magnetometer as well and use it together with the MPU-6050. Note that "GY-521" is just the name of the breakout board and not the sensor. The sensor on the "GY-521" is the MPU-6050.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: enkaytynguyen91 on Apr 26, 2014, 07:28 am
i see that:
Code: [Select]
roll = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

the angle will change : -180 to +180 degree
whether i change:
Code: [Select]
roll = (atan2(accY, accZ)+PI) * RAD_TO_DEG;
  pitch = (atan(-accX / sqrt(accY * accY + accZ * accZ))+PI) * RAD_TO_DEG;

To the angle change: 0 to 360?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Apr 27, 2014, 12:42 am
@enkaytynguyen91
Yes you can do that just fine ;)
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: timtianyang on May 09, 2014, 10:28 pm
Hi Lauszus, can you post a video in which you lay the board flat and slide it in x and y directions? I wanna to see if your code is susceptible to linear accelerations. I wanna see if pitch and roll change as you accelerate the board. Thanks!
Tim
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on May 15, 2014, 04:02 pm
@timtianyang
No I haven't got time to do that at the moment, as I got my finals coming up.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Geoffroy on May 27, 2014, 07:09 pm
Hi,

I just finished reading the whole thread  XD
As a project for a university course, we are making a 3 axis camera stabilizer. We are using a 6DOF IMU MPU6050 accelerometer and gyroscope. I'm using your code to control the pitch and the roll of the camera (huge thanks by the way for the code and all those explanations you gave to everybody that helped me). As it is impossible to calculate the yaw with this code, I'm trying to combine it with the I2Cdevlib by means of the quaternions.

My idea would be to combine both and so to be able to control the corrected pitch and roll and the non-drifting yaw. But I'm having FIFO overflow problems.
Does anyone try this kind of combination?

Thanks in advance for your reply.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ozzie2005 on Jun 01, 2014, 08:24 pm
Quote

I just updated the MPU-6050 code. The changes newest code can be found in the Github repository: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050.


Hi Lauszus,

The link above seems to be broken, is there anyway you can fix the link?

I searched for your MPU6050 library in google and got the your code for the MPU6050 but I don't know if that is the most recent version. There is something I don't understand from your code:

 accX = ((i2cData[0] << 8) | i2cData[1]) + 2000.0;
 accY = ((i2cData[2] << 8) | i2cData[3]) + 3319.84;
 accZ = ((i2cData[4] << 8) | i2cData[5]) + 664.48;
 
Where did you get these values in red from? I checked the MPU-6050 datasheet and could not find any of these values. Is there any reason why you add them to the ACCs? and if so, why these values?

I really appreciate your help.
Thank you,
Ozzie
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: michinyon on Jun 02, 2014, 01:14 am
You obviously didn't read the thread very carefully,   because only a few posts ago,   it was stated that those values are to compensate for the zeroing error of his particular device.    Your device will be different.      If you think that the zeroing error,  or asymetrical response,  of the device matters,   you can do you own experiments to measure it,   and then put those numbers into the equation.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ozzie2005 on Jun 02, 2014, 05:16 am
I must've missed that post but thank you for your time explaining the reason for these values.

cheers.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Geoffroy on Jun 14, 2014, 10:51 am
I recently bought a 10 DOF IMU from Drotek (MPU6050 gyrometre & accelerometre + HMC5883 magnetometre + MS5611 altimetre )

I wanted to use the library with Kalman filter  you provided for this board (MPU6050_HMC5883L) with my Arduino Uno.
But I get the following error : i2cWrite failed 2.
So I suspect it's a problem of address (I changed the address of the MPU6050 to 0x69 as mentioned by Drotek without success).
Let's acknowledge that I can run the MPU6050 code without a problem setting the address to 0x69.

Does anyone experienced and solved that problem?

Thanks in advance for any reply.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: enkaytynguyen91 on Jun 18, 2014, 05:03 am
Hi,
We know that: The default sensitivity is high, and the sensor returnes 16 bits,a variation of 50 is just a very small variation.
So, when we calculate the angle base on the raw data, what is the resolution of sensor (smallest angle that the sensor can detect)?
Thanks!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jun 21, 2014, 01:39 am
@Geoffroy
Are you by any chance using a Due? There was a bug in the Wire library for the Due: https://github.com/arduino/Arduino/issues/1976 which will be available when a new release of the Arduino IDE is available - or simply compile the IDE yourself.
For now simply ignore the returned value.

@enkaytynguyen91
It depends on what sensitivity you use. Please see these datasheets for more information: http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf and http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A-00v4.2.pdf.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Geoffroy on Jun 24, 2014, 09:27 pm
@Lauszus
No, I'm using a Arduino Uno. It seems that the problem lies in the fact that I can't access directly the magnetometer. Then I found that I should access it through the MPU6050 using it in the by-pass mode. But I'm not sure of the code I add.

Code: [Select]
  // Bypass mode
  while (i2cWrite(MPU6050, 0x6A, 0x00, true));
  while (i2cWrite(MPU6050, 0x37, 0x02, true));


Using that extra code, I'm able to get values of yaw, pitch and roll. But the yaw seems to be drifting. I don't know if it could be linked with the by-pass mode and if I can use another method to get those values.

Geoffroy
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 02, 2014, 12:35 am
@Geoffroy
It seems weird that you can't access both sensors on the same bus, but yes your code looks good to me if you want to use it in bypass mode.

You should be able to simply connect both sensors to SCL and SDA at the same time. Maybe try adding a pullup resistor to 3.3V and see if that solves the problem.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: caymanguy on Jul 04, 2014, 11:04 am
im using a different code
I like to use yours

https://www.youtube.com/watch?v=kIss1mZJWPs


mpu 6050
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: desraino on Jul 05, 2014, 11:38 am
First sorry for my bad english. I want to know something from your project MPU6050_HMC5883L Kalman Filter https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050_HMC5883L.

I recently bought a 10 DOF IMU GY-86 (MPU6050 gyrometre & accelerometre + HMC5883 magnetometre + MS5611 altimetre ) and use arduino mega 2560.

But I get the following error : i2cWrite failed 2. I attached Screen Shot for my problem.
I have changed the address of the MPU6050 to 0x69 as the comment, but didnt change anything.

Could everyone solved this? Or maybe have some experienced as me?

Thanks in advance for any reply.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: michinyon on Jul 05, 2014, 04:58 pm
Your wiring of the I2C connection is probably wrong.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: desraino on Jul 05, 2014, 07:05 pm
Whats wrong with the wiring  michinyon?

Could you tell me more about this? I really need it.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: shrims4u on Jul 13, 2014, 10:11 am
Can this combination give me an accuracy of angle measurement upto 5minutes(0.083333degrees).

If not Which sensors/IMU do I use to achieve such high precision/accuracy ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Barsil on Jul 16, 2014, 11:04 am
Hi Lauszus,

I am currently trying to read my digital IMU, the "6 Degrees of Freedom IMU Digital Combo Board - ITG3200/ADXL345" from Sparkfun with my Arduino Due and I am facing some trouble since it's my first time working with Arduino and Microcontrollers at all. My coding skills are the ones of a typical Mechanical Engineer: I know how code looks like.
I read through this blog and saw, that you and Kashif already solved that particular problem but that was a few years ago and I didn't become smarter by reading your exchange of replies.
I do get some data in the Serial Monitor by using the code from the late Fabio Versano (http://www.varesano.net/blog/fabio/my-first-6-dof-imu-sensors-fusion-implementation-adxl345-itg3200-arduino-and-processing) but it doesn't work 100% and I can't find the mistake. The newer FreeIMU Version doesn't work either, because the Due doesn't support EEPROM.
The code I like the best is yours but unfortunately it is for analog IMUs and even though I tried to combine the code of Versano and yours I couldn't get it work.
If there is an easy way to change your code to work with digital IMUs or even better, if you still have some working code for that problem I would be super appreciative if you could send it to me.

Thanks in advance and thanks for the great blog you're having here!
Barsil
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Lauszus on Jul 17, 2014, 08:03 am
@Barsil
Please have a look at the following example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/ITG3205_ADXL345.

Regards
Lauszus
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: laiiha on Jul 17, 2014, 06:11 pm
hello Lauszus,

Can i use this example with a 10dof (accelero, gyro, magneto) ?

thanks

laiiha
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Barsil on Jul 22, 2014, 08:07 am
Thanks for the response Lauszus,

the reason I got confused probably was that either my Arduino due or my Imu are broken. I tried different codes from different people including yours and after almost two weeks of failing I'm now more or less able to write my own code - which also doesn't work. If I print the raw readings I get values around 65k for the Accelerometers in X an Y direction and for the Gyros while the X Accelerometer changes to low values around 20 - 200 when moving it in the right way. The Accelerometer in Z shows constant values of 1023.

If something similar occured to you or you can tell me if this probably is because of broken gear or not, I would be helped very much.

Thanks again,
Barsil.

edit: The Gyro readings change from values between 65k and 0 when moving the Imu but return to around 65k when holding it still. Sometimes the values switch from values around 65k to values around 0 very fast when holding the IMU still.

Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Devkota on Jul 24, 2014, 03:24 am
@ Lauszus
I tried the same code (the Kalman FIltering part) that you've given the link to. Not as a header file, but as a loop in the main sketch.
Initially, I put the same values for the Q_angle and R_angle. The response of the Kalman FIlter was very slow as a result of it. It took forever to catchup with the angle of the accelerometer(which is the value that I am comparing the filter with). You change the direction of the tilt, and it doesn't even move from positive to negative quickly, it takes a lot of time.
As I try to decrease the values of the Q_angle and R_angle, even when one of the values reaches the 5th decimal, the output of the filter gets stuck on the initial value(that of the accelerometer) and does not change. How ever the same code in C(I printed the values of the accelerometer and gyroscope in a file and read it in the program) produces a very fast response and works.
So. my problem is that I'm sure the lower values of Q_angle and R_angle will do the trick and I will just need to tune them, but the output gets stuck in Arduino.
What do you think should I do?
Arduino 2560,
IMU=MultiWii MWC Flight Control Module,Gyro= ITG3205, Accelerometer= BMA180.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ashwin1224 on Jul 25, 2014, 09:46 am
I wish to add a lean sensor readout to my motorcycle. However I dont have strong physics on me. I can tinker with the tutorial codes a bit to get the angle readout but will this work on a moving motorcycle? From what limited research Ive done on the internet, it seems to me calculating the angle of a stationary object is fairly simple but when it comes to a motorcycle its a different thing altogether. If Ive understood it correctly then I think the combination of both the gyro and accelerometer will work, but again I am not too good at this.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Vpooler on Aug 07, 2014, 10:33 am
Hello Lauszus and others!

I too am making a balancing bot with PID controlling (I will get a Bachelors degree for it!) and good PID loop relies on good input data so I tried out your sample sketch for ADXL/ITG combo (got mine of dealextreme for pennies). For some reason, the Kalman filter seemed to be too reliant on accelerometer so the output data was rather noisy for me and jumped all over the place during rotation.

First column is Gyro, second is Accelerometer, third is Kalman and fourth is Complementary. As you can see, Kalman pretty much follows the accelerometer. Since I could not figure this out, I gave up on Kalman and looked into ways to improve complementary filter and I did succeed in this!
First I changed tuning to 99.7 and 0.03, which improved the stability a bit. Second, I implemented two instances of running median filters to smooth out spikes in raw data - I had problems with large spikes coming in from both gyro and accelerometer, gyro ones making the result drift more than 10 degrees ever so often and afterwards rolling back to right results over the course of several seconds - unacceptable. Sudden odd spikes from the accelerometer on the other hand were compromising the static stability, with fast jumps of +/- 1.5 degrees.
Now, the simple median filter works by looking at three readings and organizing them from smallest to biggest value, returning the middle one; so if we get three readings i.e. 190; 25; 192 it would be simple to see which of them is the bogus one and by rearranging them to 25; 190; 192 the 190 would be pretty much on spot. Unless we get more than one spike in a row, simple median filter will do the trick just fine. And since it is rolling, the correct value of 192 will also pass - it wont be omitted permanently, just used in the next calculation! If it is right, it will pass. But you can use more elaborate median filters with i.e 13 data points also, there is a lot written about implementing it on the web.
This is more precise than rolling average filter, which would've given us the result of 135.66. I also tried that and I also tried running complementary filter's result through Kalman, the second integer being gyro rate.
I will stick by the improved complementary from now on, saves up a few process cycles for me!

The microcontroller I am running my sketch on for now is a humble, honest Arduino Micro. And the data looks damn sexy on that Processing graph :)

PS: code is attached to this post! I left Kalman in for reference. If it works on your processor just as well as it should and is up par with complementary or even better then good, it is something I did wrong then :D
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: samhuang on Oct 30, 2014, 02:59 pm
Hello!
First sorry for my bad english.
I used mpu6050 + kalman filter+ servo control cameras on a motorcycle.I want to keep the camera level, but to consider the centrifugal force or acceleration component dynamic when it?
Now with mpu6050 + kalman filter seem to shake not work on a motorcycle, for a mpu6050 dmp6 static may be normal, but the actual loading on the car but can not achieve the desired angle. I have to modify the program? You can give directions? I do not know how write program. there are examples to follow?
I really need it.
Please provide comments to help me.
Thank you.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: JotaStar on Nov 05, 2014, 06:55 pm
Hi Lauszus

I'm trying to use ITG3205 and ADXL335 (not ADXL345) and I have a doubt about your code:

Code: [Select]
void loop() {
  double gyroXrate = -(((double)readGyroX() - zeroValue[3]) / 14.375);
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = (((double)readGyroY() - zeroValue[4]) / 14.375);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double accXangle = getXangle();
  double accYangle = getYangle();

  compAngleX = (0.93 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle);
  compAngleY = (0.93 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);

  double xAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter
  double yAngle = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter

  timer = micros();

  /* print data to processing */
  Serial.print(gyroXangle); Serial.print("\t");
  Serial.print(gyroYangle); Serial.print("\t");

  Serial.print(accXangle); Serial.print("\t");
  Serial.print(accYangle); Serial.print("\t");

  Serial.print(compAngleX); Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");

  Serial.print(xAngle); Serial.print("\t");
  Serial.print(yAngle); Serial.print("\t");

  Serial.print("\n");

  delay(10);
}


What is the purpose of gyroXangle / gyroYangle variables? You get the gyro rate (º/sec) over time (and it will generate drift) but yo dont use it to calculate the angles, or in complementary/kalman filter, isnt it?

I think you use it only for graphic / processing purposes... Am I right?

Another question that I have is that when you roll or pitch PCB close to 90º, the values get erroneous... this code only work for rotations < 90º in X and Y, is that correct? (I made a tipical 3D box with processing and using rotateX/rotateZ with the angles that I obtain with your code)

Thank you very much for your code and help.


Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mutenroch on Dec 02, 2014, 12:19 am
Dear Lauszus,
You said that hopefuly we have millis() in order to measure the time lapse between readings...
I'm searching the way to measure delta time with millis or micros all around the forum and the playground and i just  get dispersed clues but nothing useful for a newbie like me...
Could you please take a minute, or any other samaritan soul, to extend the explanation on how to calculate d-time from a sensor reading? Or suggest a paper or a link to work it out by myself?
Thanks in advance!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: satishr on Jan 23, 2015, 07:09 pm
Hi Lauszus,

Great guide on IMUs and the Kalman filter!

I seem to have a problem that some of the others faced but there is no mention of a solution in the thread.

I'm using a SEN10121 board (https://www.sparkfun.com/products/10121) which has an ADXL345 and an ITG-3200 with an Arduino Mega ADK.
I'm using your code from here - https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/ITG3205_ADXL345

Although you mention that the angle range at the output end of the Kalman filter should be -90 to 90, I'm getting values as follows -
(Let us consider anticlockwise rotation starting from 0 degrees on y axis)

Actual Rotation (Degrees)        Output from Kalman Filter (Degrees)
0                                                        0/360
90                                                      22-23
180                                                    0/360
270                                                    342-343
360                                                    0/360

Is there a way this can be corrected?
Your response will be really helpful.

Thank you!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mrburnette on Jan 25, 2015, 03:46 pm
Dear Lauszus,
You said that hopefuly we have millis() in order to measure the time lapse between readings...
I'm searching the way to measure delta time with millis or micros all around the forum and the playground and i just  get dispersed clues but nothing useful for a newbie like me...
Could you please take a minute, or any other samaritan soul, to extend the explanation on how to calculate d-time from a sensor reading? Or suggest a paper or a link to work it out by myself?
Thanks in advance!
I always think that examples are easier to understand than a long-winded old man. So, you can take a look at this project: HERE (http://www.hackster.io/rayburne/smells-like-air-pressure-bmp180-mems-on-arduino-pro-mini) and this code is responsible for setting up the timing loop for the graph:

In the ScrnFuncts Tab, the function:
Code: [Select]

void drawGraph(unsigned long Pressure)
  const unsigned long GraphResponsemS = 514286 ; // full screen of 84 slots every 12 hours


To use the time as an event, this code makes it happen:
Code: [Select]

  PlotTime = TimeMarker + GraphResponsemS;    // this is our trigger to plot

  if (PlotTime < millis() )  // machine clock has advanced beyond trigger, so react
  {


So, what we are really doing is setting up a situation where we wish something to happen at a future time. Then we check with each iteration of the main loop() to see if that has happened; that is we compare the current time (milliseconds) to that future time - when the "future" event is current or past, it is time to act.

After acting, we need to set our trigger event into the future again...
Code: [Select]

    PlotTime = millis();              // Reset with current time for future event triggering
  }


I have a lots more examples of simple projects on my project page (http://www.hackster.io/rayburne).

Sorry for hijacking the topic.

Ray
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: chfakht on Feb 04, 2015, 04:09 am
Hi ,
Firstly i thank you a lot for your effort and for sharing your knowledge
i'm actually working on a project for indoor localisation so i need to know how to use
the MPU9060 (interpretating accceleration anf gyroscope values ) to do so ...
 and please another question : how can i save all values in matlab workspace

Thanks a lot
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: max25 on Feb 04, 2015, 08:08 pm
@ Lauszus Hi I'm using the ITG 30205 and ADXL345 and want to know how you got the values of bias zeroValue [5] = {-200, 44, 660, 52.3, -18.5} to your ITG3205_ADXL345.ino code, I made a code just for this, the more the values do not agree especially in the case of the gyro, you can help me.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: cbassett1000 on Feb 06, 2015, 06:11 am
Hi all, I am new to the accelerometer/gyro world but am eager to learn.  I have the ITG30205/ADXL345 IMU with I2C.  I have used Lauszus's code (from Github) and Vpooler's code.  I get data values only of 225, 270, 180, and 325-335.  I was expecting 0, 45, 90, 135, 180, 225, 270, 315, 360 or 90 degree combinations something to that effect.  Is this correct?  Could there be something wrong?  Also my goal is to have the output "stable" (1 to 5 degree accuracy)  I hope that's not a pipe dream.  Could there be a way to get the code to go to 1 degree increments if not real-time output in degrees?  Any help is appreciated.

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: _MeRKeZ_ on Feb 26, 2015, 11:25 am
Hi friends,

Thanks for your all sharings. I need to ask a few questions. If you help me, I appreciate

To find 'gyroRate' , we will use this formula " gyroRate = (gyroAdc-gyroZero)/sensitivity "

(http://hizliresim.com/XB6Jko)

Is this 'sensitivity' value for my choice?

How can I find the 'gyroZero' value?

To find 'accVal' we will use this formula " accVal = accAdc-accZero "

(http://hizliresim.com/7oDPXr)

How can I find zero voltage and also accZero? Is not sensitivity needed to calculate acc value?

Thanks

Edit: This data belongs to MPU6050
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Abdul_Rehman on Jul 15, 2015, 10:16 pm
Q_loc_estimate[][]={{Q_loc_estimate},{Qestimate[1][1]}};
is this correct initialization?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: HAmzaaaaa-500 on Sep 12, 2015, 01:56 am
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;

unsigned long timer;

double zeroValue[5] = {950, -400, 13500, -100, -500};

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;

double compAngleX = 180;
double compAngleY = 180;


void setup() {

   // join I2C bus (I2Cdev library doesn't do this automatically)
   Wire.begin();
   Serial.begin(115200);

   // initialize device
   Serial.println("Initializing I2C devices...");
   accelgyro.initialize();

   // verify connection
   Serial.println("Testing device connections...");
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");


  timer = micros();
}

void loop() {
   // read raw accel/gyro measurements from device
   //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

   accelgyro.getRotation(&gx, &gy, &gz);
   
   double gyroXrate = -((gx-zeroValue[3])/131); //Change the sensitivity after your sensor
   gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
   
   double gyroYrate = ((gy-zeroValue[4])/131);
   gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Without any filter
   
   
   ///////////////////////////
   //The acc X and Y angle///
   //////////////////////////
   accelgyro.getAcceleration(&ax, &ay, &az);
   
   double accXval = ax-zeroValue[0];
   double accZval = ay-zeroValue[2];   
   double accXangle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
   
   double accYval = ay-zeroValue[1];
   accZval = ay-zeroValue[2];   
   double accYangle = (atan2(accYval,accZval)+PI)*RAD_TO_DEG;
   
   //////////////////////////
   //////////////////////////
   
   compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle);
   compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

   timer = micros();
   
   
 Serial.print(compAngleX);Serial.print("\t"); 
 Serial.print(compAngleY); Serial.print("\t");
 //Serial.print(timer); Serial.print("\t");
 Serial.print("\n");
   
  delay(10);


   
}
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: ArthurD on Sep 12, 2015, 09:47 am
Having got desperate about MPU6050 I meanwhile use a completely different IMU sensor (3D Gyro, 3D Accelerometer, 3D Compass, feat. onboard Kalman filtering and sensor fusion by an onboard-cpu - no calculation any more, no monster libs to #include, just read single I2C register or read by UART.


http://www.robot-electronics.co.uk/htm/cmps11i2c.htm

http://de.manu-systems.com/CMPS11.shtml (http://de.manu-systems.com/CMPS11.shtml)
http://www.hobbytronics.co.uk/cmps11-tilt-compass (http://www.hobbytronics.co.uk/cmps11-tilt-compass)

Quote
The CMPS11 module is a true tilt compensated compass for a fraction of the price of competing modules. Unlike similar modules which just give you the raw data, this little board incorporates a powerful 16-bit processor which performs all the complex calculations to give an output of 0 to 359.9 degrees.

Employing a 3-axis magnetometer, a 3-axis gyro and a 3-axis accelerometer. A Kalman filter combines the gyro and accelerometer to remove the errors caused by tilting of the PCB. The CMPS11 produces a result of 0-3599 representing 0-359.9 or 0 to 255. The output of the three sensors measuring x, y and z components of the magnetic field, together with the pitch and roll are used to calculate the bearing, each of these components are also made available in there raw form. The CMPS11 module requires a power supply at  3.6 - 5v and draws a nominal 25mA of current. A choice of  serial or I2C interfaces are provided.
I'm perfectly fine with it. This is example code for fusioned+filtered heading, pitch, and roll , but all single 3D sensor values also can be polled quite easily (CMPS10 is the predesessor, same code):

/****************************************************************
*                  Arduino CMPS10 example code                  *
*                    CMPS10 running I2C mode                    *
*                    by James Henderson, 2012                   *
*****************************************************************/
#include <Wire.h>
#include <SoftwareSerial.h>

#define ADDRESS 0x60                                          // Defines address of CMPS10

#define LCD_RX              0x02                              // RX and TX pins used for LCD0303 serial port
#define LCD_TX              0x03
#define LCD03_HIDE_CUR      0x04
#define LCD03_CLEAR         0x0C
#define LCD03_SET_CUR       0x02

SoftwareSerial lcd03 =  SoftwareSerial(LCD_RX, LCD_TX);      // Defines software serial port for LCD03

void setup(){
 Wire.begin();                                               // Conects I2C
 lcd03.begin(9600);
 lcd03.write(LCD03_HIDE_CUR);
 lcd03.write(LCD03_CLEAR);
}

void loop(){
  byte highByte, lowByte, fine;              // highByte and lowByte store high and low bytes of the bearing and fine stores decimal place of bearing
  char pitch, roll;                          // Stores pitch and roll values of CMPS10, chars are used because they support signed value
  int bearing;                               // Stores full bearing
 
  Wire.beginTransmission(ADDRESS);           //starts communication with CMPS10
  Wire.write(2);                              //Sends the register we wish to start reading from
  Wire.endTransmission();

  Wire.requestFrom(ADDRESS, 4);              // Request 4 bytes from CMPS10
  while(Wire.available() < 4);               // Wait for bytes to become available
  highByte = Wire.read();         
  lowByte = Wire.read();           
  pitch = Wire.read();             
  roll = Wire.read();             
 
  bearing = ((highByte<<8)+lowByte)/10;      // Calculate full bearing
  fine = ((highByte<<8)+lowByte)%10;         // Calculate decimal place of bearing
 
  display_data(bearing, fine, pitch, roll);  // Display data to the LCD03
 
  delay(100);
}

void display_data(int b, int f, int p, int r){    // pitch and roll (p, r) are recieved as ints instead oif bytes so that they will display corectly as signed values.
 
 lcd03.write(LCD03_SET_CUR);                     // Set the LCD03 cursor position
 lcd03.write(1); 
 lcd03.print("CMPS10 Example V:");
 lcd03.print(soft_ver());                        // Display software version of the CMPS10
 
 delay(5);                                       // Delay to allow LCD03 to proscess data 
 
 lcd03.write(LCD03_SET_CUR);
 lcd03.write(21); 
 lcd03.print("Bearing = ");                      // Display the full bearing and fine bearing seperated by a decimal poin on the LCD03
 lcd03.print(b);                             
 lcd03.print(".");
 lcd03.print(f);
 lcd03.print("  ");

 delay(5);

 lcd03.write(LCD03_SET_CUR);                     // Display the Pitch value to the LCD03
 lcd03.write(41);
 lcd03.print("Pitch = ");
 lcd03.print(p);
 lcd03.print(" ");

 delay(5);
 
 lcd03.write(LCD03_SET_CUR);                     // Display the roll value to the LCD03
 lcd03.write(61);
 lcd03.print("Roll = ");
 lcd03.print(r);
 lcd03.print(" ");
}

int soft_ver(){
  int data;                                      // Software version of  CMPS10 is read into data and then returned
 
  Wire.beginTransmission(ADDRESS);   
  Wire.write((byte)0);                           // Sends the register we wish to start reading from
  Wire.endTransmission();

  Wire.requestFrom(ADDRESS, 1);                  // Request byte from CMPS10
  while(Wire.available() < 1);
  data = Wire.read();         
 
  return(data);
}







Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Fraser-Island on Nov 30, 2015, 08:20 pm
I have a problem using the sketch made by Lauszus  ( downloaded from github) and my mpu6050 with the arduino mega2650

The complementary and Kalman angle both move back to 0° very soon, but my gyro is still in displaced position and should display e.g 30°

But it goes back to zero


Is this normal?

Can anyone help me please?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: fjarias on Dec 16, 2015, 04:39 pm
Hi everybody, im from Argentina, and i have an mpu-6050 with Atmega328p-pu. I can not determine whether this MPU6050 is moving horizontally on the Y axis as values fluctuate far as anyone knows whether the MPU is moved on said shaft for feacientemente going forward or backward UNUSED CORNERS !!! ?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: MicroNUno on Jan 09, 2016, 01:39 pm
I'm a total beginner so this probably is a  silly question:

When I compile the gitHub code, it Shows the following Errors in the Consol :

"

MPU6050_with_Kalman_Filter.ino: In function 'void setup()':
MPU6050_with_Kalman_Filter:58: error: 'i2cWrite' was not declared in this scope
MPU6050_with_Kalman_Filter:59: error: 'i2cWrite' was not declared in this scope
MPU6050_with_Kalman_Filter:61: error: 'i2cRead' was not declared in this scope
MPU6050_with_Kalman_Filter:70: error: 'i2cRead' was not declared in this scope
MPU6050_with_Kalman_Filter.ino: In function 'void loop()':
MPU6050_with_Kalman_Filter:98: error: 'i2cRead' was not declared in this scope
'i2cWrite' was not declared in this scope

"

Does anyone kno how I can fix these?

 
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mahim on Feb 16, 2016, 01:15 pm
Hi,
I am working on Inertial sensors based gait analysis. I am using the inertial sensor module- GY80. Using various sites online, I have written a code to determine the angles using ADXL345 and L3G4200D.
I have somehow obtained angles individually. But when I plot them in MATLAB, comparing the angle values, the plots do not follow the same patterns. So I am not sure if I can apply Complementary filters or such Sensor fusion algorithms.
I have pasted my code and a sample plot here.
The commented parts of the code were for my experiments.

Please tell me what I am doing wrong.

Thank you.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: hamood_elazhare on Feb 27, 2016, 08:45 pm
heey sir :
im now Working On IMU 9DOF and i read your great topic about Klman filter and im from your fans in your method of explaning
so i have an 10 DOF IMU (MPU6050+HMC5883L+BMP08)
so i saw u have so many many codes for every thing and i get confused how can i select my one and also if i want to detect it

i want u plez to just show me the really sutable one of  code that reads this data from 3-axis Accel+3-AxisGyro +3-Axis Compass with Kalman filter
and thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Darshandarshu on Mar 10, 2016, 10:13 am
Can anyone suggest me a good nonlatching hall effect sensor to find rpm of cvt in an off road car
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: aammrruull on Apr 06, 2016, 03:47 am
-Lauszus

i want to ask a question about your code kalman mpu6050

how to change te max of pitch to 180 degree? because in your code the pitch just can make the max is 90 degree

thankyou
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: NST1992 on Apr 26, 2016, 05:34 am
Hi Laszaus,

Is there a way to change the output of the Kalman filter to give me the data of yaw, pitch and roll instead of the pitch and roll data?

How do i put the code into the 9 dof razor firmware for me to filter out the data output?

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: NST1992 on Apr 26, 2016, 05:44 am
Hi Laszaus,

Is there a way to change the output of the Kalman filter to give me the data of yaw, pitch and roll instead of the pitch and roll data?

How do i put the code into the 9 dof razor firmware for me to filter out the data output?

Thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mario123 on Jul 12, 2016, 05:36 pm

Hi Lauszus , When you give instructions for Measure 360 , exactly where that portion of code will , in addition to What do you mean the values ​​of the variables acceleration on the axes, and angles . These variables are not defined. If you can publish the code with the changes that you mention it, that seriously simpler
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: mario123 on Jul 12, 2016, 06:08 pm
Hi Lauszus , When you give instructions for Measure 360 , exactly where that portion of code will , in addition to What do you mean the values of the variables acceleration on the axes, and angles . These variables are not defined. If you can publish the code with the changes that you mention it, that seriously simpler
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: DimitrisTzam on Sep 12, 2016, 04:53 pm
Hi I'm using mpu9250 digital 9dof  sensor so I get digital data and not analog. Which is the formula to calculate pitch and roll with digital data?
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: dhal_sauce on Dec 12, 2016, 02:00 am
Good day Mr Varesano, I love the work that you have done with FreeIMU. I am currently trying to use your source code(particularly your yaw_pitch_roll example) but unfortunately, I have a sensor that is not supported by your code ( I am using an AltIMU 10 v5 which is comprised of the LSM6DS33 accel and Gyro, the LIS3MDL magnetometer and the LPS25G Barometer). I am able to read the raw data from all of the sensors but I do not know how to incorporate the new sensors into your code.
My question to you is which parts of your existing code would I have to include the different information for the individual sensors? I am really finding it difficult to follow where your code is referencing all of the functions for the different sensors from.
Any assistance will be greatly appreciated.

Regards.
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: albannava on Oct 29, 2017, 11:44 am
Would it be possible for you to list the components needed for the project?

Thank you!
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: tim77777 on Jan 04, 2018, 01:47 am
regarding the forumla
angle = 0.98 *(angle+gyro*dt) + 0.02*acc

which gyro output is used. If it is pitch angle, then is the Y gyro output used?

thanks
Title: Re: Guide to gyro and accelerometer with Arduino including Kalman filtering
Post by: Soofiyan on Jan 12, 2018, 06:03 pm
i have a problem using gyro with arduino if i power with external power supply of 5V then the arduino is callibrating again and again and if connect it directly it has no issues please look at my problem.