28 files changed, 1659 insertions, 2640 deletions

diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
new file mode 100644
index 000000000..68db382cf
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
@@ -0,0 +1,1456 @@
+/*
+ * 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) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
new file mode 100644
index 000000000..7094da1da
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
@@ -0,0 +1,26 @@
+/*
+ * AttitudeEKF.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_H__
+#define __ATTITUDEEKF_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "AttitudeEKF_types.h"
+
+/* Function Declarations */
+extern 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]);
+extern void AttitudeEKF_initialize(void);
+extern void AttitudeEKF_terminate(void);
+#endif
+/* End of code generation (AttitudeEKF.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
new file mode 100644
index 000000000..3f7522ffa
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
@@ -0,0 +1,17 @@
+/*
+ * AttitudeEKF_types.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_TYPES_H__
+#define __ATTITUDEEKF_TYPES_H__
+
+/* Include files */
+#include "rtwtypes.h"
+
+#endif
+/* End of code generation (AttitudeEKF_types.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
deleted file mode 100755
index 9e97ad11a..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ /dev/null
@@ -1,1148 +0,0 @@
-/*

- * attitudeKalmanfilter.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-#include "norm.h"

-#include "cross.h"

-#include "eye.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  int32_T b_u0;

-  int32_T b_u1;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else if (rtIsInfF(u0) && rtIsInfF(u1)) {

-    if (u0 > 0.0F) {

-      b_u0 = 1;

-    } else {

-      b_u0 = -1;

-    }

-

-    if (u1 > 0.0F) {

-      b_u1 = 1;

-    } else {

-      b_u1 = -1;

-    }

-

-    y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);

-  } else if (u1 == 0.0F) {

-    if (u0 > 0.0F) {

-      y = RT_PIF / 2.0F;

-    } else if (u0 < 0.0F) {

-      y = -(RT_PIF / 2.0F);

-    } else {

-      y = 0.0F;

-    }

-  } else {

-    y = (real32_T)atan2(u0, u1);

-  }

-

-  return y;

-}

-

-/*

- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)

- */

-void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const

-  real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T

-  P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T

-  eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T

-  P_aposteriori[144])

-{

-  real32_T wak[3];

-  real32_T O[9];

-  real_T dv0[9];

-  real32_T a[9];

-  int32_T i;

-  real32_T b_a[9];

-  real32_T x_n_b[3];

-  real32_T b_x_aposteriori_k[3];

-  real32_T z_n_b[3];

-  real32_T c_a[3];

-  real32_T d_a[3];

-  int32_T i0;

-  real32_T x_apriori[12];

-  real_T dv1[144];

-  real32_T A_lin[144];

-  static const int8_T iv0[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 };

-

-  real32_T b_A_lin[144];

-  real32_T b_q[144];

-  real32_T c_A_lin[144];

-  real32_T d_A_lin[144];

-  real32_T e_A_lin[144];

-  int32_T i1;

-  real32_T P_apriori[144];

-  real32_T b_P_apriori[108];

-  static const int8_T iv1[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 };

-

-  real32_T K_k[108];

-  real32_T fv0[81];

-  static const int8_T iv2[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 };

-

-  real32_T b_r[81];

-  real32_T fv1[81];

-  real32_T f0;

-  real32_T c_P_apriori[36];

-  static const int8_T iv3[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 };

-

-  real32_T fv2[36];

-  static const int8_T iv4[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 };

-

-  real32_T c_r[9];

-  real32_T b_K_k[36];

-  real32_T d_P_apriori[72];

-  static const int8_T iv5[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 };

-

-  real32_T c_K_k[72];

-  static const int8_T iv6[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 };

-

-  real32_T b_z[6];

-  static const int8_T iv7[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 };

-

-  static const int8_T iv8[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 };

-

-  real32_T fv3[6];

-  real32_T c_z[6];

-

-  /*  Extended Attitude Kalmanfilter */

-  /*  */

-  /*  state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */

-  /*  measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */

-  /*  knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */

-  /*  */

-  /*  [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */

-  /*  */

-  /*  Example.... */

-  /*  */

-  /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */

-  /* coder.varsize('udpIndVect', [9,1], [1,0]) */

-  /* udpIndVect=find(updVect); */

-  /* process and measurement noise covariance matrix */

-  /* Q = diag(q.^2*dt); */

-  /* observation matrix */

-  /* 'attitudeKalmanfilter:33' wx=  x_aposteriori_k(1); */

-  /* 'attitudeKalmanfilter:34' wy=  x_aposteriori_k(2); */

-  /* 'attitudeKalmanfilter:35' wz=  x_aposteriori_k(3); */

-  /* 'attitudeKalmanfilter:37' wax=  x_aposteriori_k(4); */

-  /* 'attitudeKalmanfilter:38' way=  x_aposteriori_k(5); */

-  /* 'attitudeKalmanfilter:39' waz=  x_aposteriori_k(6); */

-  /* 'attitudeKalmanfilter:41' zex=  x_aposteriori_k(7); */

-  /* 'attitudeKalmanfilter:42' zey=  x_aposteriori_k(8); */

-  /* 'attitudeKalmanfilter:43' zez=  x_aposteriori_k(9); */

-  /* 'attitudeKalmanfilter:45' mux=  x_aposteriori_k(10); */

-  /* 'attitudeKalmanfilter:46' muy=  x_aposteriori_k(11); */

-  /* 'attitudeKalmanfilter:47' muz=  x_aposteriori_k(12); */

-  /* % prediction section */

-  /* body angular accelerations */

-  /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */

-  wak[0] = x_aposteriori_k[3];

-  wak[1] = x_aposteriori_k[4];

-  wak[2] = x_aposteriori_k[5];

-

-  /* body angular rates */

-  /* 'attitudeKalmanfilter:54' wk =[wx;  wy; wz] + dt*wak; */

-  /* derivative of the prediction rotation matrix */

-  /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

-  O[0] = 0.0F;

-  O[1] = -x_aposteriori_k[2];

-  O[2] = x_aposteriori_k[1];

-  O[3] = x_aposteriori_k[2];

-  O[4] = 0.0F;

-  O[5] = -x_aposteriori_k[0];

-  O[6] = -x_aposteriori_k[1];

-  O[7] = x_aposteriori_k[0];

-  O[8] = 0.0F;

-

-  /* prediction of the earth z vector */

-  /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* prediction of the magnetic vector */

-  /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    b_a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */

-  /* 'attitudeKalmanfilter:66'     -zez,0,zex; */

-  /* 'attitudeKalmanfilter:67'     zey,-zex,0]'; */

-  /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */

-  /* 'attitudeKalmanfilter:69'     -muz,0,mux; */

-  /* 'attitudeKalmanfilter:70'     zey,-mux,0]'; */

-  /* 'attitudeKalmanfilter:74' E=eye(3); */

-  /* 'attitudeKalmanfilter:76' Z=zeros(3); */

-  /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */

-  x_n_b[0] = x_aposteriori_k[0];

-  x_n_b[1] = x_aposteriori_k[1];

-  x_n_b[2] = x_aposteriori_k[2];

-  b_x_aposteriori_k[0] = x_aposteriori_k[6];

-  b_x_aposteriori_k[1] = x_aposteriori_k[7];

-  b_x_aposteriori_k[2] = x_aposteriori_k[8];

-  z_n_b[0] = x_aposteriori_k[9];

-  z_n_b[1] = x_aposteriori_k[10];

-  z_n_b[2] = x_aposteriori_k[11];

-  for (i = 0; i < 3; i++) {

-    c_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];

-    }

