Madgwick Algorithm With Magnetometer: Issues in True North Detection

[gafa7758]

Hello,

I am currently calculating quaternions using the Madgwick algorithm with magnetometer data. The calculation works well, as the quaternion values change correctly when the sensor is rotated. I have verified the results using a quaternion calculator and visualized them with a 3D cube model.

However, I am encountering an issue with the qz value (yaw/heading).
When the sensor is powered on while lying flat, with the magnetometer's X-axis pointing north, the initial quaternion is (1, 0, 0, 0). If I power off the sensor, rotate it 90 degrees about the Z-axis (so the magnetometer's X-axis points west), and power it back on, the quaternion remains (1, 0, 0, 0).

This behavior seems incorrect since the magnetometer readings have to change after the rotation. I also tested rotating the sensor so that the magnetometer's X-axis points south or east, but the quaternion still does not change (still 1, 0, 0,0) when the program is restarted.

Could I have missed something that prevents the quaternion's Z-axis rotation from updating properly, even after changing the sensor's position and restarting the system (turning off the sensor, adjusting its position, and turning it back on)?

Note: Restarting means turning off the sensor, adjusting its position, and then powering it back on.

Thank you for your attention.

[kriswiner]

How are you calibrating the three sensors?

…

On Fri, Dec 20, 2024 at 12:15 AM gafa7758 ***@***.***> wrote: Hello, I am currently calculating quaternions using the Madgwick algorithm with magnetometer data. The calculation works well, as the quaternion values change correctly when the sensor is rotated. I have verified the results using a quaternion calculator and visualized them with a 3D cube model. However, I am encountering an issue with the qz value (yaw/heading). When the sensor is powered on while lying flat, with the magnetometer's X-axis pointing north, the initial quaternion is (1, 0, 0, 0). If I power off the sensor, rotate it 90 degrees about the Z-axis (so the magnetometer's X-axis points west), and power it back on, the quaternion remains (1, 0, 0, 0). This behavior seems incorrect since the magnetometer readings have to change after the rotation. I also tested rotating the sensor so that the magnetometer's X-axis points south or east, but the quaternion still does not change (still 1, 0, 0,0) when the program is restarted. Could I have missed something that prevents the quaternion's Z-axis rotation from updating properly, even after changing the sensor's position and restarting the system (turning off the sensor, adjusting its position, and turning it back on)? Note: Restarting means turning off the sensor, adjusting its position, and then powering it back on. Thank you for your attention. — Reply to this email directly, view it on GitHub <#495>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKQXRA777DBNYNECP232GPGYXAVCNFSM6AAAAABT6UDX5OVHI2DSMVQWIX3LMV43ASLTON2WKOZSG42TEMJYHE4TOMI> . You are receiving this because you are subscribed to this thread.Message ID: ***@***.***>

[gafa7758]

How are you calibrating the three sensors?
…
On Fri, Dec 20, 2024 at 12:15 AM gafa7758 @.> wrote: Hello, I am currently calculating quaternions using the Madgwick algorithm with magnetometer data. The calculation works well, as the quaternion values change correctly when the sensor is rotated. I have verified the results using a quaternion calculator and visualized them with a 3D cube model. However, I am encountering an issue with the qz value (yaw/heading). When the sensor is powered on while lying flat, with the magnetometer's X-axis pointing north, the initial quaternion is (1, 0, 0, 0). If I power off the sensor, rotate it 90 degrees about the Z-axis (so the magnetometer's X-axis points west), and power it back on, the quaternion remains (1, 0, 0, 0). This behavior seems incorrect since the magnetometer readings have to change after the rotation. I also tested rotating the sensor so that the magnetometer's X-axis points south or east, but the quaternion still does not change (still 1, 0, 0,0) when the program is restarted. Could I have missed something that prevents the quaternion's Z-axis rotation from updating properly, even after changing the sensor's position and restarting the system (turning off the sensor, adjusting its position, and turning it back on)? Note: Restarting means turning off the sensor, adjusting its position, and then powering it back on. Thank you for your attention. — Reply to this email directly, view it on GitHub <#495>, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKQXRA777DBNYNECP232GPGYXAVCNFSM6AAAAABT6UDX5OVHI2DSMVQWIX3LMV43ASLTON2WKOZSG42TEMJYHE4TOMI . You are receiving this because you are subscribed to this thread.Message ID: @.>

