├── .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 |
--------------------------------------------------------------------------------