-

-    d_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];

-    }

-

-    x_apriori[i] = x_n_b[i] + dt * wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 3] = wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 6] = c_a[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 9] = d_a[i];

-  }

-

-  /* 'attitudeKalmanfilter:81' A_lin=[ Z,  E,  Z,  Z */

-  /* 'attitudeKalmanfilter:82'     Z,  Z,  Z,  Z */

-  /* 'attitudeKalmanfilter:83'     EZ, Z,  O,  Z */

-  /* 'attitudeKalmanfilter:84'     MA, Z,  Z,  O]; */

-  /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */

-  b_eye(dv1);

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];

-    }

-

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * i) + 3] = 0.0F;

-    }

-  }

-

-  A_lin[6] = 0.0F;

-  A_lin[7] = x_aposteriori_k[8];

-  A_lin[8] = -x_aposteriori_k[7];

-  A_lin[18] = -x_aposteriori_k[8];

-  A_lin[19] = 0.0F;

-  A_lin[20] = x_aposteriori_k[6];

-  A_lin[30] = x_aposteriori_k[7];

-  A_lin[31] = -x_aposteriori_k[6];

-  A_lin[32] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;

-    }

-  }

-

-  A_lin[9] = 0.0F;

-  A_lin[10] = x_aposteriori_k[11];

-  A_lin[11] = -x_aposteriori_k[10];

-  A_lin[21] = -x_aposteriori_k[11];

-  A_lin[22] = 0.0F;

-  A_lin[23] = x_aposteriori_k[9];

-  A_lin[33] = x_aposteriori_k[7];

-  A_lin[34] = -x_aposteriori_k[9];

-  A_lin[35] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *

-        dt;

-    }

-  }

-

-  /* 'attitudeKalmanfilter:88' Qtemp=[ q(1),     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:89'         0,     q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:90'         0,     0,      q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:91'         0,     0,      0,      q(2),   0,      0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:92'         0,     0,      0,      0,      q(2),   0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:93'         0,     0,      0,      0,      0,      q(2),   0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:94'         0,     0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:95'         0,     0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:96'         0,     0,      0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0; */

-  /* 'attitudeKalmanfilter:97'         0,     0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0,      0; */

-  /* 'attitudeKalmanfilter:98'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0; */

-  /* 'attitudeKalmanfilter:99'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4)]; */

-  /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */

-  /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

-  b_q[0] = q[0];

-  b_q[12] = 0.0F;

-  b_q[24] = 0.0F;

-  b_q[36] = 0.0F;

-  b_q[48] = 0.0F;

-  b_q[60] = 0.0F;

-  b_q[72] = 0.0F;

-  b_q[84] = 0.0F;

-  b_q[96] = 0.0F;

-  b_q[108] = 0.0F;

-  b_q[120] = 0.0F;

-  b_q[132] = 0.0F;

-  b_q[1] = 0.0F;

-  b_q[13] = q[0];

-  b_q[25] = 0.0F;

-  b_q[37] = 0.0F;

-  b_q[49] = 0.0F;

-  b_q[61] = 0.0F;

-  b_q[73] = 0.0F;

-  b_q[85] = 0.0F;

-  b_q[97] = 0.0F;

-  b_q[109] = 0.0F;

-  b_q[121] = 0.0F;

-  b_q[133] = 0.0F;

-  b_q[2] = 0.0F;

-  b_q[14] = 0.0F;

-  b_q[26] = q[0];

-  b_q[38] = 0.0F;

-  b_q[50] = 0.0F;

-  b_q[62] = 0.0F;

-  b_q[74] = 0.0F;

-  b_q[86] = 0.0F;

-  b_q[98] = 0.0F;

-  b_q[110] = 0.0F;

-  b_q[122] = 0.0F;

-  b_q[134] = 0.0F;

-  b_q[3] = 0.0F;

-  b_q[15] = 0.0F;

-  b_q[27] = 0.0F;

-  b_q[39] = q[1];

-  b_q[51] = 0.0F;

-  b_q[63] = 0.0F;

-  b_q[75] = 0.0F;

-  b_q[87] = 0.0F;

-  b_q[99] = 0.0F;

-  b_q[111] = 0.0F;

-  b_q[123] = 0.0F;

-  b_q[135] = 0.0F;

-  b_q[4] = 0.0F;

-  b_q[16] = 0.0F;

-  b_q[28] = 0.0F;

-  b_q[40] = 0.0F;

-  b_q[52] = q[1];

-  b_q[64] = 0.0F;

-  b_q[76] = 0.0F;

-  b_q[88] = 0.0F;

-  b_q[100] = 0.0F;

-  b_q[112] = 0.0F;

-  b_q[124] = 0.0F;

-  b_q[136] = 0.0F;

-  b_q[5] = 0.0F;

-  b_q[17] = 0.0F;

-  b_q[29] = 0.0F;

-  b_q[41] = 0.0F;

-  b_q[53] = 0.0F;

-  b_q[65] = q[1];

-  b_q[77] = 0.0F;

-  b_q[89] = 0.0F;

-  b_q[101] = 0.0F;

-  b_q[113] = 0.0F;

-  b_q[125] = 0.0F;

-  b_q[137] = 0.0F;

-  b_q[6] = 0.0F;

-  b_q[18] = 0.0F;

-  b_q[30] = 0.0F;

-  b_q[42] = 0.0F;

-  b_q[54] = 0.0F;

-  b_q[66] = 0.0F;

-  b_q[78] = q[2];

