Go Down

Topic: Complementary filter with the MPU6050 (Read 37671 times) previous topic - next topic

stratjnd

Hi,

I recently acquired an MPU6050. After playing around a bit using code I found online, I have managed to be able to read data from it. Now, I would like to use a complementary filter to give me 1 angle for the board. I know that a complementary filter combines accelerometer and gyroscope data together. I have been trying to find some arduino code that shows me the filter. I have found many useful tutorials but no code. Where I need help is how to find out 'dt' J have read it has something to do with the sample rate. Also, if you could help me in 'streamlining' the code from the Arduino MPU-6050 tutorial that would be much appreciated.

Thanks!
StratJnd

PaulS

Quote
Also, if you could help me in 'streamlining' the code from the Arduino MPU-6050 tutorial that would be much appreciated.

Sure. What functionality do you want to leave out?
The art of getting good answers lies in asking good questions.

Caltoa

This is a good guide:
http://forum.arduino.cc/index.php?topic=58048.0

This site has a full library to use the chip:
http://www.i2cdevlib.com/

stratjnd

Thanks!!

Caltona, I read through the  guide you gave me and I can get the MPU-6050 working fine, the problem comes with the filter.

I don't understand how to calculate the value dt in the complementary filter.

PaulS, the code on the Arduino tutorial page seems to have a lot of variables at the beginning. I am no expert, but if could I just have a simple I2C program that recieves data of the MPU-6050?

Thanks again!

AWOL

Quote
PaulS, the code on the Arduino tutorial page seems to have a lot of variables at the beginnin

The one I can see has no variables whatsoever at the top of the sketch, just a lot of definitions.
If you don't like them, put them in a header.

racquemis

#5
Feb 07, 2014, 03:27 pm Last Edit: Feb 07, 2014, 11:21 pm by racquemis Reason: 1
Quote from: stratjnd

Where I need help is how to find out 'dt' J have read it has something to do with the sample rate.


I don't understand how to calculate the value dt in the complementary filter.

Do you actually understand why you need dt? and what it is used for?

The gyroscope measures angular the velocity but what we actually want from it is a position (angle). If you knew the angular velocity at any given time you could always know the exact position of the sensor. Our gyroscope however doesn't sample at an infinite frequency. It knows the angular velocity at A and the angular velocity at B, but it doesn't know what happens to the velocity in between. We do however know the time between A and B. (which is dt). By assuming the velocity remains constant between A and B and multiplying by dt you can estimate the position. A smaller dt leads to a more accurate estimate.

Now, how do we find dt?:
1) For a rough estimate you can use the value of the delay in your sketch which you used to set the samplerate. dt is in this case a constant.

2) For a slightly more accurate estimate you can add a Serial.println(millis()) to area in your sketch where you print out the raw mpu6050 values. It will print our the elapsed time since startup.Let it run for a while and calculate the difference between two consecutive values. this will be your dt. add it to your sketch as a constant.

3) If you really want the most accurate dt you need to recalculate the time interval everytime a take a new sample. You could add something like below right before or after you have taken a sample. Keep in mind that if your sketch is periodically doing something else, like executing an interrupt or postprocessing data. your dt will increase in value causing errors in your gyro angle. Compensate for this by disabling the dt calculation whenever you are doing things like postprocessing.
Code: [Select]
  if (PostProcessing==false){ //prevents delay from data processing from impacting gyro angle
      delta_t = micros() - timer;
  }
  timer=micros();


stratjnd

racquemis, thanks for your post! It helped me understand a lot more about what dt was and how to find it! I think for now, I may just stick to using delay, but may switch over to the millis() serial print.

AWOL, can I just leave them out then or do I need to include all of the definitions? Would the code work without them in or would the sketch no longer work?

Also, if anyone can tell me, can I just use a simple I2C sketch to read data off the MPU-6050 or does it need to have more components?

Thanks!
StratJnd

racquemis

About the definitions, for basic readout of gyro and accelerometer most of them are not needed.  If you however want more control over your mpu6050 it's good to have them around. I would keep them.
If the definitions are getting in the way of readability you can always move them to a seperate header file and include that file on the top of your sketch. If you put that header file in your project folder it will also appear in the IDE for easy access.

stratjnd

Thanks! I will do that once it starts working.... ;)

I tried to play around with the complementary filter, using dt as my sketch delay but when I ran the sketch, the filtered numbers were very erratic.

I looked on the datasheet and saw that accelerometer outputs were in g forces so how do I convert the g forces into degrees per second to use in the complementary filter?

Thanks
StratJnd

racquemis

#9
Feb 08, 2014, 11:56 am Last Edit: Feb 08, 2014, 12:05 pm by racquemis Reason: 1
You don't convert the g values to degrees per seconds.
Besides linear acceleration the accelerometer also measures the static acceleration of the gravity force.
Take two perpendicular accelerometer axis and allign one with the direction of gravity. the first axis will now measure an acceleration of 1g, while the other one measures 0g. Now if you rotate the the accelerometer by 45 degrees. Both axis will now return you the same acceleration. rotate 45 degrees further and the first axis now show 0g and the second axis 1g.
You can now see that with the ratio between the two perpendicular accelerometer axis you can determine the pitch or roll angle
This angle is very stable when the accelerometer is not being moved, however if it's moved it tends to contain alot of noise and error. Therefore it is combined with the angle derived from the gyroscope. It gives you the best of both worlds. The accelerometer angle compensated for the drift caused by the integration of the angular velocity of the gyroscope.

