yaw/heading changes when moving straight ahead! what am I doing wrong ?

I have mounted a MPU9250 to a small robot car and am totally stuck on getting a correct yaw/heading calculation. The sensor is mounted with Z down, X forward and Y sideways. The testing has been performed on a flat desk and floor. (so no or minimal roll or pitch)

I have tried different ways of calibration. 1) spinning around the z-axes 2) making the figure 8 by hand. And I have put the various bias and scaling parameters into the programs. The sample values show nice round blobs around the (0,0) point when put in scatter graphs.

I am calculating the yaw with the madgwickfilter and then to euler coordinates, as per your examples. For comparison I am also calculating the heading by a simple atan of my/mx. (and the two calculations show consistent results, with some delay visible in the yaw calculation)

When I keep the robot on the desk and turn it around, the yaw/heading changes as it should in all directions. Nice and accurate.

**The problem is when I move the robot straight ahead, the yaw/heading also changes (as much as 30 degrees when moving just 1 meter straight ahead). And when I move it back to the original position, the yaw/heading changes back to what it was.

I cannot find what I am doing wrong. Any idea where I should be looking for a fix?**

Thanks in advance for your help.

Just one question, when you move the robot straight ahead, are you doing this by hand or using the robot motors?

Could be the magnetic field generated by current flowing to power the motors is affecting your heading estimation.

If by hand, then have you properly calibrated the accelerometer and gyro?

On Tue, Mar 1, 2022 at 6:26 AM WillemD61 @.***> wrote:

I have mounted a MPU9250 to a small robot car and am totally stuck on getting a correct yaw/heading calculation. The sensor is mounted with Z down, X forward and Y sideways. The testing has been performed on a flat desk and floor. (so no or minimal roll or pitch)

I have tried different ways of calibration. 1) spinning around the z-axes 2) making the figure 8 by hand. And I have put the various bias and scaling parameters into the programs. The sample values show nice round blobs around the (0,0) point when put in scatter graphs.

I am calculating the yaw with the madgwickfilter and then to euler coordinates, as per your examples. For comparison I am also calculating the heading by a simple atan of my/mx. (and the two calculations show consistent results, with some delay visible in the yaw calculation)

When I keep the robot on the desk and turn it around, the yaw/heading changes as it should in all directions. Nice and accurate.

**The problem is when I move the robot straight ahead, the yaw/heading also changes (as much as 30 degrees when moving just 1 meter straight ahead). And when I move it back to the original position, the yaw/heading changes back to what it was.

I cannot find what I am doing wrong. Any idea where I should be looking for a fix?**

Thanks in advance for your help.

— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/474, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKTQL4WWCPNMFSOGJCTU5YSJBANCNFSM5PUGLESQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

You are receiving this because you are subscribed to this thread.Message ID: @.***>

Thanks for the quick response. I have tried both moving it by hand and by motors. It does not make any difference. Yes, I have also calibrated accelerometer and gyro, although, since the simple calculation using only magnetometer x and y value (without fusion) shows the same results, I think this is not a factor. I will try moving to a different location to see whether it could be an environmental factor. And I will print/debug all step in the full calculation. I must be doing something wrong somewhere......I just fail to understand how the spin values can be correct while the straight line values are wrong.

I don;t have any obvious idas for you to try. It is possible the data scaling is wrong so that the motion is not being accurately measured, but otherwise this should work. It has on thousands of applications.

On Tue, Mar 1, 2022 at 8:48 AM WillemD61 @.***> wrote:

Thanks for the quick response. I have tried both moving it by hand and by motors. It does not make any difference. Yes, I have also calibrated accelerometer and gyro, although, since the simple calculation using only magnetometer x and y value (without fusion) shows the same results, I think this is not a factor. I will try moving to a different location to see whether it could be an environmental factor. And I will print/debug all step in the full calculation. I must be doing something wrong somewhere......I just fail to understand how the spin values can be correct while the straight line values are wrong.

— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/474#issuecomment-1055642650, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKUWZ3ZDVSV52MFWDPDU5ZC7VANCNFSM5PUGLESQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

You are receiving this because you commented.Message ID: @.***>

Thanks anyway for your response and advice.

One more question about calibration of the magnetometer: is it normal to get quite different values each time the calibration is repeated?

I used the figure-8 movement calibration and find that the bias values are quite different each time.

