mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-21 00:03:19 +08:00
Use quaternion multiplaction for quaternion derivative.
This commit is contained in:
+1
-1
@@ -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");
|
||||
}
|
||||
|
||||
+2
-13
@@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
+5
-1
@@ -157,7 +157,11 @@ int main()
|
||||
TEST(isEqual(q_from_m, m4));
|
||||
|
||||
// quaternion derivate
|
||||
Vector<float, 4> q_dot = q.derivative(Vector3f(1, 2, 3));
|
||||
Quatf q1(0, 1, 0, 0);
|
||||
Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
|
||||
float data_q_dot_check[] = {-0.5f, 0.0f, -1.5f, 1.0f};
|
||||
Vector<float, 4> q1_dot_check(data_q_dot_check);
|
||||
TEST(isEqual(q1_dot, q1_dot_check));
|
||||
|
||||
// quaternion product
|
||||
Quatf q_prod_check(
|
||||
|
||||
Reference in New Issue
Block a user