Complete position estimator implemented (GPS + Baro + Accel)

8 files changed, 207 insertions, 241 deletions

diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 000000000..b70d3504e
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,28 @@
+/*
+ * inertial_filter.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float dt, float x[3], int i, float z, float w)
+{
+	float e = z - x[i];
+	x[i] += e * w * dt;
+
+	if (i == 0) {
+		x[1] += e * w * w * dt;
+		//x[2] += e * w * w * w * dt / 3.0; // ~ 0.0
+
+	} else if (i == 1) {
+		x[2] += e * w * w * dt;
+	}
+}
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
new file mode 100644
index 000000000..18c194abf
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -0,0 +1,13 @@
+/*
+ * inertial_filter.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void inertial_filter_predict(float dt, float x[3]);
+
+void inertial_filter_correct(float dt, float x[3], int i, float z, float w);
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c
deleted file mode 100644
index 390e1b720..000000000
--- a/src/modules/position_estimator_inav/kalman_filter_inertial.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * kalman_filter_inertial.c
- *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
- */
-
-#include "kalman_filter_inertial.h"
-
-void kalman_filter_inertial_predict(float dt, float x[3]) {
-	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
-	x[1] += x[2] * dt;
-}
-
-void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) {
-	float y[2];
-	// y = z - H x
-	y[0] = z[0] - x[0];
-	y[1] = z[1] - x[2];
-	// x = x + K * y
-	for (int i = 0; i < 3; i++) {		// Row
-		for (int j = 0; j < 2; j++) {	// Column
-			if (use[j])
-				x[i] += k[i][j] * y[j];
-		}
-	}
-}
-
-void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) {
-	float y[2];
-	// y = z - H x
-	y[0] = z[0] - x[0];
-	y[1] = z[1] - x[1];
-	y[2] = z[2] - x[2];
-	// x = x + K * y
-	for (int i = 0; i < 3; i++) {		// Row
-		for (int j = 0; j < 3; j++) {	// Column
-			if (use[j])
-				x[i] += k[i][j] * y[j];
-		}
-	}
-}
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h
deleted file mode 100644
index d34f58298..000000000
--- a/src/modules/position_estimator_inav/kalman_filter_inertial.h
+++ /dev/null
@@ -1,14 +0,0 @@
-/*
- * kalman_filter_inertial.h
- *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
- */
-
-#include <stdbool.h>
-
-void kalman_filter_inertial_predict(float dt, float x[3]);
-
-void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]);
-
-void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index 8ac701c53..939d76849 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 	= position_estimator_inav
 SRCS		 	= position_estimator_inav_main.c \
 			position_estimator_inav_params.c \
-			kalman_filter_inertial.c
+			inertial_filter.c
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 6ecdfe01d..306ebe5cc 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -66,7 +66,7 @@
 #include <drivers/drv_hrt.h>
 
 #include "position_estimator_inav_params.h"
-#include "kalman_filter_inertial.h"
+#include "inertial_filter.h"
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
@@ -82,13 +82,15 @@ static void usage(const char *reason);
 /**
  * Print the correct usage.
  */
