kintzhao/imu_tk

[toc]

gyro,acc的校正

代码参考：

https://github.com/Kyle-ak/imu_tk https://github.com/kiyoshiiriemon/imu_tk

部分安装库：

sudo apt-get install build-essential cmake libeigen3-dev  freeglut3-dev gnuplot

记录数据集合文件，数据记录采取下面的数据格式

T_sec T_nsec Acc_xyz Gyro_xyz Vel_xyz Pos_xyz Q_wxyz

d.tv_sec>>d.tv_nsec>>d.acc_x>>d.acc_y>>d.acc_z >> d.gyro_x>>d.gyro_y>>d.gyro_z>> d.vel_x>>d.vel_y>>d.vel_z
>> d.pos_x>>d.pos_y>>d.pos_z>> d.q_w>>d.q_x>>d.q_y>>d.q_z;

运行指令与说明:

-d 指定数据所在文件夹

-c 用于标定的数据集文件

-o 原始数据文件经过校正后的数据文件

-i 原始数据文件

 
robot@robot:~/work/imu_tk$ ./bin/calib_and_correct_imufile  -d ./datas/x3_3 -c record_imu_1706679864.log  -o record_imu_1706679864.log.0131  -i  record_imu_1706679864.log
 
 
 ./bin/calib_and_correct_imufile  -d ./datas/r200_3 -c data.log  -o data.log.rec  -i  data.log

校正

  ms_mat_ = mis_mat_*scale_mat_;
  inline Eigen::Matrix< _T, 3 , 1> unbiasNormalize( const Eigen::Matrix< _T, 3 , 1> &raw_data ) const
  {
    return ms_mat_*(raw_data - bias_vec_); 
  };

include/imu_tk/calibration.h

* Triad model:
 *   
 * -Misalignment matrix:
 * 
 * general case:
 * 
 *     [    1     -mis_yz   mis_zy  ]
 * T = [  mis_xz     1     -mis_zx  ]
 *     [ -mis_xy   mis_yx     1     ]
 * 
 * "body" frame spacial case:
 * 
 *     [  1     -mis_yz   mis_zy  ]
 * T = [  0        1     -mis_zx  ]
 *     [  0        0        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 ]
 * 
 * Given a raw sensor reading X (e.g., the acceleration ), the calibrated "unbiased" reading X' is obtained
 * 
 * X' = T*K*(X - B)
 * 
 * with B the bias (variable) + offset (constant, possibbly 0), or, equivalently:
 * 
 * X' = T*K*X - B'
 * 
 * with B' = T*K*B
 * 
 * Without knowing the value of the bias (and with offset == 0), the calibrated reading X'' is simply:
 * 
 * X'' = T*K*X
*/

磁力计校正

读取数据

python3 /home/robot/work/imu_tk/scripts/read_magnet_data.py
data.shape: (28495, 10)

标准椭球的方式（不推荐）

python3 magnetometer/calibrate.py

 使用说明：
1）修改代码中self.file_path对应数据记录文件，
self.file_path = "/home/robot/work/imu_tk/datas/r200_7/data_mag.log"

2） 结果保存在self.file_path+“.result” 文件中

修改里面的磁力计原始数据的文件，执行上述命令

一般化椭球的方式

 python3 /home/robot/work/imu_tk/magnetometer/magnetometer_calibration.py

修改文件路径： self.file_path = "/home/robot/work/imu_tk/datas/r200_7/mag_data.log" 

参考论文： Least squares ellipsoid specific fitting https://www.researchgate.net/publication/4070857_Least_squares_ellipsoid_specific_fitting

参考论文： Direct Least Square Fitting of Ellipses https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/ellipse-pami.pdf

Bookstein