DMP - slight sensor moves let pitch/roll jump around

[Tom]

Hi,

I just implemented the DMP portion of the i2c dev library in a piece of Software on my Beaglebone to use the DMP of the MPU6050. I am especially interestet in pitch and roll values. The issue I habe is that the values calculated via the function dmpGetYawPitchRoll() are not very stable. a slight movement results in overshots in pos/neg. directions. After a while of leaving the sensor static at the rached angle the value settles, but as soon as I move the sensor the values jump around again until I keep the sensor static and then they settle based on the actually reached position. I also tried to calculate pitch and roll (and yaw) from the quaternions directly, but with the same result.

…
pIMU->getFIFOBytes(fifoBuffer, packetSize);
pIMU->dmpGetQuaternion(&q, fifoBuffer);
pIMU->dmpGetGravity(&gravity, &q);

 

// sensor based ypr
pIMU->dmpGetYawPitchRoll(ypr, &q, &gravity);

 

// ypr from quaternions

qroll = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w – q.x*q.x – q.y*q.y + q.z*q.z);
qpitch = -1.0 * asin(-2.0*(q.x*q.z – q.w*q.y));
qyaw = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x – q.y*q.y – q.z*q.z);
…

 

Whenever I move the sensor it is very very sensitive to changes (using both methods of pitch/roll calculation). When I e.g. move the sensor from horizontal postion slightly up to an angle of 45° pitch (3-4 seconds to reach the 45° angle) I get overshoots to +80° and -60°. After approx 2-3 seconds the value settles to the expected 45°.

 

Previously I used the raw acc/gyro values provided via the (adapted) i2c library and applied either a kalman or complementary filter to get pitch and roll. I thought when using the DMP I get better values due to the IMU internal fusion of IMU data, but the raw-method looks still better (even that it does not yet satisfy me for the application I am working with, where I have a moving and acclerating object and want to measure pitch and roll independent of the object acceleration -> pitch/roll only based on gravity).

 

Am I missing something in regard to the use of the DMP? I followed 1:1 the (6 axis-)demo in regard how to set up the IMU and how to read the FIFO. I do this in a 10ms loop in my software and have not envountered issues reading the FIFO data, but when I use the read data. The sampling rate of the DMP is set to 200/(1+9)Hz = 20Hz (DMP_FIFO_RATE = 9).

 

I appreciate any feedback on this! Thanks a lot in advance! Maybe it is only a little piece that I am missing on my side ...

 

Regards,
Tom

 

Attachement:

Attached are logs using dmpGetYawPitchRoll() where I move the sensor 3 times from horizontal to 45°.

• first=slow
• second=faster
• third=fast

I only used a value each second to print capture values for the graph (kind of "filter"), while in the background the 10ms task runs to retrieve FIFO data and continuosly calculating pintch and roll (it only reports an actually calculated value every second),

 

 

 

pitch__roll.pdf

[luisrodenas]

That is so strange.

The measure I get with DMP is almost perfect, and I am comparing it with much pricer products like MicroStrain's.

 

Did you set the correct offsets for your sensor?

 

Are you using interrupts or how are you checking fifo buffer?

 

Have you tried with an Arduino and original Jeff's sketch?

[Tom]

Thanks for your reply. That is what I expected from the DMP, to get precise values

 

I poll the FIFO in my 10ms loop (like in the example), not using interrupts. I do not see FIFO overruns. Regarding the offets values I have not set something beside what is done in the library (all set to zero). I briefly read some articles about the offset. As far as I understood they are used to help the DMP calibration process and to avoid the time to calibrate on each startup.

 

I left all settings in the dmpInit function as they were (beside the DMP_RATE adjustment).

 

I do not have an Arduino board so unfortunately I cannot test the library "as is"

 

Some more hints? ... Thanks a lot!

[luisrodenas]

Offsets are usually necesary to run DMP, if not its measures fluctuate. It is not a startup thing, it will not work if offsets are very different from what should be.

 

The easiest way is to run one of the automatic offset sketchs... but if you don't own an Arduino, it's going to take you a bit longer.

You can try 2 things:

 

-Adapt  the calibration sketch to your Beaglebone. If so, please share it later. Arduino code is here: http://www.i2cdevlib.com/forums/topic/96-arduino-sketch-to-automatically-calculate-mpu6050-offsets/

 

