Complementary filter with the MPU6050

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?

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/

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!

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.

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

stratjnd:
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.

  if (PostProcessing==false){ //prevents delay from data processing from impacting gyro angle
      delta_t = micros() - timer;
  }
  timer=micros();

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

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.

Thanks! I will do that once it starts working.... :wink:

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

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.

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.

ok, I think I understood most of that...
:wink:

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. Gyroscopes and Accelerometers on a Chip – Geek Mom Projects 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

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 (Settling time - Wikipedia).
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.

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 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,
Last_Filter_Angle = Filter_Angle ??

Thanks for your help!

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.

Last_Filter_Angle = Filter_Angle

The complimentary filter already takes care of that.
Have another look at the code example i gave

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.

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

why do you use the += sign ?

Because it is easier to write pitch += x; than it is to write pitch = pitch + x;

If you don't mind me asking, why do you use 2 accelerometer axes for the pitch?? would't it be just one??

I actually use three, but lets first explain why two are used. I've already talked about that a bit in one of my earlier posts in this topic. Your right that pitch can be calculated with one accelerometer axis. your probably thinking of a formula like this.
pitch=arcsin(ax/g) in which g is the earth's gravitational constant.
However this formula assumes that the total acceleration ( sqrt(ax^2+ay^2+az^2) ) always remains equal to g.
This is not the case in dynamic situations.
Let's say you are sitting in a parked car and your holding the accelerometer at a fixed pitch of 45 degrees. ax will measure a certain value. Now when you start the car and start accelerating(dynamic acceleration). ax will now sense a part of your car's acceleration.
Suddenly the formula is returning a larger pitch angle while your still holding the accelerometer at a fixed angle of 45 degrees.
Since the formula uses a constant (g) it can not account for dynamic acceleration.

This problem is partially solved when you use a second axis perpendicular to your first one. You can now calculate the pitch based on the ratio between the accelerations sensed on each axis. There is still some error due to dynamic acceleration but it's a bit less sensitive. The gyroscope in the complimentary filter will mostly compensate for the rest.

EDIT: I forgot the most important reason of using two perpendicular axis. When you only use one axis to calculate pitch. you can't compensate for a change in acceleration due to roll and you will be getting horrible errors when the roll-angle is not zero.

Ok, so should I use that?

AWOL, sorry if I didn't make myself clear. I meant why it is used in that formula...

i explained that in my last post on page 1.
We have the last calculated angle stored. then we calculate how much the angle has changed using the gyroscope. That change we add to the last calculated angle.

Have a look here. It might give you a better understanding of the complimentary filter. it even has some code you could try.
http://www.pieter-jan.com/node/11

Sorry for the very late reply, I have been quite busy unfortunately but now I am free I hope to finish this project.

I tried the complementary filter using the code that you provided. It looked quite good when I ran it but every time I moved the IMU around, the number increased and then stayed there when the IMU was still. Every time I moved the IMU, the number increased but would not go down. Can anybody help??

Thanks!
StratJnd

HELP!!!!!

I think that my MPU-6050 is wrong... when it outputs the values flicker between two, even though I am vigorously shaking the IMU.
Here is some output values using just the MPU-6050 code from arduino playground:

MPU-6050
Read accel, temp and gyro, error = 2
accel x,y,z: 747, 512, 12545
temperature: 24.459 degrees Celsius
gyro x,y,z : 520, 4557, 396,

MPU-6050
Read accel, temp and gyro, error = 2
accel x,y,z: -5374, 2, 305
temperature: 37.212 degrees Celsius
gyro x,y,z : 2050, -13039, -29695,

MPU-6050
Read accel, temp and gyro, error = 2
accel x,y,z: 747, 512, 12545
temperature: 24.459 degrees Celsius
gyro x,y,z : 520, 4557, 396,

MPU-6050
Read accel, temp and gyro, error = 2
accel x,y,z: -5374, 2, 305
temperature: 37.212 degrees Celsius
gyro x,y,z : 2050, -13039, -29695,

MPU-6050
Read accel, temp and gyro, error = 2
accel x,y,z: 747, 512, 12545
temperature: 24.459 degrees Celsius
gyro x,y,z : 520, 4557, 396,

Can anybody help?