Fix bugs and update unit tests

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
     ;;
   *)

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()

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)
+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)

src/attitude_check.cpp

@@ -9,38 +9,29 @@ typedef q::Quaternion<float> 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<float>(dt);
+    if(dt < DT_MIN_SEC) {
+        throw std::invalid_argument("Dt is too small.");
+    }
 
     if(gyr.norm() <= 0.0f) {
         return m_q;
     } else if(mag.norm() <= 0.0f) {
         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<float, 4, 6> 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<float, 4, 1> 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,17 +107,39 @@ Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, float dt)
         2.0f * m_q.x(), 2.0f * m_q.y(), 0.0;
 
     Eigen::Matrix<float, 4, 1> 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();
 
     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<float, float> AttitudeCheck::get_gain()
+{
+    return std::tuple<float, float>(m_imu_gain, m_marg_gain);
+}
 } // End namespace attitude_check

src/attitude_check.hpp

@@ -15,22 +15,44 @@ class AttitudeCheck {
 
     quaternion::Quaternion<float> 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<float, float>
+     */
+    std::tuple<float, float> 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<float> 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

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