Add filter implementation and modules (#54)

* Add inline

* Add utils module and unittests

* Update templates and test

* Add initializers module and unit tests

* Update quat initializers

* Make quat intialization faster

* Fix bug and update test cases

* fix bug

* Update overlaods and tests

* update comments and tests

* Update comments

* cleanup

* Add code and test structure

* update cmake

* Update github stuff

* Update build script

* Fix egien include

* Add flags

* update ci

* upadte ci

* fix again

* another try

* fix eigen dep

* add codecov yaml to ignore test folder coverage

* Update scipts and configs

* Update code and tests

* Fix bugs and update unit tests

* Cleanup

* Fix bug and update unit tests

.github/ISSUE_TEMPLATE/custom_template.md

@@ -3,11 +3,6 @@ name: Custom Template
 about: For other types of issues.
 title: ''
 labels: ''
-assignees: adrian-soch
+assignees:
 
 ---
-
-**Describe the problem/comment/request**
-A clear and concise description.
-
-

.github/ISSUE_TEMPLATE/review_request.md

@@ -3,7 +3,6 @@ name: Review Request
 about: Review documentation to help us improve
 title: ''
 labels: ''
-assignees: adrian-soch
 
 ---
 

.github/workflows/ci.yaml

@@ -1,38 +1,31 @@
-name: C/C++ CI # The name of the workflow
+name: C/C++ CI
 
-on: # The trigger for the workflow
-  push: # When you push to any branch
-    branches: [ main ] # Only for the main branch
-  pull_request: # When you create or update a pull request
-    branches: [ main ] # Only for the main branch
+on:
+  push:
+    branches: [main]
+  pull_request:
+    branches: [main]
 
-jobs: # The jobs for the workflow
-  build-and-unittest: # The name of the job
+jobs:
+  build-and-unittest:
     if: github.event.pull_request.draft == false
-    runs-on: ubuntu-latest # The operating system to run the job on
-    steps: # The steps for the job
-      - name: Checkout # The name of the step
-        # uses: actions/checkout@v2 # The action to use for the step
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout
         uses: actions/checkout@v4
         with:
           sparse-checkout: |
             .github
             src
             test
-      - name: Install dependencies # The name of the step
-        run: sudo apt-get install -y cmake lcov cppcheck # The command to run for the step
+      - name: Install dependencies
+        run: sudo apt-get install -y cmake lcov cppcheck
+      - name: Install Eigen3
+        run: sudo apt-get install -y libeigen3-dev && sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen
       - name: Install gtest
         run: sudo apt-get install libgtest-dev && cd /usr/src/gtest && sudo cmake CMakeLists.txt
       - name: Run custom build script
-        run: bash build.sh test
-      - name: Run unit tests
-        run: bash build.sh test
-      - name: Generate coverage report
-        run: |
-          lcov --capture --directory . --output-file coverage.info
-          lcov --remove coverage.info '/usr/*' --output-file coverage.info
-          lcov --list coverage.info
-          genhtml coverage.info --output-directory out
+        run: bash ./build.sh test
       - name: Upload coverage reports to Codecov
         uses: codecov/codecov-action@v4
         with:

CMakeLists.txt

@@ -7,6 +7,7 @@ project(attitude_check)
 # Default to C++17
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
 
 # Add cppCheck for static checks
 # set(CMAKE_CXX_CPPCHECK "cppcheck")

build.sh

@@ -1,34 +1,37 @@
 #!/bin/bash
 # Usage: ./build.sh [test|clean|debug]
 
+ROOT="$PWD"
+BUILD="$ROOT/build"
+
 # Create a build directory if it does not exist
-mkdir -p build && cd build
+mkdir -p $BUILD && cd $BUILD
 
 # Check the first argument
 case $1 in
   test)
     # Build the project with tests enabled
-    cmake -DBUILD_TESTS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fprofile-arcs -ftest-coverage" ..
-    make
-    cd ./test
+    cmake -DBUILD_TESTS=ON ..
+    make || { echo "Build failed, stopping script."; exit 1; }
+    cd "$BUILD/test"
     # Run the tests
-    ctest
+    ctest --rerun-failed --output-on-failure
 
     # Generate the coverage report using gcov and lcov
-    cd ../build/test/CMakeFiles/attitude_check_test.dir
+    cd "$BUILD/test/CMakeFiles/attitude_check_test.dir"
 
-    lcov --capture --directory . --output-file coverage.info
+    lcov --capture --rc lcov_branch_coverage=1 --directory . --output-file coverage.info
     lcov --remove coverage.info '/usr/*' '*/test*' --output-file coverage.info
     lcov --list coverage.info
-    genhtml coverage.info --output-directory out
+    genhtml coverage.info --output-directory $BUILD/test/lcov_out
     ;;
   clean)
     # Remove the build directory
-    cd .. && rm -rf build
+    cd "$ROOT" && rm -rf build
     ;;
   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)

setup.sh

@@ -2,5 +2,6 @@ pip install pre-commit
 pre-commit install
 sudo apt install cmake lcov cppcheck
 sudo apt-get install libgtest-dev && cd /usr/src/gtest && sudo cmake CMakeLists.txt
+sudo apt-get install -y libeigen3-dev && sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen
 
 echo "Done setup."