- Faster way maybe is to read raw values from sensors, and change offsets iteratively until you get that raw measures from every gyro and Xaccel and Yaccel are 0, and Zaccel is 16384.  Then use those offsets with DMP.

[Tom]

Thanks again for your help on this! I took the arduino sketch and adapted it into my software to perform the calibration (... simply replacing the I2cDev calls with the calls to the BB I2cDev library). The IMU was leveled as required. I ran it several times, with always almost the same result:

 

 

Sensor readings with offsets:
0       -2      16387   0       0       0
Your offsets:   -4545   731     1784    21      19      -31
Data is printed as: acelX acelY acelZ giroX giroY giroZ
Check that your sensor readings are close to 0 0 16384 0 0 0

 

 

I update the initialization of the MPU to use those values:

 

MPU6050::initialize() - as given in MPU6050.cpp, only shown for completeness of initialization sequence:

    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
    setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    setSleepEnabled(false);
 

DMP setup calls the following functions:

pIMU->    dmpInitialize() -> content as given in "MPU6050_6Axis_MotionApps20.h" of i2c lib

    pIMU->setXGyroOffset(21);
    pIMU->setYGyroOffset(19);
    pIMU->setZGyroOffset(-31);
    pIMU->setXAccelOffset(-4545);
    pIMU->setYAccelOffset(731);
    pIMU->setZAccelOffset(1784);

    pIMU->setDMPEnabled(true);
    packetSize = pIMU->dmpGetFIFOPacketSize();
 

Then I do the following in my 10ms loop:

 

    mpuIntStatus = pIMU->getIntStatus();
    fifoCount = pIMU->getFIFOCount();

    if ((mpuIntStatus & MPU6050_INTERRUPT_FIFO_OFLOW_BIT) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        pIMU->resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & MPU6050_INTERRUPT_DMP_INT_BIT)
    {
        while (fifoCount < packetSize) fifoCount = pIMU->getFIFOCount();

        // read a packet from FIFO
        pIMU->getFIFOBytes(fifoBuffer, packetSize);

        pIMU->dmpGetQuaternion(&q, fifoBuffer);
        pIMU->dmpGetGravity(&gravity, &q);
        pIMU->dmpGetYawPitchRoll(ypr, &q, &gravity);
    }
 

I still get ypr values which jump around ... strange. However, the sensor seems to be initialized immediately due to the offset settings.

What I also notice is that the yaw turns +/-180° several times while I turn the sensor contineously around its Z-axis (still leveled, so Y and Z do not turn).

 

Any idea what I miss in addition?

[luisrodenas]

 

 

What I also notice is that the yaw turns +/-180° several times while I turn the sensor contineously around its Z-axis

 

The [-180,+180] yaw range is normal. 

 

 

I still get ypr values which jump around 

 

Maybe your problem is something related to sensor ranges? Try to change gyro's range to something higher (500 or more). I believe this change should be made between offsets and setDMPenabled.

You can also check this getting accel and gyro info from dmp, something like dmpGetAccel, and dmpGetgyro. Check those values. I think the first one should be in gs already, and the second one in degrees/second. This way you can move your board and you can manually check if values have sense or not.

 

 

I can't think of any other possible error, apart from your coding or the sensor itself being faulty. An arduino would be nice to check this last thing fast.

[titous]

I still get ypr values which jump around ... strange.

 

 

What do you mean by jump around?  Can you post some of the serial output?  Also, how are you calculating your ypr values?  Perhaps you haven't implemented the conversion from quaternion to ypr correctly.  If you mix up the order of the quaterion (w, x, y, z or x, y, z, w for example) that will make your ypr go crazy

[Tom]

What I mean with multiple +/-180° values on yaw is that one physical 360° turn results in x times +/-180° while it is expect to be only one time +/-180°.

I did what you proposed and retrieved the acc and gyro values from the fifo buffer via

        dmpGetAccel(acc, fifoBuffer);
        dmpGetGyro(gyro, fifoBuffer);
        
