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

  t2 = SF(1.5E1);
  t3 = SF(1.4E1);
  t4 = SF(1.3E1);
  t5 = SF(1.2E1);
  t6 = SF(1.1E1);
  t7 = SF(1.0E1);
  t8 = SF(9.0);
  t9 = SF(8.0);
  t10 = SF(7.0);
  t11 = SF(6.0);
  t12 = SF(5.0);
  t13 = SF(4.0);
  t14 = SF(1.7E1);
  t15 = SF(2.3E1);
  t16 = SF(2.0E1);
  t17 = SF(1.8E1);
  t18 = SF(2.1E1);
  t19 = SF(2.4E1);
  t20 = SF(2.5E1);
  t21 = SF(2.2E1);
  t22 = SF(1.9E1);
  A0[0][0] = q0*t3*-2.0+q1*t2*2.0+q2*t5*2.0+q3*t4*2.0;
  A0[1][0] = q0*t7*2.0+q1*t6*2.0+q2*t9*2.0-q3*t8*2.0;
  A0[2][0] = q0*t2*2.0+q1*t3*2.0-q2*t4*2.0+q3*t5*2.0;
  A0[3][0] = q0*t6*2.0-q1*t7*2.0+q2*t8*2.0+q3*t9*2.0;
  A0[4][0] = q0*t5*2.0+q1*t4*2.0+q2*t3*2.0-q3*t2*2.0;
  A0[5][0] = q0*t9*-2.0+q1*t8*2.0+q2*t7*2.0+q3*t6*2.0;
  A0[6][0] = q0*t11*2.0+q1*t10*2.0-q2*t13*2.0+q3*t12*2.0;
  A0[7][0] = q0*t10*-2.0+q1*t11*2.0+q2*t12*2.0+q3*t13*2.0;
  A0[8][0] = q0*t13*2.0-q1*t12*2.0+q2*t11*2.0+q3*t10*2.0;
  A0[9][0] = dvy*t14-dvz*t15;
  A0[10][0] = dvx*t17-dvy*t19;
  A0[11][0] = dvx*t20-dvz*t22;
  A0[12][0] = dvx*t14+dvz*t16;
  A0[13][0] = dvx*t15+dvy*t16;
  A0[14][0] = dvy*t18+dvz*t17;
  A0[15][0] = dvx*t18+dvz*t19;
  A0[16][0] = dvy*t20+dvz*t21;
  A0[17][0] = dvx*t21+dvy*t22;
  A0[18][0] = SF(1.6E1);