I just reposted it, so even more will get use of it. The code can be found here:
https://rapidshare.com/files/1073395588/IMU6DOFVer2.zip
https://rapidshare.com/files/3588312346/IMU6DOF.zip
https://rapidshare.com/files/4139847836/Graph.zip
Hallo everybody
I recently bought this analog 6DOF (six degrees of freedom) IMU board (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. I have attached my code for Arduino. 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 Earth gravitational acceleration (g) 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 10 bit resolution, for example this should be 4095 for 12 a ADC bit module). 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 horizontal (this is done in the code - look under "Calibration") 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 and 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 and thereby calculate the angle of the gyro. The final equation will look like this:
gyroAngle=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 gravitational acceleration (g) in three dimensions. To translate the analog readings I use this equation:
accVal = (accAdc-accZero)/sensitivity - where accVal are the g's measured by the sensor, accAdc is the analog reading, accZero is the value when it is horizontal (this is done in the code - look under "Calibration"). Sensitivity is the sensitivity found in the datasheet, but translated into quids.
This is done the same way as with the gyro. If you look in the datasheet (http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf ) you will see that the sensitivity is about 270-330 mV/g. I use the top one (330mv/g) because the the Vs is connected to 3.3V and not 3V. You might want to find out what your true sensitivity is by experimenting.
To translate this into quids again use this equation: sensitivity/3.3*1023.
So in this example I get:
0.33/3.3*1023=102,3.
The final equation will look like this:
accVal = (accAdc-accZero)/102,3
To calculate the angle you first have to calculate the force vector (see http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/#step1 for information) to do so I just use Pythagoras in three dimensions:
R = sqrt(accXval2+accYval2+accZval2).
You can then calculate the angle between the x-axis and the force vector (R) by using acos (inverse of cosinus):
accXangle = acos(accXval/R)
It is the same with the y-axis:
accYangle = acos(accYval/R)
The output will be as radians. To tranform it into degrees you just multiply the result with 57.295779513082320876798154814105.
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. I do not know the exact theory of how it works, but a simple explanation is that it combines the two calculated angles and then "guesses" the true angle with mathematics.
Basically it works in three steps:
First it gets the accelerometer angle
It then compares it with the angle calculated by the gyroscope as well as the last angle
Outputs the estimated angle based on the previous step
I did not make my own Kalman filter, so I can not tell exactly how it works, instead I used the Kalman filter from this project: http://www.x-firm.com/?page_id=145 .
If you do not like using something you do not understand, you can use something 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 attach my own code for my 6DOF IMU (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.
What do you think? Did I get something wrong or any ideas for improvements?
Sources:
http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
http://www.x-firm.com/?page_id=148
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.
I have attached my code, both the updated code for the Arduino and the Processing code. It is also possible to see the data from the y-axis. Just uncomment drawAxisY(); in the code.
My Balancing robot
Below is a video of my balancing robot. It uses the same IMU and algorithm as described in the post above.


































Secondly, I want to develop quadrotor. I'm newbie but I started learning it.
1. How many IMU DOF is minimum required for it ?
2. Can I use Arduino Uno for it ?
will this solve my purpose?
I don't think a Arduino Uno has enough I/O's and enough SRAM and flash. Try have a look at the Arduino Mega board instead: http://arduino.cc/en/Main/ArduinoBoardMega.
Best Regards
Kristian Lauszus
0v on pot to 0v on arduino and middle pot to any analog of arduino ....and then conncet the s1 and 0v on sabertooth to pin 5 on pwm arduino and pin 0 arduino ...is that enough ....? is that all? can any answer
while trying to search to built my owen segway ....i bought to accomplished my project .
1-Sabertooth 2x25 motor controller http://www.dimensionengineering.com/datasheets/Sabertooth2x25.pdf
2-imu razor combo analog 6 dof sensor http://www.sparkfun.com/products/10010
3-arduino mega 2560..(micro controller )
http://arduino.cc/en/Main/ArduinoBoardMega2560
kindly, could you have look to this connection on link below ..between the arduino and sensor?
http://voidbot.net/razor-6dof.html
could you help me find the best connection between the sabertooth and the micorcontroller ?
and then i'm thinking (( Just ))connect S1 and 0v on sabertooth to to pin 4 on arduino (for example) and ground on arduion again...
and ignore S2, and 5v.....? what do you think ?
do you can make me small diagram??
To use the sabertooth in analog mode, you have to connect it to the Arduino like so:
Sabertooth - Arduino
0V - GND
S1 - PWM pin1
S2 - PWM pin2
Remember that you have to filter the output of the PWM signal, to convert it to a analog signal. See "Note on using filtered PWM in Analog Mode" section at page 11.
Then you have to connect the battery as so:
Sabertooth - Battery
B+ - Positive pole
B- - Negative pole
And your motors:
Sabertooth - Motors
M1A - Motor1
M1B - Motor1
M2A - Motor2
M2B - Motor2
To enable Analog input mode, see page 10: "Analog input mode is selected by setting switches 1 and 2 to the UP position. Switch 3 should be either up or down, depending on the battery type being use."
You can also drive the motors using UART (Serial). The connection would be like this:
Sabertooth - Arduino
GND - GND
S1 - TX
See section "Baud Rate Selection" to see how to select the correct baudrate.
Regards
Lauszus
you know ...so far and as far as i know .. you the first useing the analog 6 dof sensor to built segway...it will be great experience..
.please put me in touch if you got website ...or youtube...show the steps...all the best.
becouse i use the arduino mega 2560..the connection to sabertooth
Sabertooth - Arduino
0V - GND
S1 - PWM pin2
S2 - PWM pin3
Is that right?
also to filter the signal from pwm useing 10k ohm ressistance ..should i take two ground from the arduino....and connect the ressistace in between...or one ground and connect all togather ....? or my quistione confused you...
Thanks...
..
I'm not sure what you mean, but take a look at "Figure 4.1: Filtered PWM" at page 11 of the datasheet: http://www.dimensionengineering.com/datasheets/Sabertooth2x25.pdf.
Just hook it up like the diagram. It doesn't matter if you only hook one of the GND pin to 0V on the Sabertooth.
Regards
Lauszus
Would this 6 DOF Kalman filtering work fast enough in order to provide roll stabilization for a sounding rocket? Would this code also work with other accelerometer/gyros (especially ones resistant at high g's)? What should be changed in the code if one changes the type of accelerometer/gyros?
In a sounding rocket we are talking of speeds ~ 600 m/s (Mach 2) and accelerations (during boost phase) of about 30g's.
Of course the roll stabilization would only be needed during the coast phase, when the motor already burnout, so the acceleration is way smaller (probably around 2-3 g since it is generated only by gravity and an ever decreasing drag force).
Best regards.
It will differently work with other IMU's. If you change the measurement and process noise covariance:
const float Q_angle = 0.001; // Process noise covariance for the accelerometer
const float Q_gyro = 0.003; // Process noise covariance for the gyro
const float R_angle = 0.03; // Measurement noise covariance
Another thing you have to notice is, you will then need a accelerometer that can angle 30g. The one I used is limited to only -+3g and you can even damage the accelerometer if you expose it to that amount of acceleration. My best advice is to go at some forum for amateur rockets entuatist and ask them what kind of sensors they are using :)
As it stands right now the IMU just gives you attitude information.
How would you blend the GPS data into the Kalman filtering in order to give you full autopilot capabilities?
Best regards!
You might want to ask at the aeroquad forum for more info: http://aeroquad.com/forum.php
- Lauszus
I have got a bit of a hick-up i wanted to try your code with a 5DOF from http://www.sparkfun.com/commerce/product_info.php?products_id=741
getting rid of the zgyro, but im having troubles because the gyro and accelerometer are not configured as yours, I tried inverting the gyro but to no avail,
I cannot get any results from the graph, please help
Hows your balancing robot going anyway?
Do you mind telling me why we are dividing dtime by 1000,thanks in advance
What is dtime?