├── Script.py ├── ahrs ├── __init__.py └── ahrs.py ├── README.md ├── .gitignore ├── LICENSE └── quaternion.py /Script.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ahrs/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Oscillatory-Motion-Tracking 2 | 3 | A python based imu AHRS system to provide position data from gyroscope and acceleration sensors. 4 | It is based on the Open source IMU and AHRS algorithms by xioTechnologies 5 | 6 | 7 | ''' 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | This program is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this program. If not, see . 20 | ''' -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | 49 | # Translations 50 | *.mo 51 | *.pot 52 | 53 | # Django stuff: 54 | *.log 55 | local_settings.py 56 | 57 | # Flask stuff: 58 | instance/ 59 | .webassets-cache 60 | 61 | # Scrapy stuff: 62 | .scrapy 63 | 64 | # Sphinx documentation 65 | docs/_build/ 66 | 67 | # PyBuilder 68 | target/ 69 | 70 | # Jupyter Notebook 71 | .ipynb_checkpoints 72 | 73 | # pyenv 74 | .python-version 75 | 76 | # celery beat schedule file 77 | celerybeat-schedule 78 | 79 | # SageMath parsed files 80 | *.sage.py 81 | 82 | # dotenv 83 | .env 84 | 85 | # virtualenv 86 | .venv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | .spyproject 93 | 94 | # Rope project settings 95 | .ropeproject 96 | 97 | # mkdocs documentation 98 | /site 99 | 100 | # mypy 101 | .mypy_cache/ 102 | -------------------------------------------------------------------------------- /ahrs/ahrs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from quaternion import Quaternion 3 | 4 | 5 | class MahonyAHRS: 6 | """ 7 | Madgwick's implementation of Mayhony's AHRS algorithm 8 | 9 | For more information see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 10 | or https://github.com/xioTechnologies/Oscillatory-Motion-Tracking-With-x-IMU/blob/master/%40MahonyAHRS/MahonyAHRS.m 11 | 12 | This is ported from the MATLAB class by SOH Madgwick from 28.09.2011 13 | 14 | :author: basti-schr (basti.schr@gmx.de) 15 | """ 16 | 17 | sample_period = 1 / 256 18 | quaternion = Quaternion(1, 0, 0, 0) # output quaternion describing the Earth relative to the sensor 19 | kp = 1 # algorithm proportional gain 20 | ki = 0 # algorithm integral gain 21 | _e_int = np.array([.0, .0, .0]) # integral error 22 | 23 | @staticmethod 24 | def norm(vector): 25 | size = 0 26 | for i in vector: 27 | size += i ** 2 28 | if size <= 0: 29 | raise ZeroDivisionError 30 | size = np.sqrt(size) 31 | return (np.array(vector) / size).tolist() 32 | 33 | def __init__(self, sample_period=None, quaternion=None, kp=None, ki=None): 34 | if sample_period is None: 35 | sample_period = 1 / 256 36 | if quaternion is None: 37 | quaternion = Quaternion(1, 0, 0, 0) 38 | if kp is None: 39 | kp = 1 40 | if ki is None: 41 | ki = 0 42 | 43 | self.sample_period = sample_period 44 | self.quaternion = quaternion # output quaternion describing the Earth relative to the sensor 45 | self.kp = kp # algorithm proportional gain 46 | self.ki = ki # algorithm integral gain 47 | self._e_int = np.array([.0, .0, .0]) # integral error 48 | 49 | def update_imu(self, gyroscope, accelerometer): 50 | gyroscope = np.array(gyroscope) 51 | accelerometer = np.array(accelerometer) 52 | q = self.quaternion # short name local variable for readability 53 | 54 | # normalize accelerometer measurement 55 | self.norm(accelerometer) 56 | 57 | # estimate the direction of gravity 58 | v = [2 * (q[1] * q[3] - q[0] * q[2]), 59 | 2 * (q[0] * q[1] + q[2] * q[3]), 60 | q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2 61 | ] 62 | 63 | # Error is sum of cross product between estimated direction and measured direction of field 64 | e = np.cross(accelerometer, v) 65 | if self.ki > 0: 66 | self._e_int += e * self.sample_period 67 | else: 68 | self._e_int = np.array([.0, .0, .0]) 69 | 70 | # Apply feedback terms 71 | gyroscope = gyroscope + self.kp * e + self.ki * self._e_int 72 | 73 | # Compute rate of change of quaternion 74 | q_dot = q * Quaternion(np.hstack((0, gyroscope))) 75 | q_dot *= 0.5 76 | 77 | # Integrate to yield quaternion 78 | q += q_dot * self.sample_period 79 | self.quaternion = q.norm() 80 | 81 | def update(self, gyroscope, accelerometer, magnetometer): 82 | print("not implemented at this time! Continue as IMU ...") 83 | self.update_imu(gyroscope, accelerometer) 84 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /quaternion.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu 4 | supplemented by Sebastian Schröder, basti.schr@gmx.de 5 | 6 | 7 | This program is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU Lesser General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General Public License 18 | along with this program. If not, see . 19 | 20 | """ 21 | 22 | import numpy as np 23 | import numbers 24 | 25 | 26 | class Quaternion: 27 | """ 28 | A simple class implementing basic quaternion arithmetic. 29 | """ 30 | 31 | def __init__(self, w_or_q, x=None, y=None, z=None): 32 | """ 33 | Initializes a Quaternion object 34 | :param w_or_q: A scalar representing the real part of the quaternion, another Quaternion object or a 35 | four-element array containing the quaternion values 36 | :param x: The first imaginary part if w_or_q is a scalar 37 | :param y: The second imaginary part if w_or_q is a scalar 38 | :param z: The third imaginary part if w_or_q is a scalar 39 | """ 40 | self._q = np.array([1.0, .0, .0, .0]) 41 | 42 | if x is not None and y is not None and z is not None: 43 | w = w_or_q 44 | q = np.array([float(w), float(x), float(y), float(z)]) 45 | elif isinstance(w_or_q, Quaternion): 46 | q = np.array(w_or_q.q) 47 | else: 48 | q = np.array(w_or_q) 49 | if len(q) != 4: 50 | raise ValueError("Expecting a 4-element array or w x y z as parameters") 51 | 52 | self._set_q(q) 53 | 54 | # Quaternion specific interfaces 55 | 56 | def conj(self): 57 | """ 58 | Returns the conjugate of the quaternion 59 | :rtype : Quaternion 60 | :return: the conjugate of the quaternion 61 | """ 62 | return Quaternion(self._q[0], -self._q[1], -self._q[2], -self._q[3]) 63 | 64 | def to_angle_axis(self): 65 | """ 66 | Returns the quaternion's rotation represented by an Euler angle and axis. 67 | If the quaternion is the identity quaternion (1, 0, 0, 0), a rotation along the x axis with angle 0 is returned. 68 | :return: rad, x, y, z 69 | """ 70 | if self[0] == 1 and self[1] == 0 and self[2] == 0 and self[3] == 0: 71 | return 0, 1, 0, 0 72 | rad = np.arccos(self[0]) * 2 73 | imaginary_factor = np.sin(rad / 2) 74 | if abs(imaginary_factor) < 1e-8: 75 | return 0, 1, 0, 0 76 | x = self._q[1] / imaginary_factor 77 | y = self._q[2] / imaginary_factor 78 | z = self._q[3] / imaginary_factor 79 | return rad, x, y, z 80 | 81 | @staticmethod 82 | def from_angle_axis(rad, x, y, z): 83 | s = np.sin(rad / 2) 84 | return Quaternion(np.cos(rad / 2), x * s, y * s, z * s) 85 | 86 | @staticmethod 87 | def rotate_vector(q, vector): 88 | """ 89 | :author basti-schr 90 | :param q: a Quaternion 91 | :param vector: a Point or vector to be rotated 92 | :return: the rotated vector or Point 93 | """ 94 | if not isinstance(q, Quaternion): 95 | if len(q) != 4: 96 | raise TypeError("q have to be a Quatenion") 97 | else: 98 | q = Quaternion(q) 99 | 100 | p = np.hstack(([0], vector)) 101 | r = Quaternion(p) 102 | rotation = (q * r) * q.conj() 103 | 104 | return rotation[1:] 105 | 106 | def to_euler_angles(self): 107 | pitch = np.arcsin(2 * self[1] * self[2] + 2 * self[0] * self[3]) 108 | if np.abs(self[1] * self[2] + self[3] * self[0] - 0.5) < 1e-8: 109 | roll = 0 110 | yaw = 2 * np.arctan2(self[1], self[0]) 111 | elif np.abs(self[1] * self[2] + self[3] * self[0] + 0.5) < 1e-8: 112 | roll = -2 * np.arctan2(self[1], self[0]) 113 | yaw = 0 114 | else: 115 | roll = np.arctan2(2 * self[0] * self[1] - 2 * self[2] * self[3], 1 - 2 * self[1] ** 2 - 2 * self[3] ** 2) 116 | yaw = np.arctan2(2 * self[0] * self[2] - 2 * self[1] * self[3], 1 - 2 * self[2] ** 2 - 2 * self[3] ** 2) 117 | return roll, pitch, yaw 118 | 119 | def to_euler123(self): 120 | roll = np.arctan2(-2 * (self[2] * self[3] - self[0] * self[1]), 121 | self[0] ** 2 - self[1] ** 2 - self[2] ** 2 + self[3] ** 2) 122 | pitch = np.arcsin(2 * (self[1] * self[3] + self[0] * self[1])) 123 | yaw = np.arctan2(-2 * (self[1] * self[2] - self[0] * self[3]), 124 | self[0] ** 2 + self[1] ** 2 - self[2] ** 2 - self[3] ** 2) 125 | return roll, pitch, yaw 126 | 127 | def to_rotation_matrix(self): 128 | """ 129 | Converts a quaternion orientation to a rotation matrix 130 | For more information see: https://www.x-io.co.uk/node/8#quaternions 131 | :return: the rotation matrix of the quaternion 132 | """ 133 | q = self._q 134 | r = np.array([[0, 0, 0], 135 | [0, 0, 0], 136 | [0, 0, 0]]) 137 | 138 | r[0, 0] = (2 * q[0] ** 2) - 1 + (2 * q[1] ** 2) 139 | r[0, 1] = 2 * (q[1] * q[2] + q[0] * q[3]) 140 | r[0, 2] = 2 * (q[1] * q[3] - q[0] * q[2]) 141 | r[1, 0] = 2 * (q[1] * q[2] - q[0] * q[3]) 142 | r[1, 1] = (2 * q[0] ** 2) - 1 + (2 * q[2] ** 2) 143 | r[1, 2] = (q[2] * q[3] + q[0] * q[4]) 144 | r[2, 0] = (q[1] * q[3] + q[0] * q[2]) 145 | r[2, 1] = (q[2] * q[3] - q[0] * q[1]) 146 | r[2, 2] = (2 * q[0] ** 2) - 1 + (2 * q[3] ** 2) 147 | 148 | return r 149 | 150 | def __mul__(self, other): 151 | """ 152 | multiply the given quaternion with another quaternion or a scalar 153 | :param other: a Quaternion object or a number 154 | :return: 155 | """ 156 | if isinstance(other, Quaternion): 157 | w = self._q[0] * other._q[0] - self._q[1] * other._q[1] - self._q[2] * other._q[2] - self._q[3] * other._q[ 158 | 3] 159 | x = self._q[0] * other._q[1] + self._q[1] * other._q[0] + self._q[2] * other._q[3] - self._q[3] * other._q[ 160 | 2] 161 | y = self._q[0] * other._q[2] - self._q[1] * other._q[3] + self._q[2] * other._q[0] + self._q[3] * other._q[ 162 | 1] 163 | z = self._q[0] * other._q[3] + self._q[1] * other._q[2] - self._q[2] * other._q[1] + self._q[3] * other._q[ 164 | 0] 165 | 166 | return Quaternion(w, x, y, z) 167 | elif isinstance(other, numbers.Number): 168 | q = self._q * other 169 | return Quaternion(q) 170 | 171 | def __add__(self, other): 172 | """ 173 | add two quaternions element-wise or add a scalar to each element of the quaternion 174 | :param other: 175 | :return: 176 | """ 177 | if not isinstance(other, Quaternion): 178 | if len(other) != 4: 179 | raise TypeError("Quaternions must be added to other quaternions or a 4-element array") 180 | q = self.q + other 181 | else: 182 | q = self.q + other.q 183 | 184 | return Quaternion(q) 185 | 186 | def size(self): 187 | """ 188 | :author: basti-schr 189 | :return: the length of the quaternion 190 | """ 191 | return np.sqrt(self.q[0] ** 2 + self.q[1] ** 2 + self.q[2] ** 2 + self.q[3] ** 2) 192 | 193 | def norm(self): 194 | """ 195 | :author: basti-schr 196 | :return: the normalized quaternion 197 | """ 198 | return self._q / self.size() 199 | 200 | # Implementing other interfaces to ease working with the class 201 | 202 | def _set_q(self, q): 203 | self._q = q 204 | 205 | def _get_q(self): 206 | return self._q 207 | 208 | q = property(_get_q, _set_q) 209 | 210 | def __getitem__(self, item): 211 | return self._q[item] 212 | 213 | def __array__(self): 214 | return self._q 215 | 216 | def __str__(self): 217 | """ 218 | :author: basti-schr 219 | """ 220 | 221 | return str(self.__array__().tolist()) 222 | --------------------------------------------------------------------------------