Shifted pitch and yaw measurement

janq0 commented 1 year ago

It's difficult to describe the issue in words, so I made an illustration of it.

The following output is from the GY521_pitch_roll_yaw example after calibration. The actual pitch and roll movement was gradual and slow. From line 34 to 35, the measured pitch shifts by around -17. A similar shift occurs in the roll from line 36 to 37.

CNT PITCH   ROLL    YAW
30  0.772   345.005 358.791
31  0.669   344.142 358.607
32  0.797   342.071 358.438
33  0.244   338.645 358.423
34  0.171   335.321 358.668
35  343.608 341.118 357.288
36  341.322 342.920 355.539
37  342.271 2.811   354.572
38  342.297 1.450   354.143
39  342.342 2.843   353.974

I'm not sure why this behavior happens, but I assume that the problem is caused by this library, since it does the computation of pitch/roll/yaw from the raw sensor output. I'm using version 0.3.9.

added the image for future reference

RobTillaart commented 1 year ago

Thanks for the issue. Quite busy so it will take a while before I can Investigate..

RobTillaart commented 1 year ago

Q: are the jumps also occurring if you go the other way / from 350ish to 0.xxx?

RobTillaart commented 1 year ago

Notes to myself

check math (done, but do it again)
simulate raw sensor to stress math (lot of work)
error ~17 could be 16 + some normal variation ==> bit error?
pitch and roll correct with _aax _aay while yaw doesn't. ==> Are the _gaN values smooth or does jump comes from _aaN ?

RobTillaart commented 1 year ago

Q: what are your 6 calibration values?

RobTillaart commented 1 year ago

Scanned several tutorials / libraries and most had identical or similar math. There was this remark that might fix the issue. Remark in https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/

Can you verify if the fix below works as I have no setup to test.

Change in GY521.cpp the following lines (181-183) and give it a try.

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

into

  _gax = 0.96 * _gax + 0.04 * _aax;
  _gay = 0.96 * _gay + 0.04 * _aay;

  _roll  = _gax;
  _pitch = _gay;
  _yaw   = _gaz;

The difference is that the correction factor 0.96 * _gax + 0.04 * _aax adds up or not in the _gax (Y -axis idem).

If this fixes your issue I will update the library asap.

janq0 commented 1 year ago

Q: what are your 6 calibration values?

The calibration in the example i gave was generated by the GY521_readCalibration_2 as follows:

  sensor.axe = 0.0786060;
  sensor.aye = 0.0127197;
  sensor.aze = -1.0656372;
  sensor.gxe = 1.3613740;
  sensor.gye = 2.9567937;
  sensor.aze = 0.1717557;

Scanned several tutorials / libraries and most had identical or similar math. There was this remark that might fix the issue. Remark in https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/

Can you verify if the fix below works as I have no setup to test.

Change in GY521.cpp the following lines (181-183) and give it a try.
  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;
into
  _gax = 0.96 * _gax + 0.04 * _aax;
  _gay = 0.96 * _gay + 0.04 * _aay;

  _roll  = _gax;
  _pitch = _gay;
  _yaw   = _gaz;
The difference is that the correction factor 0.96 * _gax + 0.04 * _aax adds up or not in the _gax (Y -axis idem).

If this fixes your issue I will update the library asap.

Changing the lines you specified makes the measurement very imprecise. (Is there some mistake in the code you included?) The values pitch and roll change at about 1 degree per second even though the GY-521 is stationary. Changing the actual pitch and roll of the gyroscope impacted the measured value, but not in a predictable way, which means I wasn't able to tell if the original issue was fixed by this. I made sure that the sensor is property calibrated before testing:

  sensor.axe = 0.0841863441;
  sensor.aye = 0.0117734062;
  sensor.aze = -1.0599359273;
  sensor.gxe = 1.4209155082;
  sensor.gye = 2.9071882247;
  sensor.aze = 0.1559407472;

RobTillaart commented 1 year ago

The calibration in the example i gave was generated by the GY521_readCalibration_2 as follows: sensor.axe = 0.0786060; sensor.aye = 0.0127197; sensor.aze = -1.0656372; sensor.gxe = 1.3613740; sensor.gye = 2.9567937; sensor.aze = 0.1717557;

Looks reasonable, no extremes.

RobTillaart commented 1 year ago

