Merge branch 'master' into inav_alt_gps. Use GPS for altitude estimation.

3 files changed, 164 insertions, 127 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 ddd2f62c0..8f4f9e080 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -71,6 +71,8 @@
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
 
+#define MIN_VALID_W 0.00001f
+
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
@@ -180,7 +182,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	int baro_init_cnt = 0;
 	int baro_init_num = 200;
-	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized only once
+	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
 	float surface_offset = 0.0f;	// ground level offset from reference altitude
 	float surface_offset_rate = 0.0f;	// surface offset change rate
 	float alt_avg = 0.0f;
@@ -192,9 +194,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
 
-	bool ref_xy_inited = false;
-	hrt_abstime ref_xy_init_start = 0;
-	const hrt_abstime ref_xy_init_delay = 1000000;	// wait for 1s after 3D fix
+	bool ref_inited = false;
+	hrt_abstime ref_init_start = 0;
+	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
 
 	uint16_t accel_updates = 0;
 	uint16_t baro_updates = 0;
@@ -211,18 +213,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	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] = {
+	float corr_acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
+	float corr_baro = 0.0f;		// D
+	float corr_gps[3][2] = {
 		{ 0.0f, 0.0f },		// N (pos, vel)
 		{ 0.0f, 0.0f },		// E (pos, vel)
+		{ 0.0f, 0.0f },		// D (pos, vel)
 	};
-	float sonar_corr = 0.0f;
-	float sonar_corr_filtered = 0.0f;
+	float w_gps_xy = 1.0f;
+	float w_gps_z = 1.0f;
+	float corr_sonar = 0.0f;
+	float corr_sonar_filtered = 0.0f;
 
-	float flow_corr[] = { 0.0f, 0.0f };	// X, Y
-	float flow_w = 0.0f;
+	float corr_flow[] = { 0.0f, 0.0f };	// N E
+	float w_flow = 0.0f;
 
 	float sonar_prev = 0.0f;
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
@@ -307,13 +312,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					} else {
 						wait_baro = false;
 						baro_offset /= (float) baro_init_cnt;
-						warnx("init ref: alt=%.3f", baro_offset);
-						mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_offset);
-						local_pos.ref_alt = baro_offset;
-						local_pos.ref_timestamp = hrt_absolute_time();
+						corr_baro = -baro_offset;
+						warnx("baro offs: %.2f", baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						local_pos.z_global = true;
 					}
 				}
 			}
@@ -370,10 +373,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (armed.armed && !flag_armed) {
 					flag_armed = armed.armed;
 					baro_offset -= z_est[0];
+					corr_baro = -baro_offset;
+					local_pos.ref_alt -= z_est[0];
+					local_pos.ref_timestamp = t;
 					z_est[0] = 0.0f;
 					alt_avg = 0.0f;
-					local_pos.ref_alt = baro_offset;
-					local_pos.ref_timestamp = t;
 				}
 			}
 
@@ -386,9 +390,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[0] -= accel_bias[0];
-						sensor.accelerometer_m_s2[1] -= accel_bias[1];
-						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+						sensor.accelerometer_m_s2[0] -= acc_bias[0];
+						sensor.accelerometer_m_s2[1] -= acc_bias[1];
+						sensor.accelerometer_m_s2[2] -= acc_bias[2];
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
@@ -399,12 +403,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							}
 						}
 
-						accel_corr[0] = accel_NED[0] - x_est[2];
-						accel_corr[1] = accel_NED[1] - y_est[2];
-						accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+						corr_acc[0] = accel_NED[0] - x_est[2];
+						corr_acc[1] = accel_NED[1] - y_est[2];
+						corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
 
 					} else {
-						memset(accel_corr, 0, sizeof(accel_corr));
+						memset(corr_acc, 0, sizeof(corr_acc));
 					}
 
 					accel_counter = sensor.accelerometer_counter;
@@ -412,7 +416,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 
 				if (sensor.baro_counter > baro_counter) {
-					baro_corr = baro_offset - sensor.baro_alt_meter - z_est[0];
+					corr_baro = - sensor.baro_alt_meter - z_est[0];
 					baro_counter = sensor.baro_counter;
 					baro_updates++;
 				}