-  b_q[90] = 0.0F;

-  b_q[102] = 0.0F;

-  b_q[114] = 0.0F;

-  b_q[126] = 0.0F;

-  b_q[138] = 0.0F;

-  b_q[7] = 0.0F;

-  b_q[19] = 0.0F;

-  b_q[31] = 0.0F;

-  b_q[43] = 0.0F;

-  b_q[55] = 0.0F;

-  b_q[67] = 0.0F;

-  b_q[79] = 0.0F;

-  b_q[91] = q[2];

-  b_q[103] = 0.0F;

-  b_q[115] = 0.0F;

-  b_q[127] = 0.0F;

-  b_q[139] = 0.0F;

-  b_q[8] = 0.0F;

-  b_q[20] = 0.0F;

-  b_q[32] = 0.0F;

-  b_q[44] = 0.0F;

-  b_q[56] = 0.0F;

-  b_q[68] = 0.0F;

-  b_q[80] = 0.0F;

-  b_q[92] = 0.0F;

-  b_q[104] = q[2];

-  b_q[116] = 0.0F;

-  b_q[128] = 0.0F;

-  b_q[140] = 0.0F;

-  b_q[9] = 0.0F;

-  b_q[21] = 0.0F;

-  b_q[33] = 0.0F;

-  b_q[45] = 0.0F;

-  b_q[57] = 0.0F;

-  b_q[69] = 0.0F;

-  b_q[81] = 0.0F;

-  b_q[93] = 0.0F;

-  b_q[105] = 0.0F;

-  b_q[117] = q[3];

-  b_q[129] = 0.0F;

-  b_q[141] = 0.0F;

-  b_q[10] = 0.0F;

-  b_q[22] = 0.0F;

-  b_q[34] = 0.0F;

-  b_q[46] = 0.0F;

-  b_q[58] = 0.0F;

-  b_q[70] = 0.0F;

-  b_q[82] = 0.0F;

-  b_q[94] = 0.0F;

-  b_q[106] = 0.0F;

-  b_q[118] = 0.0F;

-  b_q[130] = q[3];

-  b_q[142] = 0.0F;

-  b_q[11] = 0.0F;

-  b_q[23] = 0.0F;

-  b_q[35] = 0.0F;

-  b_q[47] = 0.0F;

-  b_q[59] = 0.0F;

-  b_q[71] = 0.0F;

-  b_q[83] = 0.0F;

-  b_q[95] = 0.0F;

-  b_q[107] = 0.0F;

-  b_q[119] = 0.0F;

-  b_q[131] = 0.0F;

-  b_q[143] = q[3];

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *

-          i0];

-      }

-

-      c_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];

-      }

-    }

-

-    for (i0 = 0; i0 < 12; i0++) {

-      d_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-

-      e_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];

-    }

-  }

-

-  /* % update */

-  /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

-  if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {

-    /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */

-    if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-      /* 'attitudeKalmanfilter:112' r(2)=10000; */

-      r[1] = 10000.0F;

-    }

-

-    /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:115'         0,r(1),0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:116'         0,0,r(1),0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:117'         0,0,0,r(2),0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:118'         0,0,0,0,r(2),0,0,0,0; */

-    /* 'attitudeKalmanfilter:119'         0,0,0,0,0,r(2),0,0,0; */

-    /* 'attitudeKalmanfilter:120'         0,0,0,0,0,0,r(3),0,0; */

-    /* 'attitudeKalmanfilter:121'         0,0,0,0,0,0,0,r(3),0; */

-    /* 'attitudeKalmanfilter:122'         0,0,0,0,0,0,0,0,r(3)]; */

-    /* observation matrix */

-    /* [zw;ze;zmk]; */

-    /* 'attitudeKalmanfilter:125' H_k=[  E,     Z,      Z,    Z; */

-    /* 'attitudeKalmanfilter:126'         Z,     Z,      E,    Z; */

-    /* 'attitudeKalmanfilter:127'         Z,     Z,      Z,    E]; */

-    /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */

-    /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */

-    /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        b_P_apriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1

-            + 12 * i0];

-        }

-      }

-    }

-

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        K_k[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];

-        }

-      }

-

-      for (i0 = 0; i0 < 9; i0++) {

-        fv0[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];

-        }

-      }

-    }

-

-    b_r[0] = r[0];

-    b_r[9] = 0.0F;

-    b_r[18] = 0.0F;

-    b_r[27] = 0.0F;

-    b_r[36] = 0.0F;

-    b_r[45] = 0.0F;

-    b_r[54] = 0.0F;

-    b_r[63] = 0.0F;

-    b_r[72] = 0.0F;

-    b_r[1] = 0.0F;

-    b_r[10] = r[0];

-    b_r[19] = 0.0F;

-    b_r[28] = 0.0F;

-    b_r[37] = 0.0F;

-    b_r[46] = 0.0F;

-    b_r[55] = 0.0F;

-    b_r[64] = 0.0F;

-    b_r[73] = 0.0F;

-    b_r[2] = 0.0F;

-    b_r[11] = 0.0F;

-    b_r[20] = r[0];

-    b_r[29] = 0.0F;

-    b_r[38] = 0.0F;

-    b_r[47] = 0.0F;

-    b_r[56] = 0.0F;

-    b_r[65] = 0.0F;

-    b_r[74] = 0.0F;

-    b_r[3] = 0.0F;

-    b_r[12] = 0.0F;

-    b_r[21] = 0.0F;

-    b_r[30] = r[1];

-    b_r[39] = 0.0F;

-    b_r[48] = 0.0F;

-    b_r[57] = 0.0F;

-    b_r[66] = 0.0F;

-    b_r[75] = 0.0F;

-    b_r[4] = 0.0F;

-    b_r[13] = 0.0F;

-    b_r[22] = 0.0F;

-    b_r[31] = 0.0F;

-    b_r[40] = r[1];

-    b_r[49] = 0.0F;

-    b_r[58] = 0.0F;

-    b_r[67] = 0.0F;

-    b_r[76] = 0.0F;

-    b_r[5] = 0.0F;

-    b_r[14] = 0.0F;

-    b_r[23] = 0.0F;

-    b_r[32] = 0.0F;

-    b_r[41] = 0.0F;

-    b_r[50] = r[1];

-    b_r[59] = 0.0F;

-    b_r[68] = 0.0F;

-    b_r[77] = 0.0F;

-    b_r[6] = 0.0F;

-    b_r[15] = 0.0F;

-    b_r[24] = 0.0F;

