position_estimator_inav rewrite, publishes vehicle_global_position now

1 files changed, 5 insertions, 5 deletions

diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index b70d3504e..8cdde5e1a 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -13,16 +13,16 @@ void inertial_filter_predict(float dt, float x[3])
 	x[1] += x[2] * dt;
 }
 
-void inertial_filter_correct(float dt, float x[3], int i, float z, float w)
+void inertial_filter_correct(float edt, float x[3], int i, float w)
 {
-	float e = z - x[i];
-	x[i] += e * w * dt;
+	float ewdt = w * edt;
+	x[i] += ewdt;
 
 	if (i == 0) {
-		x[1] += e * w * w * dt;
+		x[1] += w * ewdt;
 		//x[2] += e * w * w * w * dt / 3.0; // ~ 0.0
 
 	} else if (i == 1) {
-		x[2] += e * w * w * dt;
+		x[2] += w * ewdt;
 	}
 }