Merge pull request #2013 from PX4/attitude_estimator_q

attitude_estimator_q added

8 files changed, 680 insertions, 31 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 6517e026a..a28ff1bbe 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -8,7 +8,7 @@
 # INAV, which defaults to 0 now. 
 if param compare INAV_ENABLED 1
 then
-	attitude_estimator_ekf start
+	attitude_estimator_q start
 	position_estimator_inav start
 else
 	ekf_att_pos_estimator start
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index cbfda9735..7884b94cb 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -77,6 +77,7 @@ MODULES 	+= modules/land_detector
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_q
 MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f6f4fc5ea..2a7b61238 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -136,6 +136,24 @@ public:
 #endif
 
 	/**
+	 * set row from vector
+	 */
+	void set_row(unsigned int row, const Vector<N> v) {
+		for (unsigned i = 0; i < N; i++) {
+			data[row][i] = v.data[i];
+		}
+	}
+
+	/**
+	 * set column from vector
+	 */
+	void set_col(unsigned int col, const Vector<M> v) {
+		for (unsigned i = 0; i < M; i++) {
+			data[i][col] = v.data[i];
+		}
+	}
+
+	/**
 	 * access by index
 	 */
 	float &operator()(const unsigned int row, const unsigned int col) {
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index d28966fca..b7cb068dd 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -94,6 +94,19 @@ public:
 	}
 
 	/**
+	 * division
+	 */
+	Quaternion operator /(const Quaternion &q) const {
+		float norm = q.length_squared();
+		return Quaternion(
+				(  data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
+			    (- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
+			    (- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
+			    (- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
+		);
+	}
+
+	/**
 	 * derivative
 	 */
 	const Quaternion derivative(const Vector<3> &w) {
@@ -109,39 +122,73 @@ public:
 	}
 
 	/**
-	 * imaginary part of quaternion
+	 * conjugate
 	 */
-	Vector<3> imag(void) {
-		return Vector<3>(&data[1]);
+	Quaternion conjugated() const {
+		return Quaternion(data[0], -data[1], -data[2], -data[3]);
 	}
 
 	/**
-	 * inverse of quaternion
+	 * inversed
 	 */
-	math::Quaternion inverse() {
-		Quaternion res;
-		memcpy(res.data,data,sizeof(res.data));
-		res.data[1] = -res.data[1];
-		res.data[2] = -res.data[2];
-		res.data[3] = -res.data[3];
-		return res;
+	Quaternion inversed() const {
+		float norm = length_squared();
+		return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
 	}
 
+	/**
+	 * conjugation
+	 */
+	Vector<3> conjugate(const Vector<3> &v) const {
+		float q0q0 = data[0] * data[0];
+		float q1q1 = data[1] * data[1];
+		float q2q2 = data[2] * data[2];
+		float q3q3 = data[3] * data[3];
+
+		return Vector<3>(
+				v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
+				v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
+				v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
+
+				v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
+				v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
+				v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
+
+				v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
+				v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
+				v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
+		);
+	}
 
 	/**
-	* rotate vector by quaternion
-	*/
-	Vector<3> rotate(const Vector<3> &w) {
-		Quaternion q_w;	// extend vector to quaternion
-		Quaternion q = {data[0],data[1],data[2],data[3]};
-		Quaternion q_rotated;	// quaternion representation of rotated vector
-		q_w(0) = 0;
-		q_w(1) = w.data[0];
-		q_w(2) = w.data[1];
-		q_w(3) = w.data[2];
-		q_rotated = q*q_w*q.inverse();
-		Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
-		return res;
+	 * conjugation with inversed quaternion
+	 */
+	Vector<3> conjugate_inversed(const Vector<3> &v) const {
+		float q0q0 = data[0] * data[0];
+		float q1q1 = data[1] * data[1];
+		float q2q2 = data[2] * data[2];
+		float q3q3 = data[3] * data[3];
+
+		return Vector<3>(
+				v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
+				v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
+				v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
+
+				v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
+				v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
+				v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
+
+				v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
+				v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
+				v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
+		);
+	}
+
+	/**
+	 * imaginary part of quaternion
+	 */
+	Vector<3> imag(void) {
+		return Vector<3>(&data[1]);
 	}
 
 	/**
@@ -165,6 +212,17 @@ public:
 	}
 
 	/**
+	 * create Euler angles vector from the quaternion
+	 */
+	Vector<3> to_euler() const {
+		return Vector<3>(
+			atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
+			asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
+			atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
+		);
+	}
+
+	/**
 	 * set quaternion to rotation by DCM
 	 * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
 	 */
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
new file mode 100644
index 000000000..a1c56aa1f
--- /dev/null
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -0,0 +1,479 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file attitude_estimator_q_main.cpp
+ *
+ * Attitude estimator (quaternion based)
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <float.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/debug_key_value.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_hrt.h>
+
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
+
+using math::Vector;
+using math::Matrix;
+using math::Quaternion;
+
+class AttitudeEstimatorQ;
+
+namespace attitude_estimator_q {
+AttitudeEstimatorQ *instance;
+}
+
+
+class AttitudeEstimatorQ {
+public:
+	/**
+	 * Constructor
+	 */
+	AttitudeEstimatorQ();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~AttitudeEstimatorQ();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	void		task_main();
+
+private:
+	static constexpr float _dt_max = 0.02;
+	bool	_task_should_exit = false;		/**< if true, task should exit */
+	int		_control_task = -1;			/**< task handle for task */
+
+	int		_sensors_sub = -1;
+	int		_params_sub = -1;
+	int		_global_pos_sub = -1;
+	orb_advert_t		_att_pub = -1;
+
+	struct {
+		param_t	w_acc;
+		param_t	w_mag;
+		param_t	w_gyro_bias;
+		param_t	mag_decl;
+		param_t	mag_decl_auto;
+		param_t	acc_comp;
+		param_t	bias_max;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	float		_w_accel = 0.0f;
+	float		_w_mag = 0.0f;
+	float		_w_gyro_bias = 0.0f;
+	float		_mag_decl = 0.0f;
+	bool		_mag_decl_auto = false;
+	bool		_acc_comp = false;
+	float		_bias_max = 0.0f;
+
+	Vector<3>	_gyro;
+	Vector<3>	_accel;
+	Vector<3>	_mag;
+
+	Quaternion	_q;
+	Vector<3>	_rates;
+	Vector<3>	_gyro_bias;
+
+	vehicle_global_position_s _gpos = {};
+	Vector<3>	_vel_prev;
+	Vector<3>	_pos_acc;
+	hrt_abstime _vel_prev_t = 0;
+
+	bool		_inited = false;
+
+	perf_counter_t _update_perf;
+	perf_counter_t _loop_perf;
+
+	void update_parameters(bool force);
+
+	int update_subscriptions();
+
+	void init();
+
+	void update(float dt);
+};
+
+
+AttitudeEstimatorQ::AttitudeEstimatorQ() {
+	_params_handles.w_acc		= param_find("ATT_W_ACC");
+	_params_handles.w_mag		= param_find("ATT_W_MAG");
+	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
+	_params_handles.mag_decl	= param_find("ATT_MAG_DECL");
+	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
+	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
+	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
+}
+
+/**
+ * Destructor, also kills task.
+ */
+AttitudeEstimatorQ::~AttitudeEstimatorQ() {
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	attitude_estimator_q::instance = nullptr;
+}
+
+int AttitudeEstimatorQ::start() {
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("attitude_estimator_q",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2500,
+				       (main_t)&AttitudeEstimatorQ::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
+	attitude_estimator_q::instance->task_main();
+}
+
+void AttitudeEstimatorQ::task_main() {
+	warnx("started");
+
+    _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+
+	update_parameters(true);
+
+	hrt_abstime last_time = 0;
+
+	struct pollfd fds[1];
+	fds[0].fd = _sensors_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+		int ret = poll(fds, 1, 1000);
+
+		if (ret < 0) {
+			// Poll error, sleep and try again
+			usleep(10000);
+			continue;
+		} else if (ret == 0) {
+			// Poll timeout, do nothing
+			continue;
+		}
+
+		update_parameters(false);
+
+		// Update sensors
+		sensor_combined_s sensors;
+		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
+			_gyro.set(sensors.gyro_rad_s);
+			_accel.set(sensors.accelerometer_m_s2);
+			_mag.set(sensors.magnetometer_ga);
+		}
+
+		bool gpos_updated;
+		orb_check(_global_pos_sub, &gpos_updated);
+		if (gpos_updated) {
+			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
+			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
+				/* set magnetic declination automatically */
+				_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
+			}
+		}
+
+		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) {
+			/* position data is actual */
+			if (gpos_updated) {
+				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
+
+				/* velocity updated */
+				if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
+					float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
+					/* calculate acceleration in body frame */
+					_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
+				}
+				_vel_prev_t = _gpos.timestamp;
+				_vel_prev = vel;
+			}
+
+		} else {
+			/* position data is outdated, reset acceleration */
+			_pos_acc.zero();
+			_vel_prev.zero();
+			_vel_prev_t = 0;
+		}
+
+		// Time from previous iteration
+		uint64_t now = hrt_absolute_time();
+		float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.0f;
+		last_time = now;
+
+		if (dt > _dt_max) {
+			dt = _dt_max;
+		}
+
+		update(dt);
+
+		Vector<3> euler = _q.to_euler();
+
+		struct vehicle_attitude_s att = {};
+		att.timestamp = sensors.timestamp;
+
+		att.roll = euler(0);
+		att.pitch = euler(1);
+		att.yaw = euler(2);
+
+		att.rollspeed = _rates(0);
+		att.pitchspeed = _rates(1);
+		att.yawspeed = _rates(2);
+
+		for (int i = 0; i < 3; i++) {
+			att.g_comp[i] = _accel(i) - _pos_acc(i);
+		}
+
+		/* copy offsets */
+		memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
+
+		Matrix<3, 3> R = _q.to_dcm();
+
+		/* copy rotation matrix */
+		memcpy(&att.R[0], R.data, sizeof(att.R));
+		att.R_valid = true;
+
+		if (_att_pub < 0) {
+			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
+		} else {
+			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
+		}
+	}
+}
+
+void AttitudeEstimatorQ::update_parameters(bool force) {
+	bool updated = force;
+	if (!updated) {
+		orb_check(_params_sub, &updated);
+	}
+	if (updated) {
+		parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+
+		param_get(_params_handles.w_acc, &_w_accel);
+		param_get(_params_handles.w_mag, &_w_mag);
+		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
+		float mag_decl_deg = 0.0f;
+		param_get(_params_handles.mag_decl, &mag_decl_deg);
+		_mag_decl = math::radians(mag_decl_deg);
+		int32_t mag_decl_auto_int;
+		param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
+		_mag_decl_auto = mag_decl_auto_int != 0;
+		int32_t acc_comp_int;
+		param_get(_params_handles.acc_comp, &acc_comp_int);
+		_acc_comp = acc_comp_int != 0;
+		param_get(_params_handles.bias_max, &_bias_max);
+	}
+}
+
+void AttitudeEstimatorQ::init() {
+	// Rotation matrix can be easily constructed from acceleration and mag field vectors
+	// 'k' is Earth Z axis (Down) unit vector in body frame
+	Vector<3> k = -_accel;
+	k.normalize();
+
+	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
+	Vector<3> i = (_mag - k * (_mag * k));
+	i.normalize();
+
+	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
+	Vector<3> j = k % i;
+
+	// Fill rotation matrix
+	Matrix<3, 3> R;
+	R.set_row(0, i);
+	R.set_row(1, j);
+	R.set_row(2, k);
+
+	// Convert to quaternion
+	_q.from_dcm(R);
+}
+
+void AttitudeEstimatorQ::update(float dt) {
+	if (!_inited) {
+		init();
+		_inited = true;
+	}
+
+	// Angular rate of correction
+	Vector<3> corr;
+
+	// Magnetometer correction
+	// Project mag field vector to global frame and extract XY component
+	Vector<3> mag_earth = _q.conjugate(_mag);
+	float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+	// Project magnetometer correction to body frame
+	corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+
+	// Accelerometer correction
+	// Project 'k' unit vector of earth frame to body frame
+	// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
+	// Optimized version with dropped zeros
+	Vector<3> k(
+			2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
+			2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
+			(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
+	);
+
+	corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
+
+	// Gyro bias estimation
+	_gyro_bias += corr * (_w_gyro_bias * dt);
+	for (int i = 0; i < 3; i++) {
+		_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
+	}
+	_rates = _gyro + _gyro_bias;
+
+	// Feed forward gyro
+	corr += _rates;
+
+	// Apply correction to state
+	_q += _q.derivative(corr) * dt;
+
+	// Normalize quaternion
+	_q.normalize(); // TODO! NaN protection???
+}
+
+
+int attitude_estimator_q_main(int argc, char *argv[]) {
+	if (argc < 1) {
+		errx(1, "usage: attitude_estimator_q {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (attitude_estimator_q::instance != nullptr) {
+			errx(1, "already running");
+		}
+
+		attitude_estimator_q::instance = new AttitudeEstimatorQ;
+
+		if (attitude_estimator_q::instance == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != attitude_estimator_q::instance->start()) {
+			delete attitude_estimator_q::instance;
+			attitude_estimator_q::instance = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (attitude_estimator_q::instance == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete attitude_estimator_q::instance;
+		attitude_estimator_q::instance = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (attitude_estimator_q::instance) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
new file mode 100644
index 000000000..fa1592307
--- /dev/null
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file attitude_estimator_q_params.c
+ *
+ * Parameters for attitude estimator (quaternion based)
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <systemlib/param/param.h>
+
+PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
+PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
+PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);		///< magnetic declination, in degrees
+PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);		///< automatic GPS based magnetic declination
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);		///< acceleration compensation
+PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);		///< gyro bias limit, rad/s
diff --git a/src/modules/attitude_estimator_q/module.mk b/src/modules/attitude_estimator_q/module.mk
new file mode 100644
index 000000000..b3688e4a9
--- /dev/null
+++ b/src/modules/attitude_estimator_q/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+#
+# Attitude estimator (quaternion based)
+#
+
+MODULE_COMMAND	 = attitude_estimator_q
+
+SRCS		 = attitude_estimator_q_main.cpp \
+		   attitude_estimator_q_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 3a890c30b..7460f6f55 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -300,7 +300,7 @@ int test_mathlib(int argc, char *argv[])
 					R.from_euler(roll, pitch, yaw);
 					q.from_euler(roll, pitch, yaw);
 					vector_r = R * vector;
-					vector_q = q.rotate(vector);
+					vector_q = q.conjugate(vector);
 
 					for (int i = 0; i < 3; i++) {
 						if (fabsf(vector_r(i) - vector_q(i)) > tol) {
@@ -315,7 +315,7 @@ int test_mathlib(int argc, char *argv[])
 		// test some values calculated with matlab
 		tol = 0.0001f;
 		q.from_euler(M_PI_2_F, 0.0f, 0.0f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -326,7 +326,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		q.from_euler(0.3f, 0.2f, 0.1f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		vector_true = {1.1566, 0.7792, 1.0273};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -337,7 +337,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		q.from_euler(-1.5f, -0.2f, 0.5f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		vector_true = {0.5095, 1.4956, -0.7096};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -348,7 +348,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		vector_true = { -1.3660, 0.3660, 1.0000};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -359,4 +359,4 @@ int test_mathlib(int argc, char *argv[])
 		}
 	}
 	return rc;
-}
\ No newline at end of file
+}