@@ -427,22 +431,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
 					sonar_time = t;
 					sonar_prev = flow.ground_distance_m;
-					sonar_corr = flow.ground_distance_m + surface_offset + z_est[0];
-					sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
+					corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
+					corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
 
-					if (fabsf(sonar_corr) > params.sonar_err) {
+					if (fabsf(corr_sonar) > params.sonar_err) {
 						/* correction is too large: spike or new ground level? */
-						if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
+						if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
 							/* spike detected, ignore */
-							sonar_corr = 0.0f;
+							corr_sonar = 0.0f;
 							sonar_valid = false;
 
 						} else {
 							/* new ground level */
-							surface_offset -= sonar_corr;
+							surface_offset -= corr_sonar;
 							surface_offset_rate = 0.0f;
-							sonar_corr = 0.0f;
-							sonar_corr_filtered = 0.0f;
+							corr_sonar = 0.0f;
+							corr_sonar_filtered = 0.0f;
 							sonar_valid_time = t;
 							sonar_valid = true;
 							local_pos.surface_bottom_timestamp = t;
@@ -494,21 +498,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					}
 
 					/* velocity correction */
-					flow_corr[0] = flow_v[0] - x_est[1];
-					flow_corr[1] = flow_v[1] - y_est[1];
+					corr_flow[0] = flow_v[0] - x_est[1];
+					corr_flow[1] = flow_v[1] - y_est[1];
 					/* adjust correction weight */
 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
-					flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
+					w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
 
 					/* if flow is not accurate, reduce weight for it */
 					// TODO make this more fuzzy
 					if (!flow_accurate)
-						flow_w *= 0.05f;
+						w_flow *= 0.05f;
 
 					flow_valid = true;
 
 				} else {
-					flow_w = 0.0f;
+					w_flow = 0.0f;
 					flow_valid = false;
 				}
 
