imu_tk note

Last updated on May 7, 2023 pm

[TOC]

Overview

本文以标定 Realsense D435i 为例。

加速度计的标定利用了加速度计在静止状态下的三轴数据模值等于重力加速度这一条件，采集加速度计在不同状态的静止数据，通过优化算法即可求出标定参数。加速度计单独即可完成标定，陀螺仪的标定需要加速度计数据的参与，所以需要先标定好加速度计。在标定陀螺仪时，IMU从静止状态A，通过一段时间，到了静止状态B，通过陀螺仪可以算出来A到B的旋转矩阵，而通过加速度数据也可以算出来A到B的旋转矩阵，最小化这一差异即可实现优化求解。

Code

Collect IMU data

  • Procedure:
    1. Left the IMU static for 50 seconds.
    2. Rotate the IMU and then lay it in a different attitude.
    3. Wait for at least 1 seconds.
    4. Have you rotated the IMU 36 ~ 50 times? If not, go back to step 2.
    5. Done.

  • 录制 rosbag

    1
    2
    3
    4
    5
    # start
    roslaunch realsense2_camera rs_camera.launch

    # record
    rosbag record /camera/imu

  • rosbag 转 csv 文件

    1
    kalibr_bagextractor --bag ~/Downloads/imu_d435i_imu_tk.bag --imu-topics /camera/imu

  • 拆分 csv 文件 为 acc.mat 和 gyro.mat
    • mat数据格式为：时间戳（单位为秒） X轴数据 Y轴数据 Z轴数据，文本的格式为UTF-8无BOM

Build

需要修改代码里的offset和scale，把offset设置为0，把scale设置为1

1
2
3
4
5
// test_imu_calib.cpp
init_acc_calib.setBias( Vector3d(0, 0, 0) );
init_gyro_calib.setScale( Vector3d(1.0, 1.0, 1.0) );

mp_calib.setGravityMagnitude(9.8016); // 9.81744

Run

1
2
cd bin/
./test_imu_calib acc.mat gyro.mat

Output

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
Importing IMU data from the Matlab matrix file : /home/cg/Downloads/acc_d435i.mat
Importing IMU data from the Matlab matrix file : /home/cg/Downloads/gyr_d435i.mat
Accelerometers calibration: calibrating...
Accelerometers calibration: extracted 46 intervals using threshold multiplier 2 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.303187e+01    0.00e+00    1.16e+03   0.00e+00   0.00e+00  1.00e+04        0    7.22e-03    7.64e-03
   1  2.903204e-01    7.27e+01    5.90e+01   2.87e-01   1.00e+00  3.00e+04        1    8.33e-03    1.60e-02
   2  2.617510e-01    2.86e-02    1.15e-02   3.27e-03   1.00e+00  9.00e+04        1    7.94e-03    2.40e-02
residual 0.261751
Accelerometers calibration: extracted 46 intervals using threshold multiplier 3 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.320660e+01    0.00e+00    1.17e+03   0.00e+00   0.00e+00  1.00e+04        0    7.39e-03    7.68e-03
   1  2.920290e-01    7.29e+01    5.97e+01   2.87e-01   1.00e+00  3.00e+04        1    8.28e-03    1.60e-02
   2  2.637732e-01    2.83e-02    1.15e-02   3.30e-03   1.00e+00  9.00e+04        1    7.47e-03    2.35e-02
residual 0.263773
Accelerometers calibration: extracted 46 intervals using threshold multiplier 4 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.320781e+01    0.00e+00    1.17e+03   0.00e+00   0.00e+00  1.00e+04        0    6.83e-03    7.05e-03
   1  2.906375e-01    7.29e+01    5.99e+01   2.87e-01   1.00e+00  3.00e+04        1    7.65e-03    1.47e-02
   2  2.624371e-01    2.82e-02    1.14e-02   3.27e-03   1.00e+00  9.00e+04        1    7.99e-03    2.27e-02
residual 0.262437
Accelerometers calibration: extracted 46 intervals using threshold multiplier 5 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.316700e+01    0.00e+00    1.17e+03   0.00e+00   0.00e+00  1.00e+04        0    7.16e-03    7.64e-03
   1  2.997427e-01    7.29e+01    6.00e+01   2.87e-01   1.00e+00  3.00e+04        1    7.92e-03    1.56e-02
   2  2.717076e-01    2.80e-02    1.17e-02   3.22e-03   1.00e+00  9.00e+04        1    7.55e-03    2.31e-02
residual 0.271708
Accelerometers calibration: extracted 46 intervals using threshold multiplier 6 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.318641e+01    0.00e+00    1.17e+03   0.00e+00   0.00e+00  1.00e+04        0    6.83e-03    7.18e-03
   1  2.925727e-01    7.29e+01    6.03e+01   2.88e-01   1.00e+00  3.00e+04        1    7.51e-03    1.47e-02
   2  2.646989e-01    2.79e-02    1.15e-02   3.19e-03   1.00e+00  9.00e+04        1    8.01e-03    2.27e-02
residual 0.264699
Accelerometers calibration: extracted 46 intervals using threshold multiplier 7 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.322846e+01    0.00e+00    1.16e+03   0.00e+00   0.00e+00  1.00e+04        0    7.31e-03    7.56e-03
   1  2.962436e-01    7.29e+01    6.06e+01   2.88e-01   1.00e+00  3.00e+04        1    7.85e-03    1.55e-02
   2  2.684434e-01    2.78e-02    1.12e-02   3.14e-03   1.00e+00  9.00e+04        1    7.80e-03    2.33e-02