-    b_r[33] = 0.0F;

-    b_r[42] = 0.0F;

-    b_r[51] = 0.0F;

-    b_r[60] = r[2];

-    b_r[69] = 0.0F;

-    b_r[78] = 0.0F;

-    b_r[7] = 0.0F;

-    b_r[16] = 0.0F;

-    b_r[25] = 0.0F;

-    b_r[34] = 0.0F;

-    b_r[43] = 0.0F;

-    b_r[52] = 0.0F;

-    b_r[61] = 0.0F;

-    b_r[70] = r[2];

-    b_r[79] = 0.0F;

-    b_r[8] = 0.0F;

-    b_r[17] = 0.0F;

-    b_r[26] = 0.0F;

-    b_r[35] = 0.0F;

-    b_r[44] = 0.0F;

-    b_r[53] = 0.0F;

-    b_r[62] = 0.0F;

-    b_r[71] = 0.0F;

-    b_r[80] = r[2];

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];

-      }

-    }

-

-    mrdivide(b_P_apriori, fv1, K_k);

-

-    /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */

-    for (i = 0; i < 9; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];

-      }

-

-      O[i] = z[i] - f0;

-    }

-

-    for (i = 0; i < 12; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 9; i0++) {

-        f0 += K_k[i + 12 * i0] * O[i0];

-      }

-

-      x_aposteriori[i] = x_apriori[i] + f0;

-    }

-

-    /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

-    b_eye(dv1);

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];

-        }

-

-        b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-      }

-    }

-

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        P_aposteriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12

-            * i0];

-        }

-      }

-    }

-  } else {

-    /* 'attitudeKalmanfilter:138' else */

-    /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

-    if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {

-      /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */

-      /* 'attitudeKalmanfilter:142'             0,r(1),0; */

-      /* 'attitudeKalmanfilter:143'             0,0,r(1)]; */

-      /* observation matrix */

-      /* 'attitudeKalmanfilter:146' H_k=[  E,     Z,      Z,    Z]; */

-      /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

-      /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

-      /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          c_P_apriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-              iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          fv2[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *

-              i0];

-          }

-        }

-

-        for (i0 = 0; i0 < 3; i0++) {

-          O[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      c_r[0] = r[0];

-      c_r[3] = 0.0F;

-      c_r[6] = 0.0F;

-      c_r[1] = 0.0F;

-      c_r[4] = r[0];

-      c_r[7] = 0.0F;

-      c_r[2] = 0.0F;

-      c_r[5] = 0.0F;

-      c_r[8] = r[0];

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];

-        }

-      }

-

-      b_mrdivide(c_P_apriori, a, b_K_k);

-

-      /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */

-      for (i = 0; i < 3; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];

-        }

-

-        x_n_b[i] = z[i] - f0;

-      }

-

-      for (i = 0; i < 12; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 3; i0++) {

-          f0 += b_K_k[i + 12 * i0] * x_n_b[i0];

-        }

-

-        x_aposteriori[i] = x_apriori[i] + f0;

-      }

-

-      /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

-      b_eye(dv1);

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 = 0.0F;

-          for (i1 = 0; i1 < 3; i1++) {

-            f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];

-          }

-

-          b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-        }

-      }

-

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          P_aposteriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +

-              12 * i0];

-          }

-        }

-      }

-    } else {

-      /* 'attitudeKalmanfilter:156' else */

-      /* 'attitudeKalmanfilter:157' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

-      if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))

-      {

-        /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */

-        if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-          /* 'attitudeKalmanfilter:159' r(2)=10000; */

-          r[1] = 10000.0F;

-        }

-

-        /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */

-        /* 'attitudeKalmanfilter:163'                 0,r(1),0,0,0,0; */

-        /* 'attitudeKalmanfilter:164'                 0,0,r(1),0,0,0; */

-        /* 'attitudeKalmanfilter:165'                 0,0,0,r(2),0,0; */

-        /* 'attitudeKalmanfilter:166'                 0,0,0,0,r(2),0; */

-        /* 'attitudeKalmanfilter:167'                 0,0,0,0,0,r(2)]; */

-        /* observation matrix */

-        /* 'attitudeKalmanfilter:170' H_k=[  E,     Z,      Z,    Z; */

-        /* 'attitudeKalmanfilter:171'                 Z,     Z,      E,    Z]; */

-        /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

-        /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-        /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            d_P_apriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            c_K_k[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12

-                * i0];

-            }

-          }

-

-          for (i0 = 0; i0 < 6; i0++) {

-            fv2[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        b_K_k[0] = r[0];

-        b_K_k[6] = 0.0F;

-        b_K_k[12] = 0.0F;

-        b_K_k[18] = 0.0F;

-        b_K_k[24] = 0.0F;

-        b_K_k[30] = 0.0F;

-        b_K_k[1] = 0.0F;

-        b_K_k[7] = r[0];

-        b_K_k[13] = 0.0F;

-        b_K_k[19] = 0.0F;

-        b_K_k[25] = 0.0F;

-        b_K_k[31] = 0.0F;

-        b_K_k[2] = 0.0F;

-        b_K_k[8] = 0.0F;

-        b_K_k[14] = r[0];

-        b_K_k[20] = 0.0F;

-        b_K_k[26] = 0.0F;

-        b_K_k[32] = 0.0F;

-        b_K_k[3] = 0.0F;

-        b_K_k[9] = 0.0F;

-        b_K_k[15] = 0.0F;

-        b_K_k[21] = r[1];

-        b_K_k[27] = 0.0F;

-        b_K_k[33] = 0.0F;

-        b_K_k[4] = 0.0F;

-        b_K_k[10] = 0.0F;

-        b_K_k[16] = 0.0F;

-        b_K_k[22] = 0.0F;

-        b_K_k[28] = r[1];

-        b_K_k[34] = 0.0F;

-        b_K_k[5] = 0.0F;

-        b_K_k[11] = 0.0F;

-        b_K_k[17] = 0.0F;

-        b_K_k[23] = 0.0F;

-        b_K_k[29] = 0.0F;

-        b_K_k[35] = r[1];

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-          }

-        }

-

-        c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-        /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */

-        for (i = 0; i < 6; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

-          }

-

-          b_z[i] = z[i] - f0;

-        }

-

-        for (i = 0; i < 12; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 6; i0++) {

-            f0 += c_K_k[i + 12 * i0] * b_z[i0];

-          }

-

-          x_aposteriori[i] = x_apriori[i] + f0;

-        }

-

-        /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-        b_eye(dv1);

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 = 0.0F;

