Merge branch 'master' of github.com:PX4/Firmware into pca8574

1 files changed, 7 insertions, 8 deletions

diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index 2f1b3c014..a36a4688d 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -9,15 +9,18 @@
 
 #include "inertial_filter.h"
 
-void inertial_filter_predict(float dt, float x[3])
+void inertial_filter_predict(float dt, float x[2], float acc)
 {
 	if (isfinite(dt)) {
-		x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
-		x[1] += x[2] * dt;
+		if (!isfinite(acc)) {
+			acc = 0.0f;
+		}
+		x[0] += x[1] * dt + acc * dt * dt / 2.0f;
+		x[1] += acc * dt;
 	}
 }
 
-void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
 {
 	if (isfinite(e) && isfinite(w) && isfinite(dt)) {
 		float ewdt = e * w * dt;
@@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
 
 		if (i == 0) {
 			x[1] += w * ewdt;
-			x[2] += w * w * ewdt / 3.0;
-
-		} else if (i == 1) {
-			x[2] += w * ewdt;
 		}
 	}
 }