diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile
old mode 100755
new mode 100644
index d6ec98f0bf..4d531867ce
--- a/apps/attitude_estimator_ekf/Makefile
+++ b/apps/attitude_estimator_ekf/Makefile
@@ -48,6 +48,7 @@ CSRCS		 = attitude_estimator_ekf_main.c \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
 		   codegen/diag.c \
+		   codegen/power.c \
 		   codegen/cross.c 
 
 
diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m
old mode 100755
new mode 100644
diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj
old mode 100755
new mode 100644
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
old mode 100755
new mode 100644
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
old mode 100755
new mode 100644
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
old mode 100755
new mode 100644
index 26a696af2c..885c01bf3d
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 17:51:09 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -16,6 +16,7 @@
 #include "eye.h"
 #include "mrdivide.h"
 #include "diag.h"
+#include "power.h"
 
 /* Type Definitions */
 
@@ -74,11 +75,14 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
   P_aposteriori[144])
 {
+  real32_T a[12];
+  real32_T b_a[12];
+  int32_T i;
+  real32_T Q[144];
   real32_T O[9];
   real_T dv0[9];
-  real32_T a[9];
-  int32_T i;
-  real32_T b_a[9];
+  real32_T c_a[9];
+  real32_T d_a[9];
   real32_T x_n_b[3];
   real32_T b_x_aposteriori_k[3];
   real32_T m_n_b[3];
@@ -91,69 +95,55 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   int32_T i1;
   real32_T P_apriori[144];
   real32_T f0;
-  static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F,
-    0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
-    0.1F };
-
   real32_T R[81];
   real32_T b_P_apriori[108];