@@ -524,13 +528,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (gps.fix_type >= 3) {
 					/* hysteresis for GPS quality */
 					if (gps_valid) {
-						if (gps.eph_m > 10.0f) {
+						if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
 							gps_valid = false;
 							mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 						}
 
 					} else {
-						if (gps.eph_m < 5.0f) {
+						if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
 							gps_valid = true;
 							mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
 						}
@@ -542,50 +546,58 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if (gps_valid) {
 					/* initialize reference position if needed */
-					if (!ref_xy_inited) {
-						if (ref_xy_init_start == 0) {
-							ref_xy_init_start = t;
+					if (!ref_inited) {
+						if (ref_init_start == 0) {
+							ref_init_start = t;
 
-						} else if (t > ref_xy_init_start + ref_xy_init_delay) {
-							ref_xy_inited = true;
+						} else if (t > ref_init_start + ref_init_delay) {
+							ref_inited = true;
 							/* reference GPS position */
 							double lat = gps.lat * 1e-7;
 							double lon = gps.lon * 1e-7;
+							float alt = gps.alt * 1e-3;
 
 							local_pos.ref_lat = gps.lat;
 							local_pos.ref_lon = gps.lon;
+							local_pos.ref_alt = alt + z_est[0];
 							local_pos.ref_timestamp = t;
 
 							/* initialize projection */
 							map_projection_init(lat, lon);
-							warnx("init ref: lat=%.7f, lon=%.7f", lat, lon);
-							mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f", lat, lon);
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
 						}
 					}
 
-					if (ref_xy_inited) {
+					if (ref_inited) {
 						/* project GPS lat lon to plane */
 						float gps_proj[2];
 						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
 						/* calculate correction for position */
-						gps_corr[0][0] = gps_proj[0] - x_est[0];
-						gps_corr[1][0] = gps_proj[1] - y_est[0];
+						corr_gps[0][0] = gps_proj[0] - x_est[0];
+						corr_gps[1][0] = gps_proj[1] - y_est[0];
+						corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
 
 						/* calculate correction for velocity */
 						if (gps.vel_ned_valid) {
-							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
-							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+							corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
+							corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
+							corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
 
 						} else {
-							gps_corr[0][1] = 0.0f;
-							gps_corr[1][1] = 0.0f;
+							corr_gps[0][1] = 0.0f;
+							corr_gps[1][1] = 0.0f;
+							corr_gps[2][1] = 0.0f;
 						}
+
+						w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
+						w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
 					}
 
 				} else {
 					/* no GPS lock */
-					memset(gps_corr, 0, sizeof(gps_corr));
-					ref_xy_init_start = 0;
+					memset(corr_gps, 0, sizeof(corr_gps));
+					ref_init_start = 0;
 				}
 
 				gps_updates++;
@@ -609,7 +621,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		/* check for sonar measurement timeout */
 		if (sonar_valid && t > sonar_time + sonar_timeout) {
-			sonar_corr = 0.0f;
+			corr_sonar = 0.0f;
 			sonar_valid = false;
 		}
 
@@ -617,12 +629,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		t_prev = t;
 
 		/* use GPS if it's valid and reference position initialized */
-		bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f;
+		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
+		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
 		/* use flow if it's valid and (accurate or no GPS available) */
-		bool use_flow = flow_valid && (flow_accurate || !use_gps);
+		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
 
 		/* try to estimate position during some time after position sources lost */
-		if (use_gps || use_flow) {
+		if (use_gps_xy || use_flow) {
 			xy_src_time = t;
 		}
 
@@ -636,36 +649,47 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			/* surface distance correction */
 			if (sonar_valid) {
-				surface_offset_rate -= sonar_corr * 0.5f * params.w_alt_sonar * params.w_alt_sonar * dt;
-				surface_offset -= sonar_corr * params.w_alt_sonar * dt;
+				surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
+				surface_offset -= corr_sonar * params.w_z_sonar * dt;
 			}
 		}
 
-		/* reduce GPS weight if optical flow is good */
-		float w_pos_gps_p = params.w_pos_gps_p;
-		float w_pos_gps_v = params.w_pos_gps_v;
+		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
+		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
+		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
+		float w_z_gps_v = params.w_z_gps_v * w_gps_z;
 
+		/* reduce GPS weight if optical flow is good */
 		if (use_flow && flow_accurate) {
-			w_pos_gps_p *= params.w_gps_flow;
-			w_pos_gps_v *= params.w_gps_flow;
+			w_xy_gps_p *= params.w_gps_flow;
+			w_xy_gps_v *= params.w_gps_flow;
 		}
 
+		/* baro offset correction */
+        if (use_gps_z) {
+            baro_offset += corr_gps[2][0] * w_z_gps_p * dt;
+        }
+
 		/* accelerometer bias correction */
 		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
 
-		if (use_gps) {
-			accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p;
-			accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v;
-			accel_bias_corr[1] -= gps_corr[1][0] * w_pos_gps_p * w_pos_gps_p;
-			accel_bias_corr[1] -= gps_corr[1][1] * w_pos_gps_v;
+		if (use_gps_xy) {
+			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
+			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
+			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
+			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
+		}
+		if (use_gps_z) {
+			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
+			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
 		}
 
 		if (use_flow) {
-			accel_bias_corr[0] -= flow_corr[0] * params.w_pos_flow;
-			accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow;
+			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
+			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
 		}
 
-		accel_bias_corr[2] -= baro_corr * params.w_alt_baro * params.w_alt_baro;
+		accel_bias_corr[2] -= (corr_baro + baro_offset) * params.w_z_baro * params.w_z_baro;
 
 		/* transform error vector from NED frame to body frame */
 		for (int i = 0; i < 3; i++) {
@@ -675,15 +699,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				c += att.R[j][i] * accel_bias_corr[j];
 			}
 
-			accel_bias[i] += c * params.w_acc_bias * dt;
+			acc_bias[i] += c * params.w_acc_bias * dt;
 		}
 
 		/* inertial filter prediction for altitude */
 		inertial_filter_predict(dt, z_est);
 
 		/* inertial filter correction for altitude */
-		inertial_filter_correct(baro_corr, dt, z_est, 0, params.w_alt_baro);
-		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
+		inertial_filter_correct(corr_baro + baro_offset, dt, z_est, 0, params.w_z_baro);
+		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+		inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
+		inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
 
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
@@ -696,27 +722,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 
 			/* inertial filter correction for position */
-			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
-			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
+			inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
+			inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
 
 			if (use_flow) {
-				inertial_filter_correct(flow_corr[0], dt, x_est, 1, params.w_pos_flow * flow_w);
-				inertial_filter_correct(flow_corr[1], dt, y_est, 1, params.w_pos_flow * flow_w);
+				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
+				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
 			}
 
-			if (use_gps) {
-				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, w_pos_gps_p);
-				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, w_pos_gps_p);
+			if (use_gps_xy) {
+				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
+				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_v);
 
 				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
-					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, w_pos_gps_v);
-					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, w_pos_gps_v);
+					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
+					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
 				}
 			}
 
 			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
 				warnx("BAD ESTIMATE AFTER CORRECTION dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]);
-				warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", accel_corr[0], accel_corr[1], gps_corr[0][0], gps_corr[0][1], gps_corr[1][0], gps_corr[1][1]);
+				warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", corr_acc[0], corr_acc[1], corr_gps[0][0], corr_gps[0][1], corr_gps[1][0], corr_gps[1][1]);
 				thread_should_exit = true;
 			}
 		}
