position_estimator_inav: accelerometer bias estimation for Z, default weights for GPS updated

3 files changed, 22 insertions, 9 deletions

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index a9bc09e0d..42b5fcb61 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -167,9 +167,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int baro_init_num = 200;
 	float baro_alt0 = 0.0f; /* to determine while start up */
 
-	double lat_current = 0.0; //[°]] --> 47.0
-	double lon_current = 0.0; //[°]] -->8.5
-	double alt_current = 0.0; //[m] above MSL
+	double lat_current = 0.0f; //[°] --> 47.0
+	double lon_current = 0.0f; //[°] --> 8.5
+	double alt_current = 0.0f; //[m] above MSL
 
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
@@ -302,8 +302,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	hrt_abstime pub_last = hrt_absolute_time();
 	uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
 
+	/* acceleration in NED frame */
+	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
+
 	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
 	float accel_corr[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float accel_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
 	float baro_corr = 0.0f;		// D
 	float gps_corr[2][2] = {
 		{ 0.0f, 0.0f },		// N (pos, vel)
@@ -369,9 +373,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if (sensor.accelerometer_counter > accel_counter) {
 					if (att.R_valid) {
+						/* correct accel bias, now only for Z */
+						sensor.accelerometer_m_s2[2] -= accel_bias[2];
 						/* transform acceleration vector from body frame to NED frame */
-						float accel_NED[3];
-
 						for (int i = 0; i < 3; i++) {
 							accel_NED[i] = 0.0f;
 
@@ -425,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 								local_pos.home_timestamp = hrt_absolute_time();
 								z_est[0] += sonar_corr;
 								sonar_corr = 0.0f;
-								sonar_corr_filtered = 0.0;
+								sonar_corr_filtered = 0.0f;
 							}
 						}
 					}
@@ -470,6 +474,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
 		t_prev = t;
 
+		/* accel bias correction, now only for Z
+		 * not strictly correct, but stable and works */
+		accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
+
 		/* inertial filter prediction for altitude */
 		inertial_filter_predict(dt, z_est);
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index c90c611a7..70248b3b7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -44,10 +44,11 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
 PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
-PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
@@ -62,6 +63,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
 	h->w_pos_acc = param_find("INAV_W_POS_ACC");
 	h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
 	h->sonar_filt = param_find("INAV_SONAR_FILT");
 	h->sonar_err = param_find("INAV_SONAR_ERR");
@@ -79,6 +81,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
 	param_get(h->w_pos_acc, &(p->w_pos_acc));
 	param_get(h->w_pos_flow, &(p->w_pos_flow));
+	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));
 	param_get(h->sonar_filt, &(p->sonar_filt));
 	param_get(h->sonar_err, &(p->sonar_err));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index cca172b5d..1e70a3c48 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -49,6 +49,7 @@ struct position_estimator_inav_params {
 	float w_pos_gps_v;
 	float w_pos_acc;
 	float w_pos_flow;
+	float w_acc_bias;
 	float flow_k;
 	float sonar_filt;
 	float sonar_err;
@@ -63,6 +64,7 @@ struct position_estimator_inav_param_handles {
 	param_t w_pos_gps_v;
 	param_t w_pos_acc;
 	param_t w_pos_flow;
+	param_t w_acc_bias;
 	param_t flow_k;
 	param_t sonar_filt;
 	param_t sonar_err;