Go Down

### Topic: Madgwick filter algorithm for IMU (Read 8547 times)previous topic - next topic

#### AUSTeam

##### Mar 13, 2014, 01:49 pm
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);}`

#### robtillaart

#1
##### Mar 13, 2014, 07:39 pm
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 pmLast 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

#3
##### Mar 14, 2014, 12:28 am
"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

#4
##### Mar 14, 2014, 08:24 am

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

#5
##### Mar 14, 2014, 08:29 am

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?

#### Merlin513

#6
##### Jun 26, 2014, 11:02 pm
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

#### jremington

#7
##### Feb 01, 2016, 05:30 pm
Without an external reference (like a magnetometer) the yaw angle will drift. Drifting on the other rotation axes is due to gyro offset, which should be corrected by the accelerometer.

I don't know how that particular code works, but consider trying the much more recent contribution RTIMULib, which works very, very well with most popular sensors.

#### cr0sh

#8
##### Feb 01, 2016, 07:58 pm
Minor things - but everywhere you have 3.3 / 4095 (sure that shouldn't be 4096?) - you can pre-calc that and put it as a constant (ideally with a comment note for what it is).

Same for (180 / PI) and (PI / 180).

I'm not saying that these changes will fix any problems you are having (I highly doubt it) - but they are minor optimizations you could make up-front (then again, maybe the compiler already takes care of it, now that I think about it).
I will not respond to Arduino help PM's from random forum users; if you have such a question, start a new topic thread.

Go Up

Please enter a valid email to subscribe