fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 01:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

DCM: use simplified conversion from unit quaternion

Browse Source 
This is exactly equivalent for a unit quaternion (and only unit
quaternions should be used to encode a rotation)
This commit is contained in:
bresch
2023-06-05 11:58:42 -04:00
committed by Daniel Agar
parent 9d7abf2552
commit d03f242c04
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Dcm.hpp
+3 -4
View File
@@ -90,7 +90,6 @@ public:
const Type b = q(1); const Type b = q(1);
const Type c = q(2); const Type c = q(2);
const Type d = q(3); const Type d = q(3);
const Type aa = a * a;
const Type ab = a * b; const Type ab = a * b;
const Type ac = a * c; const Type ac = a * c;
const Type ad = a * d; const Type ad = a * d;
@@ -100,15 +99,15 @@ public:
const Type cc = c * c; const Type cc = c * c;
const Type cd = c * d; const Type cd = c * d;
const Type dd = d * d; const Type dd = d * d;
dcm(0, 0) = aa + bb - cc - dd; dcm(0, 0) = Type(1) - Type(2) * (cc + dd);
dcm(0, 1) = Type(2) * (bc - ad); dcm(0, 1) = Type(2) * (bc - ad);
dcm(0, 2) = Type(2) * (ac + bd); dcm(0, 2) = Type(2) * (ac + bd);
dcm(1, 0) = Type(2) * (bc + ad); dcm(1, 0) = Type(2) * (bc + ad);
dcm(1, 1) = aa - bb + cc - dd; dcm(1, 1) = Type(1) - Type(2) * (bb + dd);
dcm(1, 2) = Type(2) * (cd - ab); dcm(1, 2) = Type(2) * (cd - ab);
dcm(2, 0) = Type(2) * (bd - ac); dcm(2, 0) = Type(2) * (bd - ac);
dcm(2, 1) = Type(2) * (ab + cd); dcm(2, 1) = Type(2) * (ab + cd);
dcm(2, 2) = aa - bb - cc + dd; dcm(2, 2) = Type(1) - Type(2) * (bb + cc);
} }
/** /**