Jump to content
I2Cdevlib Forums

Roll and pitch minus gravity: already figured out or not?


Recommended Posts

I've seen a couple posts on here asking about incline. Like this one in which Jeff gives a knowing smile at the end when he says the math is beyond him: http://www.i2cdevlib.com/forums/topic/37-incline-of-the-sensor/

 

But then in this one says the DMP is awesome because it can do those calculations for us: http://www.i2cdevlib.com/forums/topic/29-how-to-calculate-yaw-roll-pitch-from-mpu6050/

 

So I'm confused as to whether or not we can already calculate pitch and roll minus any gravity affects based on the current MPU6050 library in the I2CDev package? I thought maybe OUTPUT_READABLE_YAWPITCHROLL would provide those values but they definitely seem affected by gravity when I move the sensor around. There's a note above that constant declaration that says "this also requires gravity vector calculations" but I didn't know if that meant "requires you to do them yourself" or just that a note that they are required and being performed behind the scenes.

 

Help me Jeff, you're my only hope! :)

 

Thanks for the awesome library by the way!

Link to comment
Share on other sites

Just trying do something like this without DMP for the moment while I have no Arduino, but only software I2C and MPU-6050 accessible from my PC Linux via optoisolated RS232 DE9 port and do not ported DMP code in my project for small ATTiny85 8k AVRs.

 

I think we should rather remove acceleration component and its influence on accel measured in MPU-6050 cords to be able calculate pitch and roll easy while you have only gravity component like in the case when MPU-6050 is moveing with constant speed and stright line (no curve acceleration efects or small and acceptable in some cases) or the best possible it is not moveing at all in which case it is quite easy calculate tilt and roll if we have well calibrated MPU-6050.

 

It could be nice to do it using DMP, but it has many undocumented steps and it is for me too difficult to guess what results I get and how it compares to other sensor fusion filters for the moment.

Link to comment
Share on other sites

What I'm trying to do is measure pitch and roll in a vehicle while it's moving, but without gravity being figured in. So if I'm driving up-hill it should report the actual incline angle of the road, not include additional measurements from the fact that I'm also accelerating up the hill.

 

It seems like this would be a pretty common use case but I don't hear many other people complaining about it...

Link to comment
Share on other sites

I had the thought the other night that a very rudimentary way to figure pitch and roll without gravity would be to just watch the gyroscope for changes: if the gyro isn't changing then the actual rotation of the object isn't changing—any acceleration changes are happening purely because of acceleration and can be ignored. If they gyro IS changing then at least part of the acceleration changes are coming from actual rotation.

 

How much is rotation and how much is acceleration is where a lot of scary math comes in. :)

Link to comment
Share on other sites

>>>>>>>>>>>>>>>>>>>>>>>>>>>

What I'm trying to do is measure pitch and roll in a vehicle while it's moving, but without gravity being figured in.

<<<<<<<<<<<<<<<<<<<<<<<<<<<

And this is exacly what I'd like to do, and that is why I've made this optoisolated PC Linux rs232 DE9 <-> photocouper <-> I2C bus with selectable pull-up resiztors and voltage.  eg. 3.3V for MPU-6050.

So, now I'm able connect small old laptop directly to MPU-6050 and power up its I2C bus 3.3V side from electric vehicle batery.

I2C bus is used to read MPu-6050 registers, and visualize it on large LCD screen while driving.

Small laptop touchpad is used in OpenGL application.

 

But I do not think we need remove gravity component, but rather any acceleration.

While I have electric vehicle i can read motor Hall sensors and I know vehicle distance over time, so I know velocity and acceleration component in pitch directtion,

so I'm able I think remove vehicle acceleration and get gravity component only, and then I'm able find pitch and roll like vehicle is not moveing at all.

So, beauty of this solution is when car is not moveing we have no acceleration, and we have gravity only and pitch & roll easy.

While driving vehicle we remove acceleration component and... we again have only gravity and known pitch & roll.

However sensor fusion in DMP mode using MPU-6050 maybe can do it for us, but It is too complicated to me for now to initialize DMP and configure to get expected results, so I prefere to do some math to remove (include known vehicle distance change in time) acceleration component and see what I get and visualize it on LCD while driving -maybe with car video as HUD like F16 using OpenCV image processing engine.

Something like GyroHorizon, but MPu-6050 gyro is not stable (we get degrees per second and need integrate to get degrees) so sensor fusion needed, but I think I do not need it while I know vehicle acceleration in pitch direction and total acceleration from MPU-6050...

Link to comment
Share on other sites

Just using Maxima CAS (http://maxima.sourceforge.net/)  to do this all math stuff in 3D space  and simplify solutions.

Looks good B)

Maxima is cool-world acceleration based on pitch (alfa) and roll(beta) while measured in MPU-6050 coords is a=[Ax, Ay, Az] looks like this:
 

 