-static void usage(const char *reason) {
+static void usage(const char *reason)
+{
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-		fprintf(stderr,
-				"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
-		exit(1);
-	}
+
+	fprintf(stderr,
+		"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	exit(1);
+}
 
 /**
  * The position_estimator_inav_thread only briefly exists to start
@@ -98,7 +100,8 @@ static void usage(const char *reason) {
  * The actual stack size should be set in the call
  * to task_create().
  */
-int position_estimator_inav_main(int argc, char *argv[]) {
+int position_estimator_inav_main(int argc, char *argv[])
+{
 	if (argc < 1)
 		usage("missing command");
 
@@ -108,17 +111,19 @@ int position_estimator_inav_main(int argc, char *argv[]) {
 			/* this is not an error */
 			exit(0);
 		}
+
 		if (argc > 1)
 			if (!strcmp(argv[2], "-v"))
 				verbose_mode = true;
 
 		thread_should_exit = false;
 		position_estimator_inav_task = task_spawn("position_estimator_inav",
-				SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
-				position_estimator_inav_thread_main,
-				(argv) ? (const char **) &argv[2] : (const char **) NULL );
+					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       position_estimator_inav_thread_main,
+					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
 		exit(0);
 	}
+
 	if (!strcmp(argv[1], "stop")) {
 		thread_should_exit = true;
 		exit(0);
@@ -127,9 +132,11 @@ int position_estimator_inav_main(int argc, char *argv[]) {
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			printf("\tposition_estimator_inav is running\n");
+
 		} else {
 			printf("\tposition_estimator_inav not started\n");
 		}
+
 		exit(0);
 	}
 
@@ -140,10 +147,10 @@ int position_estimator_inav_main(int argc, char *argv[]) {
 /****************************************************************************
  * main
  ****************************************************************************/
-int position_estimator_inav_thread_main(int argc, char *argv[]) {
-	/* welcome user */
-	printf("[position_estimator_inav] started\n");
-	static int mavlink_fd;
+int position_estimator_inav_thread_main(int argc, char *argv[])
+{
+	warnx("started.");
+	int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	mavlink_log_info(mavlink_fd, "[position_estimator_inav] started");
 
@@ -156,9 +163,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	int baro_loop_end = 70; /* measurement for 1 second */
 	float baro_alt0 = 0.0f; /* to determine while start up */
 
-	static double lat_current = 0.0; //[°]] --> 47.0
-	static double lon_current = 0.0; //[°]] -->8.5
-	static double alt_current = 0.0; //[m] above MSL
+	double lat_current = 0.0; //[°]] --> 47.0
+	double lon_current = 0.0; //[°]] -->8.5
+	double alt_current = 0.0; //[m] above MSL
 
 	/* declare and safely initialize all structs */
 	struct vehicle_status_s vehicle_status;
@@ -188,40 +195,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 	/* initialize parameter handles */
 	parameters_init(&pos_inav_param_handles);
 
-	bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */
-	/* FIRST PARAMETER READ at START UP*/
+	bool local_flag_baroINITdone = false;
+	/* first parameters read at start up */
 	struct parameter_update_s param_update;
-	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param to clear updated flag */
-	/* FIRST PARAMETER UPDATE */
+	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
+	/* first parameters update */
 	parameters_update(&pos_inav_param_handles, &params);
-	/* END FIRST PARAMETER UPDATE */
 
-	/* wait until gps signal turns valid, only then can we initialize the projection */
+	/* wait for GPS fix, only then can we initialize the projection */
 	if (params.use_gps) {
 		struct pollfd fds_init[1] = {
-				{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+			{ .fd = vehicle_gps_position_sub, .events = POLLIN }
 		};
 
 		while (gps.fix_type < 3) {
-			if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */
+			if (poll(fds_init, 1, 5000)) {
 				if (fds_init[0].revents & POLLIN) {
 					/* Wait for the GPS update to propagate (we have some time) */
 					usleep(5000);
 					orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 				}
 			}
+
 			static int printcounter = 0;
+
 			if (printcounter == 100) {
 				printcounter = 0;
-				printf("[position_estimator_inav] wait for GPS fix type 3\n");
+				warnx("waiting for GPS fix type 3...");
 			}
+
 			printcounter++;
 		}
 
 		/* get GPS position for first initialization */
 		orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
-		lat_current = ((double) (gps.lat)) * 1e-7;
-		lon_current = ((double) (gps.lon)) * 1e-7;
+		lat_current = ((double)(gps.lat)) * 1e-7;
+		lon_current = ((double)(gps.lon)) * 1e-7;
 		alt_current = gps.alt * 1e-3;
 
 		pos.home_lat = lat_current * 1e7;
@@ -232,15 +241,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 		map_projection_init(lat_current, lon_current);
 		/* publish global position messages only after first GPS message */
 	}
-	printf(
-			"[position_estimator_inav] initialized projection with: lat: %.10f,  lon:%.10f\n",
-			lat_current, lon_current);
 
-	hrt_abstime last_time = 0;
+	warnx("initialized projection with: lat: %.10f,  lon:%.10f", lat_current, lon_current);
+
+	hrt_abstime t_prev = 0;
 	thread_running = true;
-	uint32_t accelerometer_counter = 0;
+	uint32_t accel_counter = 0;
+	hrt_abstime accel_t = 0;
+	float accel_dt = 0.0f;
 	uint32_t baro_counter = 0;
-	uint16_t accelerometer_updates = 0;
+	hrt_abstime baro_t = 0;
+	hrt_abstime gps_t = 0;
+	uint16_t accel_updates = 0;
 	uint16_t baro_updates = 0;
 	uint16_t gps_updates = 0;
 	uint16_t attitude_updates = 0;
@@ -251,173 +263,184 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 
 	/* main loop */
 	struct pollfd fds[5] = {
-			{ .fd = parameter_update_sub, .events = POLLIN },
-			{ .fd = vehicle_status_sub, .events = POLLIN },
-			{ .fd = vehicle_attitude_sub, .events = POLLIN },
-			{ .fd = sensor_combined_sub, .events = POLLIN },
-			{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+		{ .fd = parameter_update_sub, .events = POLLIN },
+		{ .fd = vehicle_status_sub, .events = POLLIN },
+		{ .fd = vehicle_attitude_sub, .events = POLLIN },
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
 	};
 	printf("[position_estimator_inav] main loop started\n");
+
 	while (!thread_should_exit) {
 		bool accelerometer_updated = false;
 		bool baro_updated = false;
 		bool gps_updated = false;
-		float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f };
+		float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f };
 
 		int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		hrt_abstime t = hrt_absolute_time();
+
 		if (ret < 0) {
 			/* poll error */
-			printf("[position_estimator_inav] subscriptions poll error\n");
+			warnx("subscriptions poll error.");
 			thread_should_exit = true;
 			continue;
+
 		} else if (ret > 0) {
 			/* parameter update */
 			if (fds[0].revents & POLLIN) {
 				/* read from param to clear updated flag */
 				struct parameter_update_s update;
 				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
-						&update);
+					 &update);
 				/* update parameters */
 				parameters_update(&pos_inav_param_handles, &params);
 			}
+
 			/* vehicle status */
 			if (fds[1].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_status), vehicle_status_sub,
-						&vehicle_status);
+					 &vehicle_status);
 			}
+
 			/* vehicle attitude */
 			if (fds[2].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
 				attitude_updates++;
 			}
+
 			/* sensor combined */
 			if (fds[3].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
-				if (sensor.accelerometer_counter > accelerometer_counter) {
+
+				if (sensor.accelerometer_counter > accel_counter) {
 					accelerometer_updated = true;
-					accelerometer_counter = sensor.accelerometer_counter;
-					accelerometer_updates++;
+					accel_counter = sensor.accelerometer_counter;
+					accel_updates++;
+					accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f;
+					accel_t = t;
 				}
+
 				if (sensor.baro_counter > baro_counter) {
 					baro_updated = true;
 					baro_counter = sensor.baro_counter;
 					baro_updates++;
 				}
-				// barometric pressure estimation at start up
+
+				/* barometric pressure estimation at start up */
 				if (!local_flag_baroINITdone && baro_updated) {
-					// mean calculation over several measurements
+					/* mean calculation over several measurements */
 					if (baro_loop_cnt < baro_loop_end) {
 						baro_alt0 += sensor.baro_alt_meter;
 						baro_loop_cnt++;
+
 					} else {
-						baro_alt0 /= (float) (baro_loop_cnt);
+						baro_alt0 /= (float)(baro_loop_cnt);
 						local_flag_baroINITdone = true;
-						char str[80];
-						sprintf(str,
-								"[position_estimator_inav] baro_alt0 = %.2f",
-								baro_alt0);
-						printf("%s\n", str);
-						mavlink_log_info(mavlink_fd, str);
+						mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0);
 					}
 				}
 			}
+
 			if (params.use_gps) {
 				/* vehicle GPS position */
 				if (fds[4].revents & POLLIN) {
 					/* new GPS value */
 					orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
-					/* Project gps lat lon (Geographic coordinate system) to plane */
-					map_projection_project(((double) (gps.lat)) * 1e-7,
-							((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
-							&(local_pos_gps[1]));
-					local_pos_gps[2] = (float) (gps.alt * 1e-3);
+					/* project GPS lat lon (Geographic coordinate system) to plane */
+					map_projection_project(((double)(gps.lat)) * 1e-7,
+							       ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]),
+							       &(proj_pos_gps[1]));
+					proj_pos_gps[2] = (float)(gps.alt * 1e-3);
 					gps_updated = true;
 					pos.valid = gps.fix_type >= 3;
 					gps_updates++;
 				}
+
 			} else {
 				pos.valid = true;
 			}
 
-		} /* end of poll return value check */
+		}
+
+		/* end of poll return value check */
+
+		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f;
+		t_prev = t;
 
-		hrt_abstime t = hrt_absolute_time();
-		float dt = (t - last_time) / 1000000.0;
-		last_time = t;
 		if (att.R_valid) {
 			/* 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] * sensor.accelerometer_m_s2[j];
 				}
 			}
+
 			accel_NED[2] += CONSTANTS_ONE_G;
 
-			/* kalman filter for altitude */
-			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 };
+			/* inertial filter prediction for altitude */
+			inertial_filter_predict(dt, z_est);
+
+			/* inertial filter correction for altitude */
 			if (local_flag_baroINITdone && baro_updated) {
-				z_meas[0] = baro_alt0 - sensor.baro_alt_meter;	// Z = -alt
-				use_z[0] = true;
+				if (baro_t > 0) {
+					inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro);
+				}
+
+				baro_t = t;
 			}