-            for (i1 = 0; i1 < 6; i1++) {

-              f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

-            }

-

-            b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-          }

-        }

-

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            P_aposteriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1

-                + 12 * i0];

-            }

-          }

-        }

-      } else {

-        /* 'attitudeKalmanfilter:181' else */

-        /* 'attitudeKalmanfilter:182' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

-        if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))

-        {

-          /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */

-          /* 'attitudeKalmanfilter:184'                     0,r(1),0,0,0,0; */

-          /* 'attitudeKalmanfilter:185'                     0,0,r(1),0,0,0; */

-          /* 'attitudeKalmanfilter:186'                     0,0,0,r(3),0,0; */

-          /* 'attitudeKalmanfilter:187'                     0,0,0,0,r(3),0; */

-          /* 'attitudeKalmanfilter:188'                     0,0,0,0,0,r(3)]; */

-          /* observation matrix */

-          /* 'attitudeKalmanfilter:191' H_k=[  E,     Z,      Z,    Z; */

-          /* 'attitudeKalmanfilter:192'                     Z,     Z,      Z,    E]; */

-          /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

-          /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-          /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              d_P_apriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                  iv7[i1 + 12 * i0];

-              }

-            }

-          }

-

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              c_K_k[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +

-                  12 * i0];

-              }

-            }

-

-            for (i0 = 0; i0 < 6; i0++) {

-              fv2[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *

-                  i0];

-              }

-            }

-          }

-

-          b_K_k[0] = r[0];

-          b_K_k[6] = 0.0F;

-          b_K_k[12] = 0.0F;

-          b_K_k[18] = 0.0F;

-          b_K_k[24] = 0.0F;

-          b_K_k[30] = 0.0F;

-          b_K_k[1] = 0.0F;

-          b_K_k[7] = r[0];

-          b_K_k[13] = 0.0F;

-          b_K_k[19] = 0.0F;

-          b_K_k[25] = 0.0F;

-          b_K_k[31] = 0.0F;

-          b_K_k[2] = 0.0F;

-          b_K_k[8] = 0.0F;

-          b_K_k[14] = r[0];

-          b_K_k[20] = 0.0F;

-          b_K_k[26] = 0.0F;

-          b_K_k[32] = 0.0F;

-          b_K_k[3] = 0.0F;

-          b_K_k[9] = 0.0F;

-          b_K_k[15] = 0.0F;

-          b_K_k[21] = r[2];

-          b_K_k[27] = 0.0F;

-          b_K_k[33] = 0.0F;

-          b_K_k[4] = 0.0F;

-          b_K_k[10] = 0.0F;

-          b_K_k[16] = 0.0F;

-          b_K_k[22] = 0.0F;

-          b_K_k[28] = r[2];

-          b_K_k[34] = 0.0F;

-          b_K_k[5] = 0.0F;

-          b_K_k[11] = 0.0F;

-          b_K_k[17] = 0.0F;

-          b_K_k[23] = 0.0F;

-          b_K_k[29] = 0.0F;

-          b_K_k[35] = r[2];

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-            }

-          }

-

-          c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-          /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */

-          for (i = 0; i < 3; i++) {

-            b_z[i] = z[i];

-          }

-

-          for (i = 0; i < 3; i++) {

-            b_z[i + 3] = z[i + 6];

-          }

-

-          for (i = 0; i < 6; i++) {

-            fv3[i] = 0.0F;

-            for (i0 = 0; i0 < 12; i0++) {

-              fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];

-            }

-

-            c_z[i] = b_z[i] - fv3[i];

-          }

-

-          for (i = 0; i < 12; i++) {

-            f0 = 0.0F;

-            for (i0 = 0; i0 < 6; i0++) {

-              f0 += c_K_k[i + 12 * i0] * c_z[i0];

-            }

-

-            x_aposteriori[i] = x_apriori[i] + f0;

-          }

-

-          /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-          b_eye(dv1);

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              f0 = 0.0F;

-              for (i1 = 0; i1 < 6; i1++) {

-                f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];

-              }

-

-              b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-            }

-          }

-

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              P_aposteriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *

-                  P_apriori[i1 + 12 * i0];

-              }

-            }

-          }

-        } else {

-          /* 'attitudeKalmanfilter:202' else */

-          /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */

-          for (i = 0; i < 12; i++) {

-            x_aposteriori[i] = x_apriori[i];

-          }

-

-          /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */

-          memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));

-        }

-      }

-    }

-  }

-

-  /* % euler anglels extraction */

-  /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = -x_aposteriori[i + 6];

-  }

-

-  rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);

-

-  /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

-  rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&

-           x_aposteriori[9]), wak);

-

-  /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  cross(z_n_b, x_n_b, wak);

-

-  /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  rdivide(x_n_b, norm(wak), wak);

-

-  /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */

-  cross(wak, z_n_b, x_n_b);

-

-  /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */

-  for (i = 0; i < 3; i++) {

-    b_x_aposteriori_k[i] = x_n_b[i];

-  }

-

-  rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);

-

-  /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

-  for (i = 0; i < 3; i++) {

-    Rot_matrix[i] = x_n_b[i];

-    Rot_matrix[3 + i] = wak[i];

-    Rot_matrix[6 + i] = z_n_b[i];

-  }

-

-  /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

-  /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */

-  eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

-  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);

-  eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

-}

-

-/* End of code generation (attitudeKalmanfilter.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
deleted file mode 100755
index afa63c1a9..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_H__

-#define __ATTITUDEKALMANFILTER_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

-#endif

-/* End of code generation (attitudeKalmanfilter.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
deleted file mode 100755
index 7d620d7fa..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.c

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_initialize.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_initialize(void)

-{

-  rt_InitInfAndNaN(8U);

-}

-

-/* End of code generation (attitudeKalmanfilter_initialize.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
deleted file mode 100755
index 8b599be66..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.h

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__

-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_initialize(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_initialize.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
deleted file mode 100755
index 7f9727419..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.c

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_terminate.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_terminate(void)

-{

-  /* (no terminate code required) */

-}

-

-/* End of code generation (attitudeKalmanfilter_terminate.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
deleted file mode 100755
index da84a5024..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.h

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__

-#define __ATTITUDEKALMANFILTER_TERMINATE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_terminate(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_terminate.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
deleted file mode 100755
index 30fd1e75e..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*

- * attitudeKalmanfilter_types.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TYPES_H__

-#define __ATTITUDEKALMANFILTER_TYPES_H__

-

-/* Type Definitions */

-

