fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 13:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quaternion: Implemented the largest pivot element method for conversion from Dcm to Quaternion to avoid numerical problems

Browse Source
This commit is contained in:
Matthias Grob
2016-12-14 17:55:05 +01:00
parent bb1a321639
commit 2f4427a923
1 changed files with 30 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+30 -15
View File
@@ -90,24 +90,39 @@ public:
*
* @param dcm dcm to set quaternion to
*/
Quaternion(const Dcm<Type> &dcm) :
Quaternion(const Dcm<Type> &R) :
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = 1;
q(1) = 0;
q(2) = 0;
q(3) = 0;
Type q0_tmp = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
if (fabsf(q0_tmp) > 0) {
q(0) = q0_tmp;
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(Type(4) * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(Type(4) * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(Type(4) * q(0)));
Type t = R.trace();
if (t > Type(0)) {
t = sqrtf(Type(1) + t);
q(0) = Type(0.5) * t;
t = Type(0.5) / t;
q(1) = (R(2,1) - R(1,2)) * t;
q(2) = (R(0,2) - R(2,0)) * t;
q(3) = (R(1,0) - R(0,1)) * t;
} else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
t = sqrtf(Type(1) + R(0,0) - R(1,1) - R(2,2));
q(1) = Type(0.5) * t;
t = Type(0.5) / t;
q(0) = (R(2,1) - R(1,2)) * t;
q(2) = (R(1,0) + R(0,1)) * t;
q(3) = (R(0,2) + R(2,0)) * t;
} else if (R(1,1) > R(2,2)) {
t = sqrtf(Type(1) - R(0,0) + R(1,1) - R(2,2));
q(2) = Type(0.5) * t;
t = Type(0.5) / t;
q(0) = (R(0,2) - R(2,0)) * t;
q(1) = (R(1,0) + R(0,1)) * t;
q(3) = (R(2,1) + R(1,2)) * t;
} else {
t = sqrtf(Type(1) - R(0,0) - R(1,1) + R(2,2));
q(3) = Type(0.5) * t;
t = Type(0.5) / t;
q(0) = (R(1,0) - R(0,1)) * t;
q(1) = (R(0,2) + R(2,0)) * t;
q(2) = (R(2,1) + R(1,2)) * t;
}
}