Merge branch 'master' into seatbelt_multirotor

6 files changed, 695 insertions, 0 deletions

diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c
new file mode 100644
index 000000000..64031ee7b
--- /dev/null
+++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c
@@ -0,0 +1,27 @@
+/*
+ * 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_inertial_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];
+		}
+	}
+}
diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h
new file mode 100644
index 000000000..3e5134c33
--- /dev/null
+++ b/src/modules/position_estimator_inav/kalman_filter_inertial.h
@@ -0,0 +1,12 @@
+/*
+ * 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_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
new file mode 100644
index 000000000..8ac701c53
--- /dev/null
+++ b/src/modules/position_estimator_inav/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build position_estimator_inav
+#
+
+MODULE_COMMAND	 	= position_estimator_inav
+SRCS		 	= position_estimator_inav_main.c \
+			position_estimator_inav_params.c \
+			kalman_filter_inertial.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
new file mode 100644
index 000000000..2b485f895
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -0,0 +1,441 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator_inav_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/conversions.h>
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_inav_params.h"
+#include "kalman_filter_inertial.h"
+
+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 */
+static bool verbose_mode = false;
+
+__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
+
+int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
+/**
+ * Print the correct usage.
+ */
+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);
+	}
+
+/**
+ * The position_estimator_inav_thread only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int position_estimator_inav_main(int argc, char *argv[]) {
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+		if (thread_running) {
+			printf("position_estimator_inav already running\n");
+			/* 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 );
+		exit(0);
+	}
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	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);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_inav_thread_main(int argc, char *argv[]) {
+	/* welcome user */
+	printf("[position_estimator_inav] started\n");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[position_estimator_inav] started");
+
+	/* initialize values */
+	float x_est[3] = { 0.0f, 0.0f, 0.0f };
+	float y_est[3] = { 0.0f, 0.0f, 0.0f };
+	float z_est[3] = { 0.0f, 0.0f, 0.0f };
+
+	int baro_loop_cnt = 0;
+	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
+
+	/* declare and safely initialize all structs */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	/* make sure that baroINITdone = false */
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_local_position_s pos;
+	memset(&pos, 0, sizeof(pos));
+
+	/* subscribe */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	/* advertise */
+	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos);
+
+	struct position_estimator_inav_params params;
+	struct position_estimator_inav_param_handles pos_inav_param_handles;
+	/* 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*/
+	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 */
+	parameters_update(&pos_inav_param_handles, &params);
+	/* END FIRST PARAMETER UPDATE */
+
+	/* wait until gps signal turns valid, only then can we initialize the projection */
+	if (params.use_gps) {
+		struct pollfd fds_init[1] = {
+				{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+		};
+
+		while (gps.fix_type < 3) {
+			if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */
+				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");
+			}
+			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;
+		alt_current = gps.alt * 1e-3;
+
+		pos.home_lat = lat_current * 1e7;
+		pos.home_lon = lon_current * 1e7;
+		pos.home_timestamp = hrt_absolute_time();
+
+		/* initialize coordinates */
+		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;
+	thread_running = true;
+	uint32_t accelerometer_counter = 0;
+	uint32_t baro_counter = 0;
+	uint16_t accelerometer_updates = 0;
+	uint16_t baro_updates = 0;
+	uint16_t gps_updates = 0;
+	uint16_t attitude_updates = 0;
+	hrt_abstime updates_counter_start = hrt_absolute_time();
+	uint32_t updates_counter_len = 1000000;
+	hrt_abstime pub_last = hrt_absolute_time();
+	uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+
+	/* 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 }
+	};
+	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 };
+
+		int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		if (ret < 0) {
+			/* poll error */
+			printf("[position_estimator_inav] subscriptions poll error\n");
+			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 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 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) {
+					accelerometer_updated = true;
+					accelerometer_counter = sensor.accelerometer_counter;
+					accelerometer_updates++;
+				}
+				if (sensor.baro_counter > baro_counter) {
+					baro_updated = true;
+					baro_counter = sensor.baro_counter;
+					baro_updates++;
+				}
+				// barometric pressure estimation at start up
+				if (!local_flag_baroINITdone && baro_updated) {
+					// 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);
+						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);
+					}
+				}
+			}
+			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);
+					gps_updated = true;
+					pos.valid = gps.fix_type >= 3;
+					gps_updates++;
+				}
+			} else {
+				pos.valid = true;
+			}
+
+		} /* end of poll return value check */
+
+		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 };
+			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_alt, use_z);
+			}
+
+			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[2];	// position, acceleration
+				float y_meas[2];	// position, acceleration
+				bool use_xy[2] = { false, false };
+				if (gps_updated) {
+					x_meas[0] = local_pos_gps[0];
+					y_meas[0] = local_pos_gps[1];
+					use_xy[0] = true;
+				}
+				if (accelerometer_updated) {
+					x_meas[1] = accel_NED[0];
+					y_meas[1] = accel_NED[1];
+					use_xy[1] = true;
+				}
+				if (use_xy[0] || use_xy[1]) {
+					/* correction */
+					kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy);
+					kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy);
+				}
+			}
+		}
+		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);
+				updates_counter_start = t;
+				accelerometer_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];
+			pos.vx = x_est[1];
+			pos.y = y_est[0];
+			pos.vy = y_est[1];
+			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))) {
+				orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos);
+			}
+		}
+	}
+
+	printf("[position_estimator_inav] exiting.\n");
+	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
new file mode 100644
index 000000000..8466bcd0a
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @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_FLOAT(INAV_K_POS_00, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_01, 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_20, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f);
+
+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_10 = param_find("INAV_K_POS_10");
+	h->k_pos_11 = param_find("INAV_K_POS_11");
+	h->k_pos_20 = param_find("INAV_K_POS_20");
+	h->k_pos_21 = param_find("INAV_K_POS_21");
+
+	return OK;
+}
+
+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_10, &(p->k_pos[1][0]));
+	param_get(h->k_pos_11, &(p->k_pos[1][1]));
+	param_get(h->k_pos_20, &(p->k_pos[2][0]));
+	param_get(h->k_pos_21, &(p->k_pos[2][1]));
+
+	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
new file mode 100644
index 000000000..8cdc2e10f
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_inav_params.h
+ * 
+ * Parameters for Position Estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct position_estimator_inav_params {
+	int use_gps;
+	float k_alt[3][2];
+	float k_pos[3][2];
+};
+
+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_10;
+	param_t k_pos_11;
+	param_t k_pos_20;
+	param_t k_pos_21;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_inav_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);