and calculated pitch and roll manually:

        pitchAcc = ((atan2(acc[0], acc[2]) + M_PI) * (180 / 3.14159265)) - 180;
        rollAcc  = ((atan2(acc[1], acc[2]) + M_PI) * (180 / 3.14159265)) - 180;

I get fine (non-fused) values for ACC based pitch and roll without any oscillating or overshoots.
The sensor is initialized with gyroRate=250 and g=2g.

I am not sure if the gyro rate is OK. What is the resolution it is reported with? Is it really deg/sec? When using the values 1:1 and fusing them with the ACC values manually then the large deg/sec values end up in high overshoots of the angles.
Applying the scaling according to the configured gyro scaling results in very small values resulting in a kind of low-pass filtered result value for the angel.
When I fuse the DMP acc and gyro values manually (complementary filter) applying the 1:1 gyro scaling I get large overshoots, applying the gyro scale dependent resolution results in a damped signal.
Fusing the acc and gyro values when the sensor operates in standard mode not utilizing the DMP work better than the values I get with the DMP based fusion.

Changing the gyro rate to 2000 after the call to dmpInit(), in between setting the offset and enabling the dmp, didn't help to get good DMP calucated ypr values.
 

[Tom]

For completeness    Here is the test code I used to retrieve sensor data via DMP (fusing excluded).

 

mpu.initialize();

mpu.dmpInitialize();

mpu.setXGyroOffset(21);
mpu.setYGyroOffset(19);
mpu.setZGyroOffset(-31);
mpu.setXAccelOffset(-4545);
mpu.setYAccelOffset(731);
mpu.setZAccelOffset(1784);

//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
//mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);

mpu.setDMPEnabled(true);
int packetSize = mpu.dmpGetFIFOPacketSize();

int fifoCount = 0;
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

while(1)
{
    int mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & MPU6050_INTERRUPT_FIFO_OFLOW_BIT) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & MPU6050_INTERRUPT_DMP_INT_BIT)
    {
        int16_t acc[3];
        int16_t gyro[3];

        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);

        // Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        float q_roll = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
        float q_pitch = -1.0 * asin(-2.0*(q.x*q.z - q.w*q.y));
        float q_yaw = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

//        cout << "yaw=" << (int)(ypr[0]*180/M_PI) << " pitch=" << (int)(ypr[1]*180/M_PI) << " roll=" << (int)(ypr[2]*180/M_PI) << endl;
//        cout << "qyaw=" << (int)(q_yaw*180/M_PI) << " qpitch=" << (int)(q_pitch*180/M_PI) << " qroll=" << (int)(q_roll*180/M_PI) << endl;

        mpu.dmpGetAccel(acc, fifoBuffer);
        mpu.dmpGetGyro(gyro, fifoBuffer);

//        cout << "accX=" << acc[0] << " accY=" << acc[1] << " accZ=" << acc[2] << endl;
//        cout << "gyroX=" << gyro[0] << " gyroY=" << gyro[1] << " gyroZ=" << gyro[2] << endl;

        int pitchAcc = ((atan2(acc[0], acc[2]) + M_PI) * (180 / 3.14159265)) - 180;
        int rollAcc  = ((atan2(acc[1], acc[2]) + M_PI) * (180 / 3.14159265)) - 180;

//        cout << "pitchAcc=" << pitchAcc << " rollAcc=" << rollAcc << endl;
        
        ... fuse acc and gyro here manually

        usleep(10000);
    }
}
              

[luisrodenas]

Ok.

I think we narrowed the error to the gyros scale.

Try to get YPR integrating gyro's measures.  Then you will see if it is deg/sec or what constant you have to multiply them for.

Try this setting the scale to 250 degrees, 500, and 2000.

Im interested only in the gyro's estimate, do not fuse it yet.

 

To check scale you can do a simple experiment: try to rotate your board at constant speed. For example rotate it 360º in 10 sec (try to do it at constant speed), and you know you should get something close to 36º/sec

[Tom]

I did my best moving the board ...

 

I set the scale to 250 and 500 when in DMP mode (as suggested between dmpInit and dmpEnable) and integrated the pitch value. I cannot move the board for 360° :-) but I moved it several times to 45° within approx 5 seonds and tried to do this at constant speed. What I noticed is that sometines there were jumps of the gyro value which caused a wrong integration (even when it lies down flat). I had not seen this before when doing the integration with the standard mode values. I measured the following (when no jump occurred):

 

