Estimate accelerometers offset in position_estimator_inav

3 files changed, 53 insertions, 32 deletions

diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c
index 97d669612..6e8c0ab5f 100644
--- a/apps/position_estimator_inav/position_estimator_inav_main.c
+++ b/apps/position_estimator_inav/position_estimator_inav_main.c
@@ -280,6 +280,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	static float x_est[3] = { 0.0f, 0.0f, 0.0f };
 	static float y_est[3] = { 0.0f, 0.0f, 0.0f };
 	static float z_est[3] = { 0.0f, 0.0f, 0.0f };
+	float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f };
 
 	int baro_loop_cnt = 0;
 	int baro_loop_end = 70; /* measurement for 1 second */
@@ -471,36 +472,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		hrt_abstime t = hrt_absolute_time();
 		float dt = (t - last_time) / 1000000.0;
 		last_time = t;
-		/* calculate corrected acceleration vector in UAV frame */
-		float accel_corr[3];
-		acceleration_correct(accel_corr, sensor.accelerometer_raw,
-				params.acc_T, params.acc_offs);
-		/* transform acceleration vector from UAV frame to NED frame */
-		float accel_NED[3];
-		for (int i = 0; i < 3; i++) {
-			accel_NED[i] = 0.0f;
-			for (int j = 0; j < 3; j++) {
-				accel_NED[i] += att.R[i][j] * accel_corr[j];
+		if (att.R_valid) {
+			/* calculate corrected acceleration vector in UAV frame */
+			float accel_corr[3];
+			acceleration_correct(accel_corr, sensor.accelerometer_raw,
+					params.acc_T, params.acc_offs);
+			/* transform acceleration vector from UAV frame to NED frame */
+			float accel_NED[3];
+			for (int i = 0; i < 3; i++) {
+				accel_NED[i] = 0.0f;
+				for (int j = 0; j < 3; j++) {
+					accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]);
+				}
+			}
+			accel_NED[2] += const_earth_gravity;
+			/* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */
+			for (int i = 0; i < 3; i++) {
+				for (int j = 0; j < 3; j++) {
+					/* the inverse of a rotation matrix is its transpose, just swap i and j */
+					accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt;
+				}
+			}
+			/* kalman filter prediction */
+			kalman_filter_inertial_predict(dt, z_est);
+			/* prepare vectors for kalman filter correction */
+			float z_meas[2];	// position, acceleration
+			bool use_z[2] = { false, false };
+			if (local_flag_baroINITdone && baro_updated) {
+				z_meas[0] = baro_alt0 - sensor.baro_alt_meter;	// Z = -alt
+				use_z[0] = true;
+			}
+			if (accelerometer_updated) {
+				z_meas[1] = accel_NED[2];
+				use_z[1] = true;
+			}
+			if (use_z[0] || use_z[1]) {
+				/* correction */
+				kalman_filter_inertial_update(z_est, z_meas, params.k,
+						use_z);
 			}
-		}
-		accel_NED[2] += const_earth_gravity;
-		/* kalman filter prediction */
-		kalman_filter_inertial_predict(dt, z_est);
-		/* prepare vectors for kalman filter correction */
-		float z_meas[2];	// position, acceleration
-		bool use_z[2] = { false, false };
-		if (local_flag_baroINITdone && baro_updated) {
-			z_meas[0] = baro_alt0 - sensor.baro_alt_meter;	// Z = -alt
-			use_z[0] = true;
-		}
-		if (accelerometer_updated) {
-			z_meas[1] = accel_NED[2];
-			use_z[1] = true;
-		}
-		if (use_z[0] || use_z[1]) {
-			/* correction */
-			kalman_filter_inertial_update(z_est, z_meas, params.k,
-					use_z);
 		}
 		if (verbose_mode) {
 			/* print updates rate */
@@ -521,9 +531,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		}
 		if (t - pub_last > pub_interval) {
 			pub_last = t;
-			pos.x = 0.0f;
-			pos.vx = 0.0f;
-			pos.y = 0.0f;
+			pos.x = accel_offs_est[0];
+			pos.vx = accel_offs_est[1];
+			pos.y = accel_offs_est[2];
 			pos.vy = 0.0f;
 			pos.z = z_est[0];
 			pos.vz = z_est[1];
diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c
index d3a165947..8cd9844c7 100644
--- a/apps/position_estimator_inav/position_estimator_inav_params.c
+++ b/apps/position_estimator_inav/position_estimator_inav_params.c
@@ -39,6 +39,7 @@
  */
 
 #include "position_estimator_inav_params.h"
+#include "acceleration_transform.h"
 
 /* Kalman Filter covariances */
 /* gps measurement noise standard deviation */
@@ -51,6 +52,8 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f);
 
+PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f);
+
 PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0);
 PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0);
 PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0);
@@ -75,6 +78,8 @@ int parameters_init(struct position_estimator_inav_param_handles *h) {
 	h->k_alt_20 = param_find("INAV_K_ALT_20");
 	h->k_alt_21 = param_find("INAV_K_ALT_21");
 
+	h->acc_offs_w = param_find("INAV_ACC_OFFS_W");
+
 	h->acc_offs_0 = param_find("INAV_ACC_OFFS_0");
 	h->acc_offs_1 = param_find("INAV_ACC_OFFS_1");
 	h->acc_offs_2 = param_find("INAV_ACC_OFFS_2");
@@ -101,6 +106,8 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->k_alt_20, &(p->k[2][0]));
 	param_get(h->k_alt_21, &(p->k[2][1]));
 
+	param_get(h->acc_offs_w, &(p->acc_offs_w));
+
 	/* read int32 and cast to int16 */
 	int32_t t;
 	param_get(h->acc_offs_0, &t);
@@ -119,5 +126,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->acc_t_20, &(p->acc_T[2][0]));
 	param_get(h->acc_t_21, &(p->acc_T[2][1]));
 	param_get(h->acc_t_22, &(p->acc_T[2][2]));
+
 	return OK;
 }
diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h
index 51e57a914..2c6fb3df2 100644
--- a/apps/position_estimator_inav/position_estimator_inav_params.h
+++ b/apps/position_estimator_inav/position_estimator_inav_params.h
@@ -43,6 +43,7 @@
 struct position_estimator_inav_params {
 	int use_gps;
 	float k[3][2];
+	float acc_offs_w;
 	int16_t acc_offs[3];
 	float acc_T[3][3];
 };
@@ -57,6 +58,8 @@ struct position_estimator_inav_param_handles {
 	param_t k_alt_20;
 	param_t k_alt_21;
 
+	param_t acc_offs_w;
+
 	param_t acc_offs_0;
 	param_t acc_offs_1;
 	param_t acc_offs_2;