-  static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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,
+  static const int8_T iv0[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];
   static const int8_T iv1[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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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 fv1[81];
+  real32_T fv0[81];
   real32_T c_P_apriori[36];
-  static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+  static const int8_T iv2[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 iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0,
+  real32_T fv1[36];
+  static const int8_T iv3[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 S_k[36];
   real32_T d_P_apriori[72];
-  static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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,
+  static const int8_T iv4[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 b_K_k[72];
   static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
-    0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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, 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_r[6];
   static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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,
     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 int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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,
+  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 };
 
-  real32_T fv3[6];
+  real32_T fv2[6];
   real32_T b_z[6];
 
   /*  Extended Attitude Kalmanfilter */
@@ -170,19 +160,26 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   /* coder.varsize('udpIndVect', [9,1], [1,0]) */
   /* udpIndVect=find(updVect); */
   /* process and measurement noise covariance matrix */
-  /* Q = diag(q.^2*dt); */
-  /* 'attitudeKalmanfilter:29' Q=[1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */
-  /* 'attitudeKalmanfilter:30'      0,     1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */
-  /* 'attitudeKalmanfilter:31'      0,     0,      1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0; */
-  /* 'attitudeKalmanfilter:32'      0,     0,      0,      1e-10,   0,      0,      1e-4,      1e-4,      0,      1e-4,      1e-4,      0; */
-  /* 'attitudeKalmanfilter:33'      0,     0,      0,      0,      1e-10,   0,      1e-4,      1e-4,      0,      1e-4,      1e-4,      0; */
-  /* 'attitudeKalmanfilter:34'      0,     0,      0,      0,      0,      1e-10,   0,      0,      0,      0,      0,      0; */
-  /* 'attitudeKalmanfilter:35'      0,     0,      0,      0,      0,      0,      1e-1,   0,      0,      0,      0,      0; */
-  /* 'attitudeKalmanfilter:36'      0,     0,      0,      0,      0,      0,      0,      1e-1,   0,      0,      0,      0; */
-  /* 'attitudeKalmanfilter:37'      0,     0,      0,      0,      0,      0,      0,      0,      1e-1,   0,      0,      0; */
-  /* 'attitudeKalmanfilter:38'      0,     0,      0,      0,      0,      0,      0,      0,      0,      1e-1,   0,      0; */
-  /* 'attitudeKalmanfilter:39'      0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      1e-1,   0; */
-  /* 'attitudeKalmanfilter:40'      0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      1e-1]; */
+  /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */
+  power(q, a);
+  for (i = 0; i < 12; i++) {
+    b_a[i] = a[i] * dt;
+  }
+
+  diag(b_a, Q);
+
+  /*   Q=[1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */
+  /*       0,     1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */
+  /*       0,     0,      1e-1,    0,      0,      0,      0,      0,      0,      0,      0,      0; */
+  /*       0,     0,      0,      1e-10,   0,      0,      1e-4,      1e-4,      0,      1e-4,      1e-4,      0; */
+  /*       0,     0,      0,      0,      1e-10,   0,      1e-4,      1e-4,      0,      1e-4,      1e-4,      0; */
+  /*       0,     0,      0,      0,      0,      1e-10,   0,      0,      0,      0,      0,      0; */
+  /*       0,     0,      0,      0,      0,      0,      1e-1,   0,      0,      0,      0,      0; */
+  /*       0,     0,      0,      0,      0,      0,      0,      1e-1,   0,      0,      0,      0; */
+  /*       0,     0,      0,      0,      0,      0,      0,      0,      1e-1,   0,      0,      0; */
+  /*       0,     0,      0,      0,      0,      0,      0,      0,      0,      1e-1,   0,      0; */
+  /*       0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      1e-1,   0; */
+  /*       0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      1e-1]; */
   /* observation matrix */
   /* 'attitudeKalmanfilter:44' wx=  x_aposteriori_k(1); */
   /* 'attitudeKalmanfilter:45' wy=  x_aposteriori_k(2); */
@@ -214,13 +211,13 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   /* 'attitudeKalmanfilter:67' 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;
+    c_a[i] = (real32_T)dv0[i] + O[i] * dt;
   }
 
   /* 'attitudeKalmanfilter:68' 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;
+    d_a[i] = (real32_T)dv0[i] + O[i] * dt;
   }
 
   /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */
@@ -250,12 +247,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   for (i = 0; i < 3; i++) {
     m_n_b[i] = 0.0F;
     for (i0 = 0; i0 < 3; i0++) {
-      m_n_b[i] += a[i + 3 * i0] * x_n_b[i0];
+      m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];
     }
 
     z_n_b[i] = 0.0F;
     for (i0 = 0; i0 < 3; i0++) {
-      z_n_b[i] += b_a[i + 3 * i0] * b_x_aposteriori_k[i0];
+      z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0];
     }
 
     x_apriori[i + 6] = m_n_b[i];
@@ -360,7 +357,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
       }
 
-      P_apriori[i + 12 * i0] = f0 + fv0[i + 12 * i0];
+      P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0];
     }
   }
 
@@ -368,10 +365,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
   if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
     /* 'attitudeKalmanfilter:103' R=diag(r); */
-    diag(r, R);
+    b_diag(r, R);
 
     /* observation matrix */
-    /* 'attitudeKalmanfilter:106' H_k=[  E,     Es,      Z,    Z; */
+    /* 'attitudeKalmanfilter:106' H_k=[  E,     Z,      Z,    Z; */
     /* 'attitudeKalmanfilter:107'         Z,     Z,      E,    Z; */
     /* 'attitudeKalmanfilter:108'         Z,     Z,      Z,    E]; */
     /* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */
@@ -403,11 +400,11 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
           f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
         }
 
-        fv1[i + 9 * i0] = f0 + R[i + 9 * i0];
+        fv0[i + 9 * i0] = f0 + R[i + 9 * i0];
       }
     }
 
-    mrdivide(b_P_apriori, fv1, K_k);
+    mrdivide(b_P_apriori, fv0, K_k);
 
     /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */
     for (i = 0; i < 9; i++) {
@@ -416,13 +413,13 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
       }
 
