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

Use quaternion multiplaction for quaternion derivative.

Browse Source
This commit is contained in:
James Goppert
2016-08-16 01:03:23 -04:00
parent c563c41cd4
commit e6a6b4680c
3 changed files with 8 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Matrix.hpp
+1 -1
View File
@@ -278,7 +278,7 @@ public:
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
snprintf(buf + strlen(buf), n - strlen(buf), "\t%.2g", double(self(i, j))); // directly append to the string buffer
snprintf(buf + strlen(buf), n - strlen(buf), "\t%g", double(self(i, j))); // directly append to the string buffer
}
snprintf(buf + strlen(buf), n - strlen(buf), "\n");
}
matrix/Quaternion.hpp
+2 -13
View File
@@ -238,19 +238,8 @@ public:
Matrix41 derivative(const Matrix31 &w) const
{
const Quaternion &q = *this;
Type dataQ[] = {
q(0), -q(1), -q(2), -q(3),
q(1), q(0), -q(3), q(2),
q(2), q(3), q(0), -q(1),
q(3), -q(2), q(1), q(0)
};
Matrix<Type, 4, 4> Q(dataQ);
Vector<Type, 4> v;
v(0) = 0;
v(1) = w(0, 0);
v(2) = w(1, 0);
v(3) = w(2, 0);
return Q * v * Type(0.5);
Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
return v * q * Type(0.5);
}
/**