Complete position estimator implemented (GPS + Baro + Accel)

1 files changed, 28 insertions, 0 deletions

diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 000000000..b70d3504e
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,28 @@
+/*
+ * inertial_filter.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float dt, float x[3], int i, float z, float w)
+{
+	float e = z - x[i];
+	x[i] += e * w * dt;
+
+	if (i == 0) {
+		x[1] += e * w * w * dt;
+		//x[2] += e * w * w * w * dt / 3.0; // ~ 0.0
+
+	} else if (i == 1) {
+		x[2] += e * w * w * dt;
+	}
+}