-#endif

-/* End of code generation (attitudeKalmanfilter_types.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c
deleted file mode 100755
index 84ada9002..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/cross.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/*

- * cross.c

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "cross.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])

-{

-  c[0] = a[1] * b[2] - a[2] * b[1];

-  c[1] = a[2] * b[0] - a[0] * b[2];

-  c[2] = a[0] * b[1] - a[1] * b[0];

-}

-

-/* End of code generation (cross.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h
deleted file mode 100755
index e727f0684..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/cross.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * cross.h

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __CROSS_H__

-#define __CROSS_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);

-#endif

-/* End of code generation (cross.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c
deleted file mode 100755
index b89ab58ef..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/eye.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*

- * eye.c

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "eye.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_eye(real_T I[144])

-{

-  int32_T i;

-  memset(&I[0], 0, 144U * sizeof(real_T));

-  for (i = 0; i < 12; i++) {

-    I[i + 12 * i] = 1.0;

-  }

-}

-

-/*

- *

- */

-void eye(real_T I[9])

-{

-  int32_T i;

-  memset(&I[0], 0, 9U * sizeof(real_T));

-  for (i = 0; i < 3; i++) {

-    I[i + 3 * i] = 1.0;

-  }

-}

-

-/* End of code generation (eye.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h
deleted file mode 100755
index 94fbe7671..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/eye.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*

- * eye.h

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __EYE_H__

-#define __EYE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_eye(real_T I[144]);

-extern void eye(real_T I[9]);

-#endif

-/* End of code generation (eye.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
deleted file mode 100755
index a810f22e4..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
+++ /dev/null
@@ -1,357 +0,0 @@
-/*

- * mrdivide.c

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])

-{

-  real32_T b_A[9];

-  int32_T rtemp;

-  int32_T k;

-  real32_T b_B[36];

-  int32_T r1;

-  int32_T r2;

-  int32_T r3;

-  real32_T maxval;

-  real32_T a21;

-  real32_T Y[36];

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];

-    }

-  }

-

-  for (rtemp = 0; rtemp < 12; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];

-    }

-  }

-

-  r1 = 0;

-  r2 = 1;

-  r3 = 2;

-  maxval = (real32_T)fabs(b_A[0]);

-  a21 = (real32_T)fabs(b_A[1]);

-  if (a21 > maxval) {

-    maxval = a21;

-    r1 = 1;

-    r2 = 0;

-  }

-

-  if ((real32_T)fabs(b_A[2]) > maxval) {

-    r1 = 2;

-    r2 = 1;

-    r3 = 0;

-  }

-

-  b_A[r2] /= b_A[r1];

-  b_A[r3] /= b_A[r1];

-  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];

-  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];

-  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];

-  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];

-  if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {

-    rtemp = r2;

-    r2 = r3;

-    r3 = rtemp;

-  }

-

-  b_A[3 + r3] /= b_A[3 + r2];

-  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];

-  for (k = 0; k < 12; k++) {

-    Y[3 * k] = b_B[r1 + 3 * k];

-    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];

-    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3

-      + r3];

-    Y[2 + 3 * k] /= b_A[6 + r3];

-    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];

-    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];

-    Y[1 + 3 * k] /= b_A[3 + r2];

-    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];

-    Y[3 * k] /= b_A[r1];

-  }

-

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 12; k++) {

-      y[k + 12 * rtemp] = Y[rtemp + 3 * k];

-    }

-  }

-}

-

-/*

- *

- */

-void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])

-{

-  real32_T b_A[36];

-  int8_T ipiv[6];

-  int32_T i3;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[72];

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      b_A[iy + 6 * i3] = B[i3 + 6 * iy];

-    }

-

-    ipiv[i3] = (int8_T)(1 + i3);

-  }

-

-  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] = (int8_T)((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;

-        }

-      }

-

-      i3 = (c - j) + 6;

-      for (jy = c + 1; jy + 1 <= i3; 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;

-        i3 = (iy - j) + 12;

-        for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 6;

-      iy += 6;

-    }

-  }

-

-  for (i3 = 0; i3 < 12; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      Y[iy + 6 * i3] = A[i3 + 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 (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i3] = Y[i3 + 6 * iy];

-    }

-  }

-}

-

-/*

- *

- */

-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])

-{

-  real32_T b_A[81];

-  int8_T ipiv[9];

-  int32_T i2;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[108];

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      b_A[iy + 9 * i2] = B[i2 + 9 * iy];

-    }

-

-    ipiv[i2] = (int8_T)(1 + i2);

-  }

-

-  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] = (int8_T)((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;

-        }

-      }

-

-      i2 = (c - j) + 9;

-      for (jy = c + 1; jy + 1 <= i2; 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;

-        i2 = (iy - j) + 18;

-        for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 9;

-      iy += 9;

-    }

-  }

-

-  for (i2 = 0; i2 < 12; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      Y[iy + 9 * i2] = A[i2 + 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 (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i2] = Y[i2 + 9 * iy];

-    }

-  }

-}

-

-/* End of code generation (mrdivide.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
deleted file mode 100755
index 2d3b0d51f..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*

- * mrdivide.h

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __MRDIVIDE_H__

-#define __MRDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);

-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);

-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);

-#endif

-/* End of code generation (mrdivide.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c
deleted file mode 100755
index 0c418cc7b..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/norm.c
+++ /dev/null
@@ -1,54 +0,0 @@
-/*

- * norm.c

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "norm.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-real32_T norm(const real32_T x[3])

-{

-  real32_T y;

-  real32_T scale;

-  int32_T k;

-  real32_T absxk;

-  real32_T 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);

-}

-

-/* End of code generation (norm.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h
deleted file mode 100755
index 60cf77b57..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/norm.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * norm.h

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __NORM_H__

-#define __NORM_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern real32_T norm(const real32_T x[3]);

-#endif

-/* End of code generation (norm.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
deleted file mode 100755
index d035dae5e..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c
+++ /dev/null
@@ -1,38 +0,0 @@
-/*

- * rdivide.c

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void rdivide(const real32_T x[3], real32_T y, real32_T z[3])

-{

-  int32_T i;

-  for (i = 0; i < 3; i++) {

-    z[i] = x[i] / y;

-  }

-}

-

-/* End of code generation (rdivide.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
deleted file mode 100755
index 4bbebebe2..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * rdivide.h

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RDIVIDE_H__

-#define __RDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);

-#endif

-/* End of code generation (rdivide.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
deleted file mode 100755
index 34164d104..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*

- * rtGetInf.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf

- */

