Yaw angle..How to correct unstable values ??

Using Arduino Sensors
1

Hello !

I'm working with Arduino Due and a 9DOF accel/gyro/mag, and I'm trying to get Yaw, Pitch and Roll.
I managed to get stable Roll and Pitch angles using kalman filter...

But I can't get stable Yaw values, although, I managed to get stable raw data from the magnetometer. So I'm presuming there's something wrong with calibration and normalization...raw data process (correct me if I'm wrong).

this is how I calculate the bias:

     for (i=0;i<100;i++)
     {       
       mx = IMU_getData (AK8963_ADDRESS, MAG_XOUT_H, MAG_XOUT_L);
       my = IMU_getData (AK8963_ADDRESS, MAG_YOUT_H, MAG_YOUT_L);
       mz = IMU_getData (AK8963_ADDRESS, MAG_ZOUT_H, MAG_ZOUT_L);
       Status2_reg = I2CreadByte (AK8963_ADDRESS, 0x09);
      magXbias += mx ;
      magYbias += my ;
      magZbias += mz ;
     }
      magXbias  = magXbias  / 100 ;
      magYbias  = magYbias  / 100 ;
      magZbias  = magZbias  / 100 ;

and Yaw angle:

     mx = ( mx - magXbias ) * magCalibration[0] * 10 / mag_sensitivity ; // milligauss
     my = ( my - magYbias ) * magCalibration[1] * 10 / mag_sensitivity ; // milligauss
     mz = ( mz - magZbias ) * magCalibration[2] * 10 / mag_sensitivity ; // milligauss
  
     mag_norm = sqrt (mx*mx + my*my + mz*mz);
     mx_norm = mx / mag_norm ;
     my_norm = my / mag_norm ;
     mz_norm = mz / mag_norm ;
  
     yawX = mx_norm*cos(Pitch) + my_norm*sin(Pitch)*sin(Roll) + mz_norm*sin(Pitch)*cos(Roll) ;
     yawY = my_norm*cos(Roll) - mz_norm*sin(Roll) ;

     Yaw = ( atan2 (-yawY, yawX) ) * 180 / PI;

Please, Anyone who can tell what wrong with my code ?
...Thank you

2

any ideas ? any help Please ??

this is what I get from the serial monitor using my code
Raw data || calibration data || Normalization data || yawX YawY || Yaw (in Degrees)

R:0 49	32||cal:-38.93	19.47	18.82 ||Norm:-0.82  0.41   0.40 ||yX:-0.87   yY: -0.44||Y 153.25
R:0 51	33||cal:-38.93	58.40	37.63 ||Norm:-0.49  0.73   0.47 ||yX:-0.78   yY: -0.63||Y 141.15
R:4 48	32||cal:38.93	0.00	18.82 ||Norm:0.90   0.00   0.44 ||yX:0.23    yY: -0.43||Y 61.56
R:2 50	33||cal:0.00	38.93	37.63 ||Norm:0.00   0.72   0.69 ||yX:-0.77   yY: -0.64||Y 140.14
R:4 48	31||cal:38.93	0.00	0.00  ||Norm:1.00   0.00   0.00 ||yX:0.16    yY: 0.00 ||Y 0.00
R:2 47	34||cal:0.00	-19.47	56.45 ||Norm:0.00   -0.33  0.95 ||yX:-0.17   yY: -0.99||Y 99.70
R:2 48	30||cal:0.00	0.00	-18.82||Norm:0.00   0.00   -1.00||yX:0.67    yY: 0.72 ||Y -47.11
R:3 49	31||cal:19.47	19.47	0.00  ||Norm:0.71   0.71   0.00 ||yX:-0.36   yY: 0.47 ||Y -127.32
R:2 50	33||cal:0.00	38.93	37.63 ||Norm:0.00   0.72   0.69 ||yX:-0.87   yY: -0.46||Y 152.25
R:2 46	31||cal:0.00	-38.93	0.00  ||Norm:0.00   -1.00  0.00 ||yX:0.97    yY: -0.00||Y 0.21

And I'm not even moving the sensor !

3

The problem is in the part of the code you did not post.

4

Thank you soo much for replying,
I didn't want to throw hundreds of lines, just those the post may concern, or so I thought...

But, please be my guest here's attached the whole code..

_9DOF_YawPitchRoll.ino (9.79 KB)

5

It is hard to see how that code could ever be made to work. What is the theory behind it?

If you want to study or use code that actually works, try RTIMULib for Arduino.

6

First of all thank you so much for replying and really sorry for being late, I was trying to understand the link you posted (and still a little bit lost)

