Search code examples
pythonpositionaccelerometer

A library to get position with accelerometer gyro and magneto

I'm using a sensor which is a accelerometer+gyro+magneto (all in one chip). I was wondering if anyone knows a library which can help me get the position from those values.

I mean starting with a 0.0 (lat/long) and updating values. If there are no library, I'm still seeking links to get clues on how to do the maths. I've seen posts like this one or this one but I didn't find what I was looking for.

Thanks

Solution

  • If it can help anyone, I've found the exact topic I was looking for. Here it is :

    https://lb.raspberrypi.org/forums/viewtopic.php?t=127930

    [EDITED]

    Here's the idea :

    enter image description here

    Algorithm 2 :

    I have found two method, I do not know which one could be better,

    Accroll = atan2( accy / (accy^2 + accz^2) )
Accpitch = atan2( accx / (accx^2 + accz^2) )

    source: http://franciscoraulortega.com/pubs/Algo3DFusionsMems.pdf

    Accroll = atan2(accy, accz) 
Accpitch = atan2( -accx, sqrt(accy^2 + accz^2) )

    source: https://www.nxp.com/files-tatic/sensors/doc/app_note/AN3461.pdf page 10

    Algorithm 3

    I think it should be a complementary filter like this maybe:

    Calculate pitch and roll with gyroscope data:

    Gyrpitch = gyrox * dt;
Gyrroll = gyroy * dt;

    Calculate pitch and roll after the filter:

    pitch = Gyrpitch * 0.98 + Accpitch * 0.02;
roll = Gyrroll * 0.98 + Accroll * 0.02;

    Algorithm 1

    XH = mx cos(pitch) + my sin(pitch) sin(roll) + mz sin(pitch) cos(roll)
YH = my cos(roll) + mz sin(roll) 
yaw = atan2(−YH / XH)

    where: m: magnetometer data source: http://franciscoraulortega.com/pubs/Algo3DFusionsMems.pdf