-#include "rtGetInf.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetInf ==================================================

- * Abstract:

- * Initialize rtInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T inf = 0.0;

-  if (bitsPerReal == 32U) {

-    inf = rtGetInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return inf;

-}

-

-/* Function: rtGetInfF ==================================================

- * Abstract:

- * Initialize rtInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetInfF(void)

-{

-  IEEESingle infF;

-  infF.wordL.wordLuint = 0x7F800000U;

-  return infF.wordL.wordLreal;

-}

-

-/* Function: rtGetMinusInf ==================================================

- * Abstract:

- * Initialize rtMinusInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetMinusInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T minf = 0.0;

-  if (bitsPerReal == 32U) {

-    minf = rtGetMinusInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return minf;

-}

-

-/* Function: rtGetMinusInfF ==================================================

- * Abstract:

- * Initialize rtMinusInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetMinusInfF(void)

-{

-  IEEESingle minfF;

-  minfF.wordL.wordLuint = 0xFF800000U;

-  return minfF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetInf.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
deleted file mode 100755
index 145373cd0..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*

- * rtGetInf.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETINF_H__

-#define __RTGETINF_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetInf(void);

-extern real32_T rtGetInfF(void);

-extern real_T rtGetMinusInf(void);

-extern real32_T rtGetMinusInfF(void);

-

-#endif

-/* End of code generation (rtGetInf.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
deleted file mode 100755
index d84ca9573..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*

- * rtGetNaN.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, NaN

- */

-#include "rtGetNaN.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetNaN ==================================================

- * Abstract:

- * Initialize rtNaN needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetNaN(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T nan = 0.0;

-  if (bitsPerReal == 32U) {

-    nan = rtGetNaNF();

-  } else {

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      union {

-        LittleEndianIEEEDouble bitVal;

-        real_T fltVal;

-      } tmpVal;

-

-      tmpVal.bitVal.words.wordH = 0xFFF80000U;

-      tmpVal.bitVal.words.wordL = 0x00000000U;

-      nan = tmpVal.fltVal;

-      break;

-    }

-

-   case BigEndian:

-    {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;

-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;

-        nan = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return nan;

-}

-

-/* Function: rtGetNaNF ==================================================

- * Abstract:

- * Initialize rtNaNF needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetNaNF(void)

-{

-  IEEESingle nanF = { { 0 } };

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      nanF.wordL.wordLuint = 0xFFC00000U;

-      break;

-    }

-

-   case BigEndian:

-    {

-      nanF.wordL.wordLuint = 0x7FFFFFFFU;

-      break;

-    }

-  }

-

-  return nanF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetNaN.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
deleted file mode 100755
index 65fdaa96f..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*

- * rtGetNaN.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETNAN_H__

-#define __RTGETNAN_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetNaN(void);

-extern real32_T rtGetNaNF(void);

-

-#endif

-/* End of code generation (rtGetNaN.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
deleted file mode 100755
index 356498363..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*

- * rt_defines.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_DEFINES_H__

-#define __RT_DEFINES_H__

-

-#include <stdlib.h>

-

-#define RT_PI	3.14159265358979323846

-#define RT_PIF	3.1415927F

-#define RT_LN_10	2.30258509299404568402

-#define RT_LN_10F	2.3025851F

-#define RT_LOG10E	0.43429448190325182765

-#define RT_LOG10EF	0.43429449F

-#define RT_E	2.7182818284590452354

-#define RT_EF	2.7182817F

-#endif

-/* End of code generation (rt_defines.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
deleted file mode 100755
index 303d1d9d2..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*

- * rt_nonfinite.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *      MATLAB for code generation function to initialize non-finites,

- *      (Inf, NaN and -Inf).

- */

-#include "rt_nonfinite.h"

-#include "rtGetNaN.h"

-#include "rtGetInf.h"

-

-real_T rtInf;

-real_T rtMinusInf;

-real_T rtNaN;

-real32_T rtInfF;

-real32_T rtMinusInfF;

-real32_T rtNaNF;

-

-/* Function: rt_InitInfAndNaN ==================================================

- * Abstract:

- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the

- * generated code. NaN is initialized as non-signaling. Assumes IEEE.

- */

-void rt_InitInfAndNaN(size_t realSize)

-{

-  (void) (realSize);

-  rtNaN = rtGetNaN();

-  rtNaNF = rtGetNaNF();

-  rtInf = rtGetInf();

-  rtInfF = rtGetInfF();

-  rtMinusInf = rtGetMinusInf();

-  rtMinusInfF = rtGetMinusInfF();

-}

-

-/* Function: rtIsInf ==================================================

- * Abstract:

- * Test if value is infinite

- */

-boolean_T rtIsInf(real_T value)

-{

-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);

-}

-

-/* Function: rtIsInfF =================================================

- * Abstract:

- * Test if single-precision value is infinite

- */

-boolean_T rtIsInfF(real32_T value)

-{

-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);

-}

-

-/* Function: rtIsNaN ==================================================

- * Abstract:

- * Test if value is not a number

- */