The theory behind my code; All I want is to get Roll Pitch Yaw, Euler Angles for my IMU (I didn't thought it was that hard :confused: )

Here attached again my code (with some improvements and comments).

I want to know in theory, What should be the range of Yaw angles ? something like [0, 360] or [-180, 180] right ?
should I expect accuracy in Yaw values as much as Roll and Pitch ?

Should I keep the IMU stable when calibration occurs at the beginning ? (or make an eight movement for exp)
I still get unstable values whereas the IMU is not moving

...If anything is still not clear in the code, I'll be glad to explain it (I know my code isn't well written...I'm a beginner in all this )

...Thank you in advance for any help

_9DOF_YawPitchRoll.ino (9.6 KB)

7

Yaw angles are traditionally [0,360], either from true or magnetic North.

It looks like you are trying to implement tilt compensation, but to do so correctly requires that the definitions of pitch, roll and yaw are all consistent. How have you made certain of this? Obviously, if the compass is not tilted, this correction is unnecessary.

The "Kalman filter" part is simply wrong and doesn't help. Get rid of it. You can read more about the problems, and a solution here.

You do not need to take movement into account to estimate pitch, roll and yaw when the sensor is held still. Solve that problem before you go on to include the gyro contribution.

What does the current program do when you run it? The output posted earlier is not particularly helpful.

This is not a beginner's project.

8

Hi,

I thought I have to implement tilt compensation for the gyrometer and the compass (due to drifting), if I'm wrong I'll pass tilt compensation for the compass...

I used Kalman Filter to combine both accelerometer and gyrometer data to get stable no drift Roll and Pitch angles (accelerometer is instable but doesn't drift, gyrometer is stable but does drift...) anyway Roll and Pitch are working fine as wanted :smiley:
....I'm not worried about these anymore, but I'm still unable to get proper Yaw angle.

The current program ;

  • calculates the bias for gyrometer and compass
  • gets raw data from accelerometer, gyrometer and compass
    -calculates Roll and Pitch using Kalman filter
    -calculates Yaw angle (this Yaw angle I'm trying to get is unstable)
  • display Roll Pitch and Yaw angles on the Serial monitor.
    When I run the program and the IMU is put horizontally on the table, Roll and Pitch angles are around 0 Degrees, but Yaw angle is displaying nonsense random outputs..and I can't see why ?!!

Yes, you are 100% right, this is not a beginner's project, but I'm stuck with it and have no other choice but to get it to work so I could succeed my internship..

9

For a horizontal, properly calibrated magnetometer, the yaw angle relative to magnetic North is simply calculated as atan2(magy, magx), in radians. That does not drift.

Your problem is that you are throwing everything in at once, without making sure that you completely understand any one aspect of the problem.

Unless you are being required to learn the theory and implementation of an IMU, I suggest to buy a Bosch BNO055 self contained absolute orientation sensor. It is easy to use and in my hands is accurate to about 1 degree in absolute orientation, regardless of the gyrations to which it is subjected.

10

Hello,
What do you mean by ?

jremington:
For a horizontal, properly calibrated magnetometer, the yaw angle relative to magnetic North is simply calculated as atan2(magy, magx)

I mean I'm using the IMU in 3D space, in every possible position and orientation and that shouldn't affect Euler angles measurements. For example, if the IMU is put in a vertical position does "atan2(magy, magx)" always give me a proper Yaw ?

jremington:
Your problem is that you are throwing everything in at once, without making sure that you completely understand any one aspect of the problem.

Trust me I'm trying....but honestly, I'm not required to completely understand the hole theory, I just have to get 3 proportional angles of the IMU orientation in space. So, I'm doing my best to understand so later I could explain it, but I also have a deadline to fulfill.

Unfortunately, The choice of the IMU is already done I can't change it, I have to work with the MPU9250..

Best regards

11

In order to write properly functioning code for an IMU, it is absolutely necessary to completely understand the theory.

12

I understood the theory behind Roll and Pitch using Kalman filter and successfully managed to get proper Roll and Pitch. ("mission accomplished" for this part), even though the program isn't well organized.

I read uncountable number links explaining the theory behind using the compass to get Yaw angle...but didn't understood it 100%, maybe that's why I'm "throwing everything in at once".
Can you please point me a reliable, complete, "easy to understand" link, to master the theory behind Yaw angle ?

Thanks in advance,

13

I understood the theory behind Roll and Pitch using Kalman filter and successfully managed to get proper Roll and Pitch.

Except that the code you are using does not actually implement a Kalman filter (as I pointed out earlier) and even the attempt to use it is misguided. You cannot consider two angles of the 3D problem in isolation.

To get a 3D orientation from a 9DOF sensor, with proper handling of sensor error, requires code that is considerably more complicated and quite differently organized than what you currently have.

There are currently about three open source implementations of IMU code for an Arduino that actually work. One of those is RTIMULib, another is minimu-9 v3, and of course there is Ardupilot, at diydrones.com. The rest don't work.

14

OK, so to calibrate the magnetometer using ArduinoMagCal, I have to include #include <EEPROM.h>

But, I'm working with Arduino Due wich has no EEPROM :frowning:

15

You can add a 24LC256 EEPROM to your project and use the EEPROMAnything library. It operates at a range of voltages including 3v3, jumpers to one of eight addresses, and communicates via I2C. It's a very easy way to add nonvolatile storage.

Search the network forum as I recently posted more details of the library.

16

I can only deal with what I have MPU9250 and Arduino Due...

But why should I even calibrate the magnetometer using ArduinoMagCal from RTIMULib when I already have ASAx "sensitivity adjustment value" ? isn't it enough to get calibrated data from the compass ?

17

The ArduinoMagCal function is only an example on how to get the three axis data. To calibrate, you would need an external reference of known value. For starters, assume the sensor is already correct and use the vector magnitude and compare to the earth's known field strength. You may have to move it outdoors and away from pipes and wires to see only the earth field.

18

OK, so to start I have to calibrate the compass.
For that, I need to get the max and min for magX, magY and magZ, calculate the bias to finally get a calibrated data from the compass ?

19

The bias factor is used when you want to create a compass and correct for local disturbing fields, such as in an auto. For field strength, just calculate the magnitude while the device is out in the open and away from local objects.