path: root/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c

/*
 * AttitudeEKF.c
 *
 * Code generation for function 'AttitudeEKF'
 *
 * C source code generated on: Thu Aug 21 11:17:28 2014
 *
 */

/* Include files */
#include "AttitudeEKF.h"

/* Variable Definitions */
static float Ji[9];
static boolean_T Ji_not_empty;
static float x_apo[12];
static float P_apo[144];
static float Q[144];
static boolean_T Q_not_empty;

/* Function Declarations */
static void AttitudeEKF_init(void);
static void b_mrdivide(const float A[72], const float B[36], float y[72]);
static void inv(const float x[9], float y[9]);
static void mrdivide(const float A[108], const float B[81], float y[108]);
static float norm(const float x[3]);

/* Function Definitions */
static void AttitudeEKF_init(void)
{
  int i;
  static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
    -9.81F, 1.0F, 0.0F, 0.0F };

  for (i = 0; i < 12; i++) {
    x_apo[i] = fv5[i];
  }

  for (i = 0; i < 144; i++) {
    P_apo[i] = 200.0F;
  }
}

/*
 *
 */
static void b_mrdivide(const float A[72], const float B[36], float y[72])
{
  float b_A[36];
  signed char ipiv[6];
  int i1;
  int iy;
  int j;
  int c;
  int ix;
  float temp;
  int k;
  float s;
  int jy;
  int ijA;
  float Y[72];
  for (i1 = 0; i1 < 6; i1++) {
    for (iy = 0; iy < 6; iy++) {
      b_A[iy + 6 * i1] = B[i1 + 6 * iy];
    }

    ipiv[i1] = (signed char)(1 + i1);
  }

  for (j = 0; j < 5; j++) {
    c = j * 7;
    iy = 0;
    ix = c;
    temp = (real32_T)fabs(b_A[c]);
    for (k = 2; k <= 6 - j; k++) {
      ix++;
      s = (real32_T)fabs(b_A[ix]);
      if (s > temp) {
        iy = k - 1;
        temp = s;
      }
    }

    if (b_A[c + iy] != 0.0F) {
      if (iy != 0) {
        ipiv[j] = (signed char)((j + iy) + 1);
        ix = j;
        iy += j;
        for (k = 0; k < 6; k++) {
          temp = b_A[ix];
          b_A[ix] = b_A[iy];
          b_A[iy] = temp;
          ix += 6;
          iy += 6;
        }
      }

      i1 = (c - j) + 6;
      for (jy = c + 1; jy + 1 <= i1; jy++) {
        b_A[jy] /= b_A[c];
      }
    }

    iy = c;
    jy = c + 6;
    for (k = 1; k <= 5 - j; k++) {
      temp = b_A[jy];
      if (b_A[jy] != 0.0F) {
        ix = c + 1;
        i1 = (iy - j) + 12;
        for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) {
          b_A[ijA] += b_A[ix] * -temp;
          ix++;
        }
      }

      jy += 6;
      iy += 6;
    }
  }

  for (i1 = 0; i1 < 12; i1++) {
    for (iy = 0; iy < 6; iy++) {
      Y[iy + 6 * i1] = A[i1 + 12 * iy];
    }
  }

  for (jy = 0; jy < 6; jy++) {
    if (ipiv[jy] != jy + 1) {
      for (j = 0; j < 12; j++) {
        temp = Y[jy + 6 * j];
        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
        Y[(ipiv[jy] + 6 * j) - 1] = temp;
      }
    }
  }

  for (j = 0; j < 12; j++) {
    c = 6 * j;
    for (k = 0; k < 6; k++) {
      iy = 6 * k;
      if (Y[k + c] != 0.0F) {
        for (jy = k + 2; jy < 7; jy++) {
          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
        }
      }
    }
  }

  for (j = 0; j < 12; j++) {
    c = 6 * j;
    for (k = 5; k > -1; k += -1) {
      iy = 6 * k;
      if (Y[k + c] != 0.0F) {
        Y[k + c] /= b_A[k + iy];
        for (jy = 0; jy + 1 <= k; jy++) {
          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
        }
      }
    }
  }

  for (i1 = 0; i1 < 6; i1++) {
    for (iy = 0; iy < 12; iy++) {
      y[iy + 12 * i1] = Y[i1 + 6 * iy];
    }
  }
}

/*
 *
 */