residual 0.268443
Accelerometers calibration: extracted 46 intervals using threshold multiplier 8 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.324374e+01    0.00e+00    1.16e+03   0.00e+00   0.00e+00  1.00e+04        0    7.08e-03    7.37e-03
   1  2.997994e-01    7.29e+01    6.07e+01   2.88e-01   1.00e+00  3.00e+04        1    7.45e-03    1.48e-02
   2  2.720214e-01    2.78e-02    1.10e-02   3.10e-03   1.00e+00  9.00e+04        1    7.58e-03    2.24e-02
residual 0.272021
Accelerometers calibration: extracted 46 intervals using threshold multiplier 9 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.325722e+01    0.00e+00    1.16e+03   0.00e+00   0.00e+00  1.00e+04        0    6.96e-03    7.21e-03
   1  3.011574e-01    7.30e+01    6.06e+01   2.88e-01   1.00e+00  3.00e+04        1    7.99e-03    1.52e-02
   2  2.733308e-01    2.78e-02    1.11e-02   3.11e-03   1.00e+00  9.00e+04        1    7.87e-03    2.31e-02
residual 0.273331
Accelerometers calibration: extracted 46 intervals using threshold multiplier 10 -> Trying calibrate...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  7.330019e+01    0.00e+00    1.16e+03   0.00e+00   0.00e+00  1.00e+04        0    7.13e-03    7.45e-03
   1  3.019769e-01    7.30e+01    6.05e+01   2.88e-01   1.00e+00  3.00e+04        1    7.92e-03    1.54e-02
   2  2.740658e-01    2.79e-02    1.11e-02   3.12e-03   1.00e+00  9.00e+04        1    7.40e-03    2.28e-02
residual 0.274066
Accelerometers calibration: Better calibration obtained using threshold multiplier 2 with residual 0.261751
Misalignment Matrix
         1 -0.0010374 0.00307749
         0          1 -0.0119756
        -0          0          1
Scale Matrix
0.999201        0        0
       0  1.00004        0
       0        0  1.00224
Bias Vector
-0.0746601
1.5056e-05
  0.276673

Accelerometers calibration: inverse scale factors:
1.0008
0.999961
0.997769

Press Enter to continue

Gyroscopes calibration: calibrating...
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  5.638980e-03    0.00e+00    3.06e-01   0.00e+00   0.00e+00  1.00e+04        0    1.21e-01    1.21e-01
   1  1.007029e-03    4.63e-03    3.78e-03   1.86e-02   9.99e-01  3.00e+04        1    1.23e-01    2.44e-01
   2  1.006868e-03    1.61e-07    2.28e-06   7.60e-05   1.00e+00  9.00e+04        1    1.25e-01    3.69e-01

Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  9                        9
Residual blocks                            45                       45
Residuals                                 135                      135

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          5.638980e-03
Final                            1.006868e-03
Change                           4.632113e-03

Minimizer iterations                        3
Successful steps                            3
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000056

  Residual only evaluation           0.008886 (3)
  Jacobian & residual evaluation     0.363018 (3)
  Linear solver                      0.000043 (3)
Minimizer                            0.372046

Postprocessor                        0.000003
Total                                0.372105

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.906368e-10 <= 1.000000e-06)

Gyroscopes calibration: residual 0.00100687
Misalignment Matrix
          1 -0.00349484   0.0091083
 0.00376498           1  0.00256356
 -0.0125506 -0.00301254           1
Scale Matrix
0.997587        0        0
       0 0.994327        0
       0        0 0.994757
Bias Vector
-0.00401266
  -0.001156
 0.00173263

Gyroscopes calibration: inverse scale factors:
1.00242
1.00571
1.00527

  • test_imu_acc.calib

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    Accelerometers calibration: Better calibration obtained using threshold multiplier 2 with residual 0.261751

    Misalignment Matrix
             1 -0.0010374 0.00307749
             0          1 -0.0119756
            -0          0          1
    Scale Matrix
    0.999201        0        0
           0  1.00004        0
           0        0  1.00224
    Bias Vector
    -0.0746601
    1.5056e-05
      0.276673

    Accelerometers calibration: inverse scale factors:
    1.0008
    0.999961
    0.997769

  • test_imu_gyro.calib

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    Gyroscopes calibration: residual 0.00100687
    Misalignment Matrix
              1 -0.00349484   0.0091083
     0.00376498           1  0.00256356
     -0.0125506 -0.00301254           1
    Scale Matrix
    0.997587        0        0
           0 0.994327        0
           0        0 0.994757
    Bias Vector
    -0.00401266
      -0.001156
     0.00173263

    Gyroscopes calibration: inverse scale factors:
    1.00242
    1.00571
    1.00527

Correction

  • code: https://github.com/cggos/imu_tools/tree/kinetic/imu_calib/imu_tk

Given a raw sensor reading X (e.g., the acceleration ), the calibrated "unbiased" reading X' is obtained.

\[x^\prime = T \cdot K \cdot (x - b)\]

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
Misalignment matrix:

    [    1     -mis_yz   mis_zy  ]
T = [  mis_xz     1     -mis_zx  ]
    [ -mis_xy   mis_yx     1     ]

Scale matrix:

    [  s_x      0        0  ]
K = [   0      s_y       0  ]
    [   0       0       s_z ]

Bias vector:

    [ b_x ]
B = [ b_y ]
    [ b_z ]

X' = T*K*(X - B)

Accelerator

INS
#Sensor Calibration #INS #IMU #IMU Calibration
imu_tk note
https://cgabc.xyz/posts/719f0cc/
Author
Gavin Gao
Posted on
November 16, 2021
Licensed under