For example I am getting plus 22 xbias one time, plus 40 a second time and minus 20 a third time (this is after multiplication with the 1.49 scale factor and the 1.17 factory calibration factor from the sensor). This seems to be very dependent on the figure-8 movement that is made. Maybe that could be a problem in my case?

I am making a large figure 8 movement in the horizontal plane (1 m wide), with some movement up and down (approx plus or minus 30 cm), while also turning the robot with sensor almost upside down during the movement. (it was decribed as the movement of a boy with a toy plane :-) )

Poor mag calibration is common and could be the cause of the behavior you are seeing.

Figure 8 means the magnetometer orientation must sample all possibilities multiple times during the calibration phase. So imagine an arrow sticking up from the board surface and the device is inside a giant balloon. The idea is to move the device during calibration such that the tip of the arrow points to every point of the inside of the balloon surface at least once and preferably multiple times.

Once you have calibrated averages you should see the corrected mag scatter plots centered on the origin and more or les spherical in shape.

The other way to check proper mag calibration is to look up the local mag field from the USGS site or something similar for your location. The mag readings with the device flat and motionless should match the USGS values pretty closely.

Lastly, the magnitude of the mag field sqrt(Mx^2 + My^2 + Mz^2) should be about 0.5 Gauss.

So lots of ways to check if your mag values are reasonable.

On Wed, Mar 2, 2022 at 6:56 AM WillemD61 @.***> wrote:

Thanks anyway for your response and advice.

One more question about calibration of the magnetometer: is it normal to get quite different values each time the calibration is repeated?

I used the figure-8 movement calibration and find that the bias values are quite different each time.

For example I am getting plus 22 xbias one time, plus 40 a second time and minus 20 a third time (this is after multiplication with the 1.49 scale factor and the 1.17 factory calibration factor from the sensor). This seems to be very dependent on the figure-8 movement that is made. Maybe that could be a problem in my case?

I am making a large figure 8 movement in the horizontal plane (1 m wide), with some movement up and down (approx plus or minus 30 cm), while also turning the robot with sensor almost upside down during the movement. (it was decribed as the movement of a boy with a toy plane :-) )

— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/474#issuecomment-1057022569, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKS67GPFDSVOMXEVT23U556SVANCNFSM5PUGLESQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

You are receiving this because you commented.Message ID: @.***>

Just an update on the investigation, for anyone interested. I have done several different calibrations and measurements.

1) A figure 8 calibration, standing straight with the device in the hand of a stretched arm, making figure 8 movements (on the inside of the sphere as described by Kris), while slowly spinning myself around (or better : walking small circles around the z-axis of my stretched arm). 2) Separate calibrations of only 2 axes each time by sitting in a turning desk chair with the device in front of me. Each time one axis pointing down, one pointing inward and one pointing sideways, so 6 separate measurements. Making about 6-7 turns per measurement series. 3) Measurements with the device static on the floor and moving it forward 1 meter between each measurement (by hand), at 5 different positions, keeping it pointed in the same direction. Repeating the measurements a second time at each position. 4) Measurements with the device spinning two circles on the floor, at 5 different positions, each 1 meter apart. Also repeated a second time. (the sensor is mounted exactly in the middle of my spinning robot) 5) Repeating measurement 3 and 4 on the ground floor of my house and also in the garden.

Observations/conclusions so far: 1) The madgwickfilter takes quite a lot of measurements to converge to expected/stable values. Approx. 700 measurements (so 0.7 seconds) are needed to let roll and pitch converge to a value close to zero (since the device is on a flat floor) and to stabilise the yaw. This is needed each time a measurement series is started, even if the quaternion is not initialised to 0 in between measurements. Other than that, the software seems to be working fine. (I am using a python version of the software presented here) 2) The python version of the software I used (created by someone else from the C programs here) had an error, not making proper distinction between raw calibration values and processed calibration values (i.e. multiplied by the measurement scale and by the factory calibration values). I see in the C program presented here that this distinction is made in the correct way. 3) After the calibrations I end up with very nice circles around the (0,0) point in a x,y scatter graph, so any remaining issues do not seem to be caused by calibration problems. 4) The two series of measurements of point 3, 4 and 5 show very consistent results. So the measurement of a position in the first series correspond to measurements at the same position in the second series (raw values and of course also processed values), even if the device was moved in between. This seems to suggest an environmental factor. 5) The circles produced by measurement 4 indoors are nicely centered but of different size (concentric). 3 of the 5 circles have some overlap, 1 is much larger and 1 much smaller. This again suggests and environmental factor. Also this of course cannot be corrected by calibration, since this would affect all circles. 6) The measurement in the garden produces only one circle, so all 5 measurements overlap with each other. Looking at the x,y scatter only 1 out of the 5 measurements shows a slight deviation. 7) I further improved the consistency by first determining an average and standard deviation of the sqrt, based on initial 100 measurements and then in the rest of the process ignore any measurements that produce a sqrt outside 2 times the standard deviation from the average.

