Yaw value keeps decreasing?

Navio / Edge Hardware
1

Hi:

I recently found that when I run AHRS, the yaw value keeps decreasing even though I place my board still and not touch it.
The other values are correct and remain almost the same.
What’s the problem then?

Thanks!

Peter

2

This is due to how this example is written. It only uses gyros and accelerometers, which allow to determine orientation except for rotation around vector of gravity. To solve this you can add magnetometer to the filter.

3

Thanks for the explanation!
Can you provide me some references for how to correctly determine a yaw value?

Peter.

4

The easiest way would be to add magnetometer support to this example. Algorithm is already there, you need to call method imu.getMotion9 and pass magnetometer values to it. Here is the tricky part - magnetometer axis are different from accelerometer and gyroscope, so you would need to rotate magnetometer values before passing them to match others. Take a look how they are placed relative to each other (page 38). Magnetometer calibration is another important step, there are different ways to do it, search for hard and soft iron calibration.

5

I’ve noticed this problem too and have been working to get the magnetometer included in the AHRS. I performed a magnetometer calibration and found that (at least for the bench setup) a hard iron correction was all that was needed (picture attached). Based on the data sheet for the MPU9250,

mag x is inline with accel/gyro y
mag y is inline with accel/gyro x
magz is inline with accel/gryo -z

In the AHRS code I commented out the imu.getMotion6 along with ahrs.updateIMU and uncommented the next section down and modified it to include the calibration

 imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
 mx=(mx-17.0317)/100; // Gauss, IMU output = micro Tesla
 my=(my-25.7831)/100; // Gauss
 mz=(mz+12.5051)/100; // Gauss
 ahrs.update(ax, ay, az, gx/0.0175, gy/0.0175, gz/0.0175, my, mx, -mz, dt);

This does not give any meaningful results, for example

ROLL: +119.82 PITCH: +38.54 YAW: -38.54 PERIOD 0.0012s RATE 850Hz
ROLL: -22.06 PITCH: -9.71 YAW: -26.09 PERIOD 0.0012s RATE 848Hz
ROLL: -164.30 PITCH: +24.48 YAW: -71.98 PERIOD 0.0014s RATE 733Hz
ROLL: +41.79 PITCH: +28.42 YAW: -10.14 PERIOD 0.0012s RATE 846Hz
ROLL: -77.72 PITCH: -7.94 YAW: -56.35 PERIOD 0.0012s RATE 848Hz

Two questions that I have are:

  1. What units does the magnetometer reading need to have for the ahrs.update to function? (Based on the Madgwick AHRS MATLAB code, it appears Telsa or Gauss can be used.)
  2. Am I feeding in the correct magnetometer orientation into the ahrs.update?
6

Everything looks correct to me (except for the results :smile: ). It does not really matter which units you use as magnetometer measurements are normalized.

// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;

Magnetometer orientation seems correct, your calibration results are great.

There must be some bug in the code, the best idea would be to just go and check sanity of results on each step.

7

I’ve tried to include the change in AHRS that larssoltmann said but the yaw value continue decreasing. Could you upload some example of AHRS with magnetometer? Thank you.

Moreover, in the last line of larssoltmann’s code, is it possible that the code was like this:
ahrs.update(ax, ay, az, gx0.0175, gy0.0175, gz*0.0175, my, mx, -mz, dt);
Because when you use the function ahrs.updateIMU, giro values are multiplying.

1 Like
8

@alsamon1 That’s a good find! That should explain results that @larssoltmann got with jumping values. Lars, you were integrating huge angular velocities.

Magnetometer calibration is compulsory to get good results.

9

Great find alsamon1! That fixed the issue. I did notice that when the system is left untouched during and after AHRS initialization, the yaw values still wander as they did without the magnetometer. If I move the system around and then set it down, the yaws remain steady and change as expected when a rotation is applied. No big deal, I can live with moving the plane before starting data collection. Thanks again for the help!

10

Great that you got it working! As yaw value is initialized at 0 it will drift for some time until it matches magnetometer reading, after that it should fine. Is that what you are experiencing?

Maybe you can make a pull request to our repo so that others can benefit from your work?

Do not forget about magnetic declination compensation :smile:

11

You are correct about the yaw drift till magnetometer matching. I let the system still undisturbed and it took about a minute for the yaw value to stabilize (plot attached). Magnetic declination compensation is next.

12

You can also try to increase gain in the filter so that it converges faster.

13

The plot was made using the example script and was not run in real time, loop frequency was shown to be ~150Hz. Once I added the AHRS code to my data logging script in a separate thread running in real time, convergence was much faster, less than 10sec.

14

That is much better. We can probably come up with another initialization algorithm that will first calculate approximate yaw from mag and accelerometer and use it as initial value.

If you don’t mind I would really like to add your changes to our repository. Could you send a pull request on GitHub? Let me know if I could somehow help with that.

Thanks for making it work with magnetometer!