├── tests ├── __init__.py └── test_calib.py ├── requirements.txt ├── setup.cfg ├── .coveragerc ├── .travis.yml ├── LICENSE ├── setup.py ├── .gitignore ├── README.rst └── calibraxis.py /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy>=1.9.0 2 | six>=1.9.0 3 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [bdist_wheel] 2 | universal=1 3 | 4 | [aliases] 5 | test=pytest 6 | -------------------------------------------------------------------------------- /.coveragerc: -------------------------------------------------------------------------------- 1 | [run] 2 | branch = True 3 | source = calibraxis 4 | include = */calibraxis/* 5 | omit = 6 | */setup.py 7 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | sudo: false 3 | python: 4 | - 2.6 5 | - 2.7 6 | - 3.3 7 | - 3.4 8 | - 3.5 9 | branches: 10 | only: 11 | - master 12 | - develop 13 | install: 14 | install: 15 | - "pip install pytest" 16 | - "pip install pytest-cov" 17 | - "pip install python-coveralls" 18 | - "pip install -e ." 19 | script: py.test tests/ --cov calibraxis --cov-report term-missing 20 | after_success: 21 | coveralls 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Henrik Blidh 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | ''' 4 | The setup script for the calibraxis package. 5 | 6 | .. moduleauthor:: hbldh 7 | 8 | Created on 2016-04-08 9 | 10 | ''' 11 | 12 | from __future__ import division 13 | from __future__ import print_function 14 | from __future__ import absolute_import 15 | 16 | import os 17 | import sys 18 | import re 19 | from codecs import open 20 | 21 | from setuptools import setup 22 | 23 | 24 | if sys.argv[-1] == 'publish': 25 | os.system('python setup.py register') 26 | os.system('python setup.py sdist upload') 27 | os.system('python setup.py bdist_wheel upload') 28 | sys.exit() 29 | 30 | 31 | with open('calibraxis.py', 'r') as fd: 32 | version = re.search(r'^__version__\s*=\s*[\'"]([^\'"]*)[\'"]', 33 | fd.read(), re.MULTILINE).group(1) 34 | 35 | 36 | def read(f): 37 | return open(f, encoding='utf-8').read() 38 | 39 | 40 | setup( 41 | name='calibraxis', 42 | version=version, 43 | author='Henrik Blidh', 44 | author_email='henrik.blidh@nedobmkull.com', 45 | url='https://github.com/hbldh/calibraxis', 46 | description='Autocalibration method for accelerometers, implemented in Python.', 47 | long_description=read('README.rst'), 48 | license='MIT', 49 | keywords=['Calibration', 'Accelerometers'], 50 | classifiers=[ 51 | 'Development Status :: 4 - Beta', 52 | 'Operating System :: OS Independent', 53 | 'Intended Audience :: Science/Research', 54 | 'Intended Audience :: Developers', 55 | 'License :: OSI Approved :: MIT License', 56 | 'Topic :: Software Development', 57 | 'Topic :: Scientific/Engineering', 58 | 'Programming Language :: Python :: 2', 59 | 'Programming Language :: Python :: 2.6', 60 | 'Programming Language :: Python :: 2.7', 61 | 'Programming Language :: Python :: 3', 62 | 'Programming Language :: Python :: 3.3', 63 | 'Programming Language :: Python :: 3.4', 64 | 'Programming Language :: Python :: 3.5', 65 | ], 66 | py_modules=['calibraxis'], 67 | test_suite="tests", 68 | zip_safe=False, 69 | include_package_data=True, 70 | install_requires=[ 71 | 'numpy>=1.9.0', 72 | 'six>=1.9.0' 73 | ], 74 | setup_requires=['pytest-runner', ], 75 | tests_require=['pytest', ], 76 | ext_modules=[], 77 | entry_points={} 78 | ) 79 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by .ignore support plugin (hsz.mobi) 2 | 3 | ### Python template 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | env/ 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | downloads/ 19 | eggs/ 20 | .eggs/ 21 | lib/ 22 | lib64/ 23 | parts/ 24 | sdist/ 25 | var/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *,cover 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | 57 | # Sphinx documentation 58 | docs/_build/ 59 | 60 | # PyBuilder 61 | target/ 62 | 63 | 64 | ### JetBrains template 65 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion 66 | 67 | *.iml 68 | 69 | ## Directory-based project format: 70 | .idea/ 71 | # if you remove the above rule, at least ignore the following: 72 | 73 | # User-specific stuff: 74 | # .idea/workspace.xml 75 | # .idea/tasks.xml 76 | # .idea/dictionaries 77 | 78 | # Sensitive or high-churn files: 79 | # .idea/dataSources.ids 80 | # .idea/dataSources.xml 81 | # .idea/sqlDataSources.xml 82 | # .idea/dynamic.xml 83 | # .idea/uiDesigner.xml 84 | 85 | # Gradle: 86 | # .idea/gradle.xml 87 | # .idea/libraries 88 | 89 | # Mongo Explorer plugin: 90 | # .idea/mongoSettings.xml 91 | 92 | ## File-based project format: 93 | *.ipr 94 | *.iws 95 | 96 | ## Plugin-specific files: 97 | 98 | # IntelliJ 99 | /out/ 100 | 101 | # mpeltonen/sbt-idea plugin 102 | .idea_modules/ 103 | 104 | # JIRA plugin 105 | atlassian-ide-plugin.xml 106 | 107 | # Crashlytics plugin (for Android Studio and IntelliJ) 108 | com_crashlytics_export_strings.xml 109 | crashlytics.properties 110 | crashlytics-build.properties 111 | 112 | 113 | ### SublimeText template 114 | # cache files for sublime text 115 | *.tmlanguage.cache 116 | *.tmPreferences.cache 117 | *.stTheme.cache 118 | 119 | # workspace files are user-specific 120 | *.sublime-workspace 121 | 122 | # project files should be checked into the repository, unless a significant 123 | # proportion of contributors will probably not be using SublimeText 124 | # *.sublime-project 125 | 126 | # sftp configuration file 127 | sftp-config.json 128 | 129 | 130 | ### Linux template 131 | *~ 132 | 133 | # KDE directory preferences 134 | .directory 135 | 136 | # Linux trash folder which might appear on any partition or disk 137 | .Trash-* 138 | 139 | 140 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | Calibraxis 2 | ========== 3 | 4 | .. image:: https://travis-ci.org/hbldh/calibraxis.svg?branch=master 5 | :target: https://travis-ci.org/hbldh/calibraxis 6 | .. image:: http://img.shields.io/pypi/v/calibraxis.svg 7 | :target: https://pypi.python.org/pypi/calibraxis/ 8 | .. image:: http://img.shields.io/pypi/l/calibraxis.svg 9 | :target: https://pypi.python.org/pypi/calibraxis/ 10 | .. image:: https://coveralls.io/repos/github/hbldh/calibraxis/badge.svg?branch=master 11 | :target: https://coveralls.io/github/hbldh/calibraxis?branch=master 12 | 13 | An Python/NumPy implementation of the accelerometer calibration method 14 | described in [#FRO2009]_. This is a Python reimplementation of the 15 | Matlab routine found at [#MLCENTRAL]_. 16 | 17 | Installation 18 | ------------ 19 | 20 | .. code:: bash 21 | 22 | $ pip install calibraxis 23 | 24 | 25 | Usage 26 | ----- 27 | 28 | .. code-block:: python 29 | 30 | import numpy as np 31 | from calibraxis import Calibraxis 32 | 33 | c = Calibraxis() 34 | points = np.array([[-4772.38754098, 154.04459016, -204.39081967], 35 | [3525.0346179, -68.64924886, -34.54604833], 36 | [-658.17681729, -4137.60248854, -140.49377865], 37 | [-564.18562092, 4200.29150327, -130.51895425], 38 | [-543.18289474, 18.14736842, -4184.43026316], 39 | [-696.62532808, 15.70209974, 3910.20734908], 40 | [406.65271419, 18.46827992, -4064.61085677], 41 | [559.45926413, -3989.69513798, -174.71879106], 42 | [597.22629169, -3655.54153041, -1662.83257031], 43 | [1519.02616089, -603.82472204, 3290.58469588]]) 44 | # Add points to calibration object's storage. 45 | c.add_points(points) 46 | # Run the calibration parameter optimization. 47 | c.calibrate_accelerometer() 48 | 49 | # Applying the calibration parameters to the calibration data. 50 | c.apply(points[0 :]) 51 | >>> (-0.9998374717802275, 0.018413117166568103, -0.015581921828828033) 52 | c.batch_apply(points) 53 | >>> [(-0.9998374717802275, 0.018413117166568103, -0.015581921828828033), 54 | (0.9992961622260429, -0.013214366898928225, 0.02485664909901566), 55 | (-0.019529368790511807, -0.9999036558762957, -0.0016168646941819831), 56 | (0.02495705262007455, 0.9997148237911497, 0.002962712686085044), 57 | (0.01976766176204912, -0.004116860997835083, -0.9989226575863294), 58 | (-0.01861952448274546, -0.0030340053509653056, 0.9994716286085392), 59 | (0.2486658848595297, -0.0015217968569550546, -0.9695063568748282), 60 | (0.2743240898265507, -0.9612564659612206, -0.01023892300189375), 61 | (0.2845586995260631, -0.8814105592109305, -0.37753891563574526), 62 | (0.5138552246439876, -0.14594841230046982, 0.8459602354269684)] 63 | 64 | Testing 65 | ------- 66 | 67 | Run tests with: 68 | 69 | .. code:: bash 70 | 71 | $ python setup.py test 72 | 73 | or with `Pytest `_: 74 | 75 | .. code:: bash 76 | 77 | $ py.test tests.py 78 | 79 | Documentation 80 | ------------- 81 | 82 | TBW. 83 | 84 | References 85 | ---------- 86 | 87 | .. [#FRO2009] `Frosio, I.; Pedersini, F.; Alberto Borghese, N., 88 | "Autocalibration of MEMS Accelerometers," Instrumentation and Measurement, 89 | IEEE Transactions on, vol.58, no.6, pp.2034,2041, June 2009 90 | `_ 91 | 92 | .. [#MLCENTRAL] `Matlab File Central `_. 94 | -------------------------------------------------------------------------------- /tests/test_calib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | :mod:`test_standard` 5 | ================== 6 | 7 | .. module:: test_standard 8 | :platform: Unix, Windows 9 | :synopsis: 10 | 11 | .. moduleauthor:: hbldh 12 | 13 | Created on 2015-07-04, 12:00 14 | 15 | """ 16 | 17 | from __future__ import division 18 | from __future__ import print_function 19 | from __future__ import unicode_literals 20 | from __future__ import absolute_import 21 | 22 | import re 23 | 24 | import pytest 25 | import numpy as np 26 | 27 | from calibraxis import Calibraxis 28 | 29 | 30 | @pytest.fixture(scope='module') 31 | def points_1(): 32 | # Measuring range +/-8 33 | return np.array([[-4772.38754098, 154.04459016, -204.39081967], 34 | [3525.0346179, -68.64924886, -34.54604833], 35 | [-658.17681729, -4137.60248854, -140.49377865], 36 | [-564.18562092, 4200.29150327, -130.51895425], 37 | [-543.18289474, 18.14736842, -4184.43026316], 38 | [-696.62532808, 15.70209974, 3910.20734908], 39 | [406.65271419, 18.46827992, -4064.61085677], 40 | [559.45926413, -3989.69513798, -174.71879106], 41 | [597.22629169, -3655.54153041, -1662.83257031], 42 | [1519.02616089, -603.82472204, 3290.58469588]]) 43 | 44 | 45 | @pytest.fixture(scope='module') 46 | def points_2(): 47 | return np.array([[-1575.43324607, 58.07787958, -72.69371728], 48 | [1189.53102547, -11.92749837, -23.37687786], 49 | [-212.62989556, -1369.82898172, -48.73498695], 50 | [-183.42717178, 1408.61463096, -33.89745265], 51 | [-162.57253886, 23.43005181, -1394.36722798], 52 | [-216.76963011, 19.37118754, 1300.13822193], 53 | [-809.20208605, 69.1029987, -1251.60104302], 54 | [-1244.03955901, -866.0843061, -67.02594034], 55 | [-1032.3692107, 811.19178082, 699.69602087], 56 | [-538.82617188, -161.6171875, -1337.34895833]]) 57 | 58 | 59 | def test_calibration_points_1(points_1): 60 | c = Calibraxis(verbose=False) 61 | c.add_points(points_1) 62 | c.calibrate_accelerometer() 63 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 64 | 65 | 66 | def test_calibration_points_1_scaled(points_1): 67 | c = Calibraxis(verbose=False) 68 | c.add_points(points_1 / ((2 ** 15) / 8.)) 69 | c.calibrate_accelerometer() 70 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 71 | 72 | 73 | def test_calibration_points_2(points_2): 74 | c = Calibraxis(verbose=False) 75 | c.add_points(points_2) 76 | c.calibrate_accelerometer() 77 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 78 | 79 | 80 | def test_calibration_points_2_scaled(points_2): 81 | c = Calibraxis(verbose=False) 82 | c.add_points(points_2 / ((2 ** 15) / 16.)) 83 | c.calibrate_accelerometer() 84 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 85 | 86 | 87 | def test_recalibration_points_2(points_2): 88 | c = Calibraxis(verbose=False) 89 | points = points_2 / ((2 ** 15) / 16.) 90 | for p in points[:-1, :]: 91 | c.add_points(p) 92 | c.calibrate_accelerometer() 93 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 94 | c.add_points(points[-1, :]) 95 | c.calibrate_accelerometer() 96 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 97 | 98 | 99 | def test_add_points_1(points_1): 100 | c = Calibraxis(verbose=False) 101 | points = points_1 / ((2 ** 15) / 8.) 102 | for p in points: 103 | c.add_points(p) 104 | np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6) 105 | c.calibrate_accelerometer() 106 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 107 | 108 | 109 | def test_add_points_2(points_1): 110 | c = Calibraxis(verbose=False) 111 | points = points_1 / ((2 ** 15) / 8.) 112 | for p in points: 113 | c.add_points(list(p)) 114 | np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6) 115 | c.calibrate_accelerometer() 116 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 117 | 118 | 119 | def test_add_points_3(points_1): 120 | c = Calibraxis(verbose=False) 121 | points = points_1 / ((2 ** 15) / 8.) 122 | for p in points: 123 | c.add_points(tuple(p)) 124 | np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6) 125 | c.calibrate_accelerometer() 126 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 127 | 128 | 129 | def test_add_points_4(points_2): 130 | c = Calibraxis(verbose=False) 131 | points = points_2 / ((2 ** 15) / 8.) 132 | c.add_points(points.tolist()) 133 | np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6) 134 | c.calibrate_accelerometer() 135 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 136 | 137 | 138 | def test_add_points_5(points_2): 139 | c = Calibraxis(verbose=False) 140 | points = points_2 / ((2 ** 15) / 8.) 141 | c.add_points(points) 142 | c.add_points([]) 143 | np.testing.assert_almost_equal(np.linalg.norm(np.array(c._calibration_points) - points), 0.0, 6) 144 | c.calibrate_accelerometer() 145 | np.testing.assert_almost_equal(c._calibration_errors[-1], 0.0, 2) 146 | 147 | 148 | def test_apply(points_1): 149 | c = Calibraxis(verbose=False) 150 | c.add_points(points_1) 151 | c.calibrate_accelerometer() 152 | np.testing.assert_almost_equal(np.linalg.norm(c.apply(points_1[0, :])), 1.0, 2) 153 | 154 | 155 | def test_batch_apply(points_1): 156 | c = Calibraxis(verbose=False) 157 | c.add_points(points_1) 158 | c.calibrate_accelerometer() 159 | out = c.batch_apply(points_1) 160 | normed = np.sqrt((np.array(out) ** 2).sum(axis=1)) 161 | np.testing.assert_array_almost_equal(normed, 1.0, 2) 162 | 163 | 164 | def test_error_to_few_points(points_2): 165 | c = Calibraxis(verbose=False) 166 | for p in points_2[:5, :]: 167 | c.add_points(p) 168 | with pytest.raises(ValueError): 169 | c.calibrate_accelerometer() 170 | 171 | 172 | def test_verbose_prints_progress(points_2, capsys): 173 | c = Calibraxis(verbose=True) 174 | c.add_points(points_2) 175 | c.calibrate_accelerometer() 176 | out, err = capsys.readouterr() 177 | for row in filter(None, out.split('\n')): 178 | assert re.match('^([0-9]+):\s([0-9\-\.e]+)\s*(\([0-9\s\-\.e,]+\))$', row) 179 | -------------------------------------------------------------------------------- /calibraxis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | :mod:`calibraxis` 5 | ================= 6 | 7 | Created by hbldh 8 | Created on 2016-04-25 9 | 10 | Copyright (c) 2016, Nedomkull Mathematical Modeling AB. 11 | 12 | """ 13 | 14 | from __future__ import division 15 | from __future__ import print_function 16 | from __future__ import unicode_literals 17 | from __future__ import absolute_import 18 | 19 | import copy 20 | 21 | import six 22 | import numpy as np 23 | 24 | # Version information. 25 | __version__ = '0.2.0' 26 | version = __version__ # backwards compatibility name 27 | version_info = (0, 2, 0) 28 | 29 | 30 | class Calibraxis(object): 31 | """Calibration object, used for storing and applying the 32 | calibration parameters. 33 | 34 | Computes the Zero G levels, Sensitivity, Scale factor Matrix and the 35 | bias vector of a MEMS accelerometer. 36 | 37 | The procedure exploits the fact that, in static conditions, the 38 | modulus of the accelerometer output vector matches that of the 39 | gravity acceleration. The calibration model incorporates the bias 40 | and scale factor for each axis and the cross-axis symmetrical 41 | factors. The parameters are computed through Gauss-Newton 42 | nonlinear optimization. 43 | 44 | The mathematical model used is :math:`a = M(r - b)` 45 | where :math:`M` and :math:`b` are scale factor matrix and 46 | bias vector respectively and :math:`r` is the raw accelerometer 47 | reading. 48 | 49 | .. math:: 50 | 51 | M = \begin{matrix} 52 | M_{xx} & M_{xy} & M_{xz} \\ 53 | M_{yx} & M_{yy} & M_{yz} \\ 54 | M_{zx} & M_{zy} & M_{zz} 55 | \end{matrix} 56 | 57 | b = \begin{matrix} 58 | b_{x} \\ 59 | b_{y} \\ 60 | b_{z} 61 | \end{matrix} 62 | 63 | where 64 | 65 | .. math:: 66 | 67 | M_{xy} = M_{yx} 68 | 69 | M_{xz} = M_{zx} 70 | 71 | M_{yz} = M_{zy}. 72 | 73 | The diagonal elements of M represent the scale factors along the 74 | three axes, whereas the other elements of M are called cross-axis 75 | factors. These terms allow describing both the axes’ misalignment 76 | and the crosstalk effect between different channels caused 77 | by the sensor electronics. In an ideal world, :math:`M = I` and 78 | :math:`B = \mathbb{0}`. 79 | 80 | Reference: 81 | Iuri Frosio, Federico Pedersini, N. Alberto Borghese 82 | "Autocalibration of MEMS Accelerometers" 83 | IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, VOL. 58, NO. 6, JUNE 2009 84 | 85 | This is a Python reimplementation of the Matlab routines found at 86 | `Matlab File Central `_. 88 | 89 | :param bool verbose: Print optimization progress data. 90 | 91 | """ 92 | 93 | def __init__(self, verbose=False): 94 | 95 | self._points = [] 96 | self._verbose = verbose 97 | 98 | # Accelerometer calibration parameters. 99 | self._calibration_points = [] 100 | self._calibration_errors = None 101 | 102 | self.bias_vector = None 103 | self.scale_factor_matrix = None 104 | 105 | def add_points(self, points): 106 | """Add point(s) to the calibration procedure. 107 | 108 | :param list, tuple, numpy.ndarray points: The point(s) to add to the 109 | calibration point storage. 110 | 111 | """ 112 | if isinstance(points, (list, tuple)): 113 | if len(points) > 0: 114 | if isinstance(points[0], (list, tuple, np.ndarray)): 115 | # Multiple points sent as list of lists. 116 | for p in points: 117 | self._calibration_points.append(copy.deepcopy(p)) 118 | else: 119 | # Assume single point sent in as list/tuple/array. 120 | self._calibration_points.append(copy.deepcopy(points)) 121 | else: 122 | # Empty list/tuple. Skip. 123 | pass 124 | elif isinstance(points, np.ndarray): 125 | if points.ndim > 1: 126 | for p in points: 127 | self._calibration_points.append(p.copy()) 128 | elif points.ndim == 1: 129 | self._calibration_points.append(points.copy()) 130 | 131 | def calibrate_accelerometer(self): 132 | """Perform the calibration of accelerometer using the 133 | stored points. 134 | """ 135 | points = np.array(self._calibration_points) 136 | self._perform_accelerometer_calibration_optimisation(points) 137 | 138 | def _perform_accelerometer_calibration_optimisation(self, points): 139 | """Perform the Gauss-Newton optimisation for parameters.""" 140 | nbr_points = len(points) 141 | if nbr_points < 9: 142 | raise ValueError( 143 | 'Need at least 9 measurements for the calibration procedure!') 144 | 145 | def error_function(M_mat, b_vec, y): 146 | """Optimisation error function for a point. 147 | 148 | :param numpy.ndarray M_mat: The scale factor matrix 149 | of this iteration. 150 | :param numpy.ndarray b_vec: The zero-g offset vector 151 | of this iteration. 152 | :param numpy.ndarray y: The point ot estimate error for. 153 | :return: The square sum of the error of this point. 154 | :rtype: float 155 | 156 | """ 157 | return float(np.sum((M_mat.dot((y - b_vec)) ** 2)) - 1) 158 | 159 | def calculate_jacobian(M_mat, b_vec, point): 160 | """Calculate the Jacobian for a point. 161 | 162 | :param numpy.ndarray M_mat: The scale factor matrix 163 | of this iteration. 164 | :param numpy.ndarray b_vec: The zero-g offset vector 165 | of this iteration. 166 | :param numpy.ndarray y: The point ot estimate error for. 167 | :return: The square sum of the error of this point. 168 | :rtype: float 169 | 170 | """ 171 | jac = np.zeros((9,), 'float') 172 | 173 | jac[0] = 2 * (b_vec[0] - point[0]) * ( 174 | M_mat[0, 0] * (b_vec[0] - point[0]) + M_mat[0, 1] * ( 175 | b_vec[1] - point[1]) + M_mat[0, 2] * ( 176 | b_vec[2] - point[2])) 177 | jac[1] = 2 * (b_vec[1] - point[1]) * ( 178 | M_mat[0, 0] * (b_vec[0] - point[0]) + M_mat[0, 1] * ( 179 | b_vec[1] - point[1]) + M_mat[0, 2] * ( 180 | b_vec[2] - point[2])) + 2 * (b_vec[0] - point[0]) * ( 181 | M_mat[0, 1] * (b_vec[0] - point[0]) + M_mat[1, 1] * ( 182 | b_vec[1] - point[1]) + M_mat[1, 2] * ( 183 | b_vec[2] - point[2])) 184 | jac[2] = 2 * (b_vec[0] - point[0]) * ( 185 | M_mat[0, 2] * (b_vec[0] - point[0]) + M_mat[1, 2] * ( 186 | b_vec[1] - point[1]) + M_mat[2, 2] * ( 187 | b_vec[2] - point[2])) + 2 * (b_vec[2] - point[2]) * ( 188 | M_mat[0, 0] * (b_vec[0] - point[0]) + M_mat[0, 1] * ( 189 | b_vec[1] - point[1]) + M_mat[0, 2] * ( 190 | b_vec[2] - point[2])) 191 | jac[3] = 2 * (b_vec[1] - point[1]) * ( 192 | M_mat[0, 1] * (b_vec[0] - point[0]) + M_mat[1, 1] * ( 193 | b_vec[1] - point[1]) + M_mat[1, 2] * ( 194 | b_vec[2] - point[2])) 195 | jac[4] = 2 * (b_vec[1] - point[1]) * ( 196 | M_mat[0, 2] * (b_vec[0] - point[0]) + M_mat[1, 2] * ( 197 | b_vec[1] - point[1]) + M_mat[2, 2] * ( 198 | b_vec[2] - point[2])) + 2 * (b_vec[2] - point[2]) * ( 199 | M_mat[0, 1] * (b_vec[0] - point[0]) + M_mat[1, 1] * ( 200 | b_vec[1] - point[1]) + M_mat[1, 2] * ( 201 | b_vec[2] - point[2])) 202 | jac[5] = 2 * (b_vec[2] - point[2]) * ( 203 | M_mat[0, 2] * (b_vec[0] - point[0]) + M_mat[1, 2] * ( 204 | b_vec[1] - point[1]) + M_mat[2, 2] * ( 205 | b_vec[2] - point[2])) 206 | jac[6] = 2 * M_mat[0, 0] * ( 207 | M_mat[0, 0] * (b_vec[0] - point[0]) + M_mat[0, 1] * ( 208 | b_vec[1] - point[1]) + M_mat[0, 2] * ( 209 | b_vec[2] - point[2])) + 2 * M_mat[0, 1] * ( 210 | M_mat[0, 1] * (b_vec[0] - point[0]) + M_mat[1, 1] * ( 211 | b_vec[1] - point[1]) + M_mat[1, 2] * ( 212 | b_vec[2] - point[2])) + 2 * M_mat[0, 2] * ( 213 | M_mat[0, 2] * (b_vec[0] - point[0]) + M_mat[1, 2] * ( 214 | b_vec[1] - point[1]) + M_mat[2, 2] * ( 215 | b_vec[2] - point[2])) 216 | jac[7] = 2 * M_mat[0, 1] * ( 217 | M_mat[0, 0] * (b_vec[0] - point[0]) + M_mat[0, 1] * ( 218 | b_vec[1] - point[1]) + M_mat[0, 2] * ( 219 | b_vec[2] - point[2])) + 2 * M_mat[1, 1] * ( 220 | M_mat[0, 1] * (b_vec[0] - point[0]) + M_mat[1, 1] * ( 221 | b_vec[1] - point[1]) + M_mat[1, 2] * ( 222 | b_vec[2] - point[2])) + 2 * M_mat[1, 2] * ( 223 | M_mat[0, 2] * (b_vec[0] - point[0]) + M_mat[1, 2] * ( 224 | b_vec[1] - point[1]) + M_mat[2, 2] * ( 225 | b_vec[2] - point[2])) 226 | jac[8] = 2 * M_mat[0, 2] * ( 227 | M_mat[0, 0] * (b_vec[0] - point[0]) + M_mat[0, 1] * ( 228 | b_vec[1] - point[1]) + M_mat[0, 2] * ( 229 | b_vec[2] - point[2])) + 2 * M_mat[1, 2] * ( 230 | M_mat[0, 1] * (b_vec[0] - point[0]) + M_mat[1, 1] * ( 231 | b_vec[1] - point[1]) + M_mat[1, 2] * ( 232 | b_vec[2] - point[2])) + 2 * M_mat[2, 2] * ( 233 | M_mat[0, 2] * (b_vec[0] - point[0]) + M_mat[1, 2] * ( 234 | b_vec[1] - point[1]) + M_mat[2, 2] * ( 235 | b_vec[2] - point[2])) 236 | 237 | return jac 238 | 239 | def optvec_to_M_and_b(v): 240 | """ 241 | Convenience method for moving between optimisation 242 | vector and correct lin.alg. formulation. 243 | """ 244 | return (np.array([[v[0], v[1], v[2]], 245 | [v[1], v[3], v[4]], 246 | [v[2], v[4], v[5]]]), 247 | v[6:].copy()) 248 | 249 | gain = 1 # Damping Gain - Start with 1 250 | damping = 0.01 # Damping parameter - has to be less than 1. 251 | tolerance = 1e-12 252 | R_prior = 100000 253 | self._calibration_errors = [] 254 | nbr_iterations = 200 255 | 256 | # Initial Guess values of M and b. 257 | if self.bias_vector is not None: 258 | # Recalibration using prior optimization results. 259 | x = np.array([self.scale_factor_matrix[0, 0], 260 | self.scale_factor_matrix[0, 1], 261 | self.scale_factor_matrix[0, 2], 262 | self.scale_factor_matrix[1, 1], 263 | self.scale_factor_matrix[1, 2], 264 | self.scale_factor_matrix[2, 2], 265 | self.bias_vector[0], 266 | self.bias_vector[1], 267 | self.bias_vector[2]]) 268 | else: 269 | # Fresh calibration. 270 | sensitivity = 1 / np.sqrt((points ** 2).sum(axis=1)).mean() 271 | x = np.array([sensitivity, 0.0, 0.0, 272 | sensitivity, 0.0, sensitivity, 273 | 0.0, 0.0, 0.0]) 274 | last_x = x.copy() 275 | 276 | # Residuals vector 277 | R = np.zeros((nbr_points, ), 'float') 278 | 279 | # Jacobian matrix 280 | J = np.zeros((nbr_points, 9), 'float') 281 | 282 | for n in six.moves.range(nbr_iterations): 283 | # Calculate the Jacobian and error for each point. 284 | M, b = optvec_to_M_and_b(x) 285 | for i in six.moves.range(nbr_points): 286 | R[i] = error_function(M, b, points[i, :]) 287 | J[i, :] = calculate_jacobian(M, b, points[i, :]) 288 | 289 | # Calculate Hessian, Gain matrix and apply it to solution vector. 290 | H = np.linalg.inv(J.T.dot(J)) 291 | D = J.T.dot(R).T 292 | x -= gain * (D.dot(H)).T 293 | R_post = np.linalg.norm(R) 294 | if self._verbose: 295 | print("{0}: {1} ({2})".format( 296 | n, R_post, ", ".join(["{0:0.9g}".format(v) for v in x]))) 297 | 298 | # This is to make sure that the error is 299 | # decreasing with every iteration. 300 | if R_post <= R_prior: 301 | gain -= damping * gain 302 | else: 303 | gain *= damping 304 | 305 | # Iterations are stopped when the following 306 | # convergence criteria is satisfied. 307 | if abs(max(2 * (x - last_x) / (x + last_x))) <= tolerance: 308 | self.scale_factor_matrix, self.bias_vector = optvec_to_M_and_b(x) 309 | break 310 | 311 | last_x = x.copy() 312 | R_prior = R_post 313 | self._calibration_errors.append(R_post) 314 | 315 | def apply(self, acc_values): 316 | """Apply the calibration scale matrix and bias to accelerometer values. 317 | 318 | :param list, tuple, numpy.ndarray acc_values: The accelerometer data. 319 | :return: The transformed accelerometer values. 320 | :rtype: tuple 321 | 322 | """ 323 | converted_g_values = self.scale_factor_matrix.dot( 324 | np.array(acc_values) - self.bias_vector) 325 | return tuple(converted_g_values.tolist()) 326 | 327 | def batch_apply(self, acc_values): 328 | """Apply the calibration scale matrix and bias to an array of 329 | accelerometer data 330 | 331 | Assumes that the input is either a list or tuple containing three 332 | element lists, tuples or arrays or a [N x 3] NumPy array. 333 | 334 | :param list, tuple, numpy.ndarray acc_values: The accelerometer data. 335 | :return: The transformed accelerometer values. 336 | :rtype: list 337 | 338 | """ 339 | return [self.apply(a) for a in acc_values] 340 | --------------------------------------------------------------------------------