1 files changed, 16 insertions, 1 deletions
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c
index 64031ee7b..390e1b720 100644
--- a/src/modules/position_estimator_inav/kalman_filter_inertial.c
+++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c
@@ -12,7 +12,7 @@ void kalman_filter_inertial_predict(float dt, float x[3]) {
 	x[1] += x[2] * dt;
 }
 
-void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) {
+void kalman_filter_inertial2_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];
@@ -25,3 +25,18 @@ void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool u
 		}
 	}
 }
+
+void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) {
+	float y[2];
+	// y = z - H x
+	y[0] = z[0] - x[0];
+	y[1] = z[1] - x[1];
+	y[2] = z[2] - x[2];
+	// x = x + K * y
+	for (int i = 0; i < 3; i++) {		// Row
+		for (int j = 0; j < 3; j++) {	// Column
+			if (use[j])
+				x[i] += k[i][j] * y[j];
+		}
+	}
+}