Using setMagneticDeclination does not work properly

[obromios]

I am in Sydney where the magnetic declination is about 12º. If I use the setMagneticDeclination to set the declination to 12º, instead of 0º yaw pointing to True North, it is about 25º off. Upon examination, I think this is related to #46 and #42. It may also relate to #30.

In particular it relates to your code atr MPU9250.h:280,

       float an = +a[0];
        float ae = -a[1];
        float ad = -a[2];
        float gn = +g[0] * DEG_TO_RAD;
        float ge = -g[1] * DEG_TO_RAD;
        float gd = -g[2] * DEG_TO_RAD;
        float mn = +m[1];
        float me = -m[0];
        float md = +m[2];

As discussed in issue #46, this differs from Kris Winers code, MPU9250_MS5637_AHRS_t3.ino:521

MadgwickQuaternionUpdate(-ax, ay, az, gx*PI/180.0f, -gy*PI/180.0f, -gz*PI/180.0f, my, -mx, mz);
I have examined this more carefully, and realise that the only difference is that all the accelerometer values are negated. This means that unlike Kris Winer's code, your code base is not an NED coordinate system. I am not sure exactly what it should be called, but something like NWU (North West Up). This maybe clearer by reading Kris Winer comment, immediately prior to MPU9250_MS5637_AHRS_t3.ino:521

 // For the MPU9250+MS5637 Mini breakout the +x accel/gyro is North, then -y accel/gyro is East. So if we want te quaternions properly aligned
  // we need to feed into the Madgwick function Ax, -Ay, -Az, Gx, -Gy, -Gz, My, -Mx, and Mz. But because gravity is by convention
  // positive down, we need to invert the accel data, so we pass -Ax, Ay, Az, Gx, -Gy, -Gz, My, -Mx, and Mz into the Madgwick
  // function to get North along the accel +x-axis, East along the accel -y-axis, and Down along the accel -z-axis.
  // This orientation choice can be modified to allow any convenient (non-NED) orientation convention.

If I negate the accelerometer readings, so making it a NED system, then the magnetic declination adjustment works. As well, the In the NED system, according to Winer's code, the declination adjustment is added. I have not investigated in detail, but my guess is that in a NWU system, the magnetic declination adjustment should be subtracted. As well, with the negated accelerometer readings, the roll is now about 0º when horizontal rather than about +/- 180, as reported in #37.

If this is correct, then it is easy to fix. Unfortunately, unlike the other issues and fixes I have suggested (#33, #34, #39), it would be a breaking change. If people are using the setMagneticDeclination, perhaps by negating the actual value of the declination, then this change could drastically alter their yaw estimates. I note that removing the fuse values (see #45) will also be a breaking change, though will probably not cause a drastic change in the estimates.

What I propose to do is introduce a new enum

enum class COORDINATES_SEL {
   NED,
   NWU
} 

and then have a setting coordinates_sel to choose NED, or NWU.

If you do not want to make a breaking change, then we could just update the documentation to include this new setting, and indicate that for the setMagneticDeclination they should use the negative of the actual value if using NWU, or the actual value if using NED.

If you are prepared to make a breaking change, we could do the following;

• Change the code so that if NWU, then subtract rather than add the declination
• Remove the fuse values
• Bump the version number to indicate a breaking change, i.e. go to v1.0.0 I am not sure about the Arduino, but in many eco-systems this would make it clear to people that the new version may break things.
• Change the README to make it clear there is a breaking change
• Add a CHANGLOG.md to the root folder. This can be used document the important changes since the last version (this is a good thing to add anyway)
• If making a breaking change, you could also make NED the default coordinate system.

[obromios]

I just checked, and can confirm that negating the accelerometer values so the quaternion update is NED fixes the linear acceleration problem reported in #30. In particular the z values become small and comparable to the x and y values.

[obromios]

Another issue with the NWU coordinate system is that the yaw time takes very long time to settle. If you introduce a step function change in the pitch, whilst observing the roll, pitch and yaw angles, the pitch settles to the new value very quickly. Similarly for a step change in the roll. However a step change in the yaw angle takes 5 to 10 seconds to settle.

Whereas some of the other issues associated with the NWU system can be fixed very easily e.g. #30, the fix to this problem is not immediately apparent. I suspect the problem is that the magnetometer yaw angle and the change in yaw from the gyro are feeding conflicting error signals to the filter.

I no longer think that fixing the problems by reintroducing the NED coordinate system would be a breaking change. It appears that there was already a breaking change that happened at 455a9ef, when the coordinate system was changed from NED to NWU, and the fixes just restore proper operation.

[hideakitai]

@obromios This issue will be fixed by https://github.com/hideakitai/MPU9250/tree/bugfix_obromious. Please confirm if it works.

[obromios]

@hideakitai, I should be able to look at all the changes this by the weekend.

[obromios]

@hideakitai, I have checked and the setMagneticDeclination appears to be fixed on this branch.

[hideakitai]

Fixed in v0.4.3