First of all, I would like to thank you, for the respon Sir
For calibration, I used bias error correction for the gyroscope and soft iron and hard iron calibration for the magnetometer. I did not calibrate the accelerometer because I believe its values are already good enough. Below are the equations I used in the code for calibration.

1. Gyroscope Calibration

    //Finding the total value with a certain number of samples.
    avr_gyroX = avr_gyroX + gyroX;
    avr_gyroY = avr_gyroY + gyroY;
    avr_gyroZ = avr_gyroZ + gyroZ;
    
    //Finding the offset value by dividing the total value by the total number of 
    //samples
    offset_gyroX = avr_gyroX/sample;
    offset_gyroY = avr_gyroY/sample;
    offset_gyroZ = avr_gyroZ/sample;
   

   Result:

      Sample: 752
   
      Average X: -227.23
      Average Y: 825.67
      Average Z: 825.24
   
      Offset X: -0.30
      Offset Y: 1.10
      Offset Z: 1.10
   
      Gyro X Calibrated: 0.03
      Gyro Y Calibrated: -0.00
      Gyro Z Calibrated: -0.03
   

2. Magnetometer Calibration

   //hard iron
    offset_x = (max_magX + min_magX)/2;
    offset_y = (max_magY + min_magY)/2;
    offset_z = (max_magZ + min_magZ)/2;
   
    //soft iron
    avg_delta_x = (max_magX - min_magX)/2;
    avg_delta_y = (max_magY - min_magY)/2;
    avg_delta_z = (max_magZ - min_magZ)/2;
   
    avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z)/3;
   
    //Scale
    scale_x = avg_delta/avg_delta_x;
    scale_y = avg_delta/avg_delta_y;
    scale_z = avg_delta/avg_delta_z;
   

   Result:

    avg_delta: 46.44
   
    Max X: 89.72	Min X: 6.30
    Max Y: 41.71	Min Y: -42.46
    Max Z: -63.17	Min Z: -174.19
   
    Offset X: 48.01
    Offset Y: -0.38
    Offset Z: -118.68
   
    Scale X: 1.11
    Scale Y: 1.10
    Scale Z: 0.84
   

3. Example of accelerometer value that i think it's already proper value, so i decide didn't calibrate it.
   ax: -0.01, ay: 0.04, az: 0.97 g
   ax: -0.02, ay: 0.03, az: 0.98 g
   ax: 0.01, ay:0.04, az: 0.98 g

So, the calibration I did is like that, do you think it’s correct or not? Here, I am using the 9DoF Razor IMU sensor from SparkFun to get raw data from each sensor, and it is done in real-time.
I have also uploaded the code and a short video related to the simulation that might help in understanding the issue I am facing. If you are willing to visit it, you can open my GitHub link: https://github.com/gafa7758/Quaternion_heading_issue.git

Thank you

[kriswiner]