-boolean_T rtIsNaN(real_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan(value)? TRUE:FALSE;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-/* Function: rtIsNaNF =================================================

- * Abstract:

- * Test if single-precision value is not a number

- */

-boolean_T rtIsNaNF(real32_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan((real_T)value)? true:false;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-

-/* End of code generation (rt_nonfinite.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
deleted file mode 100755
index bd56b30d9..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*

- * rt_nonfinite.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_NONFINITE_H__

-#define __RT_NONFINITE_H__

-

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-#include <float.h>

-#endif

-#include <stddef.h>

-#include "rtwtypes.h"

-

-extern real_T rtInf;

-extern real_T rtMinusInf;

-extern real_T rtNaN;

-extern real32_T rtInfF;

-extern real32_T rtMinusInfF;

-extern real32_T rtNaNF;

-extern void rt_InitInfAndNaN(size_t realSize);

-extern boolean_T rtIsInf(real_T value);

-extern boolean_T rtIsInfF(real32_T value);

-extern boolean_T rtIsNaN(real_T value);

-extern boolean_T rtIsNaNF(real32_T value);

-

-typedef struct {

-  struct {

-    uint32_T wordH;

-    uint32_T wordL;

-  } words;

-} BigEndianIEEEDouble;

-

-typedef struct {

-  struct {

-    uint32_T wordL;

-    uint32_T wordH;

-  } words;

-} LittleEndianIEEEDouble;

-

-typedef struct {

-  union {

-    real32_T wordLreal;

-    uint32_T wordLuint;

-  } wordL;

-} IEEESingle;

-

-#endif

-/* End of code generation (rt_nonfinite.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
index 9a5c96267..b5a02a7a6 100755..100644
--- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -1,159 +1,160 @@
-/*

- * rtwtypes.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTWTYPES_H__

-#define __RTWTYPES_H__

-#ifndef TRUE

-# define TRUE (1U)

-#endif

-#ifndef FALSE

-# define FALSE (0U)

-#endif

-#ifndef __TMWTYPES__

-#define __TMWTYPES__

-

-#include <limits.h>

-

-/*=======================================================================* 

- * Target hardware information

- *   Device type: Generic->MATLAB Host Computer

- *   Number of bits:     char:   8    short:   16    int:  32

- *                       long:  32      native word size:  32

- *   Byte ordering: LittleEndian

- *   Signed integer division rounds to: Zero

- *   Shift right on a signed integer as arithmetic shift: on

- *=======================================================================*/

-

-/*=======================================================================* 

- * Fixed width word size data types:                                     * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 

- *=======================================================================*/

-

-typedef signed char int8_T;

-typedef unsigned char uint8_T;

-typedef short int16_T;

-typedef unsigned short uint16_T;

-typedef int int32_T;

-typedef unsigned int uint32_T;

-typedef float real32_T;

-typedef double real64_T;

-

-/*===========================================================================* 

- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 

- *                           ulong_T, char_T and byte_T.                     * 

- *===========================================================================*/

-

-typedef double real_T;

-typedef double time_T;

-typedef unsigned char boolean_T;

-typedef int int_T;

-typedef unsigned int uint_T;

-typedef unsigned long ulong_T;

-typedef char char_T;

-typedef char_T byte_T;

-

-/*===========================================================================* 

- * Complex number type definitions                                           * 

- *===========================================================================*/

-#define CREAL_T	

-   typedef struct {  

-     real32_T re;  

-     real32_T im;  

-   } creal32_T;  

-

-   typedef struct {  

-     real64_T re;  

-     real64_T im;  

-   } creal64_T;  

-

-   typedef struct {  

-     real_T re;  

-     real_T im;  

-   } creal_T;  

-

-   typedef struct {  

-     int8_T re;  

-     int8_T im;  

-   } cint8_T;  

-

-   typedef struct {  

-     uint8_T re;  

-     uint8_T im;  

-   } cuint8_T;  

-

-   typedef struct {  

-     int16_T re;  

-     int16_T im;  

-   } cint16_T;  

-

-   typedef struct {  

-     uint16_T re;  

-     uint16_T im;  

-   } cuint16_T;  

-

-   typedef struct {  

-     int32_T re;  

-     int32_T im;  

-   } cint32_T;  

-

-   typedef struct {  

-     uint32_T re;  

-     uint32_T im;  

-   } cuint32_T;  

-

-

-/*=======================================================================* 

- * Min and Max:                                                          * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *=======================================================================*/

-

-#define MAX_int8_T  	((int8_T)(127))

-#define MIN_int8_T  	((int8_T)(-128))

-#define MAX_uint8_T 	((uint8_T)(255))

-#define MIN_uint8_T 	((uint8_T)(0))

-#define MAX_int16_T 	((int16_T)(32767))

-#define MIN_int16_T 	((int16_T)(-32768))

-#define MAX_uint16_T	((uint16_T)(65535))

-#define MIN_uint16_T	((uint16_T)(0))

-#define MAX_int32_T 	((int32_T)(2147483647))

-#define MIN_int32_T 	((int32_T)(-2147483647-1))

-#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))

-#define MIN_uint32_T	((uint32_T)(0))

-

-/* Logical type definitions */

-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)

-#  ifndef false

-#   define false (0U)

-#  endif

-#  ifndef true

-#   define true (1U)

-#  endif

-#endif

-

-/*

- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation

- * for signed integer values.

- */

-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)

-#error "This code must be compiled using a 2's complement representation for signed integer values"

-#endif

-

-/*

- * Maximum length of a MATLAB identifier (function/variable)

- * including the null-termination character. Referenced by

- * rt_logging.c and rt_matrx.c.

- */

-#define TMW_NAME_LENGTH_MAX	64

-

-#endif

-#endif

-/* End of code generation (rtwtypes.h) */

+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================* 
+ * Target hardware information
+ *   Device type: ARM Compatible->ARM Cortex
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32
+ *                       native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Undefined
+ *   Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================* 
+ * Fixed width word size data types:                                     * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================* 
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
+ *                           ulong_T, char_T and byte_T.                     * 
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================* 
+ * Complex number type definitions                                           * 
+ *===========================================================================*/
+#define CREAL_T	
+   typedef struct {  
+      real32_T re;  
+      real32_T im;  
+   } creal32_T;  
+
+   typedef struct {  
+      real64_T re;  
+      real64_T im;  
+   } creal64_T;  
+
+   typedef struct {  
+      real_T re;  
+      real_T im;  
+   } creal_T;  
+
+   typedef struct {  
+      int8_T re;  
+      int8_T im;  
+   } cint8_T;  
+
+   typedef struct {  
+      uint8_T re;  
+      uint8_T im;  
+   } cuint8_T;  
+
+   typedef struct {  
+      int16_T re;  
+      int16_T im;  
+   } cint16_T;  
+
+   typedef struct {  
+      uint16_T re;  
+      uint16_T im;  
+   } cuint16_T;  
+
+   typedef struct {  
+      int32_T re;  
+      int32_T im;  
+   } cint32_T;  
+
+   typedef struct {  
+      uint32_T re;  
+      uint32_T im;  
+   } cuint32_T;  
+
+
+/*=======================================================================* 
+ * Min and Max:                                                          * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *=======================================================================*/
+
+#define MAX_int8_T  	((int8_T)(127))
+#define MIN_int8_T  	((int8_T)(-128))
+#define MAX_uint8_T 	((uint8_T)(255))
+#define MIN_uint8_T 	((uint8_T)(0))
+#define MAX_int16_T 	((int16_T)(32767))
+#define MIN_int16_T 	((int16_T)(-32768))
+#define MAX_uint16_T	((uint16_T)(65535))
+#define MIN_uint16_T	((uint16_T)(0))
+#define MAX_int32_T 	((int32_T)(2147483647))
+#define MIN_int32_T 	((int32_T)(-2147483647-1))
+#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T	((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+#  ifndef false
+#   define false (0U)
+#  endif
+#  ifndef true
+#   define true (1U)
+#  endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX	64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */