├── .gitignore ├── Mahony ├── MahonyAHRS.py └── __init__.py ├── README.md ├── README_UA.md ├── SixDOFAnimation.py ├── charts ├── 1_matlab.png ├── 1_python.png ├── 2_matlab.png ├── 2_python.png ├── 3_matlab.png ├── 3_python.png ├── 4_matlab.png ├── 4_python.png ├── 5_matlab.png ├── 5_python.png ├── 6_matlab.png ├── 6_python.png ├── 7_matlab.png ├── 7_python.png ├── 8_matlab.png ├── 8_python.png ├── 9_all_2D_charts.png ├── 9_all_2D_charts_full_size.png ├── CHARTS.md ├── SixDOFAnimation.gif └── SixDOFAnimationMatlab.gif ├── logged_data ├── LoggedData_CalBattAndTherm.csv ├── LoggedData_CalInertialAndMag.csv ├── LoggedData_DateTime.csv ├── LoggedData_EulerAngles.csv ├── LoggedData_Quaternion.csv ├── LoggedData_RotationMatrix.csv ├── lin_pos_hp.npy └── rot_mat.npy ├── requirements.txt ├── script.py └── ximu_python_library ├── CalInertialAndMagneticDataClass.py └── xIMUdataClass.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by .ignore support plugin (hsz.mobi) 2 | ### Python template 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | env/ 14 | build/ 15 | develop-eggs/ 16 | dist/ 17 | downloads/ 18 | eggs/ 19 | .eggs/ 20 | lib/ 21 | lib64/ 22 | parts/ 23 | sdist/ 24 | var/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .coverage 43 | .coverage.* 44 | .cache 45 | nosetests.xml 46 | coverage.xml 47 | *,cover 48 | .hypothesis/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | 58 | # Flask stuff: 59 | instance/ 60 | .webassets-cache 61 | 62 | # Scrapy stuff: 63 | .scrapy 64 | 65 | # Sphinx documentation 66 | docs/_build/ 67 | 68 | # PyBuilder 69 | target/ 70 | 71 | # IPython Notebook 72 | .ipynb_checkpoints 73 | 74 | # pyenv 75 | .python-version 76 | 77 | # celery beat schedule file 78 | celerybeat-schedule 79 | 80 | # dotenv 81 | .env 82 | 83 | # virtualenv 84 | venv/ 85 | ENV/ 86 | 87 | # Spyder project settings 88 | .spyderproject 89 | 90 | # Rope project settings 91 | .ropeproject 92 | ### VirtualEnv template 93 | # Virtualenv 94 | # http://iamzed.com/2009/05/07/a-primer-on-virtualenv/ 95 | [Bb]in 96 | [Ii]nclude 97 | [Ll]ib 98 | [Ll]ib64 99 | [Ll]ocal 100 | [Ss]cripts 101 | pyvenv.cfg 102 | .venv 103 | pip-selfcheck.json 104 | 105 | ### JetBrains template 106 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 107 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 108 | 109 | # User-specific stuff 110 | .idea/**/workspace.xml 111 | .idea/**/tasks.xml 112 | .idea/**/usage.statistics.xml 113 | .idea/**/dictionaries 114 | .idea/**/shelf 115 | 116 | # AWS User-specific 117 | .idea/**/aws.xml 118 | 119 | # Generated files 120 | .idea/**/contentModel.xml 121 | 122 | # Sensitive or high-churn files 123 | .idea/**/dataSources/ 124 | .idea/**/dataSources.ids 125 | .idea/**/dataSources.local.xml 126 | .idea/**/sqlDataSources.xml 127 | .idea/**/dynamic.xml 128 | .idea/**/uiDesigner.xml 129 | .idea/**/dbnavigator.xml 130 | 131 | # Gradle 132 | .idea/**/gradle.xml 133 | .idea/**/libraries 134 | 135 | # Gradle and Maven with auto-import 136 | # When using Gradle or Maven with auto-import, you should exclude module files, 137 | # since they will be recreated, and may cause churn. Uncomment if using 138 | # auto-import. 139 | # .idea/artifacts 140 | # .idea/compiler.xml 141 | # .idea/jarRepositories.xml 142 | # .idea/modules.xml 143 | # .idea/*.iml 144 | # .idea/modules 145 | # *.iml 146 | # *.ipr 147 | 148 | # CMake 149 | cmake-build-*/ 150 | 151 | # Mongo Explorer plugin 152 | .idea/**/mongoSettings.xml 153 | 154 | # File-based project format 155 | *.iws 156 | 157 | # IntelliJ 158 | out/ 159 | 160 | # mpeltonen/sbt-idea plugin 161 | .idea_modules/ 162 | 163 | # JIRA plugin 164 | atlassian-ide-plugin.xml 165 | 166 | # Cursive Clojure plugin 167 | .idea/replstate.xml 168 | 169 | # SonarLint plugin 170 | .idea/sonarlint/ 171 | 172 | # Crashlytics plugin (for Android Studio and IntelliJ) 173 | com_crashlytics_export_strings.xml 174 | crashlytics.properties 175 | crashlytics-build.properties 176 | fabric.properties 177 | 178 | # Editor-based Rest Client 179 | .idea/httpRequests 180 | 181 | # Android studio 3.1+ serialized cache file 182 | .idea/caches/build_file_checksums.ser 183 | 184 | # idea folder, uncomment if you don't need it 185 | .idea 186 | 187 | test_arrays.py 188 | test_3d_chart.py -------------------------------------------------------------------------------- /Mahony/MahonyAHRS.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: X-IO (https://x-io.co.uk/) 3 | Python implementer: Korzhak (GitHub) 4 | Date: 15.12.2022 5 | 6 | Filtering by Mahony method 7 | """ 8 | 9 | import numpy as np 10 | 11 | GR = 9.81 12 | 13 | 14 | class Mahony: 15 | def __init__(self, sample_freq=100, kp_def=1., ki_def=0.0): 16 | self.kp = kp_def 17 | self.ki = ki_def 18 | 19 | self.sample_freq = sample_freq 20 | self.sample_period = 1 / sample_freq 21 | 22 | self.Quaternion = np.array([1., 0., 0., 0.]) 23 | 24 | self.e_int = np.array([0., 0., 0.]) 25 | 26 | @staticmethod 27 | def prod_q(quaternion1, quaternion0): 28 | w0, x0, y0, z0 = quaternion0 29 | w1, x1, y1, z1 = quaternion1 30 | return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, 31 | x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, 32 | -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, 33 | x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64) 34 | 35 | @staticmethod 36 | def conjugate_quat(q): 37 | return np.array([q[0], -q[1], -q[2], -q[3]]) 38 | 39 | def Q_to_DCM(self): 40 | w, x, y, z = self.Quaternion 41 | return np.array([ 42 | [1.0 - 2.0 * (y ** 2 + z ** 2), 2.0 * (x * y - w * z), 43 | 2.0 * (x * z + w * y)], 44 | [2.0 * (x * y + w * z), 1.0 - 2.0 * (x ** 2 + z ** 2), 45 | 2.0 * (y * z - w * x)], 46 | [2.0 * (x * z - w * y), 2.0 * (w * x + y * z), 47 | 1.0 - 2.0 * (x ** 2 + y ** 2)]]) 48 | 49 | def update_marg(self, g: np.array, a: np.array, m: np.array) -> np.array: 50 | """ 51 | Calculating quaternion using accelerometer, gyroscope and magnetometer data 52 | 53 | :param a: Accelerometer data [ax, ay, az] 54 | :param g: Gyroscope data [wx, wy, wz] 55 | :param m: Magnetometer data [mx, my, mz] 56 | :return: Calculated quaternion [w, i, j, k]. 57 | """ 58 | 59 | g = g.copy() 60 | a = a.copy() 61 | m = m.copy() 62 | 63 | q = self.Quaternion.copy() 64 | 65 | if np.linalg.norm(a) == 0 or np.linalg.norm(m) == 0: 66 | return self.Quaternion 67 | 68 | # Normalize accelerometer and magnetometer measurement 69 | a /= np.linalg.norm(a) 70 | m /= np.linalg.norm(a) 71 | 72 | # Reference direction of Earth's magnetic feild 73 | h = self.prod_q(q, self.prod_q(np.array([0, m[0], m[1], m[2]]), self.conjugate_quat(q))) 74 | b = np.array([0, np.linalg.norm(h[1:3]), 0, h[3]]) 75 | 76 | # Estimated direction of gravity and magnetic field 77 | v = np.array([ 78 | 2 * (q[1] * q[3] - q[0] * q[2]), 79 | 2 * (q[0] * q[1] + q[2] * q[3]), 80 | q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2 81 | ]) 82 | 83 | w = np.array([ 84 | 2 * b[1] * (0.5 - q[2]**2 - q[3]**2) + 2 * b[3] * (q[1] * q[3] - q[0] * q[2]), 85 | 2 * b[1] * (q[1] * q[2] - q[0] * q[3]) + 2 * b[3] * (q[0] * q[1] + q[2] * q[3]), 86 | 2 * b[1] * (q[0] * q[2] + q[1] * q[3]) + 2 * b[3] * (0.5 - q[1]**2 - q[2]**2) 87 | ]) 88 | 89 | # Error is sum of cross product between estimated direction and measured direction of fields 90 | e = np.cross(a, v) + np.cross(m, w) 91 | 92 | # Apply feedback terms 93 | if self.ki > 0: 94 | self.e_int += e * self.sample_period 95 | 96 | # Apply feedback terms 97 | g += self.kp * e + self.ki * self.e_int 98 | 99 | # Compute rate of change of quaternion 100 | q_dot = 0.5 * self.prod_q(q, np.array([0, g[0], g[1], g[2]])) 101 | 102 | # Integrate to yield quaternion 103 | q += q_dot * self.sample_period 104 | self.Quaternion = q / np.linalg.norm(q) 105 | return self.Quaternion 106 | 107 | def update_imu(self, g: np.array, a: np.array) -> np.array: 108 | """ 109 | Calculating quaternion using accelerometer and gyroscope data 110 | 111 | :param g: Gyroscope data [wx, wy, wz] 112 | :param a: Accelerometer data [ax, ay, az] 113 | :return: Calculated quaternion [w, i, j, k]. 114 | """ 115 | 116 | q = self.Quaternion.copy() 117 | 118 | g = g.copy() 119 | a = a.copy() 120 | 121 | if np.linalg.norm(a) == 0: 122 | return self.Quaternion 123 | 124 | a /= np.linalg.norm(a) 125 | 126 | # Estimated direction of gravity and vector perpendicular to magnetic flux 127 | v = np.array([ 128 | 2 * (q[1] * q[3] - q[0] * q[2]), 129 | 2 * (q[0] * q[1] + q[2] * q[3]), 130 | q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2 131 | ]) 132 | 133 | # Error is sum of cross product between estimated and measured direction of gravity 134 | e = np.cross(a, v) 135 | 136 | # Compute and apply integral feedback if enabled 137 | if self.ki > 0: 138 | self.e_int += e * self.sample_period 139 | 140 | # Apply feedback terms 141 | g += self.kp * e + self.ki * self.e_int 142 | 143 | # Compute rate of change of quaternion 144 | q_dot = 0.5 * self.prod_q(q, np.array([0, g[0], g[1], g[2]])) 145 | 146 | q += q_dot * self.sample_period 147 | self.Quaternion = q / np.linalg.norm(q) 148 | return self.Quaternion 149 | -------------------------------------------------------------------------------- /Mahony/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/Mahony/__init__.py -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **[English](REAMDE.md)** | [Українська](README_UA.md) 2 | 3 | # Oscillatory-Motion-Tracking-With-x-IMU-Python 4 | 5 | Python realization of [Oscillatory-Motion-Tracking-With-x-IMU](https://github.com/xioTechnologies/Oscillatory-Motion-Tracking-With-x-IMU). 6 | 7 | Tracking movements of IMU sensor. Trajectory demonstration of movements. 8 | 9 | `ximu_python_library` was taken from [Gait-Tracking-With-x-IMU-Python](https://github.com/daehwa/Gait-Tracking-With-x-IMU-Python) project. 10 | 11 | 12 | ## Installation 13 | 14 | For using this code you need to clone repo: 15 | 16 | ```shell 17 | git clone https://github.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python 18 | cd Oscillatory-Motion-Tracking-With-x-IMU-Python 19 | ``` 20 | 21 | Making a virtual environment and install dependencies: 22 | 23 | ```shell 24 | virtualenv -p python3 venv 25 | . venv/bin/activate 26 | 27 | pip install -r requirements.txt 28 | ``` 29 | 30 | Running the script which calculate rotation matrix and position: 31 | 32 | ```shell 33 | python script.py 34 | ``` 35 | 36 | Running the script for visualization of movements in 3D: 37 | 38 | ```shell 39 | python SixDOFAnimation.py 40 | ``` 41 | 42 | 43 | ## Visualization and charts 44 | 45 | | Python | MATLAB | 46 | |----------------------------------|---------------------------------------| 47 | | ![](charts/SixDOFAnimation.gif) | ![](charts/SixDOFAnimationMatlab.gif) | 48 | 49 | To view all charts follow [the link](charts/CHARTS.md) or push image below: 50 | 51 | [![Show all charts](charts/9_all_2D_charts.png)](charts/CHARTS.md) 52 | -------------------------------------------------------------------------------- /README_UA.md: -------------------------------------------------------------------------------- 1 | [English](README.md) | **[Українська](README_UA.md)** 2 | 3 | # Oscillatory-Motion-Tracking-With-x-IMU-Python 4 | 5 | Python реалізація [Oscillatory-Motion-Tracking-With-x-IMU](https://github.com/xioTechnologies/Oscillatory-Motion-Tracking-With-x-IMU). 6 | 7 | Відстежування рухів IMU давача. Побудова траєкторії руху. 8 | 9 | `ximu_python_library` була взята з проекту [Gait-Tracking-With-x-IMU-Python](https://github.com/daehwa/Gait-Tracking-With-x-IMU-Python). 10 | 11 | ## Встановлення 12 | 13 | Для використання даного коду Вам потрібно клонувати репозиторій: 14 | 15 | ```shell 16 | git clone https://github.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python 17 | cd Oscillatory-Motion-Tracking-With-x-IMU-Python 18 | ``` 19 | 20 | Створити віртуальне середовище та встановити залежності: 21 | 22 | ```shell 23 | virtualenv -p python3 venv 24 | . venv/bin/activate 25 | 26 | pip install -r requirements.txt 27 | ``` 28 | 29 | Виконання скрипта, що розраховує позицію та матрицю обертання: 30 | 31 | ```shell 32 | python script.py 33 | ``` 34 | 35 | Виконання скрипта візуалізації руху в 3D: 36 | 37 | ```shell 38 | python SixDOFAnimation.py 39 | ``` 40 | 41 | ## Візуалізація та графіки 42 | 43 | | Python | MATLAB | 44 | |----------------------------------|---------------------------------------| 45 | | ![](charts/SixDOFAnimation.gif) | ![](charts/SixDOFAnimationMatlab.gif) | 46 | 47 | Щоб переглянути всі графіки перейдіть за [посиланням](charts/CHARTS.md), або натисніть на зображення внизу: 48 | 49 | [![Show all charts](charts/9_all_2D_charts.png)](charts/CHARTS.md) 50 | -------------------------------------------------------------------------------- /SixDOFAnimation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # SixDOFAnimation 4 | # Implementation of 6D animation as in the 5 | # Oscillatory-Motion-Tracking-With-x-IMU project. 6 | # https://www.youtube.com/watch?v=SI1w9uaBw6Q 7 | # For high performance, the PyQtGraph library is used. 8 | # Date: 14.01.2023 9 | # Author: Korzhak (GitHub) 10 | # Ukraine 11 | # 12 | 13 | import pyqtgraph as pg 14 | import numpy as np 15 | from pyqtgraph.Qt import QtCore, QtGui 16 | import pyqtgraph.opengl as gl 17 | 18 | pg.mkQApp() 19 | 20 | # Make view in window 21 | 22 | view = gl.GLViewWidget() 23 | view.setCameraPosition(distance=0.6) 24 | view.show() 25 | 26 | # Set vectors length 27 | 28 | vector_len = 0.1 29 | 30 | # Making grid surfaces in 3 axes 31 | 32 | gx = gl.GLGridItem() 33 | gx.setSize(x=0.2, y=0.2, z=0.2) 34 | gx.setSpacing(x=0.01, y=0.01, z=0.01) 35 | gx.rotate(90, 0, 1, 0) 36 | gx.translate(-0.1, 0, 0) 37 | view.addItem(gx) 38 | 39 | gy = gl.GLGridItem() 40 | gy.setSize(x=0.2, y=0.2, z=0.2) 41 | gy.setSpacing(x=0.01, y=0.01, z=0.01) 42 | gy.rotate(90, 1, 0, 0) 43 | gy.translate(0, 0.1, 0) 44 | view.addItem(gy) 45 | 46 | gz = gl.GLGridItem() 47 | gz.setSize(x=0.2, y=0.2, z=0.2) 48 | gz.setSpacing(x=0.01, y=0.01, z=0.01) 49 | gz.translate(0, 0, -0.1) 50 | view.addItem(gz) 51 | 52 | # Making 3 vectors in space 53 | 54 | x_point1 = np.array([0, 0, 0]) # specify the (x, y, z) values of the first point in a tuple 55 | x_point2 = np.array([vector_len, 0, 0]) 56 | 57 | y_point1 = np.array([0, 0, 0]) # specify the (x, y, z) values of the first point in a tuple 58 | y_point2 = np.array([0, vector_len, 0]) 59 | 60 | z_point1 = np.array([0, 0, 0]) # specify the (x, y, z) values of the first point in a tuple 61 | z_point2 = np.array([0, 0, vector_len]) # specify the (x, y, z) values of the second point in a tuple 62 | 63 | x_axis = gl.GLLinePlotItem(pos=np.array([x_point1, x_point2]), width=3) 64 | y_axis = gl.GLLinePlotItem(pos=np.array([y_point1, y_point2]), width=3) 65 | z_axis = gl.GLLinePlotItem(pos=np.array([z_point1, z_point2]), width=3) 66 | 67 | x_axis.setData(color=(1, 0, 0, 1)) 68 | y_axis.setData(color=(0, 1, 0, 1)) 69 | z_axis.setData(color=(0, 0, 1, 1)) 70 | 71 | view.addItem(x_axis) 72 | view.addItem(y_axis) 73 | view.addItem(z_axis) 74 | 75 | # Load position data and rotation vectors data 76 | 77 | lin_pos_hp = np.load("logged_data/lin_pos_hp.npy") 78 | R = np.load("logged_data/rot_mat.npy") 79 | 80 | i = 0 81 | step = 10 82 | 83 | 84 | def update(): 85 | """ 86 | Function which update position of vectors in space 87 | """ 88 | global x_axis 89 | global x_point1 90 | global x_point2 91 | 92 | global y_axis 93 | global y_point1 94 | global y_point2 95 | 96 | global z_axis 97 | global z_point1 98 | global z_point2 99 | 100 | global lin_pos_hp 101 | global R 102 | global i 103 | if i <= max(lin_pos_hp.shape): 104 | 105 | # Set X vector 106 | pos = lin_pos_hp[i, :].copy() 107 | a = pos.copy() 108 | b = R[:, 0, i].copy() 109 | l = np.linalg.norm(b - a) # length of vector 110 | c = b * vector_len / l # find new vector with length 'vector_len' 111 | x_point1 = pos # set start point 112 | x_point2 = pos + c # set end point 113 | x_axis.setData(pos=np.array([x_point1, x_point2])) 114 | 115 | # Set Y vector 116 | a = pos.copy() 117 | b = R[:, 1, i].copy() 118 | l = np.linalg.norm(b - a) # length of vector 119 | c = b * vector_len / l # find new vector with length 'vector_len' 120 | y_point1 = pos # set start point 121 | y_point2 = pos + c # set end point 122 | y_axis.setData(pos=np.array([y_point1, y_point2])) 123 | 124 | # Set Z vector 125 | a = pos.copy() 126 | b = R[:, 2, i].copy() 127 | l = np.linalg.norm(b - a) # length of vector 128 | c = b * vector_len / l # find new vector with length 'vector_len' 129 | z_point1 = pos # set start point 130 | z_point2 = pos + c # set end point 131 | z_axis.setData(pos=np.array([z_point1, z_point2])) 132 | i += step 133 | 134 | 135 | if __name__ == '__main__': 136 | t = QtCore.QTimer() 137 | t.timeout.connect(update) 138 | t.start(1) 139 | import sys 140 | 141 | if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'): 142 | QtGui.QGuiApplication.instance().exec_() 143 | -------------------------------------------------------------------------------- /charts/1_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/1_matlab.png -------------------------------------------------------------------------------- /charts/1_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/1_python.png -------------------------------------------------------------------------------- /charts/2_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/2_matlab.png -------------------------------------------------------------------------------- /charts/2_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/2_python.png -------------------------------------------------------------------------------- /charts/3_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/3_matlab.png -------------------------------------------------------------------------------- /charts/3_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/3_python.png -------------------------------------------------------------------------------- /charts/4_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/4_matlab.png -------------------------------------------------------------------------------- /charts/4_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/4_python.png -------------------------------------------------------------------------------- /charts/5_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/5_matlab.png -------------------------------------------------------------------------------- /charts/5_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/5_python.png -------------------------------------------------------------------------------- /charts/6_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/6_matlab.png -------------------------------------------------------------------------------- /charts/6_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/6_python.png -------------------------------------------------------------------------------- /charts/7_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/7_matlab.png -------------------------------------------------------------------------------- /charts/7_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/7_python.png -------------------------------------------------------------------------------- /charts/8_matlab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/8_matlab.png -------------------------------------------------------------------------------- /charts/8_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/8_python.png -------------------------------------------------------------------------------- /charts/9_all_2D_charts.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/9_all_2D_charts.png -------------------------------------------------------------------------------- /charts/9_all_2D_charts_full_size.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/9_all_2D_charts_full_size.png -------------------------------------------------------------------------------- /charts/CHARTS.md: -------------------------------------------------------------------------------- 1 | 2 | | Python | MATLAB | 3 | |--------------------------|--------------------------------| 4 | | ![](SixDOFAnimation.gif) | ![](SixDOFAnimationMatlab.gif) | 5 | | ![](1_python.png) | ![](1_matlab.png) | 6 | | ![](2_python.png) | ![](2_matlab.png) | 7 | | ![](3_python.png) | ![](3_matlab.png) | 8 | | ![](4_python.png) | ![](4_matlab.png) | 9 | | ![](5_python.png) | ![](5_matlab.png) | 10 | | ![](6_python.png) | ![](6_matlab.png) | 11 | | ![](7_python.png) | ![](7_matlab.png) | 12 | | ![](8_python.png) | ![](8_matlab.png) | 13 | -------------------------------------------------------------------------------- /charts/SixDOFAnimation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/SixDOFAnimation.gif -------------------------------------------------------------------------------- /charts/SixDOFAnimationMatlab.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/charts/SixDOFAnimationMatlab.gif -------------------------------------------------------------------------------- /logged_data/LoggedData_CalBattAndTherm.csv: -------------------------------------------------------------------------------- 1 | Packet number,Battery voltage (V),Thermometer (degrees C) 2 | 14,4.222168,30.82813 3 | 148,4.222168,30.82813 4 | 281,4.220459,30.82813 5 | 415,4.218994,30.88281 6 | 548,4.218994,30.88281 7 | 682,4.218994,30.82813 8 | 815,4.218994,30.94141 9 | 949,4.218994,30.82813 10 | 1082,4.218994,30.82813 11 | 1216,4.220459,30.82813 12 | 1349,4.220459,30.88281 13 | 1483,4.223633,30.82813 14 | 1616,4.217285,30.82813 15 | 1750,4.218994,30.82813 16 | 1883,4.218994,30.82813 17 | 2017,4.218994,30.82813 18 | 2150,4.222168,30.88281 19 | 2284,4.218994,30.88281 20 | 2417,4.220459,30.82813 21 | 2551,4.222168,30.82813 22 | 2684,4.220459,30.94141 23 | 2818,4.222168,30.88281 24 | 2951,4.220459,30.82813 25 | 3085,4.218994,30.82813 26 | 3218,4.218994,30.94141 27 | 3352,4.218994,30.82813 28 | 3485,4.218994,30.82813 29 | 3619,4.222168,30.82813 30 | 3752,4.220459,30.82813 31 | 3886,4.222168,30.88281 32 | 4019,4.218994,30.82813 33 | 4153,4.218994,30.94141 34 | 4286,4.220459,30.82813 35 | 4420,4.218994,30.88281 36 | 4553,4.218994,30.88281 37 | 4687,4.218994,30.94141 38 | 4820,4.220459,30.82813 39 | 4954,4.220459,30.94141 40 | 5087,4.220459,30.82813 41 | 5221,4.218994,30.88281 42 | 5354,4.218994,30.82813 43 | 5488,4.218994,30.94141 44 | 5621,4.220459,30.82813 45 | 5755,4.218994,30.82813 46 | 5888,4.218994,30.94141 47 | 6022,4.222168,30.82813 48 | 6155,4.220459,30.82813 49 | 6289,4.220459,30.82813 50 | 6422,4.218994,30.94141 51 | 6556,4.218994,30.82813 52 | 6689,4.218994,30.88281 53 | 6823,4.218994,30.94141 54 | 6956,4.220459,30.82813 55 | 7090,4.217285,30.82813 56 | 7223,4.220459,30.82813 57 | 7357,4.222168,30.82813 58 | 7490,4.220459,30.82813 59 | 7624,4.218994,30.82813 60 | 7757,4.218994,30.82813 61 | 7891,4.218994,30.88281 62 | 8024,4.218994,30.82813 63 | 8158,4.218994,30.82813 64 | 8291,4.222168,30.94141 65 | 8425,4.222168,31.22656 66 | 8558,4.220459,31.22656 67 | 8692,4.220459,30.88281 68 | 8825,4.222168,30.88281 69 | 8959,4.218994,30.88281 70 | 9092,4.218994,30.88281 71 | 9226,4.218994,30.82813 72 | 9359,4.218994,30.82813 73 | 9493,4.218994,30.82813 74 | 9626,4.218994,30.94141 75 | 9760,4.222168,30.94141 76 | 9893,4.220459,30.88281 77 | 10027,4.218994,30.94141 78 | 10160,4.218994,30.88281 79 | 10294,4.218994,30.94141 80 | 10427,4.218994,30.88281 81 | 10561,4.218994,30.94141 82 | 10694,4.218994,30.88281 83 | 10828,4.218994,30.88281 84 | 10961,4.220459,30.88281 85 | 11095,4.222168,31.16797 86 | 11228,4.222168,30.82813 87 | 11362,4.218994,30.82813 88 | 11495,4.218994,30.94141 89 | 11629,4.218994,30.94141 90 | 11762,4.218994,30.88281 91 | 11896,4.218994,30.94141 92 | 12029,4.218994,31.22656 93 | 12163,4.220459,30.94141 94 | 12296,4.222168,30.88281 95 | 12430,4.222168,30.94141 96 | 12563,4.217285,30.88281 97 | 12697,4.218994,30.94141 98 | 12830,4.218994,30.94141 99 | 12964,4.218994,30.94141 100 | 13097,4.218994,30.94141 101 | 13231,4.218994,30.82813 102 | 13364,4.220459,30.82813 103 | 13498,4.222168,30.88281 104 | 13631,4.220459,30.82813 105 | 13765,4.222168,30.88281 106 | 13898,4.222168,31.22656 107 | 14032,4.218994,30.88281 108 | 14165,4.218994,30.94141 109 | 14299,4.218994,31.22656 110 | 14432,4.218994,31.22656 111 | 14566,4.222168,30.94141 112 | 14699,4.222168,30.82813 113 | 14833,4.222168,30.88281 114 | 14966,4.218994,31.22656 115 | 15100,4.218994,31.22656 116 | 15233,4.220459,31.16797 117 | 15367,4.218994,31.16797 118 | 15500,4.218994,30.82813 119 | 15634,4.220459,30.88281 120 | 15767,4.222168,31.22656 121 | 15901,4.222168,30.94141 122 | 16034,4.222168,30.94141 123 | 16168,4.218994,30.94141 124 | 16301,4.218994,31.22656 125 | 16435,4.218994,30.82813 126 | 16568,4.222168,30.94141 127 | 16702,4.218994,31.22656 128 | 16835,4.218994,31.22656 129 | 16969,4.222168,30.94141 130 | 17102,4.222168,31.16797 131 | 17236,4.222168,31.22656 132 | 17369,4.218994,30.94141 133 | 17503,4.222168,31.22656 134 | 17636,4.218994,31.16797 135 | 17770,4.218994,31.16797 136 | 17903,4.222168,31.16797 137 | 18037,4.218994,31.39844 138 | 18170,4.222168,31.22656 139 | 18304,4.222168,31.16797 140 | 18437,4.222168,31.45703 141 | 18571,4.218994,31.39844 142 | 18704,4.218994,31.39844 143 | 18838,4.218994,31.39844 144 | 18971,4.222168,31.45703 145 | 19105,4.218994,31.22656 146 | 19238,4.222168,31.39844 147 | 19372,4.222168,31.45703 148 | 19505,4.222168,31.39844 149 | 19639,4.223633,31.11328 150 | 19772,4.220459,31.22656 151 | 19906,4.218994,31.16797 152 | 20039,4.220459,31.22656 153 | 20173,4.218994,31.16797 154 | 20306,4.218994,31.45703 155 | 20440,4.222168,31.39844 156 | 20573,4.220459,31.45703 157 | 20707,4.222168,31.22656 158 | 20840,4.222168,31.45703 159 | 20974,4.218994,31.625 160 | 21107,4.222168,31.22656 161 | 21241,4.220459,31.33984 162 | 21374,4.218994,31.16797 163 | 21508,4.218994,31.22656 164 | 21641,4.218994,31.16797 165 | 21775,4.218994,31.45703 166 | 21908,4.223633,31.39844 167 | 22042,4.223633,31.22656 168 | 22175,4.222168,30.94141 169 | 22309,4.218994,31.45703 170 | 22442,4.218994,31.22656 171 | 22576,4.222168,31.45703 172 | 22709,4.218994,30.94141 173 | 22843,4.218994,31.39844 174 | 22976,4.218994,31.57031 175 | 23110,4.222168,31.22656 176 | 23243,4.222168,31.45703 177 | 23377,4.222168,31.45703 178 | 23510,4.218994,31.45703 179 | 23644,4.220459,31.39844 180 | 23777,4.218994,31.39844 181 | 23911,4.218994,31.45703 182 | 24044,4.222168,31.45703 183 | 24178,4.218994,31.22656 184 | -------------------------------------------------------------------------------- /logged_data/LoggedData_DateTime.csv: -------------------------------------------------------------------------------- 1 | Packet number,Year,Month,Day,Hours,Minutes,Seconds 2 | 147,2000,1,5,0,1,11 3 | 414,2000,1,5,0,1,11 4 | 681,2000,1,5,0,1,12 5 | 948,2000,1,5,0,1,12 6 | 1215,2000,1,5,0,1,13 7 | 1482,2000,1,5,0,1,13 8 | 1749,2000,1,5,0,1,14 9 | 2016,2000,1,5,0,1,14 10 | 2283,2000,1,5,0,1,15 11 | 2550,2000,1,5,0,1,15 12 | 2817,2000,1,5,0,1,16 13 | 3084,2000,1,5,0,1,16 14 | 3351,2000,1,5,0,1,17 15 | 3618,2000,1,5,0,1,17 16 | 3885,2000,1,5,0,1,18 17 | 4152,2000,1,5,0,1,18 18 | 4419,2000,1,5,0,1,19 19 | 4686,2000,1,5,0,1,19 20 | 4953,2000,1,5,0,1,20 21 | 5220,2000,1,5,0,1,20 22 | 5487,2000,1,5,0,1,21 23 | 5754,2000,1,5,0,1,21 24 | 6021,2000,1,5,0,1,22 25 | 6288,2000,1,5,0,1,22 26 | 6555,2000,1,5,0,1,23 27 | 6822,2000,1,5,0,1,23 28 | 7089,2000,1,5,0,1,24 29 | 7356,2000,1,5,0,1,24 30 | 7623,2000,1,5,0,1,25 31 | 7890,2000,1,5,0,1,25 32 | 8157,2000,1,5,0,1,26 33 | 8424,2000,1,5,0,1,26 34 | 8691,2000,1,5,0,1,27 35 | 8958,2000,1,5,0,1,27 36 | 9225,2000,1,5,0,1,28 37 | 9492,2000,1,5,0,1,28 38 | 9759,2000,1,5,0,1,29 39 | 10026,2000,1,5,0,1,29 40 | 10293,2000,1,5,0,1,30 41 | 10560,2000,1,5,0,1,30 42 | 10827,2000,1,5,0,1,31 43 | 11094,2000,1,5,0,1,31 44 | 11361,2000,1,5,0,1,32 45 | 11628,2000,1,5,0,1,32 46 | 11895,2000,1,5,0,1,33 47 | 12162,2000,1,5,0,1,33 48 | 12429,2000,1,5,0,1,34 49 | 12696,2000,1,5,0,1,34 50 | 12963,2000,1,5,0,1,35 51 | 13230,2000,1,5,0,1,35 52 | 13497,2000,1,5,0,1,36 53 | 13764,2000,1,5,0,1,36 54 | 14031,2000,1,5,0,1,37 55 | 14298,2000,1,5,0,1,37 56 | 14565,2000,1,5,0,1,38 57 | 14832,2000,1,5,0,1,38 58 | 15099,2000,1,5,0,1,39 59 | 15366,2000,1,5,0,1,39 60 | 15633,2000,1,5,0,1,40 61 | 15900,2000,1,5,0,1,40 62 | 16167,2000,1,5,0,1,41 63 | 16434,2000,1,5,0,1,41 64 | 16701,2000,1,5,0,1,42 65 | 16968,2000,1,5,0,1,42 66 | 17235,2000,1,5,0,1,43 67 | 17502,2000,1,5,0,1,43 68 | 17769,2000,1,5,0,1,44 69 | 18036,2000,1,5,0,1,44 70 | 18303,2000,1,5,0,1,45 71 | 18570,2000,1,5,0,1,45 72 | 18837,2000,1,5,0,1,46 73 | 19104,2000,1,5,0,1,46 74 | 19371,2000,1,5,0,1,47 75 | 19638,2000,1,5,0,1,47 76 | 19905,2000,1,5,0,1,48 77 | 20172,2000,1,5,0,1,48 78 | 20439,2000,1,5,0,1,49 79 | 20706,2000,1,5,0,1,49 80 | 20973,2000,1,5,0,1,50 81 | 21240,2000,1,5,0,1,50 82 | 21507,2000,1,5,0,1,51 83 | 21774,2000,1,5,0,1,51 84 | 22041,2000,1,5,0,1,52 85 | 22308,2000,1,5,0,1,52 86 | 22575,2000,1,5,0,1,53 87 | 22842,2000,1,5,0,1,53 88 | 23109,2000,1,5,0,1,54 89 | 23376,2000,1,5,0,1,54 90 | 23643,2000,1,5,0,1,55 91 | 23910,2000,1,5,0,1,55 92 | 24177,2000,1,5,0,1,56 93 | -------------------------------------------------------------------------------- /logged_data/lin_pos_hp.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/logged_data/lin_pos_hp.npy -------------------------------------------------------------------------------- /logged_data/rot_mat.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/logged_data/rot_mat.npy -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Korzhak/Oscillatory-Motion-Tracking-With-x-IMU-Python/45202bbc400271ebf55a6d77937f9a9444378e01/requirements.txt -------------------------------------------------------------------------------- /script.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Oscillatory-Motion-Tracking-With-x-IMU-Python 4 | # This project is realization of Oscillatory-Motion-Tracking-With-x-IMU on python. 5 | # Link to origin code: https://github.com/xioTechnologies/Oscillatory-Motion-Tracking-With-x-IMU 6 | # Date: 10.01.2023 7 | # Author: Korzhak (GitHub) 8 | # Ukraine 9 | # 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | from scipy.signal import butter, filtfilt 14 | from Mahony.MahonyAHRS import Mahony 15 | from ximu_python_library import xIMUdataClass as xIMU 16 | 17 | 18 | # Additional functions 19 | 20 | def length(array: np.array) -> int: 21 | """ 22 | Like a length() in Matlab. 23 | 24 | :param array: numpy array. 25 | :return: the length of the largest array dimension. 26 | """ 27 | return max(array.shape) 28 | 29 | 30 | # Main settings 31 | 32 | file_path = 'logged_data/LoggedData' 33 | sample_frequency = 256 34 | sample_period = 1 / sample_frequency 35 | 36 | # Import data 37 | 38 | xIMUdata = xIMU.xIMUdataClass(file_path, sr=sample_frequency) 39 | 40 | x_time = xIMUdata.CalInertialAndMagneticData.Time 41 | 42 | gyr = np.array([xIMUdata.CalInertialAndMagneticData.gyroscope[:, 0], 43 | xIMUdata.CalInertialAndMagneticData.gyroscope[:, 1], 44 | xIMUdata.CalInertialAndMagneticData.gyroscope[:, 2]]).T 45 | 46 | acc = np.array([xIMUdata.CalInertialAndMagneticData.accelerometer[:, 0], 47 | xIMUdata.CalInertialAndMagneticData.accelerometer[:, 1], 48 | xIMUdata.CalInertialAndMagneticData.accelerometer[:, 2]]).T 49 | 50 | # Plot 51 | 52 | fig = plt.figure(figsize=(7.8, 5)) 53 | plt.plot(x_time, acc[:, 0], c='r', linewidth=0.5) 54 | plt.plot(x_time, acc[:, 1], c='g', linewidth=0.5) 55 | plt.plot(x_time, acc[:, 2], c='b', linewidth=0.5) 56 | plt.legend(["x", "y", "z"]) 57 | plt.title("acceleration") 58 | plt.xlabel("time (s)") 59 | plt.ylabel("g") 60 | plt.show(block=False) 61 | 62 | fig = plt.figure(figsize=(7.8, 5)) 63 | plt.plot(x_time, gyr[:, 0], c='r', linewidth=0.5) 64 | plt.plot(x_time, gyr[:, 1], c='g', linewidth=0.5) 65 | plt.plot(x_time, gyr[:, 2], c='b', linewidth=0.5) 66 | plt.legend(["x", "y", "z"]) 67 | plt.title("gyroscope") 68 | plt.xlabel("time (s)") 69 | plt.ylabel("rad/sec") 70 | plt.show(block=False) 71 | 72 | # Process data through AHRS algorithm (calculate orientation) 73 | # See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 74 | 75 | R = np.zeros((3, 3, length(gyr))) # rotation matrix describing sensor relative to Earth 76 | 77 | ahrs = Mahony(sample_freq=sample_frequency, ki_def=0) 78 | np.set_printoptions(suppress=True) 79 | 80 | for i in range(length(gyr)): 81 | ahrs.update_imu(gyr[i, :] * (np.pi/180), acc[i, :]) # gyroscope units must be radians 82 | R[:, :, i] = ahrs.Q_to_DCM() # transpose because ahrs provides Earth relative to sensor 83 | 84 | 85 | # Save rotation matrix for 3D Plot animation 86 | np.save("logged_data/rot_mat.npy", R) 87 | 88 | # Calculate 'tilt-compensated' accelerometer 89 | 90 | tc_acc = np.zeros(acc.shape) # accelerometer in Earth frame 91 | 92 | for i in range(length(acc)): 93 | tc_acc[i, :] = R[:, :, i] @ acc[i, :] # product rotation matrix by transpose acceleration 94 | 95 | # Plot 96 | 97 | fig = plt.figure(figsize=(7.8, 5)) 98 | plt.plot(x_time, tc_acc[:, 0], c='r', linewidth=0.5) 99 | plt.plot(x_time, tc_acc[:, 1], c='g', linewidth=0.5) 100 | plt.plot(x_time, tc_acc[:, 2], c='b', linewidth=0.5) 101 | plt.legend(["x", "y", "z"]) 102 | plt.title("'Tilt-compensated' accelerometer") 103 | plt.xlabel("time (s)") 104 | plt.ylabel("g") 105 | plt.show(block=False) 106 | 107 | # Calculate linear acceleration in Earth frame (subtracting gravity) 108 | 109 | lin_acc = tc_acc - np.array([np.zeros(length(tc_acc)), 110 | np.zeros(length(tc_acc)), 111 | np.ones(length(tc_acc))]).T 112 | 113 | lin_acc *= 9.81 # convert from 'g' to m/s^2 114 | 115 | # Plot 116 | 117 | fig = plt.figure(figsize=(7.8, 5)) 118 | plt.plot(x_time, lin_acc[:, 0], c='r', linewidth=0.5) 119 | plt.plot(x_time, lin_acc[:, 1], c='g', linewidth=0.5) 120 | plt.plot(x_time, lin_acc[:, 2], c='b', linewidth=0.5) 121 | plt.legend(["x", "y", "z"]) 122 | plt.title("Linear acceleration") 123 | plt.xlabel("time (s)") 124 | plt.ylabel("m/s^2") 125 | plt.show(block=False) 126 | 127 | # Calculate linear velocity (integrate acceleration) 128 | 129 | lin_vel = np.zeros(lin_acc.shape) 130 | 131 | for i in range(1, length(lin_acc)): 132 | lin_vel[i, :] = lin_vel[i - 1, :] + lin_acc[i, :] * sample_period 133 | 134 | # Plot 135 | 136 | fig = plt.figure(figsize=(7.8, 5)) 137 | plt.plot(x_time, lin_vel[:, 0], c='r', linewidth=0.5) 138 | plt.plot(x_time, lin_vel[:, 1], c='g', linewidth=0.5) 139 | plt.plot(x_time, lin_vel[:, 2], c='b', linewidth=0.5) 140 | plt.legend(["x", "y", "z"]) 141 | plt.title("Linear velocity") 142 | plt.xlabel("time (s)") 143 | plt.ylabel("m/s") 144 | plt.show(block=False) 145 | 146 | # High-pass filter linear velocity to remove drift 147 | 148 | order = 1 149 | filter_cut_off = 0.1 150 | [b, a] = butter(order, (2 * filter_cut_off) / sample_frequency, 'high') 151 | lin_vel_hp = filtfilt(b, a, lin_vel.T).T 152 | 153 | # Plot 154 | 155 | fig = plt.figure(figsize=(7.8, 5)) 156 | plt.plot(x_time, lin_vel_hp[:, 0], c='r', linewidth=0.5) 157 | plt.plot(x_time, lin_vel_hp[:, 1], c='g', linewidth=0.5) 158 | plt.plot(x_time, lin_vel_hp[:, 2], c='b', linewidth=0.5) 159 | plt.legend(["x", "y", "z"]) 160 | plt.title("High-pass filtered linear velocity") 161 | plt.xlabel("time (s)") 162 | plt.ylabel("m/s") 163 | plt.show(block=False) 164 | 165 | # Calculate linear position (integrate velocity) 166 | 167 | lin_pos = np.zeros(lin_vel_hp.shape) 168 | 169 | for i in range(1, length(lin_vel_hp)): 170 | lin_pos[i, :] = lin_pos[i - 1, :] + lin_vel_hp[i, :] * sample_period 171 | 172 | # Plot 173 | 174 | fig = plt.figure(figsize=(7.8, 5)) 175 | plt.plot(x_time, lin_pos[:, 0], c='r', linewidth=0.5) 176 | plt.plot(x_time, lin_pos[:, 1], c='g', linewidth=0.5) 177 | plt.plot(x_time, lin_pos[:, 2], c='b', linewidth=0.5) 178 | plt.legend(["x", "y", "z"]) 179 | plt.title("Linear position") 180 | plt.xlabel("time (s)") 181 | plt.ylabel("m") 182 | plt.show(block=False) 183 | 184 | # High-pass filter linear position to remove drift 185 | 186 | lin_pos_hp = filtfilt(b, a, lin_pos.T).T 187 | 188 | # Plot 189 | 190 | fig = plt.figure(figsize=(7.8, 5)) 191 | plt.plot(x_time, lin_pos_hp[:, 0], c='r', linewidth=0.5) 192 | plt.plot(x_time, lin_pos_hp[:, 1], c='g', linewidth=0.5) 193 | plt.plot(x_time, lin_pos_hp[:, 2], c='b', linewidth=0.5) 194 | plt.legend(["x", "y", "z"]) 195 | plt.title("High-pass filtered linear position") 196 | plt.xlabel("time (s)") 197 | plt.ylabel("m") 198 | plt.show() 199 | 200 | # Save position for 3D Plot animation 201 | np.save("logged_data/lin_pos_hp.npy", lin_pos_hp) 202 | -------------------------------------------------------------------------------- /ximu_python_library/CalInertialAndMagneticDataClass.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # class DataBaseClass(): 4 | # def __init__(self): 5 | 6 | # class TimeSeriesDataBaseClass(): 7 | # def __init__(self): 8 | # self.DataBase = DataBaseClass() 9 | 10 | # class InertialAndMagneticDataBaseClass(): 11 | # def __init__(self, data): 12 | # self.TimeSeriesDataBase = TimeSeriesDataBaseClass() 13 | # set_dataformat(data) 14 | 15 | # def set_dataformat(self, data): 16 | # self.gyroscope = data[:,1:4] 17 | # self.accelerometer = data[:,4:7] 18 | # self.magnetometer = data[:,7:10] 19 | 20 | class CalInertialAndMagneticDataClass(): 21 | def __init__(self, filename, sr): 22 | self.FileNameAppendage = '_CalInertialAndMag.csv' 23 | self.SampleRate = sr 24 | self.GyroscopeUnits = 'degrees/s' 25 | self.AccelerometerUnits = 'g' 26 | self.MagnetometerUnits = 'G' 27 | data = np.loadtxt(filename+self.FileNameAppendage, 28 | delimiter=',',skiprows=1) 29 | # self.InertialAndMagneticDataBase = InertialAndMagneticDataBaseClass(data) 30 | self.set_dataformat(data) 31 | self.packetNum = data.shape[0] 32 | self.Time = np.arange(0,self.packetNum)*(1/self.SampleRate) 33 | 34 | def set_dataformat(self, data): 35 | self.gyroscope = data[:,1:4] 36 | self.accelerometer = data[:,4:7] 37 | self.magnetometer = data[:,7:10] 38 | 39 | 40 | 41 | def get_obj(filename, sr): 42 | obj = CalInertialAndMagneticDataClass(filename, sr) 43 | return obj -------------------------------------------------------------------------------- /ximu_python_library/xIMUdataClass.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | this_dir = os.path.dirname(__file__) 5 | sys.path.append(this_dir) 6 | import CalInertialAndMagneticDataClass as calIM 7 | 8 | 9 | class xIMUdataClass(): 10 | def __init__(self, filename='', sr_type='InertialMagneticSampleRate', sr=20000 / 3): 11 | self.FileNamePrefix = filename 12 | self.ErrorData = [] 13 | self.CommandData = [] 14 | self.RegisterData = [] 15 | self.DateTimeData = [] 16 | self.RawBatteryAndThermometerData = [] 17 | self.CalBatteryAndThermometerData = [] 18 | self.RawInertialAndMagneticData = [] 19 | self.CalInertialAndMagneticData = [] 20 | self.QuaternionData = [] 21 | self.RotationMatrixData = [] 22 | self.EulerAnglesData = [] 23 | self.DigitalIOdata = [] 24 | self.RawAnalogueInputData = [] 25 | self.CalAnalogueInputData = [] 26 | self.PWMoutputData = [] 27 | self.RawADXL345busData = [] 28 | self.CalADXL345busData = [] 29 | self.sr_type = sr_type 30 | self.sr = sr 31 | self.load_data() 32 | self.apply_samplerate() 33 | # self.plot() 34 | 35 | def load_data(self): 36 | dataImported = False 37 | try: 38 | self.CalInertialAndMagneticData = calIM.get_obj(self.FileNamePrefix, self.sr) 39 | dataImported = True 40 | except: 41 | print('') 42 | if not dataImported: 43 | print('No data was imported.') 44 | exit() 45 | 46 | def apply_samplerate(self): 47 | if self.sr_type == 'InertialMagneticSampleRate': 48 | try: 49 | h = self.CalInertialAndMagneticData 50 | h.SampleRate = self.sr 51 | except: 52 | print('') 53 | else: 54 | print('Invalid argument.') 55 | exit() 56 | 57 | # def plot(self): 58 | # try: 59 | # self.CalInertialAndMagneticData.plot() ######################### 60 | # except: 61 | # continue 62 | --------------------------------------------------------------------------------