Changing the lines you specified makes the measurement very imprecise. (Is there some mistake in the code you included?) The values pitch and roll change at about 1 degree per second even though the GY-521 is stationary.

The code included was a remark on the website mentioned.

RobTillaart commented 1 year ago

my note to myself above seems to tickle a possible cause.

error ~17 could be 16 + some normal variation ==> bit error?

There is a correction factor 0.96 in the formula, or 4% when looking at it differently.

_roll  = 0.96 * _gax + 0.04 * _aax;

if gax is between 355-360 degrees. that 4% correction is about 360 degrees 4% = 14.40 if gax is between 0 and 5 degrees, that 4% correction is about 5 4% = 0.20 ==> this is a difference in the order of magnitude you are observing.

So this could imply hat the normalization in the read() function is incorrect.

So lets remove the normalization (line 173 - 192)

  //  normalize internal vars
  // if (_gax < 0) _gax += 360.0;
  // else if (_gax >= 360.0) _gax -= 360.0;
  // if (_gay < 0) _gay += 360.0;
  // else if (_gay >= 360.0) _gay -= 360.0;
  // if (_gaz < 0) _gaz += 360.0;
  // else if (_gaz >= 360.0) _gaz -= 360.0;

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

  //  normalize 
  // if (_pitch < 0) _pitch += 360;
  // else if (_pitch >= 360) _pitch -= 360;
  //  if (_roll  < 0) _roll  += 360;
  // else if (_roll  >= 360) _roll  -= 360;
  // if (_yaw  < 0) _yaw  += 360;
  // else if (_yaw  >= 360) _yaw  -= 360;

  return GY521_OK;
}

Can you test please?

RobTillaart commented 1 year ago

Labelled issue as a bug as normalization is not ok.

Did some math (manual) and this confirmed the normalization is at least at the wrong place. Needs more investigation.

janq0 commented 1 year ago

So lets remove the normalization (line 173 - 192)

  //  normalize internal vars
  // if (_gax < 0) _gax += 360.0;
  // else if (_gax >= 360.0) _gax -= 360.0;
  // if (_gay < 0) _gay += 360.0;
  // else if (_gay >= 360.0) _gay -= 360.0;
  // if (_gaz < 0) _gaz += 360.0;
  // else if (_gaz >= 360.0) _gaz -= 360.0;

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

  //  normalize 
  // if (_pitch < 0) _pitch += 360;
  // else if (_pitch >= 360) _pitch -= 360;
  //  if (_roll  < 0) _roll  += 360;
  // else if (_roll  >= 360) _roll  -= 360;
  // if (_yaw  < 0) _yaw  += 360;
  // else if (_yaw  >= 360) _yaw  -= 360;

  return GY521_OK;
}

Can you test please?

Good job! The normalization seems to cause the issue as you pointed out. The measured values are accurate when I comment out the problematic code.

RobTillaart commented 1 year ago

Good to hear the cause seems to be found. Now I need (time to) find a way to get the normalization again in the code. Without it floats will eventually loose precision.

RobTillaart commented 1 year ago

@janq0

Can you test the version below of read()?

By normalizing the _gax and _gay at 375 degrees the math should be right again. Then these values will keep highest precision and not "overflow" see issue #36.

Thanks for your time, appreciated!

