280 lines
8.8 KiB
C++
280 lines
8.8 KiB
C++
#include <cmath>
|
|
#include <sstream>
|
|
|
|
#include <scsl/Flags.h>
|
|
|
|
#include <scmp/geom/Vector.h>
|
|
#include <scmp/geom/Quaternion.h>
|
|
#include <scmp/Math.h>
|
|
|
|
#include <scmp/estimation/Madgwick.h>
|
|
#include <sctest/Assert.h>
|
|
#include <sctest/Checks.h>
|
|
#include <sctest/SimpleSuite.h>
|
|
|
|
|
|
using namespace std;
|
|
using namespace scmp;
|
|
|
|
|
|
bool
|
|
SimpleAngularOrientationFloat()
|
|
{
|
|
estimation::Madgwickf estimation;
|
|
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++) {
|
|
estimation.UpdateAngularOrientation(gyro, delta);
|
|
}
|
|
|
|
|
|
SCTEST_CHECK_EQ(estimation.Orientation(), frame20Deg);
|
|
|
|
auto euler = estimation.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()
|
|
{
|
|
estimation::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()
|
|
{
|
|
estimation::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()
|
|
{
|
|
estimation::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};
|
|
estimation::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::FloatQuaternionFromEuler({0, 0, 0});
|
|
estimation::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};
|
|
estimation::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::DoubleQuaternionFromEuler({0, 0, 0});
|
|
estimation::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)
|
|
{
|
|
auto quiet = false;
|
|
auto noReport = false;
|
|
auto *flags = new scsl::Flags("test_madgwick",
|
|
"This test validates the Madgwick estimation code");
|
|
flags->Register("-n", false, "don't print the report");
|
|
flags->Register("-q", false, "suppress test output");
|
|
|
|
auto parsed = flags->Parse(argc, argv);
|
|
if (parsed != scsl::Flags::ParseStatus::OK) {
|
|
std::cerr << "Failed to parse flags: "
|
|
<< scsl::Flags::ParseStatusToString(parsed) << "\n";
|
|
exit(1);
|
|
}
|
|
|
|
sctest::SimpleSuite suite;
|
|
flags->GetBool("-n", noReport);
|
|
flags->GetBool("-q", quiet);
|
|
if (quiet) {
|
|
suite.Silence();
|
|
}
|
|
|
|
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 MakeQuaternion)",
|
|
SimpleAngularOrientation2InitialQuaternionf);
|
|
suite.AddTest("SimpleAngularOrientationDouble (inital MakeQuaternion)",
|
|
SimpleAngularOrientation2InitialQuaterniond);
|
|
|
|
delete flags;
|
|
auto result = suite.Run();
|
|
if (!noReport) { std::cout << suite.GetReport() << "\n"; }
|
|
return result ? 0 : 1;
|
|
}
|