├── Example.ipynb ├── LICENSE ├── README.md ├── calibrate_real_imu.py ├── code ├── __init__.py ├── cost_functions.py ├── helpers.py ├── imu_model.py ├── monte_carlo.py ├── quaternion.py ├── simulator.py └── utilities.py ├── data ├── imu0.log ├── imu1.log ├── imu2.log ├── imu3.log └── imu4.log ├── images ├── gyro_calib.png ├── real_imu.png ├── simulations.png ├── synthesized_data.png └── uncalibrated_calibrated_acc.png └── run_monte_carlo.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Aleksandr Mikov 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. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # IMU-calib 2 | 3 | A novel calibration method for **gyroscopes** and accelerometers. Contrary to existing methods the 4 | proposed one **does not require a rotating table or other special equipment**. To perform the calibration a user needs to make 5 | a series of sequential rotations of inertial measurement unit 6 | (IMU) separated by standstill periods. 7 | 8 | This is the supplementary code and the implementation of the ideas, presented in [IEEE paper](#Citation) [[IEEE link]](https://ieeexplore.ieee.org/document/9133804). 9 | 10 | ## Overview 11 | The proposed approach allows to perform full IMU calibration: scale factors, non-orthogonalities and biases of accelerometer and gyroscope triads and misalignment between them. 12 | 13 | The proposed method is quite accurate: the differences between true and estimated sensor errors were less than 0.1% of their true value. 14 | 15 | The importance of full gyroscope calibration can be shown through orientation difference between true and gyroscope-integrated orientations. The three cases are shown on the plot: 16 | 1. No corrections of gyroscope raw data. 17 | 2. Only gyroscope bias was corrected. 18 | 3. Gyroscope scale, cross-coupling and bias were corrected. 19 | 20 | ![Gyro calibration importance](images/gyro_calib.png) 21 | 22 | After the calibration the orientation error is almost zero. 23 | 24 | 25 | ## Usage 26 | 27 | ### Simulation 28 | To run the monte-carlo simulations please issue the following command: 29 | ``` 30 | python3 run_monte_carlo.py --plot=True 31 | ``` 32 | As the output, you will see the true and estimated sensor error parameters and uncalibrated/calibrated measurements for both sensors. 33 | ![Simulated IMU data](images/synthesized_data.png) 34 | 35 | IMUs calibration results for five real sensors (MPU-9150) are shown here: 36 | ![Calibrated accelerometer norm](images/uncalibrated_calibrated_acc.png) 37 | 38 | ### Real IMU 39 | For real IMU calibration we provide the datasets from five different InvenSense MPU-9150 IMUs. Please note, one important requirement: the sensor has to be kept static after each rotation. The standstill flags are generated automatically from the data using ```generate_standstill_flags``` function. To find the calibration parameters issue the following command: 40 | ``` 41 | python3 calibrate_real_imu.py --sampling_frequency=100 --file=data/imu0.log 42 | ``` 43 | and see the found parameters as the output. 44 | 45 | ### Jupyter 46 | The jupyter notebook is [here](Example.ipynb). 47 | 48 | 49 | ## Results 50 | The proposed method has been proven to be unbiased via numerical simulations. 51 | The differences between true and estimated sensor error parameters for 200 Monte-Carlo simulations are shown in the following table: 52 | ![Simulation results](images/simulations.png) 53 | 54 | IMUs calibration results for five real sensors (MPU-9150) are shown here: 55 | ![Real IMU calibration parameters](images/real_imu.png) 56 | 57 | 58 | ## Paper 59 | This is the accompanying code for "In-situ gyroscope calibration based on accelerometer data" paper that was presented on 27th Saint Petersburg International Conference on Integrated Navigation Systems (ICINS). 60 | 61 | ### Citation 62 | If you use the code, the authors will be grateful for citing: 63 | ``` 64 | @inproceedings{mikov2020situ, 65 | title={In-situ Gyroscope Calibration Based on Accelerometer Data}, 66 | author={Mikov, Aleksandr and Reginya, Sergey and Moschevikin, Alex}, 67 | booktitle={2020 27th Saint Petersburg International Conference on Integrated Navigation Systems (ICINS)}, 68 | pages={1--5}, 69 | year={2020}, 70 | organization={IEEE} 71 | } 72 | ``` 73 | -------------------------------------------------------------------------------- /calibrate_real_imu.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | import time 4 | 5 | from code.helpers import * 6 | from code.cost_functions import * 7 | from code.utilities import * 8 | 9 | np.set_printoptions(edgeitems=30, linewidth=1000, formatter={'float': '{: 0.4f}'.format}) 10 | 11 | if __name__ == '__main__': 12 | parser = argparse.ArgumentParser(description = 'Run calibration on real data from IMU.') 13 | parser.add_argument('--sampling_frequency', help = 'Sampling frequency for logfile.', 14 | required = True, type = int) 15 | parser.add_argument('--file', help = 'Path to file with data from IMU.', 16 | required = True, type = str) 17 | args = parser.parse_args() 18 | 19 | dt = 1 / args.sampling_frequency 20 | datafile = args.file 21 | 22 | # read file with ax, ay, az, wx, wy, wz measurements from IMU 23 | imu_data = np.genfromtxt(datafile, delimiter=' ') 24 | standstill = generate_standstill_flags(imu_data) 25 | 26 | plot_imu_data_and_standstill(imu_data, standstill) 27 | 28 | accs, angs = imu_data[:,0:3], imu_data[:,3:6] 29 | 30 | # find accelerometer calibration parameters and calibrate accel measurements 31 | theta_found_acc = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 32 | time_start = time.time() 33 | theta_found_acc = find_calib_params_acc(True, residual_acc, theta_found_acc, accs, standstill > 0) 34 | time_end = time.time() 35 | 36 | print("ACC calibration done in: ", time_end - time_start, "seconds") 37 | print("[ S_X S_Y S_Z NO_X NO_Y NO_Z B_X B_Y B_Z ]") 38 | print(theta_found_acc) 39 | accs_calibrated = calibrate_accelerometer(accs, theta_found_acc) 40 | plot_accelerations_before_and_after(accs, accs_calibrated) 41 | 42 | 43 | # find gyroscope calibration parameters 44 | theta_found_gyr = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 45 | theta_found_gyr[-6:-3] = np.mean(angs[0:100,:], axis=0) 46 | 47 | residualSum = lambda: np.sum(np.rad2deg(residual_gyr(theta_found_gyr, 48 | angs, 49 | accs_calibrated, 50 | standstill, dt))**2) 51 | 52 | print("Gyroscope residuals before calibration: ", residualSum()) 53 | time_start = time.time() 54 | theta_found_gyr = find_calib_params_gyr(True, residual_gyr, theta_found_gyr, 55 | angs, accs_calibrated, standstill, 1.0/args.sampling_frequency) 56 | time_end = time.time() 57 | print("GYR calibration done in: ", time_end - time_start, "seconds") 58 | print("[ S_X S_Y S_Z NO_X NO_Y NO_Z B_X B_Y B_Z E_X E_Y E_Z ]") 59 | print(theta_found_gyr) 60 | print("Gyroscope residuals after calibration: ", residualSum()) -------------------------------------------------------------------------------- /code/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikoff/imu-calib/12dff577725dbc7bd7161431e4142b6249395fec/code/__init__.py -------------------------------------------------------------------------------- /code/cost_functions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from code.imu_model import sensor_error_model, misalignment 4 | from code.quaternion import Quaternion 5 | 6 | def residual_acc(theta, accs, idxs, least_squares = True): 7 | ''' 8 | Accelerometer cost function according to equation (10) in the paper 9 | ''' 10 | M, b = sensor_error_model(theta[0:9]) 11 | C = np.linalg.inv(M) 12 | 13 | z = np.zeros(3 * np.sum(idxs)) 14 | standstill_counter = 0; 15 | for i, standstill in enumerate(idxs): 16 | if standstill: 17 | acc = C @ (accs[i].T - b) 18 | # equations (11) and (12) 19 | roll = np.arctan2(acc[1], acc[2]) 20 | pitch = np.arctan2(-acc[0], np.sqrt(acc[2]**2 + acc[1]**2)) 21 | 22 | # equation (13) 23 | u = 9.81 * np.array([-np.sin(pitch), 24 | np.cos(pitch)*np.sin(roll), 25 | np.cos(pitch)*np.cos(roll)]) 26 | residual = u - acc 27 | 28 | z[3*standstill_counter] = residual[0] 29 | z[3*standstill_counter + 1] = residual[1] 30 | z[3*standstill_counter + 2] = residual[2] 31 | standstill_counter += 1 32 | 33 | if least_squares: 34 | return z.flatten() 35 | else: 36 | return z.flatten() @ z.flatten() 37 | 38 | 39 | def residual_gyr(theta, gyrs, accs, standstill_flags, dt, least_squares = True): 40 | ''' 41 | Gyroscope cost function according to equation (16) in the paper 42 | ''' 43 | M, b = sensor_error_model(theta[0:9]) 44 | C = np.linalg.inv(M) 45 | 46 | acc_to_gyro = misalignment(theta[-3:]) 47 | Rm = np.linalg.inv(acc_to_gyro) 48 | 49 | # find the indexes, when the sensor was rotated 50 | standstill_changes = np.hstack((0, np.diff(standstill_flags))) 51 | motion_starts = np.where(standstill_changes == -1) 52 | motion_ends = np.where(standstill_changes == 1) 53 | motion_start_end_idxs = [] 54 | for s, e in zip(*motion_starts, *motion_ends): 55 | motion_start_end_idxs.append([s - 5, e + 5]) 56 | 57 | z = np.zeros(2 * len(motion_start_end_idxs)) 58 | prev_idx_motion_end = 0 59 | for i, (idx_motion_start, idx_motion_end) in enumerate(motion_start_end_idxs): 60 | acc_start = np.mean(accs[prev_idx_motion_end:idx_motion_start,:], axis=0) 61 | 62 | roll_start_acc = np.arctan2(acc_start[1], acc_start[2]) 63 | pitch_start_acc = np.arctan2(-acc_start[0], np.sqrt(acc_start[2]**2 + acc_start[1]**2)) 64 | yaw_start = 0.0 65 | 66 | # gyroscope measurements during rotation, w_j in eq. (16) 67 | gyr = gyrs[idx_motion_start:idx_motion_end,:] 68 | # equivalent to R_{a, j_start} in eq. (16) 69 | q = Quaternion.from_euler(roll_start_acc, pitch_start_acc, yaw_start) 70 | for w in gyr: 71 | w_b = C @ Rm @ w - C @ b 72 | q_upd = Quaternion.exactFromOmega((w_b) * dt) 73 | q = q.prod(q_upd) 74 | 75 | roll_end_gyr, pitch_end_gyr,_ = q.to_euler() 76 | 77 | if i < len(motion_start_end_idxs) - 1: 78 | next_idx_motion_start = motion_start_end_idxs[i+1][0] 79 | else: 80 | next_idx_motion_start = -1 81 | 82 | acc_end = np.mean(accs[idx_motion_end:next_idx_motion_start], axis=0) 83 | roll_end_acc = np.arctan2(acc_end[1], acc_end[2]) 84 | pitch_end_acc = np.arctan2(-acc_end[0], np.sqrt(acc_end[2]**2 + acc_end[1]**2)) 85 | 86 | ori_diff_gyr = np.array([[roll_end_gyr, 87 | pitch_end_gyr]]) 88 | ori_diff_acc = np.array([[roll_end_acc, 89 | pitch_end_acc]]) 90 | 91 | # equivalent to (Rg{w} * R_{a, j_start})^T in eq. (16) 92 | qgyr = Quaternion.from_euler(roll_end_gyr, pitch_end_gyr, 0.0) 93 | # R_{a, j_end} in eq. (16) 94 | qacc = Quaternion.from_euler(roll_end_acc, pitch_end_acc, 0.0) 95 | 96 | qdiff = qacc.prod(qgyr.conj()) 97 | euldiff = qdiff.to_euler() 98 | 99 | z[2*i] = euldiff[0] 100 | z[2*i + 1] = euldiff[1] 101 | 102 | prev_idx_motion_end = idx_motion_end 103 | 104 | if least_squares: 105 | return z.flatten() 106 | else: 107 | return z.flatten() @ z.flatten() 108 | -------------------------------------------------------------------------------- /code/helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.optimize as optimize 3 | 4 | from code.imu_model import sensor_error_model, misalignment 5 | from code.quaternion import Quaternion 6 | 7 | def find_calib_params_acc(least_squares, residual, theta, accelerations, idxs_standstill): 8 | ''' 9 | Find calibration parameters according to cost function from eq. (10) 10 | For details see residual_acc function inside cost_functions.py 11 | ''' 12 | res = optimize.least_squares(residual, theta, 13 | args=(accelerations, 14 | idxs_standstill, 15 | least_squares,), 16 | max_nfev = 25, 17 | x_scale = [10., 10., 10., 10., 10., 10., 1., 1., 1.], 18 | method='trf', loss='soft_l1', 19 | bounds = [ 20 | (-0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -1.1, -1.1, -1.1), 21 | (0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 1.1, 1.1, 1.1)], 22 | ) 23 | return res.x 24 | 25 | def find_calib_params_gyr(least_squares, residual, theta, ang_velocities, accelerations, standstill_flags, dt): 26 | ''' 27 | Find calibration parameters according to cost function from eq. (16) 28 | For details see residual_gyr function inside cost_functions.py 29 | ''' 30 | res = optimize.least_squares( 31 | residual, 32 | theta, 33 | args=(ang_velocities, 34 | accelerations, 35 | standstill_flags, 36 | dt, 37 | least_squares,), 38 | max_nfev = 50, 39 | x_scale = [10., 10., 10., 10., 10., 10., 10., 10., 10., 10, 10, 10], 40 | method='trf', loss='soft_l1', 41 | bounds = [ 42 | (-0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -0.11, -0.11), 43 | (0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11, 0.11)], 44 | ) 45 | return res.x 46 | 47 | def calibrate_accelerometer(measurements, sensor_errors): 48 | ''' 49 | Calibrate sensor measurements according to the model. 50 | See eq. (7). 51 | ''' 52 | M, b = sensor_error_model(sensor_errors[0:9]) 53 | C = np.linalg.inv(M) 54 | 55 | calibrated_measurements = np.copy(measurements) 56 | for measurement in calibrated_measurements: 57 | measurement[:] = C @ (measurement - b) 58 | 59 | return calibrated_measurements 60 | 61 | def calibrate_gyroscope(measurements, sensor_errors, epsilon): 62 | ''' 63 | Calibrate sensor measurements according to the model. 64 | See eq. (8). 65 | ''' 66 | M, b = sensor_error_model(sensor_errors[0:9]) 67 | C = np.linalg.inv(M) 68 | Rm = np.linalg.inv(misalignment(epsilon)) 69 | 70 | calibrated_measurements = np.copy(measurements) 71 | for measurement in calibrated_measurements: 72 | measurement[:] = C @ Rm @ measurement - C @ b 73 | 74 | return calibrated_measurements 75 | 76 | def integrate_gyroscope(roll, pitch, yaw, measurements, dt, return_as = 'array'): 77 | q = Quaternion.from_euler(roll, pitch, yaw) 78 | orientations = [] 79 | for w in measurements: 80 | q = q.prod(Quaternion.exactFromOmega((w) * dt)) 81 | if return_as == 'array': 82 | orientations.append(q.q) 83 | else: 84 | orientations.append(q) 85 | return orientations -------------------------------------------------------------------------------- /code/imu_model.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def skew(v): 4 | '''Returns skew-symmetric matrix, which satisfies A^T = -A''' 5 | return np.array([[0, -v[2], v[1]], 6 | [v[2], 0, -v[0]], 7 | [-v[1], v[0], 0]]) 8 | 9 | def sensor_error_model(errors): 10 | ''' 11 | Build sensor error model matrix and bias according to 12 | eq. (1) and (2) in the paper. 13 | ''' 14 | KX, KY, KZ, NOX, NOY, NOZ, BX, BY, BZ = errors 15 | M = np.array([[1.0 + KX, NOY, NOZ], 16 | [0.0, 1.0 + KY, NOX], 17 | [0.0, 0.0, 1.0 + KZ]]) 18 | b = np.array([BX, BY, BZ]) 19 | 20 | return M, b 21 | 22 | def misalignment(epsilon): 23 | ''' 24 | Build rotation matrix from gyroscope coordinate frame 25 | to accelerometer coordinate frame according to eq. (5) 26 | ''' 27 | return np.eye(3) + skew(epsilon) -------------------------------------------------------------------------------- /code/monte_carlo.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import time 3 | 4 | from code.simulator import * 5 | from code.cost_functions import residual_acc, residual_gyr 6 | from code.helpers import * 7 | from code.utilities import * 8 | 9 | np.set_printoptions(edgeitems=30, linewidth=1000, formatter={'float': '{: 0.4f}'.format}) 10 | 11 | 12 | def monte_carlo_cycle(N_samples, dt, theta_acc, theta_gyr, randomized_rotations, plot): 13 | fail = 0 14 | 15 | simulation_results = {} 16 | simulation_results['theta_acc_generated'] = theta_acc 17 | simulation_results['theta_gyr_generated'] = theta_gyr 18 | 19 | # start orientation 20 | roll, pitch, yaw = np.deg2rad([-150, -75, -150]) 21 | yaw_increment = np.deg2rad(18) 22 | 23 | # generate ideal measurements 24 | times, ideal_gyroscope, ideal_accelerometer, standstill_flags = \ 25 | generate_ideal_imu_measurements_for_rotation_sequence( 26 | roll, pitch, yaw, yaw_increment, N_samples, dt, randomized_rotations) 27 | 28 | # restore true oreintations from ideal measurements 29 | orientations_true = integrate_gyroscope(roll, pitch, yaw, ideal_gyroscope, dt) 30 | 31 | # corrupt imu measurements according to sensor errors 32 | corrupted_gyroscope_measurements, corrupted_accelerometer_measurements = \ 33 | corrupt_imu_measurements(theta_acc, theta_gyr, ideal_gyroscope, ideal_accelerometer, 0.004, 0.04) 34 | 35 | if plot: 36 | plot_corrupted_accelerometer_and_gyro_measurements(corrupted_accelerometer_measurements, 37 | corrupted_gyroscope_measurements, standstill_flags) 38 | 39 | # find accelerometer calibration parameters and calibrate accel measurements 40 | theta_found_acc = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 41 | time_start = time.time() 42 | theta_found_acc = find_calib_params_acc(True, residual_acc, theta_found_acc, 43 | corrupted_accelerometer_measurements, standstill_flags > 0) 44 | time_end = time.time() 45 | 46 | print("ACC calibration done in: ", time_end - time_start, "seconds") 47 | print("First row - true parameters, second - their estimate, third - their difference") 48 | print("[ S_X S_Y S_Z NO_X NO_Y NO_Z B_X B_Y B_Z ]") 49 | print(theta_acc) 50 | print(theta_found_acc) 51 | print(np.abs(theta_found_acc - theta_acc)) 52 | accs_calibrated = calibrate_accelerometer(corrupted_accelerometer_measurements, theta_found_acc) 53 | 54 | if plot: 55 | plot_calibrated_and_uncalibrated_acc_norms( 56 | corrupted_accelerometer_measurements, 57 | accs_calibrated) 58 | 59 | 60 | # find gyroscope calibration parameters 61 | theta_found_gyr = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 62 | 63 | residualSum = lambda: np.sum(np.rad2deg(residual_gyr(theta_found_gyr, 64 | corrupted_gyroscope_measurements, 65 | accs_calibrated, 66 | standstill_flags, 67 | dt) ** 2)) 68 | 69 | print("Gyroscope residuals before calibration: ", residualSum()) 70 | 71 | theta_found_gyr[-6:-3] = np.mean(corrupted_gyroscope_measurements[0:N_samples,:], axis=0) 72 | gyr_calibrated_bias_only = calibrate_gyroscope(corrupted_gyroscope_measurements, theta_gyr[0:9], theta_gyr[-3:]) 73 | print("Gyroscope residuals with only bias calibrated: ", residualSum()) 74 | 75 | time_start = time.time() 76 | theta_found_gyr = find_calib_params_gyr(True, residual_gyr, theta_found_gyr, 77 | corrupted_gyroscope_measurements, accs_calibrated, standstill_flags, dt) 78 | time_end = time.time() 79 | 80 | print("GYR calibration done in: ", time_end - time_start, "seconds") 81 | print("First row - true parameters, second - their estimate, third - their difference") 82 | print("[ S_X S_Y S_Z NO_X NO_Y NO_Z B_X B_Y B_Z E_X E_Y E_Z ]") 83 | print(theta_gyr) 84 | print(theta_found_gyr) 85 | print(np.abs(theta_found_gyr - theta_gyr)) 86 | 87 | print("Gyroscope residuals after calibration: ", residualSum()) 88 | 89 | if residualSum() > 1.0: 90 | fail = 1 91 | print("Calibration failed!") 92 | 93 | gyr_calibrated = calibrate_gyroscope(corrupted_gyroscope_measurements, theta_gyr[0:9], theta_gyr[-3:]) 94 | if plot: 95 | plot_ideal_corrupted_calibrated_measurements(ideal_gyroscope, corrupted_gyroscope_measurements, gyr_calibrated) 96 | 97 | simulation_results['theta_acc_found'] = theta_found_acc 98 | simulation_results['theta_gyr_found'] = theta_found_gyr 99 | simulation_results['residual_deg'] = residualSum() 100 | simulation_results['fail'] = fail 101 | 102 | return simulation_results -------------------------------------------------------------------------------- /code/quaternion.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from code.imu_model import skew 4 | 5 | class Quaternion(): 6 | ''' 7 | Quaternion class. Supports multiplication, normalization, conversion to matrix form. 8 | Notation: right-handed, passive, local-to-global. 9 | ''' 10 | def __init__(self, q = [1, 0, 0, 0]): 11 | self._q = np.array(q).reshape((4,1)) 12 | 13 | @property 14 | def q(self): 15 | return self._q 16 | 17 | @q.setter 18 | def q(self, value): 19 | self._q = value 20 | 21 | @property 22 | def r(self): 23 | return self._q[0,0] 24 | 25 | @property 26 | def w(self): 27 | return self._q[0,0] 28 | 29 | @property 30 | def x(self): 31 | return self._q[1,0] 32 | 33 | @property 34 | def y(self): 35 | return self._q[2,0] 36 | 37 | @property 38 | def z(self): 39 | return self._q[3,0] 40 | 41 | @property 42 | def v(self): 43 | return self._q[1:,0] 44 | 45 | @classmethod 46 | def fromRV(cls, r, v): 47 | return Quaternion([r, v[0], v[1], v[2]]) 48 | 49 | @classmethod 50 | def exactFromOmega(cls, w): 51 | wnorm = np.linalg.norm(w) 52 | if (wnorm > 0.00001): 53 | return Quaternion.fromRV(np.cos(wnorm*0.5), (w / wnorm * np.sin(wnorm*0.5)).flatten()) 54 | else: 55 | return Quaternion.identity() 56 | 57 | @classmethod 58 | def from_euler(cls, roll, pitch, yaw): 59 | halfRoll = roll / 2.0 60 | halfPitch = pitch / 2.0 61 | halfYaw = yaw / 2.0 62 | w = np.cos(halfRoll) * np.cos(halfPitch) * np.cos(halfYaw) + np.sin(halfRoll) * np.sin(halfPitch) * np.sin(halfYaw) 63 | x = np.sin(halfRoll) * np.cos(halfPitch) * np.cos(halfYaw) - np.cos(halfRoll) * np.sin(halfPitch) * np.sin(halfYaw) 64 | y = np.cos(halfRoll) * np.sin(halfPitch) * np.cos(halfYaw) + np.sin(halfRoll) * np.cos(halfPitch) * np.sin(halfYaw) 65 | z = np.cos(halfRoll) * np.cos(halfPitch) * np.sin(halfYaw) - np.sin(halfRoll) * np.sin(halfPitch) * np.cos(halfYaw) 66 | return Quaternion([w, x, y, z]) 67 | 68 | @classmethod 69 | def identity(cls): 70 | return Quaternion([1, 0, 0, 0]) 71 | 72 | def norm(self): 73 | return np.linalg.norm(self._q) 74 | 75 | def conj(self): 76 | return Quaternion.fromRV(self.r, -1 * self.v) 77 | 78 | def prod(self, other): 79 | return Quaternion.fromRV(self.r * other.r - self.v.dot(other.v), 80 | self.r * other.v + other.r * self.v + np.cross(self.v, other.v)) 81 | 82 | def Rm(self): 83 | return (self.w**2 - self.v.T @ self.v) * np.eye(3)\ 84 | + 2 * np.outer(self.v, self.v.T) + 2 * self.w * skew(self.v) 85 | 86 | def normalize(self): 87 | norm = np.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2) 88 | self.q[0,0] /= norm 89 | self.q[1,0] /= norm 90 | self.q[2,0] /= norm 91 | self.q[3,0] /= norm 92 | 93 | def normalized(self): 94 | norm = np.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2) 95 | return Quaternion(self.q / norm) 96 | 97 | def to_euler(self): 98 | q = self.q 99 | t0 = 2.0 * (q[0] * q[1] + q[2] * q[3]) 100 | t1 = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]) 101 | roll = np.arctan2(t0, t1)[0] 102 | t2 = 2.0 * (q[0] * q[2] - q[3] * q[1]) 103 | t2 = 1.0 if t2 > 1.0 else t2 104 | t2 = -1.0 if t2 < -1.0 else t2 105 | pitch = np.arcsin(t2)[0] 106 | t3 = 2.0 * (q[0] * q[3] + q[1] * q[2]) 107 | t4 = 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]) 108 | yaw = np.arctan2(t3, t4)[0] 109 | return (roll, pitch, yaw) 110 | -------------------------------------------------------------------------------- /code/simulator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from code.quaternion import * 4 | from code.imu_model import sensor_error_model, misalignment 5 | 6 | def slerp(q_from, q_to, fraction): 7 | theta = q_from.q.T @ q_to.q 8 | q = np.sin((1.0 - fraction) * theta) * q_from.q / np.sin(theta) \ 9 | + np.sin(fraction * theta) / np.sin(theta) * q_to.q 10 | return Quaternion(q).normalized() 11 | 12 | def generate_angular_velocity_between_two_orientations(q0, q1, dt): 13 | return 2.0 * q0.conj().prod(Quaternion(q1.q - q0.q)).v / dt 14 | 15 | def generate_orientations(q_from, q_to, n_intermediary_points): 16 | return [slerp(q_from, q_to, fraction) 17 | for fraction in np.linspace(0.0, 1.0, n_intermediary_points)] 18 | 19 | def generate_imu_measurements_from_orientation_sequence(orientations, dt = 0.01): 20 | angular_velocities, accelerations = [], [] 21 | 22 | o_prev = orientations[0] 23 | for o in orientations[0:]: 24 | angular_velocities.append( 25 | generate_angular_velocity_between_two_orientations(o_prev, o, dt)) 26 | accelerations.append( 27 | o.Rm().T @ np.array([[0.0], [0.0], [9.81]]).flatten()) 28 | o_prev = o 29 | 30 | return np.array(angular_velocities), np.array(accelerations) 31 | 32 | def generate_imu_measurements_between_orientations(fromOrientation, toOrientation, n_intermediary_points, dt): 33 | orientations = generate_orientations( 34 | fromOrientation, toOrientation, n_intermediary_points) 35 | 36 | angular_velocities, accelerations = \ 37 | generate_imu_measurements_from_orientation_sequence(orientations, dt) 38 | 39 | return angular_velocities, accelerations 40 | 41 | def generate_ideal_imu_measurements_for_rotation_sequence(start_roll, start_pitch, start_yaw, 42 | yaw_increment, N_samples = 100, 43 | dt=0.01, randomized = False): 44 | 45 | zero_epoch_orientation = Quaternion.from_euler(start_roll, start_pitch, start_yaw) 46 | 47 | # generate first orientation chunk while the sensor was motionless 48 | ang_velocities, accelerations = generate_imu_measurements_between_orientations( 49 | zero_epoch_orientation, zero_epoch_orientation, N_samples, dt) 50 | standstill = np.ones(len(ang_velocities)) 51 | 52 | 53 | roll, pitch, yaw = start_roll, start_pitch, start_yaw 54 | roll_prev, pitch_prev, yaw_prev = start_roll, start_pitch, start_yaw 55 | 56 | times = [] 57 | for pitch in np.linspace(start_pitch + np.pi/4, start_pitch + np.pi, 4): 58 | for roll in np.linspace(start_roll, start_roll + 2 * np.pi, 6): 59 | if randomized: 60 | roll += np.deg2rad(np.random.randint(60)) * np.random.choice([-1, 0, 1]) 61 | pitch += np.deg2rad(np.random.randint(45)) * np.random.choice([-1, 0, 1]) 62 | yaw += yaw_increment 63 | 64 | # generate data through rotation 65 | fromOrientation = Quaternion.from_euler(roll_prev, pitch_prev, yaw_prev) 66 | toOrientation = Quaternion.from_euler(roll, pitch, yaw) 67 | ang, acc = generate_imu_measurements_between_orientations( 68 | fromOrientation, toOrientation, N_samples, dt) 69 | 70 | ang_velocities = np.concatenate((ang_velocities, ang), axis=0) 71 | accelerations = np.concatenate((accelerations, acc), axis=0) 72 | standstill = np.concatenate((standstill, np.zeros(N_samples)), axis=0) 73 | 74 | # generate data for standstill 75 | ang, acc = generate_imu_measurements_between_orientations( 76 | toOrientation, toOrientation, N_samples, dt) 77 | 78 | ang_velocities = np.concatenate((ang_velocities, ang), axis=0) 79 | accelerations = np.concatenate((accelerations, acc), axis=0) 80 | standstill = np.concatenate((standstill, np.ones(N_samples)), axis=0) 81 | 82 | roll_prev, pitch_prev, yaw_prev = roll, pitch, yaw 83 | 84 | if len(times): 85 | times.append(times[-1] + dt) 86 | else: 87 | times.append(0.0) 88 | 89 | return times, ang_velocities, accelerations, standstill 90 | 91 | def corrupt_imu_measurements(theta_acc, theta_gyr, ang_velocities, accelerations, 92 | gyr_noise_std = 0.001, acc_noise_std = 0.04): 93 | ang_velocities = np.copy(ang_velocities) 94 | accelerations = np.copy(accelerations) 95 | # according to equations (1), (2), (5) and (6) in the paper 96 | Ma, ba = sensor_error_model(theta_acc[0:9]) 97 | Mg, bg = sensor_error_model(theta_gyr[0:9]) 98 | 99 | Rm_gyro_to_acc = misalignment(theta_gyr[-3:]) 100 | for i, _ in enumerate(ang_velocities): 101 | ang_velocities[i] = Rm_gyro_to_acc @ (Mg @ ang_velocities[i] + bg + np.random.normal(0, gyr_noise_std, 3)) 102 | accelerations[i] = Ma @ (accelerations[i]) + ba + np.random.normal(0, acc_noise_std, 3) 103 | 104 | return ang_velocities, accelerations -------------------------------------------------------------------------------- /code/utilities.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | from code.quaternion import Quaternion 5 | from code.helpers import integrate_gyroscope 6 | 7 | def generate_standstill_flags(imu_data): 8 | standstill = np.zeros(len(imu_data)) 9 | counter_after_motion = 60 10 | # generate standstill flags 11 | for i, m in enumerate(imu_data): 12 | if np.linalg.norm(m[3:6]) < 0.13: 13 | counter_after_motion += 1 14 | if counter_after_motion > 60: 15 | standstill[i] = 1 16 | else: 17 | standstill[i] = 0 18 | standstill[i-60:i] = 0 19 | counter_after_motion = 0 20 | 21 | return standstill 22 | 23 | def plot_corrupted_accelerometer_and_gyro_measurements(corrupted_accelerometer_measurements, 24 | corrupted_gyroscope_measurements, standstill_flags): 25 | 26 | idxs_standstill = standstill_flags > 0 27 | idxs_motion = standstill_flags < 1e-8 28 | 29 | fig, ax = plt.subplots(2, 1, figsize=(16, 6)) 30 | ax[0].plot(np.linalg.norm(corrupted_accelerometer_measurements, axis=1), label = "Accelerometer norm") 31 | ax[0].plot(np.arange(0, len(standstill_flags))[idxs_standstill], 32 | standstill_flags[idxs_standstill] + 10.0, 33 | linestyle='none', marker='.', alpha=0.9, color='green', label = "Standstill") 34 | ax[0].plot(np.arange(0, len(standstill_flags))[idxs_motion], 35 | standstill_flags[idxs_motion] + 10.0, 36 | linestyle='none', marker='.', alpha=0.9, color='red', label = "In motion") 37 | ax[0].set(xlabel = "sample #", ylabel="$m/s^2$") 38 | ax[0].set_title("Accelerometer measurements norm and standstill during simulated rotations") 39 | ax[0].legend() 40 | 41 | ax[1].plot(corrupted_gyroscope_measurements) 42 | ax[1].set_title("Gyroscope measurements during simulated rotations") 43 | ax[1].set(xlabel = "sample #", ylabel="$rad/s$") 44 | ax[1].legend(["x", "y", "z"]) 45 | plt.tight_layout() 46 | plt.show() 47 | 48 | def plot_calibrated_and_uncalibrated_acc_norms(uncalibrated, calibrated): 49 | fig, ax = plt.subplots(1, 1, figsize=(16, 5)) 50 | ax.plot(np.linalg.norm(uncalibrated, axis=1)) 51 | ax.plot(np.linalg.norm(calibrated, axis=1)) 52 | ax.legend(["Uncalibrated norm", "Calibrated norm"]) 53 | ax.set(xlabel = "sample #", ylabel = "$m/s^2$", title = "Calibration results for accelerometer") 54 | plt.tight_layout() 55 | plt.show() 56 | 57 | def plot_ideal_corrupted_calibrated_measurements(ideal_gyroscope, corrupted_gyroscope_measurements, gyr_calibrated): 58 | fig, ax = plt.subplots(3, 1, figsize=(16, 9)) 59 | ax[0].plot(ideal_gyroscope[:, 0], label = 'ideal') 60 | ax[0].plot(corrupted_gyroscope_measurements[:, 0], label = 'corrupted') 61 | ax[0].plot(gyr_calibrated[:, 0], marker = 'x', markersize = 2, linestyle = 'none', label = 'calibrated') 62 | ax[0].legend() 63 | ax[0].set(xlabel = "sample #", ylabel = "$rad/sec$", title = "Calibration results for gyroscope X axis") 64 | 65 | ax[1].plot(ideal_gyroscope[:, 1], label = 'ideal') 66 | ax[1].plot(corrupted_gyroscope_measurements[:, 1], label = 'corrupted') 67 | ax[1].plot(gyr_calibrated[:, 1], marker = 'x', markersize = 2, linestyle = 'none', label = 'calibrated') 68 | ax[1].legend() 69 | ax[1].set(xlabel = "sample #", ylabel = "$rad/sec$", title = "Calibration results for gyroscope Y axis") 70 | 71 | ax[2].plot(ideal_gyroscope[:, 2], label = 'ideal') 72 | ax[2].plot(corrupted_gyroscope_measurements[:, 2], label = 'corrupted') 73 | ax[2].plot(gyr_calibrated[:, 2], marker = 'x', markersize = 2, linestyle = 'none', label = 'calibrated') 74 | ax[2].legend() 75 | ax[2].set(xlabel = "sample #", ylabel = "$rad/sec$", title = "Calibration results for gyroscope Z axis") 76 | 77 | plt.tight_layout() 78 | plt.show() 79 | 80 | def plot_imu_data_and_standstill(imu_data, standstill_flags): 81 | fig, ax = plt.subplots(2, 1, figsize=(16, 9)) 82 | ax[0].plot(imu_data[:, 0:3]) 83 | ax[0].legend(["ax", "ay", "az"]) 84 | 85 | ax[1].plot(imu_data[:, 3], label = "wx") 86 | ax[1].plot(imu_data[:, 4], label = "wx") 87 | ax[1].plot(imu_data[:, 5], label = "wx") 88 | ax[1].plot(standstill_flags) 89 | 90 | idxs_standstill = standstill_flags > 0 91 | idxs_motion = standstill_flags < 1e-8 92 | ax[1].plot(np.arange(0, len(standstill_flags))[idxs_standstill], standstill_flags[idxs_standstill], 93 | label = "standstill", linestyle='none', marker='.', alpha=0.9, color='green') 94 | ax[1].plot(np.arange(0, len(standstill_flags))[idxs_motion], standstill_flags[idxs_motion], 95 | label = "in-motion", linestyle='none', marker='.', alpha=0.9, color='red') 96 | ax[1].legend() 97 | 98 | plt.show() 99 | 100 | def plot_accelerations_before_and_after(accs, accs_calibrated): 101 | fig, ax = plt.subplots(1, 1, figsize=(8, 3)) 102 | times = np.arange(0, len(accs_calibrated)) * 0.01 103 | ax.plot(times, np.linalg.norm(accs, axis=1), alpha = 0.5) 104 | ax.plot(times, np.linalg.norm(accs_calibrated, axis=1), alpha = 0.5) 105 | ax.legend(["Uncalibrated norm", "Calibrated norm"]) 106 | ax.set(xlabel='$time, s$', ylabel='$m/s^2$', ylim = [8.81, 10.81]) 107 | plt.show() 108 | -------------------------------------------------------------------------------- /images/gyro_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikoff/imu-calib/12dff577725dbc7bd7161431e4142b6249395fec/images/gyro_calib.png -------------------------------------------------------------------------------- /images/real_imu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikoff/imu-calib/12dff577725dbc7bd7161431e4142b6249395fec/images/real_imu.png -------------------------------------------------------------------------------- /images/simulations.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikoff/imu-calib/12dff577725dbc7bd7161431e4142b6249395fec/images/simulations.png -------------------------------------------------------------------------------- /images/synthesized_data.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikoff/imu-calib/12dff577725dbc7bd7161431e4142b6249395fec/images/synthesized_data.png -------------------------------------------------------------------------------- /images/uncalibrated_calibrated_acc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mikoff/imu-calib/12dff577725dbc7bd7161431e4142b6249395fec/images/uncalibrated_calibrated_acc.png -------------------------------------------------------------------------------- /run_monte_carlo.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import argparse 3 | 4 | from code.monte_carlo import monte_carlo_cycle 5 | 6 | if __name__ == '__main__': 7 | parser = argparse.ArgumentParser(description = 'Run monte carlo simulations for accelerometer and gyroscope calibration.') 8 | parser.add_argument('--sampling_frequency', help = 'Sampling frequency for synthesized IMU measurements.', 9 | required = False, default = 100, type = float) 10 | parser.add_argument('--randomize_rotations', help = 'Flag to whether or not randomize the rotations.', 11 | required = False, default = True, type = bool) 12 | parser.add_argument('--plot', help = 'Flag to whether or not plot the results.', 13 | required = False, default = False, type = bool) 14 | parser.add_argument('--iterations_num', help = 'Number of Monte-carlo runs.', 15 | required = False, default = 1, type = int) 16 | args = parser.parse_args() 17 | 18 | N_samples = args.sampling_frequency 19 | dt = 1 / args.sampling_frequency 20 | randomize = args.randomize_rotations 21 | plot = args.plot 22 | 23 | simulation_results = {} 24 | 25 | for i in range(0, args.iterations_num): 26 | theta_acc = np.random.randint(100, size=9) / np.array([1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 27 | 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 100* np.random.choice([-1,1]), 100* np.random.choice([-1,1]), 100* np.random.choice([-1,1])]) 28 | theta_gyr = np.random.randint(100, size=12) / np.array([1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 29 | 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1]), 1000* np.random.choice([-1,1])]) 30 | 31 | simulation_results[i] = monte_carlo_cycle(N_samples, dt, theta_acc, theta_gyr, randomize, plot) --------------------------------------------------------------------------------