watchrobot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcSF.c

  t2 = SF(1.0);
  t3 = SF(2.0);
  t4 = SF(3.0);
  t5 = q3*(1.0/2.0);
  t6 = q0*t2*(1.0/2.0);
  t7 = q2*t4*(1.0/2.0);
  t8 = q0*t3*(1.0/2.0);
  t9 = q1*t2*(1.0/2.0);
  t10 = q3*t4*(1.0/2.0);
  t11 = q1*(1.0/2.0);
  t12 = q2*t2*(1.0/2.0);
  t13 = q3*t3*(1.0/2.0);
  t14 = q0*(1.0/2.0);
  t15 = q1*t4*(1.0/2.0);
  t16 = q2*t3*(1.0/2.0);
  t17 = q3*t2*(1.0/2.0);
  t18 = q0*t4*(1.0/2.0);
  t19 = q2*(1.0/2.0);
  t20 = q1*t3*(1.0/2.0);
  t21 = q0*q0;
  t22 = q1*q1;
  t23 = q2*q2;
  t24 = q3*q3;
  t25 = q0*q1*2.0;
  t26 = q0*q3*2.0;
  t27 = q0*q2*2.0;
  A0[0][0] = daz*(1.0/2.0)-daz_b*(1.0/2.0);
  A0[1][0] = day*(1.0/2.0)-day_b*(1.0/2.0);
  A0[2][0] = dax*(1.0/2.0)-dax_b*(1.0/2.0);
  A0[3][0] = q2*(-1.0/2.0)+t8+t9+t10;
  A0[4][0] = t5+t6+t7-q1*t3*(1.0/2.0);
  A0[5][0] = t14+t15+t16-q3*t2*(1.0/2.0);
  A0[6][0] = t11+t12+t13-q0*t4*(1.0/2.0);
  A0[7][0] = t5-t6+t7+t20;
  A0[8][0] = t8+t9-t10+t19;
  A0[9][0] = -t11+t12+t13+t18;
  A0[10][0] = t14+t15-t16+t17;
  A0[11][0] = t14-t15+t16+t17;
  A0[12][0] = t11-t12+t13+t18;
  A0[13][0] = -t8+t9+t10+t19;
  A0[14][0] = -t5+t6+t7+t20;
  A0[15][0] = -t21-t22-t23-t24;
  A0[16][0] = t21-t22-t23+t24;
  A0[17][0] = t21-t22+t23-t24;
  A0[18][0] = t21+t22-t23-t24;
  A0[19][0] = t27-q1*q3*2.0;
  A0[20][0] = t25-q2*q3*2.0;
  A0[21][0] = t26-q1*q2*2.0;
  A0[22][0] = t25+q2*q3*2.0;
  A0[23][0] = t26+q1*q2*2.0;
  A0[24][0] = t27+q1*q3*2.0;