static void inv(const float x[9], float y[9])
{
  float b_x[9];
  int p1;
  int p2;
  int p3;
  float absx11;
  float absx21;
  float absx31;
  int itmp;
  for (p1 = 0; p1 < 9; p1++) {
    b_x[p1] = x[p1];
  }

  p1 = 0;
  p2 = 3;
  p3 = 6;
  absx11 = (real32_T)fabs(x[0]);
  absx21 = (real32_T)fabs(x[1]);
  absx31 = (real32_T)fabs(x[2]);
  if ((absx21 > absx11) && (absx21 > absx31)) {
    p1 = 3;
    p2 = 0;
    b_x[0] = x[1];
    b_x[1] = x[0];
    b_x[3] = x[4];
    b_x[4] = x[3];
    b_x[6] = x[7];
    b_x[7] = x[6];
  } else {
    if (absx31 > absx11) {
      p1 = 6;
      p3 = 0;
      b_x[0] = x[2];
      b_x[2] = x[0];
      b_x[3] = x[5];
      b_x[5] = x[3];
      b_x[6] = x[8];
      b_x[8] = x[6];
    }
  }

  absx11 = b_x[1] / b_x[0];
  b_x[1] /= b_x[0];
  absx21 = b_x[2] / b_x[0];
  b_x[2] /= b_x[0];
  b_x[4] -= absx11 * b_x[3];
  b_x[5] -= absx21 * b_x[3];
  b_x[7] -= absx11 * b_x[6];
  b_x[8] -= absx21 * b_x[6];
  if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) {
    itmp = p2;
    p2 = p3;
    p3 = itmp;
    b_x[1] = absx21;
    b_x[2] = absx11;
    absx11 = b_x[4];
    b_x[4] = b_x[5];
    b_x[5] = absx11;
    absx11 = b_x[7];
    b_x[7] = b_x[8];
    b_x[8] = absx11;
  }

  absx11 = b_x[5] / b_x[4];
  b_x[5] /= b_x[4];
  b_x[8] -= absx11 * b_x[7];
  absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8];
  absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4];
  y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0];
  y[p1 + 1] = absx21;
  y[p1 + 2] = absx11;
  absx11 = -b_x[5] / b_x[8];
  absx21 = (1.0F - b_x[7] * absx11) / b_x[4];
  y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
  y[p2 + 1] = absx21;
  y[p2 + 2] = absx11;
  absx11 = 1.0F / b_x[8];
  absx21 = -b_x[7] * absx11 / b_x[4];
  y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
  y[p3 + 1] = absx21;
  y[p3 + 2] = absx11;
}

/*
 *
 */
static void mrdivide(const float A[108], const float B[81], float y[108])
{
  float b_A[81];
  signed char ipiv[9];
  int i0;
  int iy;
  int j;
  int c;
  int ix;
  float temp;
  int k;
  float s;
  int jy;
  int ijA;
  float Y[108];
  for (i0 = 0; i0 < 9; i0++) {
    for (iy = 0; iy < 9; iy++) {
      b_A[iy + 9 * i0] = B[i0 + 9 * iy];
    }

    ipiv[i0] = (signed char)(1 + i0);
  }

  for (j = 0; j < 8; j++) {
    c = j * 10;
    iy = 0;
    ix = c;
    temp = (real32_T)fabs(b_A[c]);
    for (k = 2; k <= 9 - j; k++) {
      ix++;
      s = (real32_T)fabs(b_A[ix]);
      if (s > temp) {
        iy = k - 1;
        temp = s;
      }
    }

    if (b_A[c + iy] != 0.0F) {
      if (iy != 0) {
        ipiv[j] = (signed char)((j + iy) + 1);
        ix = j;
        iy += j;
        for (k = 0; k < 9; k++) {
          temp = b_A[ix];
          b_A[ix] = b_A[iy];
          b_A[iy] = temp;
          ix += 9;
          iy += 9;
        }
      }

      i0 = (c - j) + 9;
      for (jy = c + 1; jy + 1 <= i0; jy++) {
        b_A[jy] /= b_A[c];
      }
    }

    iy = c;
    jy = c + 9;
    for (k = 1; k <= 8 - j; k++) {
      temp = b_A[jy];
      if (b_A[jy] != 0.0F) {
        ix = c + 1;
        i0 = (iy - j) + 18;
        for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) {
          b_A[ijA] += b_A[ix] * -temp;
          ix++;
        }
      }

      jy += 9;
      iy += 9;
    }
  }

  for (i0 = 0; i0 < 12; i0++) {
    for (iy = 0; iy < 9; iy++) {
      Y[iy + 9 * i0] = A[i0 + 12 * iy];
    }
  }

  for (jy = 0; jy < 9; jy++) {
    if (ipiv[jy] != jy + 1) {
      for (j = 0; j < 12; j++) {
        temp = Y[jy + 9 * j];
        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
        Y[(ipiv[jy] + 9 * j) - 1] = temp;
      }
    }
  }

  for (j = 0; j < 12; j++) {
    c = 9 * j;
    for (k = 0; k < 9; k++) {
      iy = 9 * k;
      if (Y[k + c] != 0.0F) {
        for (jy = k + 2; jy < 10; jy++) {
          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
        }
      }
    }
  }

  for (j = 0; j < 12; j++) {
    c = 9 * j;
    for (k = 8; k > -1; k += -1) {
      iy = 9 * k;
      if (Y[k + c] != 0.0F) {
        Y[k + c] /= b_A[k + iy];
        for (jy = 0; jy + 1 <= k; jy++) {
          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
        }
      }
    }
  }

  for (i0 = 0; i0 < 9; i0++) {
    for (iy = 0; iy < 12; iy++) {
      y[iy + 12 * i0] = Y[i0 + 9 * iy];
    }
  }
}

/*
 *
 */
static float norm(const float x[3])
{
  float y;
  float scale;
  int k;
  float absxk;
  float t;
  y = 0.0F;
  scale = 1.17549435E-38F;
  for (k = 0; k < 3; k++) {
    absxk = (real32_T)fabs(x[k]);
    if (absxk > scale) {
      t = scale / absxk;
      y = 1.0F + y * t * t;
      scale = absxk;
    } else {
      t = absxk / scale;
      y += t * t;
    }
  }

  return scale * (real32_T)sqrt(y);
}

/*
 * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
 *     = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
 */
