scsl/test/madgwick.cc

261 lines
8.1 KiB
C++

#include <cmath>
#include <sstream>
#include <scmp/geom/Vector.h>
#include <scmp/geom/Quaternion.h>
#include <scmp/Math.h>
#include <scmp/filter/Madgwick.h>
#include <sctest/Assert.h>
#include <sctest/Checks.h>
#include <sctest/SimpleSuite.h>
using namespace std;
using namespace scmp;
bool
SimpleAngularOrientationFloat()
{
filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientationFloatDefaultDT()
{
filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
mflt.DeltaT(delta);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
VerifyUpdateWithZeroDeltaTFails()
{
filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientationDouble()
{
filter::Madgwickd mflt;
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_DEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_DEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_DEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientation2InitialVector3f()
{
const geom::Vector3f initialFrame{0, 0, 0};
filter::Madgwickf mflt(initialFrame);
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientation2InitialQuaternionf()
{
const auto initialFrame = geom::quaternionf_from_euler({0, 0, 0});
filter::Madgwickf mflt(initialFrame);
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientation2InitialVector3d()
{
const geom::Vector3d initialFrame{0, 0, 0};
filter::Madgwickd mflt(initialFrame);
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_DEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_DEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_DEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientation2InitialQuaterniond()
{
const auto initialFrame = geom::quaterniond_from_euler({0, 0, 0});
filter::Madgwickd mflt(initialFrame);
const geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const double delta = 0.00917; // assume 109 updates per second, as per the paper.
const double twentyDegrees = scmp::DegreesToRadiansD(20.0);
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_DEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_DEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_DEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
int
main(int argc, char **argv)
{
(void)argc;
(void)argv;
sctest::SimpleSuite suite;
suite.AddTest("SimpleAngularOrientationFloat",
SimpleAngularOrientationFloat);
suite.AddTest("SimpleAngularOrientationFloatDefaultDT",
SimpleAngularOrientationFloatDefaultDT);
suite.AddFailingTest("VerifyUpdateWithZeroDeltaTFails",
VerifyUpdateWithZeroDeltaTFails);
suite.AddTest("SimpleAngularOrientationDouble",
SimpleAngularOrientationDouble);
suite.AddTest("SimpleAngularOrientationFloat (inital vector3f)",
SimpleAngularOrientation2InitialVector3f);
suite.AddTest("SimpleAngularOrientationDouble (inital vector3d)",
SimpleAngularOrientation2InitialVector3d);
suite.AddTest("SimpleAngularOrientationFloat (inital quaternionf)",
SimpleAngularOrientation2InitialQuaternionf);
suite.AddTest("SimpleAngularOrientationDouble (inital quaterniond)",
SimpleAngularOrientation2InitialQuaterniond);
auto result = suite.Run();
std::cout << suite.GetReport() << "\n";
return result ? 0 : 1;
}