Merge pull request #2013 from PX4/attitude_estimator_q

attitude_estimator_q added

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

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
 

src/lib/mathlib/math/Matrix.hpp

@@ -135,6 +135,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
 	 */

src/lib/mathlib/math/Quaternion.hpp

@@ -93,6 +93,19 @@ public:
 			       data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
 	}
 
+	/**
+	 * 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
 	 */
@@ -108,6 +121,69 @@ public:
 		return Q * v * 0.5f;
 	}
 
+	/**
+	 * conjugate
+	 */
+	Quaternion conjugated() const {
+		return Quaternion(data[0], -data[1], -data[2], -data[3]);
+	}
+
+	/**
+	 * inversed
+	 */
+	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)
+		);
+	}
+
+	/**
+	 * 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
 	 */
@@ -115,35 +191,6 @@ public:
 		return Vector<3>(&data[1]);
 	}
 
-	/**
-	 * inverse of quaternion
-	 */
-	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;
-	}
-
-
-	/**
-	* 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;
-	}
-
 	/**
 	 * set quaternion to rotation defined by euler angles
 	 */
@@ -164,6 +211,17 @@ public:
 		data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
 	}
 
+	/**
+	 * 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

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

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

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;
-}
+}