void AttitudeEKF(unsigned char approx_prediction, unsigned char
                 use_inertia_matrix, const unsigned char zFlag[3], float dt,
                 const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc,
                 float q_mag, float r_gyro, float r_accel, float r_mag, const
                 float J[9], float xa_apo[12], float Pa_apo[144], float
                 Rot_matrix[9], float eulerAngles[3], float debugOutput[4])
{
  int i;
  float fv0[3];
  int r2;
  float zek[3];
  float muk[3];
  float b_muk[3];
  float c_muk[3];
  float fv1[3];
  float wak[3];
  float O[9];
  float b_O[9];
  static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };

  float fv2[3];
  float maxval;
  int r1;
  float fv3[9];
  float fv4[3];
  float x_apr[12];
  signed char I[144];
  static float A_lin[144];
  static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1,
    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

  static float b_A_lin[144];
  float v[12];
  static float P_apr[144];
  float a[108];
  static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

  float S_k[81];
  static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

  float b_r_gyro[9];
  float K_k[108];
  float b_S_k[36];
  static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

  static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

  float c_r_gyro[3];
  float B[36];
  int r3;
  float a21;
  float Y[36];
  float d_a[72];
  static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0 };

  static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 1, 0, 0, 0 };

  float d_r_gyro[6];
  float c_S_k[6];
  float b_K_k[72];
  static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
    0, 0, 0, 0, 0, 1 };

  static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 1 };

  float b_z[6];

  /* LQG Postion Estimator and Controller */
  /*  Observer: */
  /*         x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */
  /*         x[n+1|n] = Ax[n|n] + Bu[n] */
  /*  */
  /*  $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $ */
  /*  */
  /*  */
  /*  Arguments: */
  /*  approx_prediction: if 1 then the exponential map is approximated with a */
  /*  first order taylor approximation. has at the moment not a big influence */
  /*  (just 1st or 2nd order approximation) we should change it to rodriquez */
  /*  approximation. */
  /*  use_inertia_matrix: set to true if you have the inertia matrix J for your */
  /*  quadrotor */
  /*  xa_apo_k: old state vectotr */
  /*  zFlag: if sensor measurement is available [gyro, acc, mag] */
  /*  dt: dt in s */
  /*  z: measurements [gyro, acc, mag] */
  /*  q_rotSpeed: process noise gyro */
  /*  q_rotAcc: process noise gyro acceleration */
  /*  q_acc: process noise acceleration */
  /*  q_mag: process noise magnetometer */
  /*  r_gyro: measurement noise gyro */
  /*  r_accel: measurement noise accel */
  /*  r_mag: measurement noise mag */
  /*  J: moment of inertia matrix */
  /*  Output: */
  /*  xa_apo: updated state vectotr */
  /*  Pa_apo: updated state covariance matrix */
  /*  Rot_matrix: rotation matrix */
  /*  eulerAngles: euler angles */
  /*  debugOutput: not used */
  /* % model specific parameters */
  /*  compute once the inverse of the Inertia */
  /* 'AttitudeEKF:48' if isempty(Ji) */
  if (!Ji_not_empty) {
    /* 'AttitudeEKF:49' Ji=single(inv(J)); */
    inv(J, Ji);
    Ji_not_empty = TRUE;
  }

  /* % init */
  /* 'AttitudeEKF:54' if(isempty(x_apo)) */
  /* 'AttitudeEKF:64' if(isempty(P_apo)) */
  /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */
  for (i = 0; i < 4; i++) {
    debugOutput[i] = 0.0F;
  }

  /* % copy the states */
  /* 'AttitudeEKF:72' wx=  x_apo(1); */
  /*  x  body angular rate */
  /* 'AttitudeEKF:73' wy=  x_apo(2); */
  /*  y  body angular rate */
  /* 'AttitudeEKF:74' wz=  x_apo(3); */
  /*  z  body angular rate */
  /* 'AttitudeEKF:76' wax=  x_apo(4); */
  /*  x  body angular acceleration */
  /* 'AttitudeEKF:77' way=  x_apo(5); */
  /*  y  body angular acceleration */
  /* 'AttitudeEKF:78' waz=  x_apo(6); */
  /*  z  body angular acceleration */
  /* 'AttitudeEKF:80' zex=  x_apo(7); */
  /*  x  component gravity vector */
  /* 'AttitudeEKF:81' zey=  x_apo(8); */
  /*  y  component gravity vector */
  /* 'AttitudeEKF:82' zez=  x_apo(9); */
  /*  z  component gravity vector */
  /* 'AttitudeEKF:84' mux=  x_apo(10); */
  /*  x  component magnetic field vector */
  /* 'AttitudeEKF:85' muy=  x_apo(11); */
  /*  y  component magnetic field vector */
  /* 'AttitudeEKF:86' muz=  x_apo(12); */
  /*  z  component magnetic field vector */
  /* % prediction section */
  /*  compute the apriori state estimate from the previous aposteriori estimate */
  /* body angular accelerations */
  /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */
  if (use_inertia_matrix == 1) {
    /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */
    fv0[0] = x_apo[3];
    fv0[1] = x_apo[4];
    fv0[2] = x_apo[5];
    for (r2 = 0; r2 < 3; r2++) {
      zek[r2] = 0.0F;
      for (i = 0; i < 3; i++) {
        zek[r2] += J[r2 + 3 * i] * fv0[i];
      }
    }

    muk[0] = x_apo[3];
    muk[1] = x_apo[4];
    muk[2] = x_apo[5];
    b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1];
    b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2];
    b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0];
    for (r2 = 0; r2 < 3; r2++) {
      c_muk[r2] = -b_muk[r2];
    }

    fv1[0] = x_apo[3];
    fv1[1] = x_apo[4];
    fv1[2] = x_apo[5];
    for (r2 = 0; r2 < 3; r2++) {
      fv0[r2] = 0.0F;
      for (i = 0; i < 3; i++) {
        fv0[r2] += Ji[r2 + 3 * i] * c_muk[i];
      }

      wak[r2] = fv1[r2] + fv0[r2] * dt;
    }
  } else {
    /* 'AttitudeEKF:96' else */
    /* 'AttitudeEKF:97' wak =[wax;way;waz]; */
    wak[0] = x_apo[3];
    wak[1] = x_apo[4];
    wak[2] = x_apo[5];
  }

  /* body angular rates */
  /* 'AttitudeEKF:101' wk =[wx;  wy; wz] + dt*wak; */
  /* derivative of the prediction rotation matrix */
  /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
  O[0] = 0.0F;
  O[1] = -x_apo[2];
  O[2] = x_apo[1];
  O[3] = x_apo[2];
  O[4] = 0.0F;
  O[5] = -x_apo[0];
  O[6] = -x_apo[1];
  O[7] = x_apo[0];
  O[8] = 0.0F;

  /* prediction of the earth z vector */
  /* 'AttitudeEKF:107' if (approx_prediction==1) */
  if (approx_prediction == 1) {
    /* e^(Odt)=I+dt*O+dt^2/2!O^2 */
    /*  so we do a first order approximation of the exponential map */
    /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */
    for (r2 = 0; r2 < 3; r2++) {
      for (i = 0; i < 3; i++) {
        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2];
      }
    }

    fv2[0] = x_apo[6];
    fv2[1] = x_apo[7];
    fv2[2] = x_apo[8];
    for (r2 = 0; r2 < 3; r2++) {
      zek[r2] = 0.0F;
      for (i = 0; i < 3; i++) {
        zek[r2] += b_O[r2 + 3 * i] * fv2[i];
      }
    }
  } else {
    /* 'AttitudeEKF:112' else */
    /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */
    maxval = dt * dt / 2.0F;
    for (r2 = 0; r2 < 3; r2++) {
      for (i = 0; i < 3; i++) {
        b_O[r2 + 3 * i] = 0.0F;
        for (r1 = 0; r1 < 3; r1++) {
          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
        }
      }
    }

    for (r2 = 0; r2 < 3; r2++) {
      for (i = 0; i < 3; i++) {
        fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
          * b_O[i + 3 * r2];
      }
    }

    fv2[0] = x_apo[6];
    fv2[1] = x_apo[7];
    fv2[2] = x_apo[8];
    for (r2 = 0; r2 < 3; r2++) {
      zek[r2] = 0.0F;
      for (i = 0; i < 3; i++) {
        zek[r2] += fv3[r2 + 3 * i] * fv2[i];
      }
    }

    /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */
    /* precision */
  }

  /* prediction of the magnetic vector */
  /* 'AttitudeEKF:121' if (approx_prediction==1) */
  if (approx_prediction == 1) {
    /* e^(Odt)=I+dt*O+dt^2/2!O^2 */
    /*  so we do a first order approximation of the exponential map */
    /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */
    for (r2 = 0; r2 < 3; r2++) {
      for (i = 0; i < 3; i++) {
        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2];
      }
    }

    fv4[0] = x_apo[9];
    fv4[1] = x_apo[10];
    fv4[2] = x_apo[11];
    for (r2 = 0; r2 < 3; r2++) {
      muk[r2] = 0.0F;
      for (i = 0; i < 3; i++) {
        muk[r2] += b_O[r2 + 3 * i] * fv4[i];
      }
    }
  } else {
    /* 'AttitudeEKF:125' else */
    /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */
    maxval = dt * dt / 2.0F;
    for (r2 = 0; r2 < 3; r2++) {
      for (i = 0; i < 3; i++) {
        b_O[r2 + 3 * i] = 0.0F;
        for (r1 = 0; r1 < 3; r1++) {
          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
        }
      }
    }

    for (r2 = 0; r2 < 3; r2++) {
      for (i = 0; i < 3; i++) {
        fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
          * b_O[i + 3 * r2];
      }
    }

    fv4[0] = x_apo[9];
    fv4[1] = x_apo[10];
    fv4[2] = x_apo[11];
    for (r2 = 0; r2 < 3; r2++) {
      muk[r2] = 0.0F;
      for (i = 0; i < 3; i++) {
        muk[r2] += fv3[r2 + 3 * i] * fv4[i];
      }
    }

    /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */
    /* precision */
  }

  /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */
  x_apr[0] = x_apo[0] + dt * wak[0];
  x_apr[1] = x_apo[1] + dt * wak[1];
  x_apr[2] = x_apo[2] + dt * wak[2];
  for (i = 0; i < 3; i++) {
    x_apr[i + 3] = wak[i];
  }

  for (i = 0; i < 3; i++) {
    x_apr[i + 6] = zek[i];
  }

  for (i = 0; i < 3; i++) {
    x_apr[i + 9] = muk[i];
  }

  /*  compute the apriori error covariance estimate from the previous */
  /* aposteriori estimate */
  /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */
  /* 'AttitudeEKF:137'     -zez,0,zex; */
  /* 'AttitudeEKF:138'     zey,-zex,0]'; */
  /* 'AttitudeEKF:139' MA=[0,muz,-muy; */
  /* 'AttitudeEKF:140'     -muz,0,mux; */
  /* 'AttitudeEKF:141'     muy,-mux,0]'; */
  /* 'AttitudeEKF:143' E=single(eye(3)); */
  /* 'AttitudeEKF:144' Z=single(zeros(3)); */
  /* 'AttitudeEKF:146' A_lin=[ Z,  E,  Z,  Z */
  /* 'AttitudeEKF:147'     Z,  Z,  Z,  Z */
  /* 'AttitudeEKF:148'     EZ, Z,  O,  Z */
  /* 'AttitudeEKF:149'     MA, Z,  Z,  O]; */
  /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */
  memset(&I[0], 0, 144U * sizeof(signed char));
  for (i = 0; i < 12; i++) {
    I[i + 12 * i] = 1;
    for (r2 = 0; r2 < 3; r2++) {
      A_lin[r2 + 12 * i] = iv1[r2 + 3 * i];
    }

    for (r2 = 0; r2 < 3; r2++) {
      A_lin[(r2 + 12 * i) + 3] = 0.0F;
    }
  }

  A_lin[6] = 0.0F;
  A_lin[7] = x_apo[8];
  A_lin[8] = -x_apo[7];
  A_lin[18] = -x_apo[8];
  A_lin[19] = 0.0F;
  A_lin[20] = x_apo[6];
  A_lin[30] = x_apo[7];
  A_lin[31] = -x_apo[6];
  A_lin[32] = 0.0F;
  for (r2 = 0; r2 < 3; r2++) {
    for (i = 0; i < 3; i++) {
      A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F;
    }
  }

  for (r2 = 0; r2 < 3; r2++) {
    for (i = 0; i < 3; i++) {
      A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2];
    }
  }

  for (r2 = 0; r2 < 3; r2++) {
    for (i = 0; i < 3; i++) {
      A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F;
    }
  }

  A_lin[9] = 0.0F;
  A_lin[10] = x_apo[11];
  A_lin[11] = -x_apo[10];
  A_lin[21] = -x_apo[11];
  A_lin[22] = 0.0F;
  A_lin[23] = x_apo[9];
  A_lin[33] = x_apo[10];
  A_lin[34] = -x_apo[9];
  A_lin[35] = 0.0F;
  for (r2 = 0; r2 < 3; r2++) {
    for (i = 0; i < 3; i++) {
      A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F;
    }
  }

  for (r2 = 0; r2 < 3; r2++) {
    for (i = 0; i < 3; i++) {
      A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F;
    }
  }

  for (r2 = 0; r2 < 3; r2++) {
    for (i = 0; i < 3; i++) {
      A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2];
    }
  }

  for (r2 = 0; r2 < 12; r2++) {
    for (i = 0; i < 12; i++) {
      b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt;
    }
  }

  /* process covariance matrix */
  /* 'AttitudeEKF:156' if (isempty(Q)) */
  if (!Q_not_empty) {
    /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */
    /* 'AttitudeEKF:158'         q_rotAcc,q_rotAcc,q_rotAcc,... */
    /* 'AttitudeEKF:159'         q_acc,q_acc,q_acc,... */
    /* 'AttitudeEKF:160'         q_mag,q_mag,q_mag]); */
    v[0] = q_rotSpeed;
    v[1] = q_rotSpeed;
    v[2] = q_rotSpeed;
    v[3] = q_rotAcc;
    v[4] = q_rotAcc;
    v[5] = q_rotAcc;
    v[6] = q_acc;
    v[7] = q_acc;
    v[8] = q_acc;
    v[9] = q_mag;
    v[10] = q_mag;
    v[11] = q_mag;
    memset(&Q[0], 0, 144U * sizeof(float));
    for (i = 0; i < 12; i++) {
      Q[i + 12 * i] = v[i];
    }

    Q_not_empty = TRUE;
  }

  /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */
  for (r2 = 0; r2 < 12; r2++) {
    for (i = 0; i < 12; i++) {
      A_lin[r2 + 12 * i] = 0.0F;
      for (r1 = 0; r1 < 12; r1++) {
        A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i];
      }
    }
  }

  for (r2 = 0; r2 < 12; r2++) {
    for (i = 0; i < 12; i++) {
      maxval = 0.0F;
      for (r1 = 0; r1 < 12; r1++) {
        maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1];
      }

      P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i];
    }
  }

  /* % update */
  /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */
  if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) {
    /*      R=[r_gyro,0,0,0,0,0,0,0,0; */
    /*          0,r_gyro,0,0,0,0,0,0,0; */
    /*          0,0,r_gyro,0,0,0,0,0,0; */
    /*          0,0,0,r_accel,0,0,0,0,0; */
    /*          0,0,0,0,r_accel,0,0,0,0; */
    /*          0,0,0,0,0,r_accel,0,0,0; */
    /*          0,0,0,0,0,0,r_mag,0,0; */
    /*          0,0,0,0,0,0,0,r_mag,0; */
    /*          0,0,0,0,0,0,0,0,r_mag]; */
    /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */
    /* observation matrix */
    /* [zw;ze;zmk]; */
    /* 'AttitudeEKF:181' H_k=[  E,     Z,      Z,    Z; */
    /* 'AttitudeEKF:182'         Z,     Z,      E,    Z; */
    /* 'AttitudeEKF:183'         Z,     Z,      Z,    E]; */
    /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */
    /* S_k=H_k*P_apr*H_k'+R; */
    /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */
    for (r2 = 0; r2 < 9; r2++) {
      for (i = 0; i < 12; i++) {
        a[r2 + 9 * i] = 0.0F;
        for (r1 = 0; r1 < 12; r1++) {
          a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i];
        }
      }

      for (i = 0; i < 9; i++) {
        S_k[r2 + 9 * i] = 0.0F;
        for (r1 = 0; r1 < 12; r1++) {
          S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i];
        }
      }
    }

    /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */
    b_r_gyro[0] = r_gyro;
    b_r_gyro[1] = r_gyro;
    b_r_gyro[2] = r_gyro;
    b_r_gyro[3] = r_accel;
    b_r_gyro[4] = r_accel;
    b_r_gyro[5] = r_accel;
    b_r_gyro[6] = r_mag;
    b_r_gyro[7] = r_mag;
    b_r_gyro[8] = r_mag;
    for (r2 = 0; r2 < 9; r2++) {
      b_O[r2] = S_k[10 * r2] + b_r_gyro[r2];
    }

    for (r2 = 0; r2 < 9; r2++) {
      S_k[10 * r2] = b_O[r2];
    }

    /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */
    for (r2 = 0; r2 < 12; r2++) {
      for (i = 0; i < 9; i++) {
        a[r2 + 12 * i] = 0.0F;
        for (r1 = 0; r1 < 12; r1++) {
          a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i];
        }
      }
    }

    mrdivide(a, S_k, K_k);

    /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */
    for (r2 = 0; r2 < 9; r2++) {
      maxval = 0.0F;
      for (i = 0; i < 12; i++) {
        maxval += (float)b_a[r2 + 9 * i] * x_apr[i];
      }

      b_O[r2] = z[r2] - maxval;
    }

    for (r2 = 0; r2 < 12; r2++) {
      maxval = 0.0F;
      for (i = 0; i < 9; i++) {
        maxval += K_k[r2 + 12 * i] * b_O[i];
      }

      x_apo[r2] = x_apr[r2] + maxval;
    }

    /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */
    memset(&I[0], 0, 144U * sizeof(signed char));
    for (i = 0; i < 12; i++) {
      I[i + 12 * i] = 1;
    }

    for (r2 = 0; r2 < 12; r2++) {
      for (i = 0; i < 12; i++) {
        maxval = 0.0F;
        for (r1 = 0; r1 < 9; r1++) {
          maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i];
        }

        A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
      }
    }

    for (r2 = 0; r2 < 12; r2++) {
      for (i = 0; i < 12; i++) {
        P_apo[r2 + 12 * i] = 0.0F;
        for (r1 = 0; r1 < 12; r1++) {
          P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
        }
      }
    }
  } else {
    /* 'AttitudeEKF:196' else */
    /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */
    if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) {
      /* 'AttitudeEKF:199' R=[r_gyro,0,0; */
      /* 'AttitudeEKF:200'             0,r_gyro,0; */
      /* 'AttitudeEKF:201'             0,0,r_gyro]; */
      /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */
      /* observation matrix */
      /* 'AttitudeEKF:205' H_k=[  E,     Z,      Z,    Z]; */
      /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */
      /*  S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */
      /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */
      for (r2 = 0; r2 < 3; r2++) {
        for (i = 0; i < 12; i++) {
          b_S_k[r2 + 3 * i] = 0.0F;
          for (r1 = 0; r1 < 12; r1++) {
            b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i];
          }
        }

        for (i = 0; i < 3; i++) {
          O[r2 + 3 * i] = 0.0F;
          for (r1 = 0; r1 < 12; r1++) {
            O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i];
          }
        }
      }

      /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */
      c_r_gyro[0] = r_gyro;
      c_r_gyro[1] = r_gyro;
      c_r_gyro[2] = r_gyro;
      for (r2 = 0; r2 < 3; r2++) {
        b_muk[r2] = O[r2 << 2] + c_r_gyro[r2];
      }

      for (r2 = 0; r2 < 3; r2++) {
        O[r2 << 2] = b_muk[r2];
      }

      /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */
      for (r2 = 0; r2 < 3; r2++) {
        for (i = 0; i < 3; i++) {
          b_O[i + 3 * r2] = O[r2 + 3 * i];
        }

        for (i = 0; i < 12; i++) {
          B[r2 + 3 * i] = 0.0F;
          for (r1 = 0; r1 < 12; r1++) {
            B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2];
          }
        }
      }

      r1 = 0;
      r2 = 1;
      r3 = 2;
      maxval = (real32_T)fabs(b_O[0]);
      a21 = (real32_T)fabs(b_O[1]);
      if (a21 > maxval) {
        maxval = a21;
        r1 = 1;
        r2 = 0;
      }

      if ((real32_T)fabs(b_O[2]) > maxval) {
        r1 = 2;
        r2 = 1;
        r3 = 0;
      }

      b_O[r2] /= b_O[r1];
      b_O[r3] /= b_O[r1];
      b_O[3 + r2] -= b_O[r2] * b_O[3 + r1];
      b_O[3 + r3] -= b_O[r3] * b_O[3 + r1];
      b_O[6 + r2] -= b_O[r2] * b_O[6 + r1];
      b_O[6 + r3] -= b_O[r3] * b_O[6 + r1];
      if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) {
        i = r2;
        r2 = r3;
        r3 = i;
      }

      b_O[3 + r3] /= b_O[3 + r2];
      b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2];
      for (i = 0; i < 12; i++) {
        Y[3 * i] = B[r1 + 3 * i];
        Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2];
        Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] *
          b_O[3 + r3];
        Y[2 + 3 * i] /= b_O[6 + r3];
        Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1];
        Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2];
        Y[1 + 3 * i] /= b_O[3 + r2];
        Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1];
        Y[3 * i] /= b_O[r1];
      }

      for (r2 = 0; r2 < 3; r2++) {
        for (i = 0; i < 12; i++) {
          b_S_k[i + 12 * r2] = Y[r2 + 3 * i];
        }
      }

      /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */
      for (r2 = 0; r2 < 3; r2++) {
        maxval = 0.0F;
        for (i = 0; i < 12; i++) {
          maxval += (float)c_a[r2 + 3 * i] * x_apr[i];
        }

        b_muk[r2] = z[r2] - maxval;
      }

      for (r2 = 0; r2 < 12; r2++) {
        maxval = 0.0F;
        for (i = 0; i < 3; i++) {
          maxval += b_S_k[r2 + 12 * i] * b_muk[i];
        }

        x_apo[r2] = x_apr[r2] + maxval;
      }

      /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */
      memset(&I[0], 0, 144U * sizeof(signed char));
      for (i = 0; i < 12; i++) {
        I[i + 12 * i] = 1;
      }

      for (r2 = 0; r2 < 12; r2++) {
        for (i = 0; i < 12; i++) {
          maxval = 0.0F;
          for (r1 = 0; r1 < 3; r1++) {
            maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i];
          }

          A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
        }
      }

      for (r2 = 0; r2 < 12; r2++) {
        for (i = 0; i < 12; i++) {
          P_apo[r2 + 12 * i] = 0.0F;
          for (r1 = 0; r1 < 12; r1++) {
            P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
          }
        }
      }
    } else {
      /* 'AttitudeEKF:217' else */
      /* 'AttitudeEKF:218' if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */
      if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) {
        /*              R=[r_gyro,0,0,0,0,0; */
        /*                  0,r_gyro,0,0,0,0; */
        /*                  0,0,r_gyro,0,0,0; */
        /*                  0,0,0,r_accel,0,0; */
        /*                  0,0,0,0,r_accel,0; */
        /*                  0,0,0,0,0,r_accel]; */
        /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */
        /* observation matrix */
        /* 'AttitudeEKF:230' H_k=[  E,     Z,      Z,    Z; */
        /* 'AttitudeEKF:231'                 Z,     Z,      E,    Z]; */
        /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */
        /*  S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */
        /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */
        for (r2 = 0; r2 < 6; r2++) {
          for (i = 0; i < 12; i++) {
            d_a[r2 + 6 * i] = 0.0F;
            for (r1 = 0; r1 < 12; r1++) {
              d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
            }
          }

          for (i = 0; i < 6; i++) {
            b_S_k[r2 + 6 * i] = 0.0F;
            for (r1 = 0; r1 < 12; r1++) {
              b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i];
            }
          }
        }

        /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */
        d_r_gyro[0] = r_gyro;
        d_r_gyro[1] = r_gyro;
        d_r_gyro[2] = r_gyro;
        d_r_gyro[3] = r_accel;
        d_r_gyro[4] = r_accel;
        d_r_gyro[5] = r_accel;
        for (r2 = 0; r2 < 6; r2++) {
          c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
        }

        for (r2 = 0; r2 < 6; r2++) {
          b_S_k[7 * r2] = c_S_k[r2];
        }

        /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */
        for (r2 = 0; r2 < 12; r2++) {
          for (i = 0; i < 6; i++) {
            d_a[r2 + 12 * i] = 0.0F;
            for (r1 = 0; r1 < 12; r1++) {
              d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i];
            }
          }
        }

        b_mrdivide(d_a, b_S_k, b_K_k);

        /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */
        for (r2 = 0; r2 < 6; r2++) {
          maxval = 0.0F;
          for (i = 0; i < 12; i++) {
            maxval += (float)e_a[r2 + 6 * i] * x_apr[i];
          }

          d_r_gyro[r2] = z[r2] - maxval;
        }

        for (r2 = 0; r2 < 12; r2++) {
          maxval = 0.0F;
          for (i = 0; i < 6; i++) {
            maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i];
          }

          x_apo[r2] = x_apr[r2] + maxval;
        }

        /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */
        memset(&I[0], 0, 144U * sizeof(signed char));
        for (i = 0; i < 12; i++) {
          I[i + 12 * i] = 1;
        }

        for (r2 = 0; r2 < 12; r2++) {
          for (i = 0; i < 12; i++) {
            maxval = 0.0F;
            for (r1 = 0; r1 < 6; r1++) {
              maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i];
            }

            A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
          }
        }

        for (r2 = 0; r2 < 12; r2++) {
          for (i = 0; i < 12; i++) {
            P_apo[r2 + 12 * i] = 0.0F;
            for (r1 = 0; r1 < 12; r1++) {
              P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
            }
          }
        }
      } else {
        /* 'AttitudeEKF:243' else */
        /* 'AttitudeEKF:244' if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */
        if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) {
          /*                  R=[r_gyro,0,0,0,0,0; */
          /*                      0,r_gyro,0,0,0,0; */
          /*                      0,0,r_gyro,0,0,0; */
          /*                      0,0,0,r_mag,0,0; */
          /*                      0,0,0,0,r_mag,0; */
          /*                      0,0,0,0,0,r_mag]; */
          /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */
          /* observation matrix */
          /* 'AttitudeEKF:254' H_k=[  E,     Z,      Z,    Z; */
          /* 'AttitudeEKF:255'                     Z,     Z,      Z,    E]; */
          /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */
          /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */
          /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */
          for (r2 = 0; r2 < 6; r2++) {
            for (i = 0; i < 12; i++) {
              d_a[r2 + 6 * i] = 0.0F;
              for (r1 = 0; r1 < 12; r1++) {
                d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
              }
            }

            for (i = 0; i < 6; i++) {
              b_S_k[r2 + 6 * i] = 0.0F;
              for (r1 = 0; r1 < 12; r1++) {
                b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i];
              }
            }
          }

          /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */
          d_r_gyro[0] = r_gyro;
          d_r_gyro[1] = r_gyro;
          d_r_gyro[2] = r_gyro;
          d_r_gyro[3] = r_mag;
          d_r_gyro[4] = r_mag;
          d_r_gyro[5] = r_mag;
          for (r2 = 0; r2 < 6; r2++) {
            c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
          }

          for (r2 = 0; r2 < 6; r2++) {
            b_S_k[7 * r2] = c_S_k[r2];
          }

          /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */
          for (r2 = 0; r2 < 12; r2++) {
            for (i = 0; i < 6; i++) {
              d_a[r2 + 12 * i] = 0.0F;
              for (r1 = 0; r1 < 12; r1++) {
                d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i];
              }
            }
          }

          b_mrdivide(d_a, b_S_k, b_K_k);

          /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */
          for (r2 = 0; r2 < 3; r2++) {
            d_r_gyro[r2] = z[r2];
          }

          for (r2 = 0; r2 < 3; r2++) {
            d_r_gyro[r2 + 3] = z[6 + r2];
          }

          for (r2 = 0; r2 < 6; r2++) {
            c_S_k[r2] = 0.0F;
            for (i = 0; i < 12; i++) {
              c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i];
            }

            b_z[r2] = d_r_gyro[r2] - c_S_k[r2];
          }

          for (r2 = 0; r2 < 12; r2++) {
            maxval = 0.0F;
            for (i = 0; i < 6; i++) {
              maxval += b_K_k[r2 + 12 * i] * b_z[i];
            }

            x_apo[r2] = x_apr[r2] + maxval;
          }

          /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */
          memset(&I[0], 0, 144U * sizeof(signed char));
          for (i = 0; i < 12; i++) {
            I[i + 12 * i] = 1;
          }

          for (r2 = 0; r2 < 12; r2++) {
            for (i = 0; i < 12; i++) {
              maxval = 0.0F;
              for (r1 = 0; r1 < 6; r1++) {
                maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i];
              }

              A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
            }
          }

          for (r2 = 0; r2 < 12; r2++) {
            for (i = 0; i < 12; i++) {
              P_apo[r2 + 12 * i] = 0.0F;
              for (r1 = 0; r1 < 12; r1++) {
                P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
              }
            }
          }
        } else {
          /* 'AttitudeEKF:267' else */
          /* 'AttitudeEKF:268' x_apo=x_apr; */
          for (i = 0; i < 12; i++) {
            x_apo[i] = x_apr[i];
          }

          /* 'AttitudeEKF:269' P_apo=P_apr; */
          memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float));
        }
      }
    }
  }

  /* % euler anglels extraction */
  /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */
  maxval = norm(*(float (*)[3])&x_apo[6]);
  a21 = norm(*(float (*)[3])&x_apo[9]);
  for (i = 0; i < 3; i++) {
    /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */
    muk[i] = -x_apo[i + 6] / maxval;
    zek[i] = x_apo[i + 9] / a21;
  }

  /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */
  wak[0] = muk[1] * zek[2] - muk[2] * zek[1];
  wak[1] = muk[2] * zek[0] - muk[0] * zek[2];
  wak[2] = muk[0] * zek[1] - muk[1] * zek[0];

  /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */
  maxval = norm(wak);
  for (r2 = 0; r2 < 3; r2++) {
    wak[r2] /= maxval;
  }

  /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */
  zek[0] = wak[1] * muk[2] - wak[2] * muk[1];
  zek[1] = wak[2] * muk[0] - wak[0] * muk[2];
  zek[2] = wak[0] * muk[1] - wak[1] * muk[0];

  /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */
  maxval = norm(zek);
  for (r2 = 0; r2 < 3; r2++) {
    zek[r2] /= maxval;
  }

  /* 'AttitudeEKF:288' xa_apo=x_apo; */
  for (i = 0; i < 12; i++) {
    xa_apo[i] = x_apo[i];
  }

  /* 'AttitudeEKF:289' Pa_apo=P_apo; */
  memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float));

  /*  rotation matrix from earth to body system */
  /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
  for (r2 = 0; r2 < 3; r2++) {
    Rot_matrix[r2] = zek[r2];
    Rot_matrix[3 + r2] = wak[r2];
    Rot_matrix[6 + r2] = muk[r2];
  }

  /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
  /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */
  /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
  /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */
  eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]);
  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
  eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]);
}

void AttitudeEKF_initialize(void)
{
  Q_not_empty = FALSE;
  Ji_not_empty = FALSE;
  AttitudeEKF_init();
}

void AttitudeEKF_terminate(void)
{
  /* (no terminate code required) */
}

/* End of code generation (AttitudeEKF.c) */