int16_t GY521::read()
{
  uint32_t now = millis();
  if (_throttle)
  {
    if ((now - _lastTime) < _throttleTime)
    {
      //  not an error.
      return GY521_THROTTLED;
    }
  }
  _lastTime = now;

  //  Connected ?
  _wire->beginTransmission(_address);
  _wire->write(GY521_ACCEL_XOUT_H);
  if (_wire->endTransmission() != 0)
  {
    _error = GY521_ERROR_WRITE;
    return _error;
  }

  //  Get the data
  int8_t n = _wire->requestFrom(_address, (uint8_t)14);
  if (n != 14)
  {
    _error = GY521_ERROR_READ;
    return _error;
  }
  //  ACCELEROMETER
  _ax = _WireRead2();           //  ACCEL_XOUT_H  ACCEL_XOUT_L
  _ay = _WireRead2();           //  ACCEL_YOUT_H  ACCEL_YOUT_L
  _az = _WireRead2();           //  ACCEL_ZOUT_H  ACCEL_ZOUT_L
  //  TEMPERATURE
  _temperature = _WireRead2();  //  TEMP_OUT_H    TEMP_OUT_L
  //  GYROSCOPE
  _gx = _WireRead2();           //  GYRO_XOUT_H   GYRO_XOUT_L
  _gy = _WireRead2();           //  GYRO_YOUT_H   GYRO_YOUT_L
  _gz = _WireRead2();           //  GYRO_ZOUT_H   GYRO_ZOUT_L

  //  duration interval
  now = micros();
  float duration = (now - _lastMicros) * 1e-6;  // duration in seconds.
  _lastMicros = now;

  //  next lines might be merged per axis. (performance)

  //  Convert raw acceleration to g's
  _ax *= _raw2g;
  _ay *= _raw2g;
  _az *= _raw2g;

  //  Error correct raw acceleration (in g) measurements  // #18 kudos to Merkxic
  _ax += axe;
  _ay += aye;
  _az += aze;

  //  prepare for Pitch Roll Yaw
  float _ax2 = _ax * _ax;
  float _ay2 = _ay * _ay;
  float _az2 = _az * _az;

  _aax = atan(       _ay / sqrt(_ax2 + _az2)) * GY521_RAD2DEGREES;
  _aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * GY521_RAD2DEGREES;
  _aaz = atan(       _az / sqrt(_ax2 + _ay2)) * GY521_RAD2DEGREES;
  //  optimize #22
  //  _aax = atan(_ay / hypot(_ax, _az)) * GY521_RAD2DEGREES;
  //  _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * GY521_RAD2DEGREES;
  //  _aaz = atan(_az / hypot(_ax, _ay)) * GY521_RAD2DEGREES;

  //  Convert to Celsius
  _temperature = _temperature * 0.00294117647 + 36.53;  //  == /340.0  + 36.53;

  //  Convert raw Gyro to degrees/seconds
  _gx *= _raw2dps;
  _gy *= _raw2dps;
  _gz *= _raw2dps;

  //  Error correct raw gyro measurements.
  _gx += gxe;
  _gy += gye;
  _gz += gze;

  //  _gax etc might loose precision after many iterations #36
  _gax += _gx * duration;
  _gay += _gy * duration;
  _gaz += _gz * duration;

  //  normalize
  bool _normalize = true;
  if (_normalize)
  {
    //  correction at 375 due to the factor 0.96 in roll formula
    if (_gax > 375)    _gax -= 375;
    else if (_gax < 0) _gax += 375;
    //  correction at 375 due to the factor 0.96 in pitch formula
    if (_gay > 375)    _gay -= 375;
    else if (_gay < 0) _gay += 375;
    // correction at 360
    if (_gaz > 360)    _gaz -= 360;
    else if (_gaz < 0) _gaz += 360;
  }

  //  Calculate Pitch Roll Yaw
  _yaw   = _gaz;
  _roll  = 0.96 * _gax + 0.04 * _aax;
  _pitch = 0.96 * _gay + 0.04 * _aay;

  if (_normalize)
  {
    if (_pitch > 360)    _pitch -= 360;
    else if (_pitch < 0) _pitch += 360;
    if (_roll > 360)     _roll -= 360;
    else if (_roll < 0)  _roll += 360;
    if (_yaw > 360)      _yaw -= 360;
    else if (_yaw < 0)   _yaw += 360;
  }

  return GY521_OK;
}

Might need to change > to >= as 0 == 360.

RobTillaart commented 1 year ago

the line bool _normalize = true; is for testing. It will be a private variable and will get a setter and getter in the public interface.

RobTillaart commented 1 year ago

Created develop branch for version 0.4.0. Build is running.

RobTillaart commented 1 year ago

Dev branch build is OK, Pull Request created PR builds OK

janq0 commented 1 year ago

@janq0

Can you test the version below of read()?

By normalizing the _gax and _gay at 375 degrees the math should be right again. Then these values will keep highest precision and not "overflow" see issue #36.

Thanks for your time, appreciated!