-> speed = 45°/5sec = 9°/sec

 

The integrated result for pitch using the gyro always ended at approx 240°

 

-> speed = 240°/5sec = 48°/sec

 

Does this means that the gyro uses an approx. 5.333 higher scaling than it should? Where can such a scaling deviation come from and how could it be configured in the chip for the DMP?

 

For testing I took the 5.333 and applied it to my fusion filter when using the DMP raw gyro and acc values and the manual fusion result looked good ... but there are the above mentioned gyro value jumps which cause the intergration to fail (an out of sudden receive value of +/- x thousand breaks the integration for a while).

 

When reading gyro values via the standard way such value jumps do not occur ...

[luisrodenas]

Yes, gyro gain or scale is broken.

Now, I think this could be caused by, from most probable to less probable:

 

- MPU broken

- Something wrong in your ported code

- DMP using wrong gain, no idea why.

 

On the other hand we have those jumps...  and you say they only occur while DMP is enabled...  so wierd.

 

To reach the end of this topic I believe you need to get another MPU or an Arduino, or better both.

If you don't need it fast and want to save some $, buy them on ebay. MPUs on board alone are like 8 $, and Arduino Pro Mini is like 5$.

If you want it fast it will cost you a bit more.

[Tom]

this does not sound good ... prior to the scaling test I already tried another MPU (had to unsolder the first one), with the same result when using DMP, so I guess the MPU is not broken (or maybe both are broken ... ordered via amazon, came from China). I am also wondering why it works in the standard mode. There I can use the scaling given in the datasheet for the certain gyro resolutions.

 

Thanks a lot for all your help ... Well, for now I will probably stick with the discrete pitch/yaw calculation. The fusion I perform seems to do the job up to a certain extent, but using the DMP would decouple measurements from the SW, which is a great thing.

 

I will also definitely look into my code to see if something is broken there, especially in the DMP portion, because the standard mode seems to work fine.

 

Have a great day. I will let you know once I probably get DMP to work!

[gregd72002]

Hi, 

 

Did you get this working?

 

I have exactly the same problem with my mpu6050. I have spent a lot of time investigating it, including one crashed quadcopter!

 

The MPU is capable of self-calibration if initialized with the correct flags. The gyro outcome is perfect, so is pitch & roll. But yaw jumps all over the place. Before, I had a different mpu6050 that worked perfectly fine but I managed to destroy it. Since, I hardly changed anything in my code and the new MPU behaves as described.

 

I'm currently waiting for a new module as I think this issue is due to faulty chip.

 

Where did you get your MPU from?

 

Thanks,

Gregory

[Tom]

Hi!

 

I did not get this issue solved yet and use the "raw" Acc/Gyro information of the sensor plus some SW-filter (complementary filter) to get pitch/roll (but not yaw).

 

I bought the chip (breadboard) at amazon and it came from a supplier in china. I have two boards, which I bought at different times, but both behave exactly the same and provide strange values.

 

Regards,

Tom

[gregd72002]

Thanks Tom.

 

I got a new MPU and now all works fine  

 

You might want to try yet another one...

[Egholm]

Hi Greg,

 

Thanks Tom.

 

I got a new MPU and now all works fine

 

You might want to try yet another one...

 

So you simply got a new MPU6050, and then it worked?

 

I'm experiencing the same problem here. If I run with Gyro FSR=250dps, my quaternions jump all over the place, if I run with 2000dps, they are rock-steady. (Haven't tried any intermediate scales yet).

 

// Egholm

[Egholm]

Hi Greg,

 

 

So you simply got a new MPU6050, and then it worked?

 

I'm experiencing the same problem here. If I run with Gyro FSR=250dps, my quaternions jump all over the place, if I run with 2000dps, they are rock-steady. (Haven't tried any intermediate scales yet).

 

// Egholm

 

Just to follow up on this: It's simply not allowed to run with FSR other than 2000dps.

 

Look at the comment inside InvenSense's "mllite_test.c" example code provided with eMPL 6.1:

 

"DMP sensor fusion works only with gyro at +-2000dps and accel +-2G"

[mllite_test.c, l. 967]

 

So, that't it! :-/