Go Down

Topic: Madgwick filter algorithm for IMU (Read 1 time) previous topic - next topic

AUSTeam

Hello,

We are trying to implement Mayhony & Madgwick IMU filter algorithm on the Arduino megaboard 2560 (we tried both filters). We got the algorithm from this site: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

So far, we pretty much copy pasted the algorithm onto the board. We are using the Oilpan IMU (its a 6 axis with gyro and accelerometer w/o magnetometer). Also, we tried adjusting the gyromeasurement definition in the code but the results didn't improve.

Problems:

We are getting readings from the board but they seem to drift over time. For example, when we pitch the board the results show some value. This value then starts to decrease to the negative of the value and then increases again. It keeps oscillating like this. In certain cases, however, this doesn't happen and the value remains stable.

Another problem is that when we pitch the board we get both pitch and roll readings and vice versa with the roll. So for instance, if we pitch for 20 degrees we get some value for pitch and some value for roll (and then the drift problem comes in with both).

Our code is as follows:

Code: [Select]
void loop(){

      //Read Sensors Voltage levels
AZ = Read_ADC(3)*(3.3/4095);
AY = Read_ADC(6)*(3.3/4095);
AX = Read_ADC(2)*(3.3/4095);

AccelZ = (AZ-1.66) * 3.3333 * g; // Calculate Acceleration in Z
AccelY = (AY-1.66) * 3.3333 * g; // Calculate Acceleration in Y
AccelX = (AX-1.66) * 3.3333 * g;

GZ = (Read_ADC(0)*(3.3/4095)) - 1.35; // Read Gyro sensor
        GX = (Read_ADC(4)*(3.3/4095)) - 1.36;
        GY = (Read_ADC(1)*(3.3/4095)) - 1.35;
       
GyroRateZ = (GZ * 500)* PI/180; // Convert to rads/s
        GyroRateX = (GX * 500)* PI/180;
        GyroRateY = (GY * 500)* PI/180;

MadgwickAHRSupdateIMU(GyroRateX,GyroRateY,GyroRateZ,AccelX,AccelY,AccelZ);

       
        Yaw_m=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1)*180/PI;
        Pitch_m=-1*asin(2*q1*q3+2*q0*q2)*180/PI;
        Roll_m=atan2(2*q2*q3-2*q0*q1,2*q0*q0+2*q3*q3-1)*180/PI;

// The data is then sent through a serial port to Matlab for plotting
        Serial.println("B"); // Indicate Beginning of data
Serial.println(Pitch_m);
        Serial.println(Roll_m);

}

Thanks in advance!

robtillaart

just on what I see in the code:

- you do not use readADC(5)  is this correct?  // you use 0,1,2,3,4,6
- first you have the sequence Z,Y,X  and suddenly you use Z,X,Y ; is this right in all case? better be consequent in "rythms"
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

pito

#2
Mar 13, 2014, 09:14 pm Last Edit: Mar 13, 2014, 09:17 pm by pito Reason: 1
Quote
We are getting readings from the board but they seem to drift over time.

The MEMS gyros drift a lot, as they do not provide an attitude (like the old style mechanical gyros) but "an angular acceleration only" what is a bit difference (because you have to integrate their output in time in order to get an attitude).
The MEMS gyros drift degrees/second, the old mechanical one degrees/hour.
So you cannot avoid that. Therefore the guys use GPS or magnetic compass to "eliminate" the drift..

michinyon

"drift over time"  is inevitable,    about the vertical axis.

The system can use gravity to estimate which way is up and down,   but without a magnetometer is cannot correct for drift around the vertical axis.

AUSTeam


just on what I see in the code:

- you do not use readADC(5)  is this correct?  // you use 0,1,2,3,4,6
- first you have the sequence Z,Y,X  and suddenly you use Z,X,Y ; is this right in all case? better be consequent in "rythms"



Read_ADC(5) will read Channel 3 on the A/D (it's ADS7844) chip, which is connected to a temperature sensor.  We double-checked the reading of each axis from the A/D converter and they are consistent from what we have expected.


AUSTeam


Quote
We are getting readings from the board but they seem to drift over time.

The MEMS gyros drift a lot, as they do not provide an attitude (like the old style mechanical gyros) but "an angular acceleration only" what is a bit difference (because you have to integrate their output in time in order to get an attitude).
The MEMS gyros drift degrees/second, the old mechanical one degrees/hour.
So you cannot avoid that. Therefore the guys use GPS or magnetic compass to "eliminate" the drift..


So, are you saying that Madgwick or Mahony's algorithms are designed to work with 9-axis IMUs? If so, then why do they implement a standalone function that only accepted data from 6-axis IMUs?

Hi AUSTeam,

What robtillaart says is quite true you do tend to get varying yaw drift with mems gyros but there are things that you can do minimize to quite acceptable levels. I have been using the Mahoney algorithm for quite some time with several different IMUs - FreeIMU v4, Altimu-10, DFROBOT 10dof, sparkfun 10724 sensor, mpu-9150 and mpu-9250. So far the mpu-6050 with the hmc5883l has the best results. Either way the algorithm works with either the 6 DoF or 9 DoF sensors.  Experience so far is the 6DOF is the most stable.  If you want to see what it can do check this youtube video that I made: http://youtu.be/lwlOZzNJqJc, using the Mahoney algorithm. I am not familiar with IMU you are using and can't really find anything on line about it.

Two big things that I found is that you have to tune Kp and Ki for the sensors that you using and make sure you have calibrated the accelerometer and magnetometer. Not sure where you do that in your code.  The notes in the video also show you how to tune Kp and Ki based on suggestion by Madgwick on one of the DIYdrone forums.  Also, suggest that you zero out the gyros when they are not moving, i.e., take about 100 or so readings, average and the subtract from you gyro readings as you go.

Mike

Go Up