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