From facb11ab6f4fa169d820dcea807a5fc9b0327688 Mon Sep 17 00:00:00 2001 From: adrian-soch Date: Sat, 23 Mar 2024 18:09:24 -0400 Subject: [PATCH] Fix bugs and update unit tests --- build.sh | 4 +- scripts/generate_test_cases.py | 118 +++++++++++++++++++++++ scripts/utils/read_mat_data.py | 16 +++- src/attitude_check.cpp | 67 +++++++------ src/attitude_check.hpp | 40 ++++++-- test/CMakeLists.txt | 3 +- test/attitude_check_test.cpp | 166 +++++++++++++++++++++++++++++---- 7 files changed, 352 insertions(+), 62 deletions(-) create mode 100644 scripts/generate_test_cases.py diff --git a/build.sh b/build.sh index b4c60ef..c871b19 100755 --- a/build.sh +++ b/build.sh @@ -12,7 +12,7 @@ case $1 in test) # Build the project with tests enabled cmake -DBUILD_TESTS=ON .. - make + make || { echo "Build failed, stopping script."; exit 1; } cd "$BUILD/test" # Run the tests ctest --rerun-failed --output-on-failure @@ -31,7 +31,7 @@ case $1 in ;; debug) # Build the project with debug mode enabled - cmake -DBUILD_TESTS=ON -DCMAKE_BUILD_TYPE=Debug .. + cmake -DBUILD_TESTS=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo .. make ;; *) diff --git a/scripts/generate_test_cases.py b/scripts/generate_test_cases.py new file mode 100644 index 0000000..bc59b99 --- /dev/null +++ b/scripts/generate_test_cases.py @@ -0,0 +1,118 @@ +''' +This script will use the python AHRS (https://ahrs.readthedocs.io/en/latest/) +as a psuedo oracle for generating test cases. +''' + +from ahrs.common.orientation import acc2q, am2angles, ecompass, rpy2q +from ahrs.filters import Madgwick +import numpy as np + + +def generate_acc2q_tests(acc): + # acc = + quat = acc2q(a=acc) + + print(f'In: {acc} Out:', end=" ") + for q in quat: + print(f'{q:.9f}', end=" ") + print("\n") + + +def generate_ecompass_tests(acc, mag): + quat = ecompass(acc, mag, frame='NED', representation='quaternion') + + print(f'Acc: {acc}, Mag: {mag} Out:', end=" ") + for q in quat: + print(f'{q:.9f}', end=" ") + print("\n") + + +def generate_am2q_tests(acc, mag): + print(f'Acc: {acc}, Mag: {mag} Out:', end=" ") + + rpy = am2angles(acc, mag) + print(np.flip(rpy)) + quat = rpy2q(np.flip(rpy)) + + print(quat) + + +am2angles + + +def get_mag_to_quat_cases(): + + generate_am2q_tests(np.array( + [-6.382152, -7.969839, 0.099505]), np.array([36.700461, 22.613240, 8.881961])) + + generate_am2q_tests(np.array([0.0, 0.0, 9.81]), + np.array([16676.8, -3050.9, 49916.9])) + + +def get_acc_to_quat_cases(): + + generate_acc2q_tests([0.090941, -0.031273, 9.759028]) + + # gt = # 0.802582 & 0.209335 & 0.088698 & 0.551520 + generate_acc2q_tests([0.283217, 2.540909, 7.708738]) + + # -0.093491 & -0.892442 & 0.090310 & 0.432031 \\ + generate_acc2q_tests([7.550523, 2.391483, -0.958994]) + + generate_acc2q_tests([-9.81, 0, 0]) + + +def get_marg_estimate_cases(): + + q0 = [0.49666186227,0.034292027603,-0.0510015643620,0.86576549471] + + q_actual = [[0.49593818106, 0.0326998015547, -0.0510947167457, 0.86623632656], + [0.49519980631, 0.0312979556660, -0.0513729764280, 0.86669395238], + [0.49443437746, 0.0302718565058, -0.0520166472497, 0.86712890015], + [0.49366783997, 0.029245689468, -0.0526602014369, 0.867561903615]] + + gyr = np.array([[-0.5006895838366212, 0.8074957770569486, 0.5528888911052677], + [-0.5326483077698894, 0.6743323911175373, 0.5262569120490862], + [-0.5624759846864724, 0.5560845889656695, 0.5166698184678814], + [-0.5880433128989375, 0.39415992228264346, 0.5017542346803379]], dtype=np.float32) + acc = np.array([[0.47451228800383594, 1.0183972122654374, 8.461165125378162], + [0.3783742880038359, 0.5534032122654374, 8.264965125378161], + [0.27340728800383585, 0.05015021226543746, 8.015791125378161], + [0.3067612880038358, -0.2853517877345626, 8.159017125378163]], dtype=np.float32) + mag = np.array([[10.04650728048766, -6.088486551348383, -43.828022623283225], + [11.084734479270914, -6.77198980959856, -44.35222010806493], + [10.19482545174241, -6.923879422543039, -43.97779333322085], + [9.30491642421391, -7.075769035487526, -43.60336655837678]], dtype=np.float32) + est = Madgwick(gyr=gyr, acc=acc, mag=mag, frequency=285.71428571, q0=q0, gain_imu=0.033, gain_marg=0.041) + print(est.Q) + +def get_imu_estimate_cases(): + + q0 = [0.49666186227,0.034292027603,-0.0510015643620,0.86576549471] + + q_actual = [[0.49593818106, 0.0326998015547, -0.0510947167457, 0.86623632656], + [0.49519980631, 0.0312979556660, -0.0513729764280, 0.86669395238], + [0.49443437746, 0.0302718565058, -0.0520166472497, 0.86712890015], + [0.49366783997, 0.029245689468, -0.0526602014369, 0.867561903615]] + + gyr = np.array([[-0.5006895838366212, 0.8074957770569486, 0.5528888911052677], + [-0.5326483077698894, 0.6743323911175373, 0.5262569120490862], + [-0.5624759846864724, 0.5560845889656695, 0.5166698184678814], + [-0.5880433128989375, 0.39415992228264346, 0.5017542346803379]], dtype=np.float32) + acc = np.array([[0.47451228800383594, 1.0183972122654374, 8.461165125378162], + [0.3783742880038359, 0.5534032122654374, 8.264965125378161], + [0.27340728800383585, 0.05015021226543746, 8.015791125378161], + [0.3067612880038358, -0.2853517877345626, 8.159017125378163]], dtype=np.float32) + est = Madgwick(gyr=gyr, acc=acc, frequency=285.71428571, q0=q0, gain_imu=0.033, gain_marg=0.041) + print(est.Q) + + +def main(): + # get_acc_to_quat_cases() + # get_mag_to_quat_cases() + get_marg_estimate_cases() + get_imu_estimate_cases() + + +if __name__ == "__main__": + main() diff --git a/scripts/utils/read_mat_data.py b/scripts/utils/read_mat_data.py index 687fa71..67fca23 100644 --- a/scripts/utils/read_mat_data.py +++ b/scripts/utils/read_mat_data.py @@ -3,6 +3,7 @@ import pandas as pd file_path = '/home/adrian/Downloads/07_undisturbed_fast_rotation_B.mat' +# file_path = '/home/adrian/Downloads/01_undisturbed_slow_rotation_A.mat' # Load data from 'file.mat' data = sio.loadmat(file_path) @@ -18,8 +19,15 @@ total = np.hstack([acc, gyro, mag, gt_quat]) df = pd.DataFrame(total) -df = df.dropna(axis = 0, how = 'any') +# df = df.dropna(axis = 0, how = 'any') -head = df.head(10) -latex_table = head.style.to_latex() -print(latex_table) \ No newline at end of file +i = 13249 +data = df.iloc[i:i+5] +df.drop(df.columns[[0]], axis=1, inplace=True) +# df.to_csv('filename.csv', index=False) + +csv_string = data.to_csv(sep=',', index=False) +print(csv_string) + +# latex_table = data.style.to_latex() +# print(latex_table) diff --git a/src/attitude_check.cpp b/src/attitude_check.cpp index b5d62ca..d19d7b1 100644 --- a/src/attitude_check.cpp +++ b/src/attitude_check.cpp @@ -9,31 +9,21 @@ typedef q::Quaternion Quat; namespace attitude_check { AttitudeCheck::AttitudeCheck(float imu_gain, float marg_gain) { - if((imu_gain < 0.0f || imu_gain > 1.0f) || - (marg_gain < 0.0f || marg_gain > 1.0f)) - { - throw std::invalid_argument("Gain must be within [0.0, 1.0]."); - } + set_gain(imu_gain, marg_gain); } AttitudeCheck::AttitudeCheck(float imu_gain, float marg_gain, float q0_w, float q0_x, float q0_y, float q0_z) { - if((imu_gain < 0.0f || imu_gain > 1.0f) || - (marg_gain < 0.0f || marg_gain > 1.0f)) - { - throw std::invalid_argument("Gain must be within [0.0, 1.0]."); - } - - try { - m_q.set(q0_w, q0_x, q0_y, q0_z); - } catch (const std::invalid_argument& e) { - throw std::invalid_argument("Initial quaternion must have a norm > 0."); - } + set_gain(imu_gain, marg_gain); + set_quaternion(q0_w, q0_x, q0_y, q0_z); } Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, Vec3f& mag, float dt) { dt = static_cast(dt); + if(dt < DT_MIN_SEC) { + throw std::invalid_argument("Dt is too small."); + } if(gyr.norm() <= 0.0f) { return m_q; @@ -41,6 +31,7 @@ Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, Vec3f& mag, float dt) return update(acc, gyr, dt); } + m_q.normalize(); Quat q_dot = (m_q * Quat(0.0f, gyr[0], gyr[1], gyr[2])) * 0.5f; if(acc.norm() <= 0.0f) { Quat q_out = m_q + (q_dot * dt); @@ -50,7 +41,6 @@ Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, Vec3f& mag, float dt) acc.normalize(); mag.normalize(); - m_q.normalize(); Quat h = m_q * Quat(0.0f, mag[0], mag[1], mag[2]) * m_q.conjugate(); float bx = std::sqrt(h.x() * h.x() + h.y() * h.y()); @@ -68,24 +58,25 @@ Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, Vec3f& mag, float dt) * (0.5f - m_q.x() * m_q.x() - m_q.y() * m_q.y()) - mag[2]; Eigen::Matrix Jt; - Jt << -2.0f * m_q.y(), 2.0f * m_q.x(), 0.0, -2.0f * bz * m_q.y(), -2.0f * bx * m_q.z() + 2.0f * bz * m_q.x(), + Jt << -2.0f * m_q.y(), 2.0f * m_q.x(), 0.0f, -2.0f * bz * m_q.y(), -2.0f * bx * m_q.z() + 2.0f * bz * m_q.x(), 2.0f * bx * m_q.y(), + 2.0f * m_q.z(), 2.0f * m_q.w(), -4.0f * m_q.x(), 2.0f * bz * m_q.z(), 2.0f * bx * m_q.y() + 2.0f * bz * m_q.w(), 2.0f * bx * m_q.z() - 4.0f * bz * m_q.x(), - 2.0f * m_q.z(), 2.0f * m_q.w(), -4.0f * m_q.x(), 2.0f * bz * m_q.z(), 2.0f * bx * m_q.y() + 2.0f * bz * m_q.w(), - 2.0f * bx * m_q.z() - 4.0f * bz * m_q.x(), + -2.0f * m_q.w(), 2.0f * m_q.z(), -4.0f * m_q.y(), -4.0f * bx * m_q.y() - 2.0f * bz * m_q.w(), - 2.0f * bx * m_q.x() + 2.0f * bz * m_q.z(), - 2.0f * bx * m_q.w() - 4.0f * bz * m_q.y(), - 2.0f * m_q.x(), 2.0f * m_q.y(), 0.0, -4.0f * bx * m_q.z() + 2.0f * bz * m_q.x(), + 2.0f * bx * m_q.x() + 2.0f * bz * m_q.z(), 2.0f * bx * m_q.w() - 4.0f * bz * m_q.y(), + + 2.0f * m_q.x(), 2.0f * m_q.y(), 0.0f, -4.0f * bx * m_q.z() + 2.0f * bz * m_q.x(), -2.0f * bx * m_q.w() + 2.0f * bz * m_q.y(), 2.0f * bx * m_q.x(); Eigen::Matrix grad = Jt * func; q_dot -= (Quat(grad[0], grad[1], grad[2], grad[3]) * m_marg_gain); - Quat q_next = m_q + q_dot * dt; + Quat q_next = m_q + (q_dot * dt); q_next.normalize(); + m_q = q_next; return q_next; } // AttitudeCheck::update @@ -116,7 +107,7 @@ Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, float dt) 2.0f * m_q.x(), 2.0f * m_q.y(), 0.0; Eigen::Matrix grad = Jt * func; - q_dot -= (Quat(grad[0], grad[1], grad[2], grad[3]) * m_marg_gain); + q_dot -= (Quat(grad[0], grad[1], grad[2], grad[3]) * m_imu_gain); Quat q_next = m_q + q_dot * dt; q_next.normalize(); @@ -124,9 +115,31 @@ Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, float dt) return q_next; } // AttitudeCheck::update -void AttitudeCheck::reset(float q_w, float q_x, float q_y, float q_z) +inline void AttitudeCheck::set_quaternion(float q_w, float q_x, float q_y, float q_z) { - m_q.set(q_w, q_x, q_y, q_z); + try { + m_q.set(q_w, q_x, q_y, q_z); + } catch(const std::invalid_argument& e) { + throw std::invalid_argument("Initial quaternion must have a norm > 0."); + } + m_q.normalize(); } + +inline void AttitudeCheck::set_gain(const float imu_gain, const float marg_gain) +{ + if((imu_gain < GAIN_MIN || imu_gain > GAIN_MAX) || + (marg_gain < GAIN_MIN || marg_gain > GAIN_MAX)) + { + throw std::invalid_argument("Gain must be within [0.0, 1.0]."); + } + + m_imu_gain = imu_gain; + m_marg_gain = marg_gain; +} + +inline std::tuple AttitudeCheck::get_gain() +{ + return std::tuple(m_imu_gain, m_marg_gain); +} } // End namespace attitude_check diff --git a/src/attitude_check.hpp b/src/attitude_check.hpp index 919c28c..c129c97 100644 --- a/src/attitude_check.hpp +++ b/src/attitude_check.hpp @@ -15,22 +15,44 @@ class AttitudeCheck { quaternion::Quaternion update(Eigen::Vector3f& acc, Eigen::Vector3f& gyr, float dt); - void reset(float q_w, float q_x, float q_y, float q_z); + /** + * @brief Manually set the quaternion component. Useful when a new initial quaternion + * for subsequent calcualtion is desired. For example, after determining absolute + * orientation through another source, the quaternion can be reset such that the orientation + * is relative to the new known orientation. + * + * @param q_w + * @param q_x + * @param q_y + * @param q_z + */ + void set_quaternion(float q_w, float q_x, float q_y, float q_z); + + /** + * @brief Set the filter gain values. + * + * @param imu_gain Gain when only Accel and Gyro are used. + * @param marg_gain Gain when Accel, Gyro, and Mag are used. + */ + void + set_gain(float imu_gain, float marg_gain); + + /** + * @brief Get the gains via tuple. + * Example: `auto [imu_gain, marg_gain] = a.get_gain();` + * + * @return std::tuple + */ + std::tuple get_gain(); private: - const float RATE_MIN_HZ { 10000.0 }; - const float DT_MIN_SEC { 1 / RATE_MIN_HZ }; + const float RATE_MAX_HZ { 10000.0 }; + const float DT_MIN_SEC { 1 / RATE_MAX_HZ }; const float GAIN_MIN { 0.0 }, GAIN_MAX { 1.0 }; quaternion::Quaternion m_q; float m_dt { 100.0 }; float m_imu_gain { 0.5 }, m_marg_gain { 0.5 }; - - void - input_handler(float dt, float gain); - - void - set_gain(float imu_gain, float marg_gain); }; } // End namespace attitude_check diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c974e3c..4d5ea5e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -15,12 +15,13 @@ find_package(GTest REQUIRED) enable_testing() -set(CMAKE_CXX_FLAGS "-g -O0 -Wall -fprofile-arcs -ftest-coverage") +set(CMAKE_CXX_FLAGS "-fprofile-arcs -ftest-coverage") add_executable( ${BINARY} main.cpp attitude_check_test.cpp + # attitude_check_estimation_test.cpp quaternion_test.cpp utilities_test.cpp initializers_test.cpp diff --git a/test/attitude_check_test.cpp b/test/attitude_check_test.cpp index 8a54c58..197e99a 100644 --- a/test/attitude_check_test.cpp +++ b/test/attitude_check_test.cpp @@ -3,13 +3,13 @@ #include "gtest/gtest.h" #include "attitude_check.cpp" -#include "attitude_check.hpp" +// #include "attitude_check.hpp" const float MAX_ABS_ERROR { 0.002 }; // About 0.115 of a degree namespace ac = attitude_check; -class Attitude_Check_Test_Suite : public ::testing::Test { +class ACheck_Test_Fixture : public ::testing::Test { protected: float dt; // dt = 1/Hz Eigen::Vector3f acc { 0.0f, 0.0f, 9.81f }; @@ -25,7 +25,7 @@ class Attitude_Check_Test_Suite : public ::testing::Test { } }; -TEST_F(Attitude_Check_Test_Suite, invalid_init){ +TEST_F(ACheck_Test_Fixture, invalid_init){ EXPECT_THROW({ try { @@ -63,19 +63,36 @@ TEST_F(Attitude_Check_Test_Suite, invalid_init){ }, std::invalid_argument); } -TEST_F(Attitude_Check_Test_Suite, marg_zero_gyro){ + +TEST_F(ACheck_Test_Fixture, invalid_update){ + ac::AttitudeCheck ac(0.8, 0.999); + + EXPECT_THROW({ + try + { + ac.update(acc, gyr, mag, -0.00001f); + } + catch(const std::invalid_argument& e) + { + EXPECT_STREQ("Dt is too small.", e.what() ); + throw; + } + }, std::invalid_argument); +} + +TEST_F(ACheck_Test_Fixture, marg_zero_gyro){ gyr.setZero(); ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); - EXPECT_NEAR(out.w(), -69.0f, MAX_ABS_ERROR); - EXPECT_NEAR(out.x(), -69.0f, MAX_ABS_ERROR); - EXPECT_NEAR(out.y(), -69.0f, MAX_ABS_ERROR); - EXPECT_NEAR(out.z(), -69.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.w(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.5f, MAX_ABS_ERROR); } -TEST_F(Attitude_Check_Test_Suite, marg_zero_mag){ +TEST_F(ACheck_Test_Fixture, marg_zero_mag){ mag.setZero(); acc.setZero(); @@ -93,7 +110,7 @@ TEST_F(Attitude_Check_Test_Suite, marg_zero_mag){ EXPECT_NEAR(out.z(), expected.z(), MAX_ABS_ERROR); } -TEST_F(Attitude_Check_Test_Suite, marg_zero_acc){ +TEST_F(ACheck_Test_Fixture, marg_zero_acc){ acc.setZero(); quaternion::Quaternion expected(-69.0f, -69.0f, -69.0f, -69.0f); @@ -110,20 +127,20 @@ TEST_F(Attitude_Check_Test_Suite, marg_zero_acc){ EXPECT_NEAR(out.z(), expected.z(), MAX_ABS_ERROR); } -TEST_F(Attitude_Check_Test_Suite, imu_zero_gyro){ +TEST_F(ACheck_Test_Fixture, imu_zero_gyro){ gyr.setZero(); ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); quaternion::Quaternion out = ac.update(acc, gyr, dt); - EXPECT_NEAR(out.w(), -69.0f, MAX_ABS_ERROR); - EXPECT_NEAR(out.x(), -69.0f, MAX_ABS_ERROR); - EXPECT_NEAR(out.y(), -69.0f, MAX_ABS_ERROR); - EXPECT_NEAR(out.z(), -69.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.w(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.5f, MAX_ABS_ERROR); } -TEST_F(Attitude_Check_Test_Suite, imu_zero_acc){ +TEST_F(ACheck_Test_Fixture, imu_zero_acc){ acc.setZero(); quaternion::Quaternion expected(-69.3333f, -69.0f, -69.0f, -69.0f); @@ -140,10 +157,10 @@ TEST_F(Attitude_Check_Test_Suite, imu_zero_acc){ EXPECT_NEAR(out.z(), expected.z(), MAX_ABS_ERROR); } -TEST_F(Attitude_Check_Test_Suite, reset) { - +TEST_F(ACheck_Test_Fixture, set_quaternion){ ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); - ac.reset(1.0f, 0.0f, 0.0f, 0.0f); + + ac.set_quaternion(1.0f, 0.0f, 0.0f, 0.0f); gyr.setZero(); quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); @@ -153,3 +170,114 @@ TEST_F(Attitude_Check_Test_Suite, reset) { EXPECT_NEAR(out.y(), 0.0f, MAX_ABS_ERROR); EXPECT_NEAR(out.z(), 0.0f, MAX_ABS_ERROR); } + +TEST_F(ACheck_Test_Fixture, set_get_gain){ + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + float in1 { 1.0f }, in2 { 1.0f }; + + ac.set_gain(in1, in2); + + auto [imu, marg] = ac.get_gain(); + + ASSERT_FLOAT_EQ(in1, imu); + ASSERT_FLOAT_EQ(in2, marg); +} + +class ACheck_Estimator_Test_Fixture : public ::testing::Test { +protected: + // Tuple(Acc, Gyr, Mag, Expected_Quaternion) + typedef std::tuple T; + std::vector test_data; + + void SetUp() override { } +}; + +TEST_F(ACheck_Estimator_Test_Fixture, update_marg_with_intitial){ + ac::AttitudeCheck ac(0.033f, 0.041f, 0.49666186227f, 0.034292027603f, -0.0510015643620f, 0.86576549471f); + + /* *INDENT-OFF* */ + test_data = { + T(Eigen::Vector3f(0.47451228800383594, 1.0183972122654374, 8.461165125378162), + Eigen::Vector3f(-0.5006895838366212, 0.8074957770569486, 0.5528888911052677), + Eigen::Vector3f(10.04650728048766, -6.088486551348383, -43.828022623283225), + Eigen::Vector4f(0.49598288, 0.03266141,-0.05120194, 0.86620585), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3783742880038359, 0.5534032122654374, 8.264965125378161), + Eigen::Vector3f(-0.5326483077698894, 0.6743323911175373, 0.5262569120490862), + Eigen::Vector3f(11.084734479270914, -6.77198980959856, -44.35222010806493), + Eigen::Vector4f(0.49530668, 0.03117841, -0.05155569, 0.86662634), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.27340728800383585, 0.05015021226543746, 8.015791125378161), + Eigen::Vector3f(-0.5624759846864724, 0.5560845889656695, 0.5166698184678814), + Eigen::Vector3f(10.19482545174241, -6.923879422543039, -43.97779333322085), + Eigen::Vector4f(0.49463786, 0.0299162, -0.05209187, 0.86702067), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3067612880038358, -0.2853517877345626, 8.159017125378163), + Eigen::Vector3f(-0.5880433128989375, 0.39415992228264346, 0.5017542346803379), + Eigen::Vector3f(9.30491642421391, -7.075769035487526, -43.60336655837678), + Eigen::Vector4f(0.49463786, 0.0289162, -0.05209187, 0.86702067), + 1.0/285.71428571) + }; + /* *INDENT-ON* */ + + size_t count = 0; + for(auto tuple : test_data) { + auto [t_acc, t_gyr, t_mag, q_exp, t_dt] = tuple; + + quaternion::Quaternion out = ac.update(t_acc, t_gyr, t_mag, t_dt); + + SCOPED_TRACE("At iteration " + std::to_string(count++)); + EXPECT_NEAR(out.w(), q_exp[0], MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), q_exp[1], MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), q_exp[2], MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), q_exp[3], MAX_ABS_ERROR); + } +} + +TEST_F(ACheck_Estimator_Test_Fixture, update_imu_with_intitial){ + ac::AttitudeCheck ac(0.033f, 0.041f, 0.49666186227f, 0.034292027603f, -0.0510015643620f, 0.86576549471f); + + /* *INDENT-OFF* */ + test_data = { + T(Eigen::Vector3f(0.47451228800383594, 1.0183972122654374, 8.461165125378162), + Eigen::Vector3f(-0.5006895838366212, 0.8074957770569486, 0.5528888911052677), + Eigen::Vector3f(10.04650728048766, -6.088486551348383, -43.828022623283225), + Eigen::Vector4f(0.49596236, 0.03276414, -0.05113933, 0.86621742), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3783742880038359, 0.5534032122654374, 8.264965125378161), + Eigen::Vector3f(-0.5326483077698894, 0.6743323911175373, 0.5262569120490862), + Eigen::Vector3f(11.084734479270914, -6.77198980959856, -44.35222010806493), + Eigen::Vector4f(0.49526518, 0.03134777, -0.05143123, 0.86665134), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.27340728800383585, 0.05015021226543746, 8.015791125378161), + Eigen::Vector3f(-0.5624759846864724, 0.5560845889656695, 0.5166698184678814), + Eigen::Vector3f(10.19482545174241, -6.923879422543039, -43.97779333322085), + Eigen::Vector4f(0.49457367, 0.03111946, -0.05192297, 0.86706038), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3067612880038358, -0.2853517877345626, 8.159017125378163), + Eigen::Vector3f(-0.5880433128989375, 0.39415992228264346, 0.5017542346803379), + Eigen::Vector3f(9.30491642421391, -7.075769035487526, -43.60336655837678), + Eigen::Vector4f(0.49457367, 0.03111946, -0.05192297, 0.86706038), + 1.0/285.71428571) + }; + /* *INDENT-ON* */ + + size_t count = 0; + for(auto tuple : test_data) { + auto [t_acc, t_gyr, t_mag, q_exp, t_dt] = tuple; + + quaternion::Quaternion out = ac.update(t_acc, t_gyr, t_dt); + + SCOPED_TRACE("At iteration " + std::to_string(count++)); + EXPECT_NEAR(out.w(), q_exp[0], MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), q_exp[1], MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), q_exp[2], MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), q_exp[3], MAX_ABS_ERROR); + } +}