int16_t GY521::read()
{
  uint32_t now = millis();
  if (_throttle)
  {
    if ((now - _lastTime) < _throttleTime)
    {
      //  not an error.
      return GY521_THROTTLED;
    }
  }
  _lastTime = now;

  //  Connected ?
  _wire->beginTransmission(_address);
  _wire->write(GY521_ACCEL_XOUT_H);
  if (_wire->endTransmission() != 0)
  {
    _error = GY521_ERROR_WRITE;
    return _error;
  }

  //  Get the data
  int8_t n = _wire->requestFrom(_address, (uint8_t)14);
  if (n != 14)
  {
    _error = GY521_ERROR_READ;
    return _error;
  }
  //  ACCELEROMETER
  _ax = _WireRead2();           //  ACCEL_XOUT_H  ACCEL_XOUT_L
  _ay = _WireRead2();           //  ACCEL_YOUT_H  ACCEL_YOUT_L
  _az = _WireRead2();           //  ACCEL_ZOUT_H  ACCEL_ZOUT_L
  //  TEMPERATURE
  _temperature = _WireRead2();  //  TEMP_OUT_H    TEMP_OUT_L
  //  GYROSCOPE
  _gx = _WireRead2();           //  GYRO_XOUT_H   GYRO_XOUT_L
  _gy = _WireRead2();           //  GYRO_YOUT_H   GYRO_YOUT_L
  _gz = _WireRead2();           //  GYRO_ZOUT_H   GYRO_ZOUT_L

  //  duration interval
  now = micros();
  float duration = (now - _lastMicros) * 1e-6;  // duration in seconds.
  _lastMicros = now;

  //  next lines might be merged per axis. (performance)

  //  Convert raw acceleration to g's
  _ax *= _raw2g;
  _ay *= _raw2g;
  _az *= _raw2g;

  //  Error correct raw acceleration (in g) measurements  // #18 kudos to Merkxic
  _ax += axe;
  _ay += aye;
  _az += aze;

  //  prepare for Pitch Roll Yaw
  float _ax2 = _ax * _ax;
  float _ay2 = _ay * _ay;
  float _az2 = _az * _az;

  _aax = atan(       _ay / sqrt(_ax2 + _az2)) * GY521_RAD2DEGREES;
  _aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * GY521_RAD2DEGREES;
  _aaz = atan(       _az / sqrt(_ax2 + _ay2)) * GY521_RAD2DEGREES;
  //  optimize #22
  //  _aax = atan(_ay / hypot(_ax, _az)) * GY521_RAD2DEGREES;
  //  _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * GY521_RAD2DEGREES;
  //  _aaz = atan(_az / hypot(_ax, _ay)) * GY521_RAD2DEGREES;

  //  Convert to Celsius
  _temperature = _temperature * 0.00294117647 + 36.53;  //  == /340.0  + 36.53;

  //  Convert raw Gyro to degrees/seconds
  _gx *= _raw2dps;
  _gy *= _raw2dps;
  _gz *= _raw2dps;

  //  Error correct raw gyro measurements.
  _gx += gxe;
  _gy += gye;
  _gz += gze;

  //  _gax etc might loose precision after many iterations #36
  _gax += _gx * duration;
  _gay += _gy * duration;
  _gaz += _gz * duration;

  //  normalize
  bool _normalize = true;
  if (_normalize)
  {
    //  correction at 375 due to the factor 0.96 in roll formula
    if (_gax > 375)    _gax -= 375;
    else if (_gax < 0) _gax += 375;
    //  correction at 375 due to the factor 0.96 in pitch formula
    if (_gay > 375)    _gay -= 375;
    else if (_gay < 0) _gay += 375;
    // correction at 360
    if (_gaz > 360)    _gaz -= 360;
    else if (_gaz < 0) _gaz += 360;
  }

  //  Calculate Pitch Roll Yaw
  _yaw   = _gaz;
  _roll  = 0.96 * _gax + 0.04 * _aax;
  _pitch = 0.96 * _gay + 0.04 * _aay;

  if (_normalize)
  {
    if (_pitch > 360)    _pitch -= 360;
    else if (_pitch < 0) _pitch += 360;
    if (_roll > 360)     _roll -= 360;
    else if (_roll < 0)  _roll += 360;
    if (_yaw > 360)      _yaw -= 360;
    else if (_yaw < 0)   _yaw += 360;
  }

  return GY521_OK;
}

Might need to change > to >= as 0 == 360.

I tested this code and it seems to work. The measured values are normalized and the issue is gone.

RobTillaart commented 1 year ago

Thanks for testing, Will merge a PR later today as its time for a small siesta. (33°C today)

RobTillaart commented 1 year ago

This issue closed due to the PR (link). If there are more related questions please reopen / or open a new issue.

Again, thanks for reporting!

RobTillaart / GY521

Shifted pitch and yaw measurement #42