fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 07:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Switch to Hamilton quaternions and add Cholesky decomposition.

Browse Source
This commit is contained in:
James Goppert
2017-02-03 16:38:39 -05:00
committed by Matthias Grob
parent 471e96ff6f
commit e595ebb9a7
2 changed files with 21 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+16 -7
View File
@@ -4,11 +4,20 @@
* All rotations and axis systems follow the right-hand rule.
* The Hamilton quaternion product definition is used.
*
* In order to rotate a vector v by a righthand rotation defined by the quaternion q
* In order to rotate a vector in frame b (v_b) to frame n by a righthand
* rotation defined by the quaternion q_nb (from frame b to n)
* one can use the following operation:
* v_rotated = q^(-1) * [0;v] * q
* where q^(-1) represents the inverse of the quaternion q.
* The product z of two quaternions z = q1 * q2 represents an intrinsic rotation
* v_n = q_nb * [0;v_b] * q_nb^-1
*
* Just like DCM's: v_n = C_nb * v_b (vector rotation)
* M_n = C_nb * M_b * C_nb^(-1) (matrix rotation)
*
* or similarly
* v_b = q_nb^1 * [0;v_n] * q_nb
*
* where q_nb^(-1) represents the inverse of the quaternion q_nb = q_bn
*
* The product z of two quaternions z = q2 * q1 represents an intrinsic rotation
* in the order of first q1 followed by q2.
* The first element of the quaternion
* represents the real part, thus, a quaternion representing a zero-rotation
@@ -211,9 +220,9 @@ public:
const Quaternion &p = *this;
Quaternion r;
r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3);
r(1) = p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2);
r(2) = p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1);
r(3) = p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
r(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2);
r(2) = p(0) * q(2) - p(1) * q(3) + p(2) * q(0) + p(3) * q(1);
r(3) = p(0) * q(3) + p(1) * q(2) - p(2) * q(1) + p(3) * q(0);
return r;
}