Sliding Windows Filter（SWF）在VIO、SLAM这个领域应用非常广，比如MSCKF、OKVIS、VINS-Mono等等，几乎可以说是VIO的标配。
SWF可以分成基于滤波器的和基于优化的两种。最典型的基于滤波器的方法就是MSCKF算法了。它是基于EKF的算法，在marginalize state的时候处理比较简单，只需要把对应的covariance的对应行列直接丢弃就可以了。而基于优化的方法在边缘化时需要对Hessian矩阵做舒尔补，操作会复杂一些。
为了尝试一下SWF,我们先从简单的基于滤波的方法入手.本文实现了一个基于MSCKF的Visual Wheel融合的Odometry.
下面是分别是仿真和用KAIST数据测试的结果
更多可参考
代码语言：javascript复制https://zhuanlan.zhihu.com/p/270670373注意：这里为了凸显加了视觉校正效果，把wheel的noise设的比较大，轨迹不太平滑。如果想要平滑一些，可以适当调低wheel的noise。
代码请见
代码语言：javascript复制https://github.com/ydsf16/TinyGrapeKit
传感器配置
Visual部分，用一个单目相机。Wheel部分可以是左右轮速度或位移
坐标系统
轮速坐标系/Odometry坐标系{0}: 车辆后轴中心、贴地. x轴向前, y轴向左,z轴向上.
全局坐标系{G}:与初始时刻的轮速坐标系重合的坐标系。
相机坐标系{C}: x轴向右,y轴向下,z轴向前
内外参数
内参数：1）相机内参；
2）左右轮轴距b，左右轮速系数、 将编码器count转成距离米，或者速度转成m/s。
外参数：轮速坐标系到相机坐标系的旋转与平移.
我们假设这些内外参数都假设预先标定好的。实际上，也可以把这些内外参数放到状态向量里面估计，这也是很多论文里面常见的做法，比如OpenVINS。但是，具体这些内外参数能不能估计出来，是不是可观的，可以参考一下黄国权老师的论文
状态向量
滑窗里面的状态分成两类，一类是Odometry的位姿。
另一个类是一串相机位姿：
总的状态是当前Odometry位姿 N帧的相机位姿：
跟MSCKF一样，我们把协方差分块表示：
这里我们使用最简单的滑窗维护方式，当新的一帧进到滑窗后，就直接把老的一帧给边缘化掉。因为是EKF，就是直接把最后一帧相机pose从x中去掉，然后把对应的协方差的行和列删除掉.
Wheel Propagation
EKF算法分成两步：Propagation Update。在这里，我们用wheel的信息进行状态的propagation，用视觉信息做update。下面先推导Propagation部分。这里的推导，参考了Mingyang Li的paper[2]。
按照IMU处理的方法，首先把ODE方程列出来
下面就可以对ODE方程进行积分了。简单起见，直接用欧拉积分了。如果考虑精度，可以使用中值或者龙格库塔：
用这个式子，就可以进行均值的Propagation。对于协方差的Propgation，我们先求雅克比。这里定义旋转的Error为：
状态增广
当新来一帧图像，可以通过odometry位姿，计算出相机位姿。
然后把它放到状态向量里面。相应的要把协方差矩阵进行扩展：
Update
7.1视觉Update
MSCKF的很大的优势就是没有把特征点放到状态向量里面，降低了计算量。
当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：
- 当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：
- 另一种是边缘化的时候，也就是把最后一帧滑出窗口的时候，把在这一帧里面新建的特征点都丢弃，也就是都拿来做更新。
这里说的特征点/feature，同时表示3D点，也表示在图像上的2D位置。
A. 一个特征点的处理
因为与是正交的.所以
做了上面的操作之后，就可以得到一个不含特征点的线性方程：
B. 多个特征点的处理
上面一个特征点可以得到一个（18）式。多个特征点就可以得到很多的（18）式，把他们堆叠起来，有得到一个很大的线性方程：
这个方程的行数一般很大，直接用来做EKF更新效率很低。MSCKF又用QR分解，进行了一次压缩。具体的对H*做QR分解
带入到(19)式中，可以得到，
左右两边，同时乘以有
最后，我们得到一个压缩后的线性方程
这方程的行数最大和状态的维度相同。最终用来做EKF更新的也就是（20）式。
C. 边缘化
边缘化，或者说如何删除滑窗里的状态。前面也已经提到了，我们使用了最简单的策略。就直接把最老的一帧去掉。去掉的这帧里的所有特征点都被拿来做更新。
最后，总结一下，当一帧图像来了之后的处理步骤
- 增广状态
- 做特征点跟踪。
- 收集跟踪丢失的的所有特征点{}
- 收集将要被边缘化的帧中的所有特征点：{}
- 利用特征点{}和{}构造线性方程(20)，并执行EKF更新。
- 边缘化操作：将x中边缘化掉的pose去掉，将协方差矩阵中对应的行和列删除。
平面约束Update
一般车辆都是运动在平面上的，在更新的时候，我们引入一个平面约束。这部分参考了Stergios I. Roumeliotis 的PaperVINS On wheels[3]
实现
全部工程代码请见：
代码语言：javascript复制https://github.com/ydsf16/TinyGrapeKit实验
仿真测试,很明显,VWO-MSCKF比纯Wheel里程计精度更好.
数据集测试
我们这里使用了KAIST数据集，链接是：
代码语言：javascript复制https://irap.kaist.ac.kr/dataset/同样的,相比纯Wheel odom,精度会有所提高.
注意:这里的测试,Wheel内参数精度都是比较低的,所以raw odom的精度不是很高.如果wheel内参数精度很高的话,VWO-MSCKF精度不一定更高.
本文仅做学术分享，如有侵权，请联系删文。