@@ -775,9 +801,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (t > pub_last + pub_interval) {
 			pub_last = t;
 			/* publish local position */
-			local_pos.xy_valid = can_estimate_xy && use_gps;
+			local_pos.xy_valid = can_estimate_xy && use_gps_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
-			local_pos.xy_global = local_pos.xy_valid && use_gps;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
+			local_pos.z_global = local_pos.z_valid && use_gps_z;
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
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 b3f46fb99..77299bd71 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -40,13 +40,15 @@
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
-PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 20.0f);
-PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.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, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
@@ -59,13 +61,15 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
-	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
-	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
-	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
-	h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
-	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_z_baro = param_find("INAV_W_Z_BARO");
+	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
+	h->w_z_acc = param_find("INAV_W_Z_ACC");
+	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
+	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
+	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
+	h->w_xy_acc = param_find("INAV_W_XY_ACC");
+	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
@@ -81,13 +85,15 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 
 int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
 {
-	param_get(h->w_alt_baro, &(p->w_alt_baro));
-	param_get(h->w_alt_acc, &(p->w_alt_acc));
-	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
-	param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
-	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_z_baro, &(p->w_z_baro));
+	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
+	param_get(h->w_z_gps_v, &(p->w_z_gps_v));
+	param_get(h->w_z_acc, &(p->w_z_acc));
+	param_get(h->w_z_sonar, &(p->w_z_sonar));
+	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
+	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
+	param_get(h->w_xy_acc, &(p->w_xy_acc));
+	param_get(h->w_xy_flow, &(p->w_xy_flow));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));
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 e394edfa4..f583f4b7d 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -41,13 +41,15 @@
 #include <systemlib/param/param.h>
 
 struct position_estimator_inav_params {
-	float w_alt_baro;
-	float w_alt_acc;
-	float w_alt_sonar;
-	float w_pos_gps_p;
-	float w_pos_gps_v;
-	float w_pos_acc;
-	float w_pos_flow;
+	float w_z_baro;
+	float w_z_gps_p;
+	float w_z_gps_v;
+	float w_z_acc;
+	float w_z_sonar;
+	float w_xy_gps_p;
+	float w_xy_gps_v;
+	float w_xy_acc;
+	float w_xy_flow;
 	float w_gps_flow;
 	float w_acc_bias;
 	float flow_k;
@@ -60,13 +62,15 @@ struct position_estimator_inav_params {
 };
 
 struct position_estimator_inav_param_handles {
-	param_t w_alt_baro;
-	param_t w_alt_acc;
-	param_t w_alt_sonar;
-	param_t w_pos_gps_p;
-	param_t w_pos_gps_v;
-	param_t w_pos_acc;
-	param_t w_pos_flow;
+	param_t w_z_baro;
+	param_t w_z_gps_p;
+	param_t w_z_gps_v;
+	param_t w_z_acc;
+	param_t w_z_sonar;
+	param_t w_xy_gps_p;
+	param_t w_xy_gps_v;
+	param_t w_xy_acc;
+	param_t w_xy_flow;
 	param_t w_gps_flow;
 	param_t w_acc_bias;
 	param_t flow_k;