+
 			if (accelerometer_updated) {
-				z_meas[1] = accel_NED[2];
-				use_z[1] = true;
-			}
-			if (use_z[0] || use_z[1]) {
-				/* correction */
-				kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z);
+				inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc);
 			}
 
 			if (params.use_gps) {
-				/* kalman filter for position */
-				kalman_filter_inertial_predict(dt, x_est);
-				kalman_filter_inertial_predict(dt, y_est);
-				/* prepare vectors for kalman filter correction */
-				float x_meas[3];	// position, velocity, acceleration
-				float y_meas[3];	// position, velocity, acceleration
-				bool use_xy[3] = { false, false, false };
+				/* inertial filter prediction for position */
+				inertial_filter_predict(dt, x_est);
+				inertial_filter_predict(dt, y_est);
+
+				/* inertial filter correction for position */
 				if (gps_updated) {
-					x_meas[0] = local_pos_gps[0];
-					y_meas[0] = local_pos_gps[1];
-					x_meas[1] = gps.vel_n_m_s;
-					y_meas[1] = gps.vel_e_m_s;
-					use_xy[0] = true;
-					use_xy[1] = true;
+					if (gps_t > 0) {
+						float gps_dt = (t - gps_t) / 1000000.0f;
+						inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p);
+						inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v);
+						inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p);
+						inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v);
+					}
+
+					gps_t = t;
 				}
+
 				if (accelerometer_updated) {
-					x_meas[2] = accel_NED[0];
-					y_meas[2] = accel_NED[1];
-					use_xy[2] = true;
-				}
-				if (use_xy[0] || use_xy[2]) {
-					/* correction */
-					kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy);
-					kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy);
+					inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc);
+					inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc);
 				}
 			}
 		}
+
 		if (verbose_mode) {
 			/* print updates rate */
 			if (t - updates_counter_start > updates_counter_len) {
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
 				printf(
-						"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
-						accelerometer_updates / updates_dt,
-						baro_updates / updates_dt,
-						gps_updates / updates_dt,
-						attitude_updates / updates_dt);
+					"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
+					accel_updates / updates_dt,
+					baro_updates / updates_dt,
+					gps_updates / updates_dt,
+					attitude_updates / updates_dt);
 				updates_counter_start = t;
-				accelerometer_updates = 0;
+				accel_updates = 0;
 				baro_updates = 0;
 				gps_updates = 0;
 				attitude_updates = 0;
 			}
 		}
+
 		if (t - pub_last > pub_interval) {
 			pub_last = t;
 			pos.x = x_est[0];
@@ -427,17 +450,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
 			pos.z = z_est[0];
 			pos.vz = z_est[1];
 			pos.timestamp = hrt_absolute_time();
+
 			if ((isfinite(pos.x)) && (isfinite(pos.vx))
-					&& (isfinite(pos.y))
-					&& (isfinite(pos.vy))
-					&& (isfinite(pos.z))
-					&& (isfinite(pos.vz))) {
+			    && (isfinite(pos.y))
+			    && (isfinite(pos.vy))
+			    && (isfinite(pos.z))
+			    && (isfinite(pos.vz))) {
 				orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos);
 			}
 		}
 	}
 
-	printf("[position_estimator_inav] exiting.\n");
+	warnx("exiting.");
 	mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting");
 	thread_running = false;
 	return 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 20142b70c..8dd0ceff8 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -34,73 +34,39 @@
 
 /*
  * @file position_estimator_inav_params.c
- * 
+ *
  * Parameters for position_estimator_inav
  */
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_INT32(INAV_USE_GPS, 0);
-
-PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f);
-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_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_POS_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f);
 
-PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f);
-PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f);
-
-int parameters_init(struct position_estimator_inav_param_handles *h) {
+int parameters_init(struct position_estimator_inav_param_handles *h)
+{
 	h->use_gps = param_find("INAV_USE_GPS");
-
-	h->k_alt_00 = param_find("INAV_K_ALT_00");
-	h->k_alt_01 = param_find("INAV_K_ALT_01");
-	h->k_alt_10 = param_find("INAV_K_ALT_10");
-	h->k_alt_11 = param_find("INAV_K_ALT_11");
-	h->k_alt_20 = param_find("INAV_K_ALT_20");
-	h->k_alt_21 = param_find("INAV_K_ALT_21");
-
-	h->k_pos_00 = param_find("INAV_K_POS_00");
-	h->k_pos_01 = param_find("INAV_K_POS_01");
-	h->k_pos_02 = param_find("INAV_K_POS_02");
-	h->k_pos_10 = param_find("INAV_K_POS_10");
-	h->k_pos_11 = param_find("INAV_K_POS_11");
-	h->k_pos_12 = param_find("INAV_K_POS_12");
-	h->k_pos_20 = param_find("INAV_K_POS_20");
-	h->k_pos_21 = param_find("INAV_K_POS_21");
-	h->k_pos_22 = param_find("INAV_K_POS_22");
+	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
+	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
+	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");
 
 	return OK;
 }
 
-int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) {
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
+{
 	param_get(h->use_gps, &(p->use_gps));
-
-	param_get(h->k_alt_00, &(p->k_alt[0][0]));
-	param_get(h->k_alt_01, &(p->k_alt[0][1]));
-	param_get(h->k_alt_10, &(p->k_alt[1][0]));
-	param_get(h->k_alt_11, &(p->k_alt[1][1]));
-	param_get(h->k_alt_20, &(p->k_alt[2][0]));
-	param_get(h->k_alt_21, &(p->k_alt[2][1]));
-
-	param_get(h->k_pos_00, &(p->k_pos[0][0]));
-	param_get(h->k_pos_01, &(p->k_pos[0][1]));
-	param_get(h->k_pos_02, &(p->k_pos[0][2]));
-	param_get(h->k_pos_10, &(p->k_pos[1][0]));
-	param_get(h->k_pos_11, &(p->k_pos[1][1]));
-	param_get(h->k_pos_12, &(p->k_pos[1][2]));
-	param_get(h->k_pos_20, &(p->k_pos[2][0]));
-	param_get(h->k_pos_21, &(p->k_pos[2][1]));
-	param_get(h->k_pos_22, &(p->k_pos[2][2]));
+	param_get(h->w_alt_baro, &(p->w_alt_baro));
+	param_get(h->w_alt_acc, &(p->w_alt_acc));
+	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));
 
 	return OK;
 }
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 c0e0042b6..870227fef 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -34,7 +34,7 @@
 
 /*
  * @file position_estimator_inav_params.h
- * 
+ *
  * Parameters for Position Estimator
  */
 
@@ -42,29 +42,20 @@
 
 struct position_estimator_inav_params {
 	int use_gps;
-	float k_alt[3][2];
-	float k_pos[3][3];
+	float w_alt_baro;
+	float w_alt_acc;
+	float w_pos_gps_p;
+	float w_pos_gps_v;
+	float w_pos_acc;
 };
 
 struct position_estimator_inav_param_handles {
 	param_t use_gps;
-
-	param_t k_alt_00;
-	param_t k_alt_01;
-	param_t k_alt_10;
-	param_t k_alt_11;
-	param_t k_alt_20;
-	param_t k_alt_21;
-
-	param_t k_pos_00;
-	param_t k_pos_01;
-	param_t k_pos_02;
-	param_t k_pos_10;
-	param_t k_pos_11;
-	param_t k_pos_12;
-	param_t k_pos_20;
-	param_t k_pos_21;
-	param_t k_pos_22;
+	param_t w_alt_baro;
+	param_t w_alt_acc;
+	param_t w_pos_gps_p;
+	param_t w_pos_gps_v;
+	param_t w_pos_acc;
 };
 
 /**