-      a[i] = z[i] - f0;
+      c_a[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] * a[i0];
+        f0 += K_k[i + 12 * i0] * c_a[i0];
       }
 
       x_aposteriori[i] = x_apriori[i] + f0;
@@ -437,7 +434,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
           f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
         }
 
-        b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+        Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
       }
     }
 
@@ -445,8 +442,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
       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];
+          P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0];
         }
       }
     }
@@ -455,10 +451,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
     /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
     if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
       /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */
-      b_diag(*(real32_T (*)[3])&r[0], O);
+      c_diag(*(real32_T (*)[3])&r[0], O);
 
       /* observation matrix */
-      /* 'attitudeKalmanfilter:123' H_k=[  E,     Es,      Z,    Z]; */
+      /* 'attitudeKalmanfilter:123' H_k=[  E,     Z,      Z,    Z]; */
       /* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
       /* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
       /* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
@@ -474,9 +470,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
 
       for (i = 0; i < 3; i++) {
         for (i0 = 0; i0 < 12; i0++) {
-          fv2[i + 3 * i0] = 0.0F;
+          fv1[i + 3 * i0] = 0.0F;
           for (i1 = 0; i1 < 12; i1++) {
-            fv2[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
+            fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
               i0];
           }
         }
@@ -486,14 +482,14 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         for (i0 = 0; i0 < 3; i0++) {
           f0 = 0.0F;
           for (i1 = 0; i1 < 12; i1++) {
-            f0 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
+            f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
           }
 
-          a[i + 3 * i0] = f0 + O[i + 3 * i0];
+          c_a[i + 3 * i0] = f0 + O[i + 3 * i0];
         }
       }
 
-      b_mrdivide(c_P_apriori, a, S_k);
+      b_mrdivide(c_P_apriori, c_a, S_k);
 
       /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */
       for (i = 0; i < 3; i++) {
@@ -523,7 +519,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
             f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
           }
 
-          b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+          Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
         }
       }
 
@@ -531,8 +527,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         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];
+            P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
+              i0];
           }
         }
       }
@@ -542,10 +538,10 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
       if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
       {
         /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */
-        c_diag(*(real32_T (*)[6])&r[0], S_k);
+        d_diag(*(real32_T (*)[6])&r[0], S_k);
 
         /* observation matrix */
-        /* 'attitudeKalmanfilter:138' H_k=[  E,     Es,      Z,    Z; */
+        /* 'attitudeKalmanfilter:138' H_k=[  E,     Z,      Z,    Z; */
         /* 'attitudeKalmanfilter:139'                     Z,     Z,      E,    Z]; */
         /* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
         /* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
@@ -577,11 +573,11 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
               f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
             }
 
-            fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0];
+            fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0];
           }
         }
 
-        c_mrdivide(d_P_apriori, fv2, b_K_k);
+        c_mrdivide(d_P_apriori, fv1, b_K_k);
 
         /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */
         for (i = 0; i < 6; i++) {
@@ -611,7 +607,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
               f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
             }
 
-            b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+            Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
           }
         }
 
@@ -619,8 +615,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
           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];
+              P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 *
+                i0];
             }
           }
         }
