Fresh import of the PX4 firmware sources.

19 files changed, 1837 insertions, 0 deletions

diff --git a/apps/position_estimator/.context b/apps/position_estimator/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/position_estimator/.context
diff --git a/apps/position_estimator/Makefile b/apps/position_estimator/Makefile
new file mode 100644
index 000000000..a766f1666
--- /dev/null
+++ b/apps/position_estimator/Makefile
@@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the position estimator
+#
+
+APPNAME		 = position_estimator
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 4096
+
+CSRCS		 = position_estimator_main.c \
+		   codegen/position_estimator.c \
+		   codegen/position_estimator_initialize.c \
+		   codegen/position_estimator_terminate.c \
+		   codegen/rt_nonfinite.c \
+		   codegen/rtGetInf.c \
+		   codegen/rtGetNaN.c
+
+INCLUDES	 = $(TOPDIR)/../mavlink/include/mavlink
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/position_estimator/codegen/position_estimator.c b/apps/position_estimator/codegen/position_estimator.c
new file mode 100644
index 000000000..731ae03e3
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator.c
@@ -0,0 +1,261 @@
+/*
+ * position_estimator.c
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "position_estimator.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T
+  xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T
+  predict_only, real32_T xapo1[6], real32_T Papo1[36])
+{
+  real32_T fv0[6];
+  real32_T fv1[6];
+  real32_T I[36];
+  real32_T xapri[6];
+  int32_T i;
+  int32_T r1;
+  static const real32_T fv2[36] = { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.004F,
+    1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+    0.004F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F,
+    0.0F, 0.0F, 0.004F, 1.0F };
+
+  static const real32_T fv3[12] = { 0.0F, 0.0F, 0.1744F, 87.2F, 0.0F, 0.0F,
+    -0.1744F, -87.2F, 0.0F, 0.0F, 0.0F, 0.0F };
+
+  int32_T r2;
+  real32_T Papri[36];
+  real32_T maxval;
+  static const real32_T fv4[36] = { 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+    1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, 0.0F, 0.0F,
+    0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F,
+    0.0F, 0.0F, 0.0F, 0.0F, 1.0F };
+
+  static const real32_T fv5[36] = { 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+    1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F,
+    0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F,
+    0.0F, 0.0F, 0.0F, 0.0F, 1.0F };
+
+  real32_T K[18];
+  static const int8_T iv0[18] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0 };
+
+  real32_T fv6[9];
+  static const int8_T iv1[18] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+    1, 0 };
+
+  real32_T b_gps_covariance[9];
+  real32_T A[9];
+  real32_T B[18];
+  int32_T r3;
+  real32_T a21;
+  real32_T Y[18];
+  real32_T b_z[3];
+  int8_T b_I[36];
+
+  /* if predit_onli == 1: no update step: use this when no new gps data is available */
+  /* %initialization */
+  /* use model F=m*a x''=F/m */
+  /*  250Hz---> dT = 0.004s */
+  /* u=[phi;theta] */
+  /* x=[px;vx;py;vy]; */
+  /* %------------------------------------------ */
+  /* %------------------------------------------------ */
+  /* R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; */
+  /* process Covariance Matrix */
+  /* measurement Covariance Matrix */
+  /* %prediction */
+  for (i = 0; i < 6; i++) {
+    fv0[i] = 0.0F;
+    for (r1 = 0; r1 < 6; r1++) {
+      fv0[i] += fv2[i + 6 * r1] * xapo[r1];
+    }
+
+    fv1[i] = 0.0F;
+    for (r1 = 0; r1 < 2; r1++) {
+      fv1[i] += fv3[i + 6 * r1] * u[r1];
+    }
+
+    xapri[i] = fv0[i] + fv1[i];
+    for (r1 = 0; r1 < 6; r1++) {
+      I[i + 6 * r1] = 0.0F;
+      for (r2 = 0; r2 < 6; r2++) {
+        I[i + 6 * r1] += fv2[i + 6 * r2] * Papo[r2 + 6 * r1];
+      }
+    }
+  }
+
+  for (i = 0; i < 6; i++) {
+    for (r1 = 0; r1 < 6; r1++) {
+      maxval = 0.0F;
+      for (r2 = 0; r2 < 6; r2++) {
+        maxval += I[i + 6 * r2] * fv4[r2 + 6 * r1];
+      }
+
+      Papri[i + 6 * r1] = maxval + fv5[i + 6 * r1];
+    }
+  }
+
+  if (1 != predict_only) {
+    /* update */
+    for (i = 0; i < 3; i++) {
+      for (r1 = 0; r1 < 6; r1++) {
+        K[i + 3 * r1] = 0.0F;
+        for (r2 = 0; r2 < 6; r2++) {
+          K[i + 3 * r1] += (real32_T)iv0[i + 3 * r2] * Papri[r2 + 6 * r1];
+        }
+      }
+    }
+
+    for (i = 0; i < 3; i++) {
+      for (r1 = 0; r1 < 3; r1++) {
+        fv6[i + 3 * r1] = 0.0F;
+        for (r2 = 0; r2 < 6; r2++) {
+          fv6[i + 3 * r1] += K[r1 + 3 * r2] * (real32_T)iv1[r2 + 6 * i];
+        }
+      }
+    }
+
+    b_gps_covariance[0] = gps_covariance[0];
+    b_gps_covariance[1] = 0.0F;
+    b_gps_covariance[2] = 0.0F;
+    b_gps_covariance[3] = 0.0F;
+    b_gps_covariance[4] = gps_covariance[1];
+    b_gps_covariance[5] = 0.0F;
+    b_gps_covariance[6] = 0.0F;
+    b_gps_covariance[7] = 0.0F;
+    b_gps_covariance[8] = gps_covariance[2];
+    for (i = 0; i < 3; i++) {
+      for (r1 = 0; r1 < 3; r1++) {
+        A[r1 + 3 * i] = fv6[r1 + 3 * i] + b_gps_covariance[r1 + 3 * i];
+      }
+
+      for (r1 = 0; r1 < 6; r1++) {
+        B[i + 3 * r1] = 0.0F;
+        for (r2 = 0; r2 < 6; r2++) {
+          B[i + 3 * r1] += Papri[r1 + 6 * r2] * (real32_T)iv1[r2 + 6 * i];
+        }
+      }
+    }
+
+    r1 = 0;
+    r2 = 1;
+    r3 = 2;
+    maxval = (real32_T)fabs(A[0]);
+    a21 = (real32_T)fabs(A[1]);
+    if (a21 > maxval) {
+      maxval = a21;
+      r1 = 1;
+      r2 = 0;
+    }
+
+    if ((real32_T)fabs(A[2]) > maxval) {
+      r1 = 2;
+      r2 = 1;
+      r3 = 0;
+    }
+
+    A[r2] /= A[r1];
+    A[r3] /= A[r1];
+    A[3 + r2] -= A[r2] * A[3 + r1];
+    A[3 + r3] -= A[r3] * A[3 + r1];
+    A[6 + r2] -= A[r2] * A[6 + r1];
+    A[6 + r3] -= A[r3] * A[6 + r1];
+    if ((real32_T)fabs(A[3 + r3]) > (real32_T)fabs(A[3 + r2])) {
+      i = r2;
+      r2 = r3;
+      r3 = i;
+    }
+
+    A[3 + r3] /= A[3 + r2];
+    A[6 + r3] -= A[3 + r3] * A[6 + r2];
+    for (i = 0; i < 6; i++) {
+      Y[3 * i] = B[r1 + 3 * i];
+      Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * A[r2];
+      Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * A[r3]) - Y[1 + 3 * i] * A[3 +
+        r3];
+      Y[2 + 3 * i] /= A[6 + r3];
+      Y[3 * i] -= Y[2 + 3 * i] * A[6 + r1];
+      Y[1 + 3 * i] -= Y[2 + 3 * i] * A[6 + r2];
+      Y[1 + 3 * i] /= A[3 + r2];
+      Y[3 * i] -= Y[1 + 3 * i] * A[3 + r1];
+      Y[3 * i] /= A[r1];
+    }
+
+    for (i = 0; i < 3; i++) {
+      for (r1 = 0; r1 < 6; r1++) {
+        K[r1 + 6 * i] = Y[i + 3 * r1];
+      }
+    }
+
+    for (i = 0; i < 3; i++) {
+      maxval = 0.0F;
+      for (r1 = 0; r1 < 6; r1++) {
+        maxval += (real32_T)iv0[i + 3 * r1] * xapri[r1];
+      }
+
+      b_z[i] = z[i] - maxval;
+    }
+
+    for (i = 0; i < 6; i++) {
+      maxval = 0.0F;
+      for (r1 = 0; r1 < 3; r1++) {
+        maxval += K[i + 6 * r1] * b_z[r1];
+      }
+
+      xapo1[i] = xapri[i] + maxval;
+    }
+
+    for (i = 0; i < 36; i++) {
+      b_I[i] = 0;
+    }
+
+    for (i = 0; i < 6; i++) {
+      b_I[i + 6 * i] = 1;
+    }
+
+    for (i = 0; i < 6; i++) {
+      for (r1 = 0; r1 < 6; r1++) {
+        maxval = 0.0F;
+        for (r2 = 0; r2 < 3; r2++) {
+          maxval += K[i + 6 * r2] * (real32_T)iv0[r2 + 3 * r1];
+        }
+
+        I[i + 6 * r1] = (real32_T)b_I[i + 6 * r1] - maxval;
+      }
+    }
+
+    for (i = 0; i < 6; i++) {
+      for (r1 = 0; r1 < 6; r1++) {
+        Papo1[i + 6 * r1] = 0.0F;
+        for (r2 = 0; r2 < 6; r2++) {
+          Papo1[i + 6 * r1] += I[i + 6 * r2] * Papri[r2 + 6 * r1];
+        }
+      }
+    }
+  } else {
+    memcpy((void *)&Papo1[0], (void *)&Papri[0], 36U * sizeof(real32_T));
+    for (i = 0; i < 6; i++) {
+      xapo1[i] = xapri[i];
+    }
+  }
+}
+
+/* End of code generation (position_estimator.c) */
diff --git a/apps/position_estimator/codegen/position_estimator.h b/apps/position_estimator/codegen/position_estimator.h
new file mode 100644
index 000000000..5e2c9acd8
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator.h
@@ -0,0 +1,32 @@
+/*
+ * position_estimator.h
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#ifndef __POSITION_ESTIMATOR_H__
+#define __POSITION_ESTIMATOR_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "position_estimator_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T predict_only, real32_T xapo1[6], real32_T Papo1[36]);
+#endif
+/* End of code generation (position_estimator.h) */
diff --git a/apps/position_estimator/codegen/position_estimator_initialize.c b/apps/position_estimator/codegen/position_estimator_initialize.c
new file mode 100644
index 000000000..862381d3b
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * position_estimator_initialize.c
+ *
+ * Code generation for function 'position_estimator_initialize'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "position_estimator.h"
+#include "position_estimator_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void position_estimator_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (position_estimator_initialize.c) */
diff --git a/apps/position_estimator/codegen/position_estimator_initialize.h b/apps/position_estimator/codegen/position_estimator_initialize.h
new file mode 100644
index 000000000..71013b390
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator_initialize.h
@@ -0,0 +1,32 @@
+/*
+ * position_estimator_initialize.h
+ *
+ * Code generation for function 'position_estimator_initialize'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#ifndef __POSITION_ESTIMATOR_INITIALIZE_H__
+#define __POSITION_ESTIMATOR_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "position_estimator_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void position_estimator_initialize(void);
+#endif
+/* End of code generation (position_estimator_initialize.h) */
diff --git a/apps/position_estimator/codegen/position_estimator_terminate.c b/apps/position_estimator/codegen/position_estimator_terminate.c
new file mode 100644
index 000000000..b25aabf40
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * position_estimator_terminate.c
+ *
+ * Code generation for function 'position_estimator_terminate'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "position_estimator.h"
+#include "position_estimator_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void position_estimator_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (position_estimator_terminate.c) */
diff --git a/apps/position_estimator/codegen/position_estimator_terminate.h b/apps/position_estimator/codegen/position_estimator_terminate.h
new file mode 100644
index 000000000..d9fcf838b
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator_terminate.h
@@ -0,0 +1,32 @@
+/*
+ * position_estimator_terminate.h
+ *
+ * Code generation for function 'position_estimator_terminate'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#ifndef __POSITION_ESTIMATOR_TERMINATE_H__
+#define __POSITION_ESTIMATOR_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "position_estimator_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void position_estimator_terminate(void);
+#endif
+/* End of code generation (position_estimator_terminate.h) */
diff --git a/apps/position_estimator/codegen/position_estimator_types.h b/apps/position_estimator/codegen/position_estimator_types.h
new file mode 100644
index 000000000..cb01c7426
--- /dev/null
+++ b/apps/position_estimator/codegen/position_estimator_types.h
@@ -0,0 +1,16 @@
+/*
+ * position_estimator_types.h
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#ifndef __POSITION_ESTIMATOR_TYPES_H__
+#define __POSITION_ESTIMATOR_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (position_estimator_types.h) */
diff --git a/apps/position_estimator/codegen/rtGetInf.c b/apps/position_estimator/codegen/rtGetInf.c
new file mode 100644
index 000000000..20a64117c
--- /dev/null
+++ b/apps/position_estimator/codegen/rtGetInf.c
@@ -0,0 +1,139 @@
+/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+/*
+ * 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/apps/position_estimator/codegen/rtGetInf.h b/apps/position_estimator/codegen/rtGetInf.h
new file mode 100644
index 000000000..c74f1fc28
--- /dev/null
+++ b/apps/position_estimator/codegen/rtGetInf.h
@@ -0,0 +1,23 @@
+/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#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/apps/position_estimator/codegen/rtGetNaN.c b/apps/position_estimator/codegen/rtGetNaN.c
new file mode 100644
index 000000000..6aa2f4639
--- /dev/null
+++ b/apps/position_estimator/codegen/rtGetNaN.c
@@ -0,0 +1,96 @@
+/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+/*
+ * 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/apps/position_estimator/codegen/rtGetNaN.h b/apps/position_estimator/codegen/rtGetNaN.h
new file mode 100644
index 000000000..d3ec61c9e
--- /dev/null
+++ b/apps/position_estimator/codegen/rtGetNaN.h
@@ -0,0 +1,21 @@
+/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#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/apps/position_estimator/codegen/rt_nonfinite.c b/apps/position_estimator/codegen/rt_nonfinite.c
new file mode 100644
index 000000000..40e15fd22
--- /dev/null
+++ b/apps/position_estimator/codegen/rt_nonfinite.c
@@ -0,0 +1,87 @@
+/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+/*
+ * 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/apps/position_estimator/codegen/rt_nonfinite.h b/apps/position_estimator/codegen/rt_nonfinite.h
new file mode 100644
index 000000000..ac9660124
--- /dev/null
+++ b/apps/position_estimator/codegen/rt_nonfinite.h
@@ -0,0 +1,53 @@
+/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#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/apps/position_estimator/codegen/rtwtypes.h b/apps/position_estimator/codegen/rtwtypes.h
new file mode 100644
index 000000000..e87ae1fdc
--- /dev/null
+++ b/apps/position_estimator/codegen/rtwtypes.h
@@ -0,0 +1,175 @@
+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'position_estimator'
+ *
+ * C source code generated on: Fri Jun  8 13:31:21 2012
+ *
+ */
+
+#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:  64      native word size:  64
+ *   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 long int64_T;
+typedef unsigned long uint64_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 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;  
+
+   typedef struct {  
+     int64_T re;  
+     int64_T im;  
+   } cint64_T;  
+
+   typedef struct {  
+     uint64_T re;  
+     uint64_T im;  
+   } cuint64_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))
+#define MAX_int64_T	((int64_T)(9223372036854775807L))
+#define MIN_int64_T	((int64_T)(-9223372036854775807L-1L))
+#define MAX_uint64_T	((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
+#define MIN_uint64_T	((uint64_T)(0UL))
+
+/* 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) */
diff --git a/apps/position_estimator/position_estimator.m b/apps/position_estimator/position_estimator.m
new file mode 100644
index 000000000..2ef4d8b06
--- /dev/null
+++ b/apps/position_estimator/position_estimator.m
@@ -0,0 +1,62 @@
+function [xapo1,Papo1] = position_estimator(u,z,xapo,Papo,gps_covariance,predict_only) %if predit_onli == 1: no update step: use this when no new gps data is available
+%#codegen
+%%initialization
+%use model F=m*a x''=F/m
+% 250Hz---> dT = 0.004s
+%u=[phi;theta]
+%x=[px;vx;py;vy];
+%%------------------------------------------
+dT=0.004;
+%%------------------------------------------------
+
+%R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1];
+
+
+F=[ 1,      0.004,  0,      0,      0,      0;
+    0,      1,      0,      0,      0,      0;
+    0,      0,      1,      0.004,  0,      0;
+    0,      0,      0,      1,      0,      0;
+    0,      0,      0,      0,      1,      0.004;
+    0,      0,      0,      0,      0,      1];
+
+B=[ 0,      -0.1744;
+    0,      -87.2;
+    0.1744, 0;
+    87.2,   0;
+    0,      0;
+    0,      0];
+ 
+H=[1,0,0,0,0,0;
+   0,0,1,0,0,0;
+   0,0,0,0,1,0];
+
+
+
+ Q=[1e-007        ,0      ,0      ,0      ,0      ,0;
+    0           ,1   ,0      ,0      ,0      ,0;
+    0           ,0      ,1e-007   ,0      ,0      ,0;
+    0           ,0      ,0      ,1   ,0      ,0
+    0           ,0      ,0      ,0      ,1e-007   ,0;
+    0           ,0      ,0      ,0      ,0      ,1]; %process Covariance Matrix
+
+
+R=[gps_covariance(1),  0,          0;
+   0,       gps_covariance(2),     0;
+   0,       0,          gps_covariance(3)]; %measurement Covariance Matrix
+
+%%prediction
+
+xapri=F*xapo+B*u;
+Papri=F*Papo*F'+Q;
+
+if 1 ~= predict_only
+    %update
+    yr=z-H*xapri;
+    S=H*Papri*H'+R;
+    K=(Papri*H')/S;
+    xapo1=xapri+K*yr;
+    Papo1=(eye(6)-K*H)*Papri;
+else
+    Papo1=Papri;
+    xapo1=xapri;
+end
\ No newline at end of file
diff --git a/apps/position_estimator/position_estimator.prj b/apps/position_estimator/position_estimator.prj
new file mode 100644
index 000000000..d754f36cc
--- /dev/null
+++ b/apps/position_estimator/position_estimator.prj
@@ -0,0 +1,269 @@
+<?xml version="1.0" encoding="UTF-8"?>

+<deployment-project>

+  <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="position_estimator" location="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator" file="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator/position_estimator.prj" build-checksum="2425236060">

+    <param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>

+    <param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>

+    <param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>

+    <param.mex.general.EnableBLAS>false</param.mex.general.EnableBLAS>

+    <param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>

+    <param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>

+    <param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>

+    <param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>

+    <param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>

+    <param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>

+    <param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>

+    <param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>

+    <param.mex.paths.working>option.paths.working.project</param.mex.paths.working>

+    <param.mex.paths.working.specified />

+    <param.mex.paths.build>option.paths.build.project</param.mex.paths.build>

+    <param.mex.paths.build.specified />

+    <param.mex.paths.search />

+    <param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>

+    <param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>

+    <param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>

+    <param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>

+    <param.mex.symbols.ReservedNameArray />

+    <param.mex.customcode.CustomSourceCode />

+    <param.mex.customcode.CustomHeaderCode />

+    <param.mex.customcode.CustomInitializer />

+    <param.mex.customcode.CustomTerminator />

+    <param.mex.customcode.CustomInclude />

+    <param.mex.customcode.CustomSource />

+    <param.mex.customcode.CustomLibrary />

+    <param.mex.PostCodeGenCommand />

+    <param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>

+    <param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>

+    <param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>

+    <param.mex.InlineThreshold>10</param.mex.InlineThreshold>

+    <param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>

+    <param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>

+    <param.mex.StackUsageMax>200000</param.mex.StackUsageMax>

+    <param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>

+    <param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>

+    <param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>

+    <param.grt.CCompilerOptimization>option.CCompilerOptimization.Off</param.grt.CCompilerOptimization>

+    <param.grt.CCompilerCustomOptimizations />

+    <param.grt.general.GenerateMakefile>false</param.grt.general.GenerateMakefile>

+    <param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>

+    <param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>

+    <param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>

+    <param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>

+    <param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>

+    <param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>

+    <param.grt.paths.working>option.paths.working.project</param.grt.paths.working>

+    <param.grt.paths.working.specified />

+    <param.grt.paths.build>option.paths.build.specified</param.grt.paths.build>

+    <param.grt.paths.build.specified>./codegen</param.grt.paths.build.specified>

+    <param.grt.paths.search />

+    <param.grt.report.GenerateReport>false</param.grt.report.GenerateReport>

+    <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>

+    <param.grt.GenerateComments>true</param.grt.GenerateComments>

+    <param.grt.MATLABSourceComments>false</param.grt.MATLABSourceComments>

+    <param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>

+    <param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>

+    <param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>

+    <param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>

+    <param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>

+    <param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>

+    <param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>

+    <param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>

+    <param.grt.MaxIdLength>32</param.grt.MaxIdLength>

+    <param.grt.ReservedNameArray />

+    <param.grt.customcode.CustomSourceCode />

+    <param.grt.customcode.CustomHeaderCode />

+    <param.grt.customcode.CustomInitializer />

+    <param.grt.customcode.CustomTerminator />

+    <param.grt.customcode.CustomInclude />

+    <param.grt.customcode.CustomSource />

+    <param.grt.customcode.CustomLibrary />

+    <param.grt.PostCodeGenCommand />

+    <param.grt.Verbose>false</param.grt.Verbose>

+    <param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>

+    <param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>

+    <param.ert.TargetFunctionLibrary>C89/C90 (ANSI)</param.ert.TargetFunctionLibrary>

+    <param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>

+    <param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>

+    <param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>

+    <param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>

+    <param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>

+    <param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>

+    <param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>

+    <param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>

+    <param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>

+    <param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>

+    <param.grt.InlineThreshold>10</param.grt.InlineThreshold>

+    <param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>

+    <param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>

+    <param.grt.StackUsageMax>200000</param.grt.StackUsageMax>

+    <param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>

+    <param.UseECoderFeatures>true</param.UseECoderFeatures>

+    <param.mex.outputfile>position_estimator_mex</param.mex.outputfile>

+    <param.grt.outputfile>position_estimator</param.grt.outputfile>

+    <param.artifact>option.target.artifact.lib</param.artifact>

+    <param.mex.GenCodeOnly>true</param.mex.GenCodeOnly>

+    <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>

+    <param.version>R2011a</param.version>

+    <param.HasECoderFeatures>true</param.HasECoderFeatures>

+    <param.mex.mainhtml>/home/thomasgubler/Dropbox/Semester Project Autonomous Landing PX4/position_estimator/codegen/mex/position_estimator/html/index.html</param.mex.mainhtml>

+    <param.grt.mainhtml />

+    <unset>

+      <param.mex.general.TargetLang />

+      <param.mex.general.IntegrityChecks />

+      <param.mex.general.ResponsivenessChecks />

+      <param.mex.general.ExtrinsicCalls />

+      <param.mex.general.EchoExpressions />

+      <param.mex.general.EnableDebugging />

+      <param.mex.general.SaturateOnIntegerOverflow />

+      <param.mex.general.FilePartitionMethod />

+      <param.mex.general.GlobalDataSyncMethod />

+      <param.mex.general.EnableVariableSizing />

+      <param.mex.general.DynamicMemoryAllocation />

+      <param.mex.paths.working />

+      <param.mex.paths.working.specified />

+      <param.mex.paths.build />

+      <param.mex.paths.build.specified />

+      <param.mex.paths.search />

+      <param.mex.report.GenerateReport />

+      <param.mex.report.LaunchReport />

+      <param.mex.comments.GenerateComments />

+      <param.mex.comments.MATLABSourceComments />

+      <param.mex.symbols.ReservedNameArray />

+      <param.mex.customcode.CustomInclude />

+      <param.mex.customcode.CustomSource />

+      <param.mex.customcode.CustomLibrary />

+      <param.mex.PostCodeGenCommand />

+      <param.mex.EnableMemcpy />

+      <param.mex.MemcpyThreshold />

+      <param.mex.InitFltsAndDblsToZero />

+      <param.mex.InlineThreshold />

+      <param.mex.InlineThresholdMax />

+      <param.mex.InlineStackLimit />

+      <param.mex.StackUsageMax />

+      <param.mex.ConstantFoldingTimeout />

+      <param.grt.general.TargetLang />

+      <param.grt.CCompilerOptimization />

+      <param.grt.CCompilerCustomOptimizations />

+      <param.grt.general.MakeCommand />

+      <param.grt.general.TemplateMakefile />

+      <param.grt.general.SaturateOnIntegerOverflow />

+      <param.grt.general.FilePartitionMethod />

+      <param.grt.general.EnableVariableSizing />

+      <param.grt.general.DynamicMemoryAllocation />

+      <param.grt.paths.working />

+      <param.grt.paths.working.specified />

+      <param.grt.paths.search />

+      <param.grt.report.LaunchReport />

+      <param.grt.GenerateComments />

+      <param.grt.MATLABSourceComments />

+      <param.ert.MATLABFcnDesc />

+      <param.ert.CustomSymbolStrGlobalVar />

+      <param.ert.CustomSymbolStrType />

+      <param.ert.CustomSymbolStrField />

+      <param.ert.CustomSymbolStrFcn />

+      <param.ert.CustomSymbolStrTmpVar />

+      <param.ert.CustomSymbolStrMacro />

+      <param.ert.CustomSymbolStrEMXArray />

+      <param.grt.MaxIdLength />

+      <param.grt.ReservedNameArray />

+      <param.grt.customcode.CustomHeaderCode />

+      <param.grt.customcode.CustomInitializer />

+      <param.grt.customcode.CustomTerminator />

+      <param.grt.customcode.CustomInclude />

+      <param.grt.customcode.CustomSource />

+      <param.grt.customcode.CustomLibrary />

+      <param.grt.PostCodeGenCommand />

+      <param.grt.Verbose />

+      <param.grt.SupportNonFinite />

+      <param.ert.PurelyIntegerCode />

+      <param.ert.SupportNonFinite />

+      <param.ert.IncludeTerminateFcn />

+      <param.ert.MultiInstanceCode />

+      <param.ert.ParenthesesLevel />

+      <param.ert.ConvertIfToSwitch />

+      <param.ert.PreserveExternInFcnDecls />

+      <param.grt.EnableMemcpy />

+      <param.grt.MemcpyThreshold />

+      <param.grt.InitFltsAndDblsToZero />

+      <param.grt.InlineThreshold />

+      <param.grt.InlineThresholdMax />

+      <param.grt.InlineStackLimit />

+      <param.grt.StackUsageMax />

+      <param.grt.ConstantFoldingTimeout />

+      <param.UseECoderFeatures />

+      <param.mex.outputfile />

+      <param.grt.outputfile />

+      <param.version />

+      <param.HasECoderFeatures />

+    </unset>

+    <fileset.entrypoints>

+      <file value="${PROJECT_ROOT}/position_estimator.m" custom-data-expanded="true">

+        <Inputs fileName="position_estimator.m" functionName="position_estimator">

+          <Input Name="u">

+            <Class>single</Class>

+            <Size>2 x 1</Size>

+            <Value />

+            <InitialValue />

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="z">

+            <Class>single</Class>

+            <Size>3 x 1</Size>

+            <Value />

+            <InitialValue />

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="xapo">

+            <Class>single</Class>

+            <Size>6 x 1</Size>

+            <Value />

+            <InitialValue />

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="Papo">

+            <Class>single</Class>

+            <Size>6 x 6</Size>

+            <Value />

+            <InitialValue />

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="gps_covariance">

+            <Class>single</Class>

+            <Size>3 x 1</Size>

+            <Value />

+            <InitialValue />

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="predict_only">

+            <Class>uint8</Class>

+            <Size>1 x 1</Size>

+            <Value />

+            <InitialValue />

+            <Complex>false</Complex>

+          </Input>

+        </Inputs>

+      </file>

+    </fileset.entrypoints>

+    <fileset.package />

+    <build-deliverables />

+    <workflow />

+    <matlab>

+      <root>/home/thomasgubler/tools/matlab</root>

+    </matlab>

+    <platform>

+      <unix>true</unix>

+      <mac>false</mac>

+      <windows>false</windows>

+      <win2k>false</win2k>

+      <winxp>false</winxp>

+      <vista>false</vista>

+      <linux>true</linux>

+      <solaris>false</solaris>

+      <osver>3.2.0-25-generic</osver>

+      <os32>false</os32>

+      <os64>true</os64>

+      <arch>glnxa64</arch>

+      <matlab>true</matlab>

+    </platform>

+  </configuration>

+</deployment-project>

+

diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c
new file mode 100644
index 000000000..773cd87ff
--- /dev/null
+++ b/apps/position_estimator/position_estimator_main.c
@@ -0,0 +1,425 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *			 Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file Model-identification based position estimator for multirotors
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <v1.0/common/mavlink.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <poll.h>
+
+#include "codegen/position_estimator.h"
+
+#define N_STATES 6
+#define ERROR_COVARIANCE_INIT 3
+#define R_EARTH 6371000.0
+
+#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
+#define REPROJECTION_COUNTER_LIMIT 125
+
+__EXPORT int position_estimator_main(int argc, char *argv[]);
+
+static uint16_t position_estimator_counter_position_information;
+
+/* values for map projection */
+static double phi_1;
+static double sin_phi_1;
+static double cos_phi_1;
+static double lambda_0;
+static double scale;
+
+/**
+ * Initializes the map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	phi_1 = lat_0 / 180.0 * M_PI;
+	lambda_0 = lon_0 / 180.0 * M_PI;
+
+	sin_phi_1 = sin(phi_1);
+	cos_phi_1 = cos(phi_1);
+
+	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
+
+	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
+	const double r_earth = 6371000;
+
+	double lat1 = phi_1;
+	double lon1 = lambda_0;
+
+	double lat2 = phi_1 + 0.5 / 180 * M_PI;
+	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
+	double sin_lat_2 = sin(lat2);
+	double cos_lat_2 = cos(lat2);
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+
+	/* 2) calculate distance rho on plane */
+	double k_bar = 0;
+	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
+	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
+	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
+
+	scale = d / rho;
+
+}
+
+/**
+ * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+static void map_projection_project(double lat, double lon, float *x, float *y)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	double phi = lat / 180.0 * M_PI;
+	double lambda = lon / 180.0 * M_PI;
+
+	double sin_phi = sin(phi);
+	double cos_phi = cos(phi);
+
+	double k_bar = 0;
+	/* using small angle approximation (formula in comment is without aproximation) */
+	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	/* using small angle approximation (formula in comment is without aproximation) */
+	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
+	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
+
+//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
+}
+
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+static void map_projection_reproject(float x, float y, double *lat, double *lon)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+
+	double x_descaled = x / scale;
+	double y_descaled = y / scale;
+
+	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_sphere = 0;
+
+	if (c != 0)
+		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
+	else
+		lat_sphere = asin(cos_c * sin_phi_1);
+
+//	printf("lat_sphere = %.10f\n",lat_sphere);
+
+	double lon_sphere = 0;
+
+	if (phi_1  == M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
+
+	} else if (phi_1 == -M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
+
+	} else {
+
+		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
+		//using small angle approximation
+//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
+//    	if(denominator != 0)
+//    	{
+//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
+//    	}
+//    	else
+//    	{
+//    	...
+//    	}
+	}
+
+//	printf("lon_sphere = %.10f\n",lon_sphere);
+
+	*lat = lat_sphere * 180.0 / M_PI;
+	*lon = lon_sphere * 180.0 / M_PI;
+
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+
+int position_estimator_main(int argc, char *argv[])
+{
+
+	/* welcome user */
+	printf("[multirotor position_estimator] started\n");
+
+	/* initialize values */
+	static float u[2] = {0, 0};
+	static float z[3] = {0, 0, 0};
+	static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0};
+	static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
+			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
+			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
+			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
+			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
+			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0
+						 };
+
+	static float xapo1[N_STATES];
+	static float Papo1[36];
+
+	static float gps_covariance[3] = {0.0f, 0.0f, 0.0f};
+
+	static uint16_t counter = 0;
+	position_estimator_counter_position_information = 0;
+
+	uint8_t predict_only = 1;
+
+	bool gps_valid = false;
+
+	bool new_initialization = true;
+
+	static double lat_current = 0;//[°]] --> 47.0
+	static double lon_current = 0; //[°]] -->8.5
+
+
+	//TODO: handle flight without gps but with estimator
+
+	/* subscribe to vehicle status, attitude, gps */
+	struct vehicle_gps_position_s gps;
+	struct vehicle_status_s vstatus;
+	struct vehicle_attitude_s att;
+
+	int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* subscribe to attitude at 100 Hz */
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+
+	/* wait until gps signal turns valid, only then can we initialize the projection */
+	while (!gps_valid) {
+		struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
+
+		/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
+		 * this choice is critical, since the vehicle status might not
+		 * actually change, if this app is started after GPS lock was
+		 * aquired.
+		 */
+		if (poll(fds, 1, 5000)) {
+			/* Wait for the GPS update to propagate (we have some time) */
+			usleep(5000);
+			/* Read wether the vehicle status changed */
+			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+			gps_valid = vstatus.gps_valid;
+		}
+	}
+
+	/* get gps value for first initialization */
+	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+	lat_current = ((double)(gps.lat)) * 1e-7;
+	lon_current = ((double)(gps.lon)) * 1e-7;
+
+	/* publish global position messages only after first GPS message */
+	struct vehicle_global_position_s global_pos = {
+		.lat = lat_current * 1e7,
+		.lon = lon_current * 1e7,
+		.alt = gps.alt
+	};
+	int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
+	printf("[multirotor position estimator] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
+
+	while (1) {
+
+		/*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */
+		struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} };
+
+		if (poll(fds, 1, 5000) <= 0) {
+			/* error / timeout */
+		} else {
+
+			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+			/* got attitude, updating pos as well */
+			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+
+			/*copy attitude */
+			u[0] = att.roll;
+			u[1] = att.pitch;
+
+			/* initialize map projection with the last estimate (not at full rate) */
+			if (counter % PROJECTION_INITIALIZE_COUNTER_LIMIT == 0) {
+				map_projection_init(lat_current, lon_current);
+				new_initialization = true;
+
+			} else {
+				new_initialization = false;
+			}
+
+			/*check if new gps values are available */
+			gps_valid = vstatus.gps_valid;
+
+
+			if (gps_valid) { //we are safe to use the gps signal (it has good quality)
+
+				predict_only = 0;
+				/* Project gps lat lon (Geographic coordinate system) to plane*/
+				map_projection_project((double)(gps.lat) * 1e-7, (double)(gps.lon) * 1e-7, &(z[0]), &(z[1]));
+
+				/* copy altitude */
+				z[2] = (gps.alt) * 1e-3;
+
+				gps_covariance[0] = gps.eph; //TODO: needs scaling
+				gps_covariance[1] = gps.eph;
+				gps_covariance[2] = gps.epv;
+
+			} else {
+				/* we can not use the gps signal (it is of low quality) */
+				predict_only = 1;
+			}
+
+			//		predict_only = 0;																//TODO: only for testing, removeme, XXX
+			//		z[0] = sinf(((float)counter)/180.0f*3.14159265f);								//TODO: only for testing, removeme, XXX
+			//		usleep(100000);																	//TODO: only for testing, removeme, XXX
+
+
+			/*Get new estimation (this is calculated in the plane) */
+			//TODO: if new_initialization == true: use 0,0,0, else use xapo
+			if (true == new_initialization) {																								//TODO,XXX: uncomment!
+				xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
+				xapo[2] = 0;
+				xapo[4] = 0;
+				position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
+
+			} else {
+				position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
+			}
+
+
+
+			/* Copy values from xapo1 to xapo */
+			int i;
+
+			for (i = 0; i < N_STATES; i++) {
+				xapo[i] = xapo1[i];
+			}
+
+			if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
+				/* Reproject from plane to geographic coordinate system */
+				//		map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
+				map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
+				//		//DEBUG
+				//			if(counter%500 == 0)
+				//			{
+				//				printf("phi_1: %.10f\n", phi_1);
+				//				printf("lambda_0: %.10f\n", lambda_0);
+				//				printf("lat_estimated: %.10f\n", lat_current);
+				//				printf("lon_estimated: %.10f\n", lon_current);
+				//				printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
+				//				fflush(stdout);
+				//
+				//			}
+
+				//			if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
+				//			{
+				/* send out */
+
+				global_pos.lat = lat_current;
+				global_pos.lon = lon_current;
+				global_pos.alt = xapo1[4];
+				global_pos.vx = xapo1[1];
+				global_pos.vy = xapo1[3];
+				global_pos.vz = xapo1[5];
+
+				/* publish current estimate */
+				orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
+				//			}
+				//			else
+				//			{
+				//				printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]);
+				//				fflush(stdout);
+				//			}
+
+			}
+
+			counter++;
+		}
+
+	}
+
+	return 0;
+}
+
+