It looks like you're doing the right thing for the accel and gyro. Here, when the sensor is flat and motionless, subtracting the average bias values should give a good result. To check, you should see 0, 0, 1000 mg on the accel and 0, 0, 0 dps on the gyro when the sensor is flat and motionless after calibration to ~1% or so. For the mag, this doesn't work <https://duckduckgo.com/?q=poor+mans+magnetometer+calibration&t=brave&ia=web>. You have to move the sensor in all three dimensions sampling every possible orientation multiple times while collecting bias data. Generally, the mag response surface will be non-spherical so ellipsoidal corrections (3 x 3 matrix) are needed for best results, but OK results can be achieved just by recentering <https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration> the response surface on the origin (subtract the average of the three axes like for the accel). As a quick check for the mag, after calibration you should see that the magnitude of the magnetic field is ~0.5 Gauss (Earth's average magnetic field). Better is to find out the expected field in your location (use USGS magnetic data or equivalent) and verify that the mag shows the expected values on all three axes. It really doesn't matter what sensor fusion algorithm you use if your sensors are not properly calibrated. Once they are, I think you will find the best results using the Mahony algorithm...

…

On Fri, Dec 20, 2024 at 10:07 AM gafa7758 ***@***.***> wrote: How are you calibrating the three sensors? … <#m_7331028583157023171_> On Fri, Dec 20, 2024 at 12:15 AM gafa7758 *@*.*> wrote: Hello, I am currently calculating quaternions using the Madgwick algorithm with magnetometer data. The calculation works well, as the quaternion values change correctly when the sensor is rotated. I have verified the results using a quaternion calculator and visualized them with a 3D cube model. However, I am encountering an issue with the qz value (yaw/heading). When the sensor is powered on while lying flat, with the magnetometer's X-axis pointing north, the initial quaternion is (1, 0, 0, 0). If I power off the sensor, rotate it 90 degrees about the Z-axis (so the magnetometer's X-axis points west), and power it back on, the quaternion remains (1, 0, 0, 0). This behavior seems incorrect since the magnetometer readings have to change after the rotation. I also tested rotating the sensor so that the magnetometer's X-axis points south or east, but the quaternion still does not change (still 1, 0, 0,0) when the program is restarted. Could I have missed something that prevents the quaternion's Z-axis rotation from updating properly, even after changing the sensor's position and restarting the system (turning off the sensor, adjusting its position, and turning it back on)? Note: Restarting means turning off the sensor, adjusting its position, and then powering it back on. Thank you for your attention. — Reply to this email directly, view it on GitHub <#495 <#495>>, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKQXRA777DBNYNECP232GPGYXAVCNFSM6AAAAABT6UDX5OVHI2DSMVQWIX3LMV43ASLTON2WKOZSG42TEMJYHE4TOMI <https://github.com/notifications/unsubscribe-auth/ABTDLKQXRA777DBNYNECP232GPGYXAVCNFSM6AAAAABT6UDX5OVHI2DSMVQWIX3LMV43ASLTON2WKOZSG42TEMJYHE4TOMI> . You are receiving this because you are subscribed to this thread.Message ID: @.*> First of all, I would like to thank you, for the respon Sir For calibration, I used bias error correction for the gyroscope and soft iron and hard iron calibration for the magnetometer. I did not calibrate the accelerometer because I believe its values are already good enough. Below are the equations I used in the code for calibration. 1. Gyroscope Calibration //Finding the total value with a certain number of samples. avr_gyroX = avr_gyroX + gyroX; avr_gyroY = avr_gyroY + gyroY; avr_gyroZ = avr_gyroZ + gyroZ; //Finding the offset value by dividing the total value by the total number of //samples offset_gyroX = avr_gyroX/sample; offset_gyroY = avr_gyroY/sample; offset_gyroZ = avr_gyroZ/sample; Result: Sample: 752 Average X: -227.23 Average Y: 825.67 Average Z: 825.24 Offset X: -0.30 Offset Y: 1.10 Offset Z: 1.10 Gyro X Calibrated: 0.03 Gyro Y Calibrated: -0.00 Gyro Z Calibrated: -0.03 2. Magnetometer Calibration //hard iron offset_x = (max_magX + min_magX)/2; offset_y = (max_magY + min_magY)/2; offset_z = (max_magZ + min_magZ)/2; //soft iron avg_delta_x = (max_magX - min_magX)/2; avg_delta_y = (max_magY - min_magY)/2; avg_delta_z = (max_magZ - min_magZ)/2; avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z)/3; //Scale scale_x = avg_delta/avg_delta_x; scale_y = avg_delta/avg_delta_y; scale_z = avg_delta/avg_delta_z; Result: avg_delta: 46.44 Max X: 89.72 Min X: 6.30 Max Y: 41.71 Min Y: -42.46 Max Z: -63.17 Min Z: -174.19 Offset X: 48.01 Offset Y: -0.38 Offset Z: -118.68 Scale X: 1.11 Scale Y: 1.10 Scale Z: 0.84 3. Example of accelerometer value that i think it's already proper value, so i decide didn't calibrate it. ax: -0.01, ay: 0.04, az: 0.97 g ax: -0.02, ay: 0.03, az: 0.98 g ax: 0.01, ay:0.04, az: 0.98 g So, the calibration I did is like that, do you think it’s correct or not? Here, I am using the 9DoF Razor IMU sensor from SparkFun to get raw data from each sensor, and it is done in real-time. I have also uploaded the code and a short video related to the simulation that might help in understanding the issue I am facing. If you are willing to visit it, you can open my GitHub link: https://github.com/gafa7758/Quaternion_heading_issue.git Thank you — Reply to this email directly, view it on GitHub <#495 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKTUB2HFJXPQEY7AXRD2GRMF3AVCNFSM6AAAAABT6UDX5OVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDKNJXGQ4DIMJRHE> . You are receiving this because you commented.Message ID: ***@***.***>

