1 files changed, 136 insertions, 0 deletions
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
new file mode 100755
index 000000000..5139848bc
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
@@ -0,0 +1,136 @@
+/*
+ * positionKalmanFilter1D.c
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
+  real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
+  P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
+  Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
+  real32_T P_aposteriori[9])
+{
+  int32_T i0;
+  real32_T f0;
+  int32_T k;
+  real32_T b_A[9];
+  int32_T i1;
+  real32_T P_apriori[9];
+  real32_T y;
+  real32_T K[3];
+  real32_T S;
+  int8_T I[9];
+
+  /* prediction */
+  for (i0 = 0; i0 < 3; i0++) {
+    f0 = 0.0F;
+    for (k = 0; k < 3; k++) {
+      f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
+    }
+
+    x_aposteriori[i0] = f0 + B[i0] * u;
+  }
+
+  for (i0 = 0; i0 < 3; i0++) {
+    for (k = 0; k < 3; k++) {
+      b_A[i0 + 3 * k] = 0.0F;
+      for (i1 = 0; i1 < 3; i1++) {
+        b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
+      }
+    }
+  }
+
+  for (i0 = 0; i0 < 3; i0++) {
+    for (k = 0; k < 3; k++) {
+      f0 = 0.0F;
+      for (i1 = 0; i1 < 3; i1++) {
+        f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
+      }
+
+      P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
+    }
+  }
+
+  if ((real32_T)fabs(u) < thresh) {
+    x_aposteriori[1] *= decay;
+  }
+
+  /* update */
+  if (gps_update == 1) {
+    y = 0.0F;
+    for (k = 0; k < 3; k++) {
+      y += C[k] * x_aposteriori[k];
+      K[k] = 0.0F;
+      for (i0 = 0; i0 < 3; i0++) {
+        K[k] += C[i0] * P_apriori[i0 + 3 * k];
+      }
+    }
+
+    y = z - y;
+    S = 0.0F;
+    for (k = 0; k < 3; k++) {
+      S += K[k] * C[k];
+    }
+
+    S += R;
+    for (i0 = 0; i0 < 3; i0++) {
+      f0 = 0.0F;
+      for (k = 0; k < 3; k++) {
+        f0 += P_apriori[i0 + 3 * k] * C[k];
+      }
+
+      K[i0] = f0 / S;
+    }
+
+    for (i0 = 0; i0 < 3; i0++) {
+      x_aposteriori[i0] += K[i0] * y;
+    }
+
+    for (i0 = 0; i0 < 9; i0++) {
+      I[i0] = 0;
+    }
+
+    for (k = 0; k < 3; k++) {
+      I[k + 3 * k] = 1;
+    }
+
+    for (i0 = 0; i0 < 3; i0++) {
+      for (k = 0; k < 3; k++) {
+        b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
+      }
+    }
+
+    for (i0 = 0; i0 < 3; i0++) {
+      for (k = 0; k < 3; k++) {
+        P_aposteriori[i0 + 3 * k] = 0.0F;
+        for (i1 = 0; i1 < 3; i1++) {
+          P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
+        }
+      }
+    }
+  } else {
+    for (i0 = 0; i0 < 9; i0++) {
+      P_aposteriori[i0] = P_apriori[i0];
+    }
+  }
+}
+
+/* End of code generation (positionKalmanFilter1D.c) */