1 files changed, 27 insertions, 0 deletions
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c
new file mode 100644
index 000000000..64031ee7b
--- /dev/null
+++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c
@@ -0,0 +1,27 @@
+/*
+ * kalman_filter_inertial.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "kalman_filter_inertial.h"
+
+void kalman_filter_inertial_predict(float dt, float x[3]) {
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) {
+	float y[2];
+	// y = z - H x
+	y[0] = z[0] - x[0];
+	y[1] = z[1] - x[2];
+	// x = x + K * y
+	for (int i = 0; i < 3; i++) {		// Row
+		for (int j = 0; j < 2; j++) {	// Column
+			if (use[j])
+				x[i] += k[i][j] * y[j];
+		}
+	}
+}