├── .gitignore ├── LICENSE ├── README.md ├── __init__.py ├── madgwickahrs.py └── quaternion.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | 59 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio 60 | 61 | *.iml 62 | 63 | ## Directory-based project format: 64 | .idea/ 65 | # if you remove the above rule, at least ignore the following: 66 | 67 | # User-specific stuff: 68 | # .idea/workspace.xml 69 | # .idea/tasks.xml 70 | # .idea/dictionaries 71 | 72 | # Sensitive or high-churn files: 73 | # .idea/dataSources.ids 74 | # .idea/dataSources.xml 75 | # .idea/sqlDataSources.xml 76 | # .idea/dynamic.xml 77 | # .idea/uiDesigner.xml 78 | 79 | # Gradle: 80 | # .idea/gradle.xml 81 | # .idea/libraries 82 | 83 | # Mongo Explorer plugin: 84 | # .idea/mongoSettings.xml 85 | 86 | ## File-based project format: 87 | *.ipr 88 | *.iws 89 | 90 | ## Plugin-specific files: 91 | 92 | # IntelliJ 93 | /out/ 94 | 95 | # mpeltonen/sbt-idea plugin 96 | .idea_modules/ 97 | 98 | # JIRA plugin 99 | atlassian-ide-plugin.xml 100 | 101 | # Crashlytics plugin (for Android Studio and IntelliJ) 102 | com_crashlytics_export_strings.xml 103 | crashlytics.properties 104 | crashlytics-build.properties 105 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # madgwick_py: A Python implementation of Madgwick's IMU and AHRS algorithm. 2 | 3 | More information on the algorithm can be found at 4 | . 5 | 6 | This implementation was done at the Cognitive Systems Lab (CSL) of the 7 | Karlsruhe Institute of Technology: 8 | 9 | # Requirements 10 | * Python 3.x (tested with Python 3.4) 11 | * NumPy 12 | 13 | # License 14 | 15 | Copyright (c) 2015-2022 Jonas Böer, webmaster@morgil.de 16 | 17 | This program is free software: you can redistribute it and/or modify 18 | it under the terms of the GNU Lesser General Public License as published by 19 | the Free Software Foundation, either version 3 of the License, or 20 | (at your option) any later version. 21 | 22 | This program is distributed in the hope that it will be useful, 23 | but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | GNU Lesser General Public License for more details. 26 | 27 | You should have received a copy of the GNU Lesser General Public License 28 | along with this program. If not, see . 29 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU Lesser General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public License 16 | along with this program. If not, see . 17 | """ 18 | 19 | __all__ = ["madgwickahrs", "quaternion"] 20 | -------------------------------------------------------------------------------- /madgwickahrs.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU Lesser General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public License 16 | along with this program. If not, see . 17 | """ 18 | 19 | import warnings 20 | import numpy as np 21 | from numpy.linalg import norm 22 | from .quaternion import Quaternion 23 | 24 | 25 | class MadgwickAHRS: 26 | samplePeriod = 1/256 27 | quaternion = Quaternion(1, 0, 0, 0) 28 | beta = 1 29 | zeta = 0 30 | 31 | def __init__(self, sampleperiod=None, quaternion=None, beta=None, zeta=None): 32 | """ 33 | Initialize the class with the given parameters. 34 | :param sampleperiod: The sample period 35 | :param quaternion: Initial quaternion 36 | :param beta: Algorithm gain beta 37 | :param beta: Algorithm gain zeta 38 | :return: 39 | """ 40 | if sampleperiod is not None: 41 | self.samplePeriod = sampleperiod 42 | if quaternion is not None: 43 | self.quaternion = quaternion 44 | if beta is not None: 45 | self.beta = beta 46 | if zeta is not None: 47 | self.zeta = zeta 48 | 49 | def update(self, gyroscope, accelerometer, magnetometer): 50 | """ 51 | Perform one update step with data from a AHRS sensor array 52 | :param gyroscope: A three-element array containing the gyroscope data in radians per second. 53 | :param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used. 54 | :param magnetometer: A three-element array containing the magnetometer data. Can be any unit since a normalized value is used. 55 | :return: 56 | """ 57 | q = self.quaternion 58 | 59 | gyroscope = np.array(gyroscope, dtype=float).flatten() 60 | accelerometer = np.array(accelerometer, dtype=float).flatten() 61 | magnetometer = np.array(magnetometer, dtype=float).flatten() 62 | 63 | # Normalise accelerometer measurement 64 | if norm(accelerometer) is 0: 65 | warnings.warn("accelerometer is zero") 66 | return 67 | accelerometer /= norm(accelerometer) 68 | 69 | # Normalise magnetometer measurement 70 | if norm(magnetometer) is 0: 71 | warnings.warn("magnetometer is zero") 72 | return 73 | magnetometer /= norm(magnetometer) 74 | 75 | h = q * (Quaternion(0, magnetometer[0], magnetometer[1], magnetometer[2]) * q.conj()) 76 | b = np.array([0, norm(h[1:3]), 0, h[3]]) 77 | 78 | # Gradient descent algorithm corrective step 79 | f = np.array([ 80 | 2*(q[1]*q[3] - q[0]*q[2]) - accelerometer[0], 81 | 2*(q[0]*q[1] + q[2]*q[3]) - accelerometer[1], 82 | 2*(0.5 - q[1]**2 - q[2]**2) - accelerometer[2], 83 | 2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - magnetometer[0], 84 | 2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - magnetometer[1], 85 | 2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - magnetometer[2] 86 | ]) 87 | j = np.array([ 88 | [-2*q[2], 2*q[3], -2*q[0], 2*q[1]], 89 | [2*q[1], 2*q[0], 2*q[3], 2*q[2]], 90 | [0, -4*q[1], -4*q[2], 0], 91 | [-2*b[3]*q[2], 2*b[3]*q[3], -4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]], 92 | [-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0], 2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]], 93 | [2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2], 2*b[1]*q[1]] 94 | ]) 95 | step = j.T.dot(f) 96 | step /= norm(step) # normalise step magnitude 97 | 98 | # Gyroscope compensation drift 99 | gyroscopeQuat = Quaternion(0, gyroscope[0], gyroscope[1], gyroscope[2]) 100 | stepQuat = Quaternion(step.T[0], step.T[1], step.T[2], step.T[3]) 101 | 102 | gyroscopeQuat = gyroscopeQuat + (q.conj() * stepQuat) * 2 * self.samplePeriod * self.zeta * -1 103 | 104 | # Compute rate of change of quaternion 105 | qdot = (q * gyroscopeQuat) * 0.5 - self.beta * step.T 106 | 107 | # Integrate to yield quaternion 108 | q += qdot * self.samplePeriod 109 | self.quaternion = Quaternion(q / norm(q)) # normalise quaternion 110 | 111 | def update_imu(self, gyroscope, accelerometer): 112 | """ 113 | Perform one update step with data from a IMU sensor array 114 | :param gyroscope: A three-element array containing the gyroscope data in radians per second. 115 | :param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used. 116 | """ 117 | q = self.quaternion 118 | 119 | gyroscope = np.array(gyroscope, dtype=float).flatten() 120 | accelerometer = np.array(accelerometer, dtype=float).flatten() 121 | 122 | # Normalise accelerometer measurement 123 | if norm(accelerometer) is 0: 124 | warnings.warn("accelerometer is zero") 125 | return 126 | accelerometer /= norm(accelerometer) 127 | 128 | # Gradient descent algorithm corrective step 129 | f = np.array([ 130 | 2*(q[1]*q[3] - q[0]*q[2]) - accelerometer[0], 131 | 2*(q[0]*q[1] + q[2]*q[3]) - accelerometer[1], 132 | 2*(0.5 - q[1]**2 - q[2]**2) - accelerometer[2] 133 | ]) 134 | j = np.array([ 135 | [-2*q[2], 2*q[3], -2*q[0], 2*q[1]], 136 | [2*q[1], 2*q[0], 2*q[3], 2*q[2]], 137 | [0, -4*q[1], -4*q[2], 0] 138 | ]) 139 | step = j.T.dot(f) 140 | step /= norm(step) # normalise step magnitude 141 | 142 | # Compute rate of change of quaternion 143 | qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1], gyroscope[2])) * 0.5 - self.beta * step.T 144 | 145 | # Integrate to yield quaternion 146 | q += qdot * self.samplePeriod 147 | self.quaternion = Quaternion(q / norm(q)) # normalise quaternion 148 | -------------------------------------------------------------------------------- /quaternion.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU Lesser General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public License 16 | along with this program. If not, see . 17 | """ 18 | 19 | import numpy as np 20 | import numbers 21 | 22 | 23 | class Quaternion: 24 | """ 25 | A simple class implementing basic quaternion arithmetic. 26 | """ 27 | def __init__(self, w_or_q, x=None, y=None, z=None): 28 | """ 29 | Initializes a Quaternion object 30 | :param w_or_q: A scalar representing the real part of the quaternion, another Quaternion object or a 31 | four-element array containing the quaternion values 32 | :param x: The first imaginary part if w_or_q is a scalar 33 | :param y: The second imaginary part if w_or_q is a scalar 34 | :param z: The third imaginary part if w_or_q is a scalar 35 | """ 36 | self._q = np.array([1, 0, 0, 0]) 37 | 38 | if x is not None and y is not None and z is not None: 39 | w = w_or_q 40 | q = np.array([w, x, y, z]) 41 | elif isinstance(w_or_q, Quaternion): 42 | q = np.array(w_or_q.q) 43 | else: 44 | q = np.array(w_or_q) 45 | if len(q) != 4: 46 | raise ValueError("Expecting a 4-element array or w x y z as parameters") 47 | 48 | self.q = q 49 | 50 | # Quaternion specific interfaces 51 | 52 | def conj(self): 53 | """ 54 | Returns the conjugate of the quaternion 55 | :rtype : Quaternion 56 | :return: the conjugate of the quaternion 57 | """ 58 | return Quaternion(self._q[0], -self._q[1], -self._q[2], -self._q[3]) 59 | 60 | def to_angle_axis(self): 61 | """ 62 | Returns the quaternion's rotation represented by an Euler angle and axis. 63 | If the quaternion is the identity quaternion (1, 0, 0, 0), a rotation along the x axis with angle 0 is returned. 64 | :return: rad, x, y, z 65 | """ 66 | if self[0] == 1 and self[1] == 0 and self[2] == 0 and self[3] == 0: 67 | return 0, 1, 0, 0 68 | rad = np.arccos(self[0]) * 2 69 | imaginary_factor = np.sin(rad / 2) 70 | if abs(imaginary_factor) < 1e-8: 71 | return 0, 1, 0, 0 72 | x = self._q[1] / imaginary_factor 73 | y = self._q[2] / imaginary_factor 74 | z = self._q[3] / imaginary_factor 75 | return rad, x, y, z 76 | 77 | @staticmethod 78 | def from_angle_axis(rad, x, y, z): 79 | s = np.sin(rad / 2) 80 | return Quaternion(np.cos(rad / 2), x*s, y*s, z*s) 81 | 82 | def to_euler_angles(self): 83 | pitch = np.arcsin(2 * self[1] * self[2] + 2 * self[0] * self[3]) 84 | if np.abs(self[1] * self[2] + self[3] * self[0] - 0.5) < 1e-8: 85 | roll = 0 86 | yaw = 2 * np.arctan2(self[1], self[0]) 87 | elif np.abs(self[1] * self[2] + self[3] * self[0] + 0.5) < 1e-8: 88 | roll = -2 * np.arctan2(self[1], self[0]) 89 | yaw = 0 90 | else: 91 | roll = np.arctan2(2 * self[0] * self[1] - 2 * self[2] * self[3], 1 - 2 * self[1] ** 2 - 2 * self[3] ** 2) 92 | yaw = np.arctan2(2 * self[0] * self[2] - 2 * self[1] * self[3], 1 - 2 * self[2] ** 2 - 2 * self[3] ** 2) 93 | return roll, pitch, yaw 94 | 95 | def to_euler123(self): 96 | roll = np.arctan2(-2 * (self[2] * self[3] - self[0] * self[1]), self[0] ** 2 - self[1] ** 2 - self[2] ** 2 + self[3] ** 2) 97 | pitch = np.arcsin(2 * (self[1] * self[3] + self[0] * self[1])) 98 | yaw = np.arctan2(-2 * (self[1] * self[2] - self[0] * self[3]), self[0] ** 2 + self[1] ** 2 - self[2] ** 2 - self[3] ** 2) 99 | return roll, pitch, yaw 100 | 101 | def __mul__(self, other): 102 | """ 103 | multiply the given quaternion with another quaternion or a scalar 104 | :param other: a Quaternion object or a number 105 | :return: 106 | """ 107 | if isinstance(other, Quaternion): 108 | w = self._q[0]*other._q[0] - self._q[1]*other._q[1] - self._q[2]*other._q[2] - self._q[3]*other._q[3] 109 | x = self._q[0]*other._q[1] + self._q[1]*other._q[0] + self._q[2]*other._q[3] - self._q[3]*other._q[2] 110 | y = self._q[0]*other._q[2] - self._q[1]*other._q[3] + self._q[2]*other._q[0] + self._q[3]*other._q[1] 111 | z = self._q[0]*other._q[3] + self._q[1]*other._q[2] - self._q[2]*other._q[1] + self._q[3]*other._q[0] 112 | 113 | return Quaternion(w, x, y, z) 114 | elif isinstance(other, numbers.Number): 115 | q = self._q * other 116 | return Quaternion(q) 117 | 118 | def __add__(self, other): 119 | """ 120 | add two quaternions element-wise or add a scalar to each element of the quaternion 121 | :param other: 122 | :return: 123 | """ 124 | if not isinstance(other, Quaternion): 125 | if len(other) != 4: 126 | raise TypeError("Quaternions must be added to other quaternions or a 4-element array") 127 | q = self._q + other 128 | else: 129 | q = self._q + other._q 130 | 131 | return Quaternion(q) 132 | 133 | # Implementing other interfaces to ease working with the class 134 | 135 | @property 136 | def q(self): 137 | return self._q 138 | 139 | @q.setter 140 | def q(self, q): 141 | self._q = q 142 | 143 | def __getitem__(self, item): 144 | return self._q[item] 145 | 146 | def __array__(self): 147 | return self._q 148 | --------------------------------------------------------------------------------