fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
matrix/Quaternion.hpp
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;
}

10
test/attitude.cpp
View File

@ -170,13 +170,13 @@ int main()
// quaternion derivative in frame 1
Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
float data_q_dot1_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot1_check(data_q_dot1_check);
TEST(isEqual(q1_dot1, q1_dot1_check));
// quaternion derivative in frame 2
Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
float data_q_dot2_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
Vector<float, 4> q1_dot2_check(data_q_dot2_check);
TEST(isEqual(q1_dot2, q1_dot2_check));
@ -296,8 +296,8 @@ int main()
// conjugate
Vector3f v1(1.5f, 2.2f, 3.2f);
TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q)*v1));
TEST(isEqual(q.conjugate(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
AxisAnglef aa_q_init(q);
TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
@ -319,7 +319,7 @@ int main()
Dcmf dcm3(Eulerf(1, 2, 3));
Dcmf dcm4(Eulerf(4, 5, 6));
Dcmf dcm34 = dcm3 * dcm4;
TEST(isEqual(Eulerf(Quatf(dcm4)*Quatf(dcm3)), Eulerf(dcm34)));
TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
// check corner cases of matrix to quaternion conversion
q = Quatf(0,1,0,0); // 180 degree rotation around the x axis