(%i40) a[1]
            cos(alfa) sin(alfa) Ay sin(beta)       sin(alfa) Az cos(beta)
(%o40) [[[- -------------------------------- - -------------------------------
                        2          2                       2          2
            sqrt(1 - sin (alfa) sin (beta))    sqrt(1 - sin (alfa) sin (beta))
                                                              + cos(alfa) Ax]]]
(%i41) a[2]
                      2
                   sin (alfa) cos(beta)
(%o41) [[[Ay (-------------------------------
                          2          2
              sqrt(1 - sin (alfa) sin (beta))
                  2
               cos (alfa) cos(beta)              cos(alfa) Az sin(beta)
        + -------------------------------) - -------------------------------]]]
                      2          2                       2          2
          sqrt(1 - sin (alfa) sin (beta))    sqrt(1 - sin (alfa) sin (beta))
(%i42) a[3]
                 2
              cos (alfa) Ay sin(beta)           cos(alfa) Az cos(beta)
(%o42) [[[------------------------------- + -------------------------------
                      2          2                      2          2
          sqrt(1 - sin (alfa) sin (beta))   sqrt(1 - sin (alfa) sin (beta))
                                                              + sin(alfa) Ax]]]
(%o43)

 

 

When checking this by calculating acceleration length: |a|=sqrt(a[1]^2+a[2]^2+a[3]^2) we get something which looks very complicated:
 

 

(%i44) len(a);

...

(%i45) factor(%);

(%o45) [[[sqrt(- (Ax  (- sin (alfa) (sin (alfa) sin (beta) - 1)
      2           2          2
 - cos (alfa) (sin (alfa) sin (beta) - 1))
       2         2       2          2            4          2     2
 + (cos (alfa) Az  + (cos (alfa) sin (alfa) + cos (alfa)) Ay ) sin (beta)
        2            2          2       4              2          2
 + ((sin (alfa) + cos (alfa)) Az  + (sin (alfa) + 2 cos (alfa) sin (alfa)
      4          2     2            2          2
 + cos (alfa)) Ay ) cos (beta))/(sin (alfa) sin (beta) - 1))]]]

 

 

However simplyfing in one line of code gives what we expect:
 

 

(%i46) trigsimp(%);
                                                 2        2       2
(%o46)                    [[[sqrt(Az  + Ay  + Ax )]]]

 

 

So, it looks like that my transformations from unit coords to world coords based on pitch&roll are good B)

 

Now it is time to add gravity and known in my case acceleration in pitch direction from Hall sensors (a0) and solve for pitch(alfa) and roll(beta) knowing total acceleration Ax, Ay, Az given by MPU-6050 raw outputs and gravity G=9.81 m/s^2 ;)

I assume the only forces significant are gravity and wehicle acceleration in pitch direction a0, then:

a= g+ a0

a,g, and a0 are 3d vectors there.

Link to comment
Share on other sites

Accelerator based equations leads to something like this:

mpu_accel_eqs.jpg

A0 - acceleration in pitch direction

Ax,Ay,Az - raw total acceleration data get from MPU calibrated to have it in [m/s^2] units or [G] then G in those equations above is replaced by 1  [G]

G - 9.81 [m/s^s]

If pitch: alfa=0 [rad] and roll: beta=0 [rad] then those equations leads to solution: A=(Ax=A0, Ay=0, Az= -G ) and it is what we expect, so it looks good and it is time to verify this theory in practice and solve those equations while reading MPU-6050 sensor in realtime environment.

 

However I'm interested in how DMP code separates Earth acceleration G from total accelaration A=(Ax,Ay,Az) and how accurate is it?

I'm determined to solve those equations above using numerical methods (realtime if possible), while I need to know how it is calculated to maybe fit it better in my case of moveing electric vehicle with known A0 acceleration from its motor Hall sensors than by using DMP or other G estimations.

 

Update:

I've changed my mind and found easier solution using Maxima algebra CAS when mpu is not moveing:

mpupr_max.jpg

I use asin() while taylor series can be used to obtain nice pitch & roll +/- angles and it can be optimized for speed for smaller pitch & roll angles .

pitch= asin(-Ax/sqrt(Ax*Ax+Ay*Ay+Az*Az) )   [rad]

 

roll= asin(Ay/(cos(pitch)*sqrt(Ax*Ax+Ay*Ay+Az*Az)) ) [rad]

 

There similar equations are used by someone else with simply accelerometer coords filter (before calculating pitch & roll ):

Pitch & Roll from Accelerometer

 

But as we can see, we do not remove gravity, but assume acceleration we got from MPU-6050 is gravity itself if no other accelerations in system..

So, now it is time to add gyro and fuse raw data from MPU-6050.

Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Loading...
 Share

×
×
  • Create New...