This is the code i used earlier for a project. Here i use a full gyro range of 1000 degrees/s (see mpu6050 datasheet) hence the 32.8 sensitivity. delta_t is calculated using the third method i gave in my earlier post. I have chosen a filter coëfficiënt of 0.98.
Code: [Select]

float P_CompCoeff= 0.98;
void ComplementaryFilter(int ax,int ay,int az,int gy,int gz) {
 long squaresum=(long)ay*ay+(long)az*az;
 pitch+=((-gy/32.8f)*(delta_t/1000000.0f));
 pitchAcc =atan(ax/sqrt(squaresum))*RAD_TO_DEG;
 pitch =P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc;
}

This can give you very smooth results but it also has a downside, The response is quite slow with a low samplerate. With high frequency movements the returned angle might not reach it's real maximum value. At a samplerate of 50 Hz this is already noticable at oscillations of 2 Hz. In most of the examples online examples they tend to ignore this even when there is a quite a simple solution. In the available examples they take a measurement, run it through the complimentary filter, return the value,and then they delay for lets say 20ms to achieve a samplerate of 50 Hz.
The better thing to do is actually to take measurements as fast as possible (delete the delay) and then only store a measurement when more then 20000 microseconds have passed (50 Hz). This way the complimentary filter
will be using a smaller dt for more accurate results and the respons is alot faster since the complimentary filter now processes data close to a 1000 Hz. To calculate the time passed since the last stored measurement you can just sum all the delta_t values (calculated with the third method i gave in the earlier post) , when the sum is larger then 20000 microsconds you store the current measurement and reset the sum to zero.

stratjnd

ok, I think I understood most of that...
;)

I found and downloaded the MPU-6050 library by Jeff Rowberg. When I ran the 'raw' sketch, It says that the 6050 is not connected. How do I find out what is wrong? Is there something wrong with the code I ran? I find that the code is not as crammed as the other code I was using before...

About the complementary filter, I read a post tutorial about it online. http://www.geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/ I have read it and it provided me with a little bit of help about the complementary filter. One thing that confuses me is how you find the 'last filtered angle' the first time round? Also, the squiggly w, what is the angular velocity?

Thanks for your help everyone!

StratJnd

racquemis

#11
Feb 08, 2014, 09:13 pm Last Edit: Feb 08, 2014, 09:19 pm by racquemis Reason: 1
the squiggly w as you call it is called omega. and is the symbol used to represent angular velocity.
The first time the calculation is done the 'last filtered angle' is just zero since you can't know what happens before you start measuring.  This results in something called settling time (http://en.wikipedia.org/wiki/Settling_time).
And it not that big of a deal if you just discard the data of the first couple of seconds.

About the MPU6050 from rowberg. try changing the MPU6050 address. the MPU6050 has a selectable I2C address, 0x68 and 0x69. it could be that library is trying to communicate at the wrong address.
However i would advise using the mpu6050 example given on the playground website. The library can be to complicated to understand if your troubleshooting issues with the sensor(code). And you probably won't even use most of the functions it provides.
I had a lot of problems with that library myself. couldn't get it work out of the box (the raw sketch) while the example on the playground website worked great from the start.

stratjnd

Thanks for the correction! I will try what you told me to do when I have some free time...
So, when I initialise the code, Should I just put
Code: [Select]
Last_Filter_Angle = 0 then after a while the code will settle down and the angles will be correct? Then at the end of my loop, after the angle has been filtered, I should put,
Code: [Select]
Last_Filter_Angle = Filter_Angle ??


Thanks for your help!

racquemis

Yeah, you can just initialize the variables used for the complimentary filter to zero. It will automatically settle down to the correct angles.If your sampling fast enough you might not even notice it.

No, you don't need to add a line like.
Code: [Select]
Last_Filter_Angle = Filter_Angle
The complimentary filter already takes care of that.
Have another look at the code example i gave
Code: [Select]

float pitch=0;
float pitchAcc;
float P_CompCoeff= 0.98;
void ComplementaryFilter(int ax,int ay,int az,int gy,int gz) {
 long squaresum=(long)ay*ay+(long)az*az;
 pitch+=((-gy/32.8f)*(delta_t/1000000.0f));
 pitchAcc =atan(ax/sqrt(squaresum))*RAD_TO_DEG;
 pitch =P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc;
}

First, notice that pitch is declared outside the filter function. It will keep it's value even after the filter has been executed. After the first filter execution pitch will contain your current Angle. However, when the filter is executed for the second time pitch actually becomes your Previous Angle. (pitch+= in line 6 of the code above)

Does this clear things up a bit? Let me know how it goes.

stratjnd

If you don't mind me asking, why do you use 2 accelerometer axes for the pitch?? would't it be just one??
Also why do you use the += sign ??
It just seems a bit confusing thats all!


Thanks,
StratJnd

Go Up