IMU标定算法

欢迎交流，有评论就更

IMU运动模型

IMU由加速计和陀螺仪构成，理想情况下，加速度计和陀螺仪的3个轴都是正交的。但是对于一般的IMU，这是不现实的。
那么，在加速度计和陀螺仪的测量值$s^S$sS与准确值$S^B$SB存在转换矩阵：

$T^a = \begin{bmatrix}1&-\alpha_{yz}&\alpha_{zy}\\ 0&1&-\alpha_{zx}\\ 0&0&1 \end{bmatrix}$Ta=⎣⎡​100​−αyz​10​αzy​−αzx​1​⎦⎤​
$T^g = \begin{bmatrix}1&-\gamma_{yz}&\gamma_{zy}\\ \gamma_{xz}&1&-\gamma_{zx}\\ -\gamma_{xy}&\gamma_{yx}&1 \end{bmatrix}$Tg=⎣⎡​1γxz​−γxy​​−γyz​1γyx​​γzy​−γzx​1​⎦⎤​
这里，假设校正后加速度计的坐标系与IMU基座坐标系重合。
另外，传感器输出与实际物理量之间存在比例因子$K^a$Ka、$K^g$Kg，参考之前博客的IMU预积分，还存在零偏$b^a$ba、$b^g$bg和测量噪声$v^a$va、$v^g$vg。
综上，加速度计和陀螺仪的测量模型为：
$z^O=T^aK^a(a^S+b^a+v^a) \\w^O=T^gK^g(a^S+b^g+v^g)$zO=TaKa(aS+ba+va)wO=TgKg(aS+bg+vg)

IMU标定流程

• 获取数据的过程如下：
  
• 静止过程检测
  
• 在初始静止过程计算陀螺仪的零偏使用的Allen方差
  
• 在所有静止过程中标定加速度计参数：
  损失函数：
  这里的$g$g是重力加速度。
• 在静止过程之间的运动过程标定陀螺仪参数
  损失函数：
  
  这里,
  是通过龙格-库塔方法数值积分得到的重力向量。

参考：
https://www.cnblogs.com/feifanrensheng/p/10439057.html
http://www.dis.uniroma1.it/~pretto/papers/pg_imeko2014.pdf
http://www.dis.uniroma1.it/~pretto/papers/tpm_icra2014.pdf
参考代码：
https://github.com/Kyle-ak/imu_tk