So there seems to be an environmental factor in my house since the garden measurement is much better, although not perfect. Maybe the metal of the reinforced concrete floor or water and/or electricity lines in the floors? If that is the issue then my problem is not solvable.

I also wonder whether wifi or bluetooth signals could be of influence. The sensors are mounted at short distance of a Raspberry Pi, controlled headless via wifi. (although I don't know how such disturbance could be depending on position)

I will now first continue the investigation with the sensors detached from my robot, using long wires. I will also try to do measurements with wifi and bluetooth switched off.

And I need still to check how the Z axis varies between positions and whether this can be used in some way to correct any x,y values (not logical on a flat floor...)

If you have a strong environmental magnetic source in the area where you want to do AHRS estimation about all you can do is either drop the mag completely and use 6 DoF sensor fusion or de-weight the mag field contribution to the 9 DoF fusion so the distortions affects the heading estimation less...but no good choice here....

On Sun, Mar 13, 2022 at 12:37 PM WillemD61 @.***> wrote:

Just an update on the investigation, for anyone interested. I have done several different calibrations and measurements.

A figure 8 calibration, standing straight with the device in the hand of a stretched arm, making figure 8 movements (on the inside of the sphere as described by Kris), while slowly spinning myself around (or better : walking small circles around the z-axis of my stretched arm).

Separate calibrations of only 2 axes each time by sitting in a turning desk chair with the device in front of me. Each time one axis pointing down, one pointing inward and one pointing sideways, so 6 separate measurements. Making about 6-7 turns per measurement series.

Measurements with the device static on the floor and moving it forward 1 meter between each measurement (by hand), at 5 different positions, keeping it pointed in the same direction. Repeating the measurements a second time at each position.

Measurements with the device spinning two circles on the floor, at 5 different positions, each 1 meter apart. Also repeated a second time. (the sensor is mounted exactly in the middle of my spinning robot)

Repeating measurement 3 and 4 on the ground floor of my house and also in the garden.

Observations/conclusions so far:

The madgwickfilter takes quite a lot of measurements to converge to expected/stable values. Approx. 700 measurements (so 0.7 seconds) are needed to let roll and pitch converge to a value close to zero (since the device is on a flat floor) and to stabilise the yaw. This is needed each time a measurement series is started, even if the quaternion is not initialised to 0 in between measurements. Other than that, the software seems to be working fine. (I am using a python version of the software presented here)

The python version of the software I used (created by someone else from the C programs here) had an error, not making proper distinction between raw calibration values and processed calibration values (i.e. multiplied by the measurement scale and by the factory calibration values). I see in the C program presented here that this distinction is made in the correct way.

After the calibrations I end up with very nice circles around the (0,0) point in a x,y scatter graph, so any remaining issues do not seem to be caused by calibration problems.

The two series of measurements of point 3, 4 and 5 show very consistent results. So the measurement of a position in the first series correspond to measurements at the same position in the second series (raw values and of course also processed values), even if the device was moved in between. This seems to suggest an environmental factor.

The circles produced by measurement 4 indoors are nicely centered but of different size (concentric). 3 of the 5 circles have some overlap, 1 is much larger and 1 much smaller. This again suggests and environmental factor. Also this of course cannot be corrected by calibration, since this would affect all circles.

The measurement in the garden produces only one circle, so all 5 measurements overlap with each other. Looking at the x,y scatter only 1 out of the 5 measurements shows a slight deviation.

I further improved the consistency by first determining an average and standard deviation of the sqrt, based on initial 100 measurements and then in the rest of the process ignore any measurements that produce a sqrt outside 2 times the standard deviation from the average.

So there seems to be an environmental factor in my house since the garden measurement is much better, although not perfect. Maybe the metal of the reinforced concrete floor or water and/or electricity lines in the floors? If that is the issue then my problem is not solvable.

I also wonder whether wifi or bluetooth signals could be of influence. The sensors are mounted at short distance of a Raspberry Pi, controlled headless via wifi. (although I don't know how such disturbance could be depending on position)

I will now first continue the investigation with the sensors detached from my robot, using long wires. I will also try to do measurements with wifi and bluetooth switched off.

And I need still to check how the Z axis varies between positions and whether this can be used in some way to correct any x,y values (not logical on a flat floor...)

— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/474#issuecomment-1066168013, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKXF27W6D7EUAPOMKNTU7Y7XTANCNFSM5PUGLESQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

You are receiving this because you commented.Message ID: @.***>

You might want to try something like this https://www.tindie.com/products/onehorse/usfsmax-module-and-carrier/ where we can compensate for some of this environmental field effect in firmware via dynamic hard iron calibration.

From the product page:

"The magnetometer calibration includes both soft iron and hard iron corrections and each board will deliver better than 0.5 degree rms heading accuracy (0.3 rms degree heading accuracy is typical) with no further user calibration. However, we know that local magnetic fields in the home or when the device is mounted in or on a vehicle (car, plane, etc.) can induce additional sources of hard iron offsets so we have incorporated a dynamic hard iron corrector into the MAX32660 fusion algorithms that compensates for environmental hard iron offsets during regular motion of the sensor in the environment. This compensation isn't perfect, but it allows excellent heading accuracy to be maintained even in challenging magnetic environments."

On Mon, Mar 14, 2022 at 1:35 PM Tlera Corporation @.***> wrote:

If you have a strong environmental magnetic source in the area where you want to do AHRS estimation about all you can do is either drop the mag completely and use 6 DoF sensor fusion or de-weight the mag field contribution to the 9 DoF fusion so the distortions affects the heading estimation less...but no good choice here....

On Sun, Mar 13, 2022 at 12:37 PM WillemD61 @.***> wrote:

Just an update on the investigation, for anyone interested. I have done several different calibrations and measurements.

A figure 8 calibration, standing straight with the device in the hand of a stretched arm, making figure 8 movements (on the inside of the sphere as described by Kris), while slowly spinning myself around (or better : walking small circles around the z-axis of my stretched arm).

Separate calibrations of only 2 axes each time by sitting in a turning desk chair with the device in front of me. Each time one axis pointing down, one pointing inward and one pointing sideways, so 6 separate measurements. Making about 6-7 turns per measurement series.

Measurements with the device static on the floor and moving it forward 1 meter between each measurement (by hand), at 5 different positions, keeping it pointed in the same direction. Repeating the measurements a second time at each position.

Measurements with the device spinning two circles on the floor, at 5 different positions, each 1 meter apart. Also repeated a second time. (the sensor is mounted exactly in the middle of my spinning robot)

Repeating measurement 3 and 4 on the ground floor of my house and also in the garden.

Observations/conclusions so far:

The madgwickfilter takes quite a lot of measurements to converge to expected/stable values. Approx. 700 measurements (so 0.7 seconds) are needed to let roll and pitch converge to a value close to zero (since the device is on a flat floor) and to stabilise the yaw. This is needed each time a measurement series is started, even if the quaternion is not initialised to 0 in between measurements. Other than that, the software seems to be working fine. (I am using a python version of the software presented here)

The python version of the software I used (created by someone else from the C programs here) had an error, not making proper distinction between raw calibration values and processed calibration values (i.e. multiplied by the measurement scale and by the factory calibration values). I see in the C program presented here that this distinction is made in the correct way.

After the calibrations I end up with very nice circles around the (0,0) point in a x,y scatter graph, so any remaining issues do not seem to be caused by calibration problems.

The two series of measurements of point 3, 4 and 5 show very consistent results. So the measurement of a position in the first series correspond to measurements at the same position in the second series (raw values and of course also processed values), even if the device was moved in between. This seems to suggest an environmental factor.

The circles produced by measurement 4 indoors are nicely centered but of different size (concentric). 3 of the 5 circles have some overlap, 1 is much larger and 1 much smaller. This again suggests and environmental factor. Also this of course cannot be corrected by calibration, since this would affect all circles.

The measurement in the garden produces only one circle, so all 5 measurements overlap with each other. Looking at the x,y scatter only 1 out of the 5 measurements shows a slight deviation.

I further improved the consistency by first determining an average and standard deviation of the sqrt, based on initial 100 measurements and then in the rest of the process ignore any measurements that produce a sqrt outside 2 times the standard deviation from the average.

So there seems to be an environmental factor in my house since the garden measurement is much better, although not perfect. Maybe the metal of the reinforced concrete floor or water and/or electricity lines in the floors? If that is the issue then my problem is not solvable.

I also wonder whether wifi or bluetooth signals could be of influence. The sensors are mounted at short distance of a Raspberry Pi, controlled headless via wifi. (although I don't know how such disturbance could be depending on position)

I will now first continue the investigation with the sensors detached from my robot, using long wires. I will also try to do measurements with wifi and bluetooth switched off.

And I need still to check how the Z axis varies between positions and whether this can be used in some way to correct any x,y values (not logical on a flat floor...)

— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/474#issuecomment-1066168013, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKXF27W6D7EUAPOMKNTU7Y7XTANCNFSM5PUGLESQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

You are receiving this because you commented.Message ID: @.***>

Thanks for that suggestion, but I decided not to invest in another solution and just accept that my ambition was a bit too high.

My goal was to use these sensors to do SLAM, i.e. simultaneous localisation and mapping, so the robot could return home via the shortest distance once its mission was complete (search and rescue). It can already do a traceback by keeping track of all steps made but that is not very efficient to get home.

I wanted to use the accelerometer to measure acceleration, and then integrate over time into speed, and then do a second integration to distance. Turned out with my slow moving robot that the acceleration was almost completely hidden within the noise as soon as the robot started moving. With a good low pass filter I can confirm the acceleration took place but it is too inaccurate to do any calculation. Also because errors just increase when you do integration over time.

Also I wanted to use magnetometer and gyro to track the heading, with the issues described above in this thread. I now also tested whether wireless signals cause any disturbance. It does not make a difference. I think the problems are caused by the environment. I clearly see different magnetic field strengths at different places in my house. I will have to live with the inaccuracies cause by this.

I have now also added revolution counters to the wheels (a 16 component black/white sticker to the wheels and some IR reflection sensors). I know those are not very accurate either and errors will accumulate over time.

And I was already using a camera to recognise the environment and measure distance and angle to the target.

I will still try and see how much I can still achieve. The learning aspect is part of the fun anyway.

As far as I am concerned you can close this issue. No problem with the software. Thanks again for the various suggestions provided.

Tough problem. dead reckoning based on inertial sensors is not very accurate right now. For direct speed measurements you might consider the PMW3901 https://www.tindie.com/products/onehorse/pmw3901-optical-flow-sensor/ and related https://www.tindie.com/products/onehorse/paa3905-optical-flow-camera/ optical flow sensors. These give x/y speed directly and looking up at the ceiling (indoors) could provide you with a better dead reckoning solution (only one integration).

On Wed, Mar 16, 2022 at 9:05 AM WillemD61 @.***> wrote:

Thanks for that suggestion, but I decided not to invest in another solution and just accept that my ambition was a bit too high.

My goal was to use these sensors to do SLAM, i.e. simultaneous localisation and mapping, so the robot could return home via the shortest distance once its mission was complete (search and rescue). It can already do a traceback by keeping track of all steps made but that is not very efficient to get home.

I wanted to use the accelerometer to measure acceleration, and then integrate over time into speed, and then do a second integration to distance. Turned out with my slow moving robot that the acceleration was almost completely hidden within the noise as soon as the robot started moving. With a good low pass filter I can confirm the acceleration took place but it is too inaccurate to do any calculation. Also because errors just increase when you do integration over time.

Also I wanted to use magnetometer and gyro to track the heading, with the issues described above in this thread. I now also tested whether wireless signals cause any disturbance. It does not make a difference. I think the problems are caused by the environment. I clearly see different magnetic field strengths at different places in my house. I will have to live with the inaccuracies cause by this.

I have now also added revolution counters to the wheels (a 16 component black/white sticker to the wheels and some IR reflection sensors). I know those are not very accurate either and errors will accumulate over time.

And I was already using a camera to recognise the environment and measure distance and angle to the target.

I will still try and see how much I can still achieve. The learning aspect is part of the fun anyway.

As far as I am concerned you can close this issue. No problem with the software. Thanks again for the various suggestions provided.

— Reply to this email directly, view it on GitHub https://github.com/kriswiner/MPU9250/issues/474#issuecomment-1069296376, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKSMIYBAEILYH2O65Z3VAIBGBANCNFSM5PUGLESQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

You are receiving this because you commented.Message ID: @.***>

kriswiner / MPU9250

yaw/heading changes when moving straight ahead! what am I doing wrong ? #474