wntrmute/wrmath
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Start Madwick filters.

Browse Source
This commit is contained in:
Kyle Isom
2019-08-06 22:49:20 -07:00
parent 7a932f422a
commit 971f324d7f
9 changed files with 286 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/madgwick.cc Normal file
View File

@@ -0,0 +1 @@
#include "wrmath/filter/madgwick.h"

6
src/quaternion.cc
View File

@@ -1,3 +1,4 @@
#include <iostream>
#include <wrmath/geom/quaternion.h>
@@ -39,7 +40,7 @@ quaternionf_from_euler(Vector3f euler)
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
return Quaternionf(Vector4f{x, y, z, w});
return Quaternionf(Vector4f{w, x, y, z});
}
@@ -61,7 +62,7 @@ quaterniond_from_euler(Vector3d euler)
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
return Quaterniond(Vector4d{x, y, z, w});
return Quaterniond(Vector4d{w, x, y, z});
}
@@ -78,6 +79,7 @@ Quaternion_SelfTest()
Vector3f vr {0.0, 0.0, 1.0};
assert(p.isUnitQuaternion());
std::cerr << p.rotate(v) << std::endl;
assert(p.rotate(v) == vr);
assert(p * q == p);
#endif