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

Add Euler angles to Madgwick and update docs.

This commit is contained in:
Kyle Isom 2019-08-06 23:42:51 -07:00
parent 85833c354d
commit ff3ff8d8b2
5 changed files with 22 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
docs/sphinx/index.rst
View File

@ -10,7 +10,7 @@ libwrmath: WNTRMUTE ROBOTICS' maths library
:maxdepth: 3 :maxdepth: 3
:caption: Contents: :caption: Contents:
api overview
vector vector
quaternion quaternion
resources resources

3
docs/sphinx/overview.rst
View File

@ -15,7 +15,8 @@ Coordinate systems
The library uses a left-hand coordinate system where +x is north, +y is The library uses a left-hand coordinate system where +x is north, +y is
east, and +z is up. Accordingly, where Euler angles are concerned, the east, and +z is up. Accordingly, where Euler angles are concerned, the
ZYX (yaw / pitch / roll) axes are used. ZYX (yaw / pitch / roll) axes are used. The code follows the convention
of using ψ, θ, ϕ to represent yaw, pitch, and roll.
Important classes Important classes
----------------- -----------------

10
include/wrmath/filter/madgwick.h
View File

@ -91,6 +91,16 @@ public:
this->updateFrame(this->sensorFrame + q, delta); this->updateFrame(this->sensorFrame + q, delta);
} }
/// Retrieve a vector of the Euler angles in ZYX orientation.
///
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
geom::Vector<T, 3>
euler()
{
return this->sensorFrame.euler();
}
private: private:
T deltaT; T deltaT;
geom::Quaternion<T> previousSensorFrame; geom::Quaternion<T> previousSensorFrame;

9
test/madgwick_test.cc
View File

@ -3,6 +3,7 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <wrmath/geom/vector.h> #include <wrmath/geom/vector.h>
#include <wrmath/geom/quaternion.h> #include <wrmath/geom/quaternion.h>
#include <wrmath/math.h>
#include <wrmath/filter/madgwick.h> #include <wrmath/filter/madgwick.h>
using namespace std; using namespace std;
@ -12,9 +13,10 @@ using namespace wr;
TEST(MadgwickFilter, SimpleAngularOrientation) TEST(MadgwickFilter, SimpleAngularOrientation)
{ {
filter::Madgwickd mf; filter::Madgwickd mf;
geom::Vector3d gyro {0.17453292519943295, 0.0, 0.0}; // 10° X rotation. geom::Vector3d gyro {0.174533, 0.0, 0.0}; // 10° X rotation.
geom::Quaterniond frame20Deg {0.984808, 0.173648, 0, 0}; // 20° final orientation. geom::Quaterniond frame20Deg {0.984808, 0.173648, 0, 0}; // 20° final orientation.
double delta = 0.00917; // assume 109 updates per second, as per the paper. double delta = 0.00917; // assume 109 updates per second, as per the paper.
double twentyDegrees = math::DegreesToRadiansD(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for // The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates. // two seconds, that means 218 updates.
@ -23,6 +25,11 @@ TEST(MadgwickFilter, SimpleAngularOrientation)
} }
EXPECT_EQ(mf.orientation(), frame20Deg); EXPECT_EQ(mf.orientation(), frame20Deg);
auto euler = mf.euler();
EXPECT_NEAR(euler[0], twentyDegrees, 0.01);
EXPECT_NEAR(euler[1], 0.0, 0.01);
EXPECT_NEAR(euler[2], 0.0, 0.01);
} }

3
update-docs
View File

@ -8,5 +8,4 @@ else
HOST="freeside.wntrmute.net:" HOST="freeside.wntrmute.net:"
fi fi
cd docs && doxygen && rsync -auv html/ ${HOST}sites/wntrmute-dev/wrmath/ cd docs/sphinx && make html && rsync -auv _build/html/ ${HOST}sites/wrmath/
cd sphinx && make html && rsync -auv _build/html/ ${HOST}sites/wntrmute-dev/sphinx/