Some hint or suggest for the setup:
1. the big current (2A wiring) and magnetic is too close to the sensor which will cause big distortion of magnetic including soft iron and hard iron.
2. those distortion will affect the heading output. that is the major reason of big deviation under NDoF mode than IMUPLUS mode.
3. switching between IMUPLUS mode and NDoF mode is not a good idea. the magnetic sensor needs to calibration with movement to estimate the magnetic environment change. This means, it is better to keep NDoF mode running all the time to keep the magnetic calibration running in background.
4. i suggest you to adding two BNO055 on the board, one runs in IMUPLUS mode, one runs in NDoF mode. then you can compensate from different modes output can get the accurate output.
... View more