What is wrong with my yaw IMU code?

Projects Programming
1

i got the Pololu IMU9(L3G4200D gyro and LSM303 acc/mag) and wrote my own program(Digitalduino: Pololu MiniIMU-9 Code) that reads the values from ACC, Gyro and MAG. i have gotten pitch(top row of pic) and roll(second row) quite stable with a kalman filter(and will later do PID), but i cant seem to get Yaw calculations to work-- the output(last row of pic) is completely wrong. the yaw code is based off of this: imu - Magnetometer dynamic calibration - Electrical Engineering Stack Exchange and http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267295038

code. MAGx is the raw value from the mag, same for MAGy etc

void YawCalc() // calculate yaw from mag
{
  //YAW!
  //this part is required to normalize the magnetic vector
  //get Min and Max Reading for MAGic Axis
  if (MAGx>xMAGMax) {
    xMAGMax = MAGx;
  }
  if (MAGy>yMAGMax) {
    yMAGMax = MAGy;
  }
  if (MAGz>zMAGMax) {
    zMAGMax = MAGz;
  }

  if (MAGx<xMAGMin) {
    xMAGMin = MAGx;
  }
  if (MAGy<yMAGMin) {
    yMAGMin = MAGy;
  }
  if (MAGz<zMAGMin) {
    zMAGMin = MAGz;
  }

  //Map the incoming Data from -1 to 1
  xMAGMap = float(map(MAGx, xMAGMin, xMAGMax, -30000, 30000))/30000.0;
  yMAGMap = float(map(MAGy, yMAGMin, yMAGMax, -30000, 30000))/30000.0;
  zMAGMap = float(map(MAGz, zMAGMin, zMAGMax, -30000, 30000))/30000.0;

  //normalize the magnetic vector
  float norm = sqrt( sq(xMAGMap) + sq(yMAGMap) + sq(zMAGMap));
  xMAGMap /=norm;
  yMAGMap /=norm;
  zMAGMap /=norm;

  yawRaw=atan2( (-yMAGMap*cos(rollPrediction) + zMAGMap*sin(rollPrediction) ) , xMAGMap*cos(pitchPrediction) + zMAGMap*sin(pitchPrediction)*sin(rollPrediction)+ zMAGMap*sin(pitchPrediction)*cos(rollPrediction)) *180/PI;
  //YawU=(atan2(-yMAGMap, xMAGMap) *180)/PI;
}

thanks.

yaw fail.PNG
yaw fail.PNG807×629 56.7 KB

2

ok, i made soem progress. :slight_smile: the output now looks a little less noisy after duplicating what is in the AHRS sketch for the IMU, but there is a part that i cant quite do.
this is the code from the AHRS(https://github.com/pololu/MinIMU-9-Arduino-AHRS/blob/master/MinIMU9AHRS/Compass.pde):

float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  
  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch);
  
  // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
  c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5;
  c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5;
  c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5;
  
  // Tilt compensated Magnetic filed X:
  MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
  // Tilt compensated Magnetic filed Y:
  MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
  // Magnetic Heading
  MAG_Heading = atan2(-MAG_Y,MAG_X);

i successfully have mag_heading calculated in my sketch, but as i examined the AHRS further, i realized MAG_HEADING wasn't the final yaw value; only the mag heading.

i then found in the AHRS code(https://github.com/pololu/MinIMU-9-Arduino-AHRS/blob/master/MinIMU9AHRS/DCM.pde):

 mag_heading_x = cos(MAG_Heading);
  mag_heading_y = sin(MAG_Heading);
  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error

it takes MAG_HEADING and uses cos and sin to get MAG_HEADING x and y and then uses some values(gyro or acc yaw?) from the DCM matrix to remove error in the yaw calculation.

this is where im stuck. what sensor values are DCM_Matrix[0][0] and DCM_Matrix[1][0]? how do i calculate yaw error without this fancy DCM matrix?

it then continues on to add the new yaw value to the DCM matrix:

Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
  
  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
  
  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I