IMU与轮速计联合初始化

? ? ? ? 最近在做一个有意思的项目，在地库里面实现多传感器融合里程计，采用的是滤波算法。由于地库里没有gnss信号，缺乏绝对观测，也不想用视觉特征等比较慢的算法，只采用轮速计与IMU做融合。

????????公司其他项目在地库中是主要是直接用轮速计做积分，用IMU做pitch角和roll角的更新。因此，在xy平面上实际上就是轮速计的积分，轮速计积分虽然比较稳定，但是并不是很好，有时候也会飘，而缺乏绝对观测的原因，一旦飘了一点点就再也回不来了，以至于回环的时候有一个很大的偏差。说到IMU，这真是个难搞的器件。如果直接用它做积分，立马就要飞掉，甚至你都看不出一点点趋势，发散的非常快，以至于还搞出速度用轮速计，角度用IMU这样的不伦不类的融合方式。地库里程计也是个细活，差一点点结果就千差万别，前面的一个小小偏差对后面的轨迹就是大范围的偏差。

? ? ? ? 言归正传，IMU与轮速计做融合最重要的是要做初始化，即估计IMU的角速度随机游走bg、加速度随机游走ba、初始速度v以及初始重力方向。这些量要是估计不好，IMU立马死给你看。以前在有gnss信号的情况下，根本就没做初始化，因为绝对观测会把错误位姿纠正回来，但是在地库中就不能这么搞了，所以也让我认识到初始化的重要性，还是不得不做的。我们知道，重力方向都是朝下的，大小为，这里说的初始重力方向指的是重力方向在以第一帧位姿坐标系下的表示，就是,是第一帧位姿的旋转，因此如果重力方向采用了，那么初始方向就要用,相应的初始速度v也要乘以,即,如果重力方向采用，那么初始方向就可以设为单位阵，初始速度为v。

1. 对于bg的初始化来说，陀螺仪有角速度观测，轮速计也有角速度观测。因此，, 累计一定量的观测，取平均就可以了。

2. 对于的初始化来说，可以截取两个时刻的轮速计时间段ts-te，将这两个时刻内的IMU观测找出来，对不齐的就进行插值。IMU的加速度计累计量就是这两个时刻的速度差，轮速计有两个时刻速度的观测，由此，建立公式，，为两时刻加速度计的积分。

? ? ? ? 这里得到的只是初值，没有限定的模长，实际上只有两个自由度，它的模长始终为9.81。这里采用vins里面的做法，采用球面坐标，球的半径是9.81，在的初值处张成一个切平面，在切平面处进行微调，因此可限制的模长为9.81，公式为, 其中为待优化量，为切矢量

3. 对于ba的初始化来说，再次利用公式，这里忽略了ba，由于ba通常非常小，所以这里忽略掉它以求，当求出之后就可以把它加回来，即，再次建立多时间戳观测，求得。

mark一个题外话，已经被它搞晕好几次了：

通常，看起来像是让位姿从i变到了j,应该是，但是它实际上表示的是，不是,切记。