@@ -631,7 +627,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
         {
           /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */
           /* observation matrix */
-          /* 'attitudeKalmanfilter:154' H_k=[  E,     Es,      Z,    Z; */
+          /* 'attitudeKalmanfilter:154' H_k=[  E,     Z,      Z,    Z; */
           /* 'attitudeKalmanfilter:155'                     Z,     Z,      Z,    E]; */
           /* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
           /* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
@@ -684,12 +680,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
           }
 
           for (i = 0; i < 6; i++) {
-            fv3[i] = 0.0F;
+            fv2[i] = 0.0F;
             for (i0 = 0; i0 < 12; i0++) {
-              fv3[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
+              fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
             }
 
-            b_z[i] = b_r[i] - fv3[i];
+            b_z[i] = b_r[i] - fv2[i];
           }
 
           for (i = 0; i < 12; i++) {
@@ -710,7 +706,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
                 f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
               }
 
-              b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+              Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
             }
           }
 
@@ -718,8 +714,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
             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];
+                P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12
+                  * i0];
               }
             }
           }
@@ -785,7 +781,8 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
   /* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
   /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */
   eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
-  eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
+  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/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
old mode 100755
new mode 100644
index 9d89705a6f..c93d7f4bbc
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
old mode 100755
new mode 100644
index 27eb2763e5..67b6fa4229
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
old mode 100755
new mode 100644
index 277df273df..610924d757
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
old mode 100755
new mode 100644
index ed09930394..c896027237
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
old mode 100755
new mode 100644
index 0b0ccd0730..a196307759
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
old mode 100755
new mode 100644
index 95814863fe..eba83d0d99
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c
old mode 100755
new mode 100644
index d4524fed8e..42cd709715
--- a/apps/attitude_estimator_ekf/codegen/cross.c
+++ b/apps/attitude_estimator_ekf/codegen/cross.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h
old mode 100755
new mode 100644
index d70b0adc24..c90d6b3a5c
--- a/apps/attitude_estimator_ekf/codegen/cross.h
+++ b/apps/attitude_estimator_ekf/codegen/cross.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c
old mode 100755
new mode 100644
index 58e9f553f4..7c28e873a3
--- a/apps/attitude_estimator_ekf/codegen/diag.c
+++ b/apps/attitude_estimator_ekf/codegen/diag.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'diag'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -27,7 +27,19 @@
 /*
  *
  */
-void b_diag(const real32_T v[3], real32_T d[9])
+void b_diag(const real32_T v[9], real32_T d[81])
+{
+  int32_T j;
+  memset(&d[0], 0, 81U * sizeof(real32_T));
+  for (j = 0; j < 9; j++) {
+    d[j + 9 * j] = v[j];
+  }
+}
+
+/*
+ *
+ */
+void c_diag(const real32_T v[3], real32_T d[9])
 {
   int32_T j;
   for (j = 0; j < 9; j++) {
@@ -42,7 +54,7 @@ void b_diag(const real32_T v[3], real32_T d[9])
 /*
  *
  */
-void c_diag(const real32_T v[6], real32_T d[36])
+void d_diag(const real32_T v[6], real32_T d[36])
 {
   int32_T j;
   memset(&d[0], 0, 36U * sizeof(real32_T));
@@ -54,12 +66,12 @@ void c_diag(const real32_T v[6], real32_T d[36])
 /*
  *
  */
-void diag(const real32_T v[9], real32_T d[81])
+void diag(const real32_T v[12], real32_T d[144])
 {
   int32_T j;
-  memset(&d[0], 0, 81U * sizeof(real32_T));
-  for (j = 0; j < 9; j++) {
-    d[j + 9 * j] = v[j];
+  memset(&d[0], 0, 144U * sizeof(real32_T));
+  for (j = 0; j < 12; j++) {
+    d[j + 12 * j] = v[j];
   }
 }
 
diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h
old mode 100755
new mode 100644
index 85ff0e0e90..98b411c2d1
--- a/apps/attitude_estimator_ekf/codegen/diag.h
+++ b/apps/attitude_estimator_ekf/codegen/diag.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'diag'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -29,8 +29,9 @@
 /* Variable Definitions */
 
 /* Function Declarations */
-extern void b_diag(const real32_T v[3], real32_T d[9]);
-extern void c_diag(const real32_T v[6], real32_T d[36]);
-extern void diag(const real32_T v[9], real32_T d[81]);
+extern void b_diag(const real32_T v[9], real32_T d[81]);
+extern void c_diag(const real32_T v[3], real32_T d[9]);
+extern void d_diag(const real32_T v[6], real32_T d[36]);
+extern void diag(const real32_T v[12], real32_T d[144]);
 #endif
 /* End of code generation (diag.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c
old mode 100755
new mode 100644
index d7ae20afda..dbd44c6a82
--- a/apps/attitude_estimator_ekf/codegen/eye.c
+++ b/apps/attitude_estimator_ekf/codegen/eye.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h
old mode 100755
new mode 100644
index 77099ec22e..325fd23ec2
--- a/apps/attitude_estimator_ekf/codegen/eye.h
+++ b/apps/attitude_estimator_ekf/codegen/eye.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c
old mode 100755
new mode 100644
index 1c69f1ef77..0a540775a0
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.c
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h
old mode 100755
new mode 100644
index 75a3968783..d286eda5ac
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.h
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c
old mode 100755
new mode 100644
index fbd5d43e07..deb71dc602
--- a/apps/attitude_estimator_ekf/codegen/norm.c
+++ b/apps/attitude_estimator_ekf/codegen/norm.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h
old mode 100755
new mode 100644
index a2e9d90d45..3459cbdb5f
--- a/apps/attitude_estimator_ekf/codegen/norm.h
+++ b/apps/attitude_estimator_ekf/codegen/norm.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c
new file mode 100644
index 0000000000..fc602fb5ce
--- /dev/null
+++ b/apps/attitude_estimator_ekf/codegen/power.c
@@ -0,0 +1,84 @@
+/*
+ * power.c
+ *
+ * Code generation for function 'power'
+ *
+ * C source code generated on: Tue Oct 16 15:27:58 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "power.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1)
+{
+  real32_T y;
+  real32_T f1;
+  real32_T f2;
+  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+    y = ((real32_T)rtNaN);
+  } else {
+    f1 = (real32_T)fabs(u0);
+    f2 = (real32_T)fabs(u1);
+    if (rtIsInfF(u1)) {
+      if (f1 == 1.0F) {
+        y = ((real32_T)rtNaN);
+      } else if (f1 > 1.0F) {
+        if (u1 > 0.0F) {
+          y = ((real32_T)rtInf);
+        } else {
+          y = 0.0F;
+        }
+      } else if (u1 > 0.0F) {
+        y = 0.0F;
+      } else {
+        y = ((real32_T)rtInf);
+      }
+    } else if (f2 == 0.0F) {
+      y = 1.0F;
+    } else if (f2 == 1.0F) {
+      if (u1 > 0.0F) {
+        y = u0;
+      } else {
+        y = 1.0F / u0;
+      }
+    } else if (u1 == 2.0F) {
+      y = u0 * u0;
+    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
+      y = (real32_T)sqrt(u0);
+    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
+      y = ((real32_T)rtNaN);
+    } else {
+      y = (real32_T)pow(u0, u1);
+    }
+  }
+
+  return y;
+}
+
+/*
+ *
+ */
+void power(const real32_T a[12], real32_T y[12])
+{
+  int32_T k;
+  for (k = 0; k < 12; k++) {
+    y[k] = rt_powf_snf(a[k], 2.0F);
+  }
+}
+
+/* End of code generation (power.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/power.h b/apps/attitude_estimator_ekf/codegen/power.h
new file mode 100644
index 0000000000..40a0d9353f
--- /dev/null
+++ b/apps/attitude_estimator_ekf/codegen/power.h
@@ -0,0 +1,34 @@
+/*
+ * power.h
+ *
+ * Code generation for function 'power'
+ *
+ * C source code generated on: Tue Oct 16 15:27:58 2012
+ *
+ */
+
+#ifndef __POWER_H__
+#define __POWER_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 power(const real32_T a[12], real32_T y[12]);
+#endif
+/* End of code generation (power.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c
old mode 100755
new mode 100644
index 32d75bf28d..11e4e6f1f8
--- a/apps/attitude_estimator_ekf/codegen/rdivide.c
+++ b/apps/attitude_estimator_ekf/codegen/rdivide.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'rdivide'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h
old mode 100755
new mode 100644
index e488ed87f9..4e24303748
--- a/apps/attitude_estimator_ekf/codegen/rdivide.h
+++ b/apps/attitude_estimator_ekf/codegen/rdivide.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'rdivide'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
old mode 100755
new mode 100644
index 5500ef3732..d20fa13fca
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
old mode 100755
new mode 100644
index bfef3881ea..83a3558735
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
old mode 100755
new mode 100644
index e5113aef0b..d357102afd
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
old mode 100755
new mode 100644
index 28f35e1f13..415927709b
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h
old mode 100755
new mode 100644
index 3ce17dd0fc..608391608f
--- a/apps/attitude_estimator_ekf/codegen/rt_defines.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
old mode 100755
new mode 100644
index 6f58e7265e..69069562b8
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
old mode 100755
new mode 100644
index 5c2a0aa619..9686964ae5
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
old mode 100755
new mode 100644
index 4fbb93f578..77fd3ec7ae
--- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Sat Oct 13 16:28:18 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
old mode 100755
new mode 100644
index 63c7d0097d..cb82036fe8
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -172,7 +172,7 @@ mc_thread_main(int argc, char *argv[])
 						rates_sp.pitch = offboard_sp.p2;
 						rates_sp.yaw = offboard_sp.p3;
 						rates_sp.thrust = offboard_sp.p4;
-						//printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
 						rates_sp.timestamp = hrt_absolute_time();
 						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
@@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.pitch_body = offboard_sp.p2;
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
-						//printf("thrust_att=%8.4f\n",offboard_sp.p4);
+//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
 						att_sp.timestamp = hrt_absolute_time();
 						/* STEP 2: publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -193,6 +193,7 @@ mc_thread_main(int argc, char *argv[])
 
 			if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
 				rates_sp.roll = manual.roll;
+
 				rates_sp.pitch = manual.pitch;
 				rates_sp.yaw = manual.yaw;
 				rates_sp.thrust = manual.throttle;
@@ -203,7 +204,7 @@ mc_thread_main(int argc, char *argv[])
 			if (state.flag_control_attitude_enabled) {
 				att_sp.roll_body = manual.roll;
 				att_sp.pitch_body = manual.pitch;
-				att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
+				att_sp.yaw_body = manual.yaw; // XXX Hack, clean up
 				/* set yaw rate */
 				rates_sp.yaw = manual.yaw;
 				att_sp.thrust = manual.throttle;
@@ -226,21 +227,15 @@ mc_thread_main(int argc, char *argv[])
 		}
 
 		/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
-
-
-		/* run attitude controller */
-		 if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
-		 	multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
-		 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		 } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
+		if (state.flag_control_attitude_enabled) {
 		 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
 		 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-		 }
+		}
 
 
 		if (state.flag_control_rates_enabled) {
 
-			float gyro[3] = {0.0f, 0.0f, 0.0f};
+			float gyro[3];
 
 			/* get current rate setpoint */
 			bool rates_sp_valid = false;
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
old mode 100755
new mode 100644
index 458b86057b..00b759d73a
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -54,17 +54,13 @@
 #include <systemlib/param/param.h>
 #include <arch/board/up_hrt.h>
 
-// PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
-// PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
-// PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
-// PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
-// PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
+
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
 
 PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
@@ -76,17 +72,17 @@ PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
 
 struct mc_att_control_params {
-	// float yaw_p;
-	// float yaw_i;
-	// float yaw_d;
-	// float yaw_awu;
-	// float yaw_lim;
+	float yaw_p;
+	float yaw_i;
+	float yaw_d;
+	float yaw_awu;
+	float yaw_lim;
 
-	float yawrate_p;
-	float yawrate_i;
-	float yawrate_d;
-	float yawrate_awu;
-	float yawrate_lim;
+	//	float yawrate_p;
+	//	float yawrate_i;
+	//	float yawrate_d;
+	//	float yawrate_awu;
+	//	float yawrate_lim;
 
 	float att_p;
 	float att_i;
@@ -99,17 +95,17 @@ struct mc_att_control_params {
 };
 
 struct mc_att_control_param_handles {
-	// param_t yaw_p;
-	// param_t yaw_i;
-	// param_t yaw_d;
-	// param_t yaw_awu;
-	// param_t yaw_lim;
+	param_t yaw_p;
+	param_t yaw_i;
+	param_t yaw_d;
+	param_t yaw_awu;
+	param_t yaw_lim;
 
-	param_t yawrate_p;
-	param_t yawrate_i;
-	param_t yawrate_d;
-	param_t yawrate_awu;
-	param_t yawrate_lim;
+	//	param_t yawrate_p;
+	//	param_t yawrate_i;
+	//	param_t yawrate_d;
+	//	param_t yawrate_awu;
+	//	param_t yawrate_lim;
 
 	param_t att_p;
 	param_t att_i;
@@ -137,17 +133,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 static int parameters_init(struct mc_att_control_param_handles *h)
 {
 	/* PID parameters */
-	// h->yaw_p 	=	param_find("MC_YAWPOS_P");
-	// h->yaw_i 	=	param_find("MC_YAWPOS_I");
-	// h->yaw_d 	=	param_find("MC_YAWPOS_D");
-	// h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
-	// h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
+	h->yaw_p 	=	param_find("MC_YAWPOS_P");
+	h->yaw_i 	=	param_find("MC_YAWPOS_I");
+	h->yaw_d 	=	param_find("MC_YAWPOS_D");
+	h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
+	h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
 
-	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
-	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
-	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-	h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
-	h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
+	//	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
+	//	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
+	//	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
+	//	h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
+	//	h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
 
 	h->att_p 	= 	param_find("MC_ATT_P");
 	h->att_i 	= 	param_find("MC_ATT_I");
@@ -163,17 +159,17 @@ static int parameters_init(struct mc_att_control_param_handles *h)
 
 static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p)
 {
-	// param_get(h->yaw_p, &(p->yaw_p));
-	// param_get(h->yaw_i, &(p->yaw_i));
-	// param_get(h->yaw_d, &(p->yaw_d));
-	// param_get(h->yaw_awu, &(p->yaw_awu));
-	// param_get(h->yaw_lim, &(p->yaw_lim));
+	param_get(h->yaw_p, &(p->yaw_p));
+	param_get(h->yaw_i, &(p->yaw_i));
+	param_get(h->yaw_d, &(p->yaw_d));
+	param_get(h->yaw_awu, &(p->yaw_awu));
+	param_get(h->yaw_lim, &(p->yaw_lim));
 
-	param_get(h->yawrate_p, &(p->yawrate_p));
-	param_get(h->yawrate_i, &(p->yawrate_i));
-	param_get(h->yawrate_d, &(p->yawrate_d));
-	param_get(h->yawrate_awu, &(p->yawrate_awu));
-	param_get(h->yawrate_lim, &(p->yawrate_lim));
+	//	param_get(h->yawrate_p, &(p->yawrate_p));
+	//	param_get(h->yawrate_i, &(p->yawrate_i));
+	//	param_get(h->yawrate_d, &(p->yawrate_d));
+	//	param_get(h->yawrate_awu, &(p->yawrate_awu));
+	//	param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	param_get(h->att_p, &(p->att_p));
 	param_get(h->att_i, &(p->att_i));
@@ -188,16 +184,23 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
+		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
 {
 	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+	static uint64_t last_input = 0;
+	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
 	last_run = hrt_absolute_time();
+	if (last_input != att_sp->timestamp) {
+		last_input = att_sp->timestamp;
+	}
+	static int sensor_delay;
+	sensor_delay = hrt_absolute_time() - att->timestamp;
 
 	static int motor_skip_counter = 0;
 
 	// static PID_t yaw_pos_controller;
-	static PID_t yaw_speed_controller;
+//	static PID_t yaw_speed_controller;
 	static PID_t pitch_controller;
 	static PID_t roll_controller;
 
@@ -213,112 +216,44 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 		// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
 		// 	PID_MODE_DERIVATIV_SET, 154);
-		pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
-			PID_MODE_DERIVATIV_SET);
+		//		pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
+		//			PID_MODE_DERIVATIV_SET);
 		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
-			PID_MODE_DERIVATIV_SET);
+				PID_MODE_DERIVATIV_SET);
 		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
-			PID_MODE_DERIVATIV_SET);
+				PID_MODE_DERIVATIV_SET);
 
 		initialized = true;
 	}
 
 	/* load new parameters with lower rate */
-	if (motor_skip_counter % 50 == 0) {
+	if (motor_skip_counter % 1000 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 		/* apply parameters */
 		// pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
-		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
+		//		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
+		printf("delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
 	}
 
 	/* calculate current control outputs */
-	
+
 	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
-					att->pitch, att->pitchspeed, deltaT)*1/10.0f;
+	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
+			att->pitch, att->pitchspeed, deltaT)*1/10.0f;
 	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
-					att->roll, att->rollspeed, deltaT)*1/10.0f;
+	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
+			att->roll, att->rollspeed, deltaT)*1/10.0f;
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
-
-	/*
-	 * compensate the vertical loss of thrust
-	 * when thrust plane has an angle.
-	 * start with a factor of 1.0 (no change)
-	 */
-	float zcompensation = 1.0f;
-
-	if (fabsf(att->roll) > 0.3f) {
-		zcompensation *= 1.04675160154f;
-
-	} else {
-		zcompensation *= 1.0f / cosf(att->roll);
-	}
-
-	if (fabsf(att->pitch) > 0.3f) {
-		zcompensation *= 1.04675160154f;
-
-	} else {
-		zcompensation *= 1.0f / cosf(att->pitch);
-	}
-
-	float motor_thrust = 0.0f;
-
-	motor_thrust = att_sp->thrust;
-
-	/* compensate thrust vector for roll / pitch contributions */
-	motor_thrust *= zcompensation;
-
-	/* limit yaw rate output */
-	if (yaw_rate_control > p.yawrate_lim) {
-		yaw_rate_control = p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (yaw_rate_control < -p.yawrate_lim) {
-		yaw_rate_control = -p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (pitch_control > p.att_lim) {
-		pitch_control = p.att_lim;
-		pitch_controller.saturated = 1;
-	}
-
-	if (pitch_control < -p.att_lim) {
-		pitch_control = -p.att_lim;
-		pitch_controller.saturated = 1;
-	}
+	//float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
+	rates_sp->yaw= deltaT*p.yaw_p*atan2f(sinf(att->yaw-att_sp->yaw_body),cos(att->yaw-att_sp->yaw_body));
 
 
-	if (roll_control > p.att_lim) {
-		roll_control = p.att_lim;
-		roll_controller.saturated = 1;
-	}
 
-	if (roll_control < -p.att_lim) {
-		roll_control = -p.att_lim;
-		roll_controller.saturated = 1;
-	}
+	rates_sp->thrust = att_sp->thrust;
 
-	if (actuators) {
-		actuators->control[0] = roll_control;
-		actuators->control[1] = pitch_control;
-		actuators->control[2] = yaw_rate_control;
-		actuators->control[3] = motor_thrust;
-	}
-
-	// XXX change yaw rate to yaw pos controller
-	if (rates_sp) {
-		rates_sp->roll = roll_control;
-		rates_sp->pitch = pitch_control;
-		rates_sp->yaw = yaw_rate_control;
-		rates_sp->thrust = motor_thrust;
-	}
 
 	motor_skip_counter++;
 }
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
old mode 100755
new mode 100644
diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h
old mode 100755
new mode 100644