[gafa7758]

Hi Sir, thank you. My quaternion values are now changing correctly when I face the sensor towards the south, west, or east (it no longer stays at 1,0,0,0 continuously when program is first run) after you suggested using the Mahony algorithm. It is quite stable for flat positions and rotations around the Z-axis. However, when I make sequential changes to the pitch, roll, and yaw values, the values sometimes continue to change even after I stop moving the sensor. Additionally, could you provide me with references to learn more in-depth about the Mahony algorithm? I am using the algorithm from this code: https://github.com/PaulStoffregen/MahonyAHRS/blob/master/src/MahonyAHRS.cpp

As for the magnetometer calibration, I have followed the steps described in the article you shared, and it matches what I have been doing. I have also tried recalibrating multiple times, but the result remains the same (the quaternion values always start at 1,0,0,0 when the program is first run). Is there any other way to calibrate the magnetometer? Thank you

[kriswiner]

Did you check the magnetometer output against the expected values for your area as I suggested? In other words, how well (accurately) are your sensors calibrated? Then, what is the fusion rate you are using? Laggy behavior will occur with slow MCU speeds...

…

On Mon, Dec 23, 2024 at 3:33 AM gafa7758 ***@***.***> wrote: Hi Sir, thank you. My quaternion values are now changing correctly when I face the sensor towards the south, west, or east (it no longer stays at 1,0,0,0 continuously when program is first run) after you suggested using the Mahony algorithm. It is quite stable for flat positions and rotations around the Z-axis. However, when I make sequential changes to the pitch, roll, and yaw values, the values sometimes continue to change even after I stop moving the sensor. Additionally, could you provide me with references to learn more in-depth about the Mahony algorithm? I am using the algorithm from this code: https://github.com/PaulStoffregen/MahonyAHRS/blob/master/src/MahonyAHRS.cpp As for the magnetometer calibration, I have followed the steps described in the article you shared, and it matches what I have been doing. I have also tried recalibrating multiple times, but the result remains the same (the quaternion values always start at 1,0,0,0 when the program is first run). Is there any other way to calibrate the magnetometer? Thank you — Reply to this email directly, view it on GitHub <#495 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTDLKTFLZCF3USNSJCY7ID2G7YIFAVCNFSM6AAAAABT6UDX5OVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDKNJZGUYTOMRZGA> . You are receiving this because you commented.Message ID: ***@***.***>