IMU

（1）用IMU来进行预测

读入一个10/20帧的数据集，通过IMU来初步预测出位姿以及显示其路径。

Christian Forster, Luca Carlone, Frank Dellaert, Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation," Robotics: Science and Systems (RSS), Rome, 2015. 

（2）通过光度差或者什么来作为更新部分，通过相机的观测方程算出增益，随后进行更新。

A. Filter Initialization

误差状态的运动方程

2.1连续时间的IMU系统动态方程

这里涉及到真实值(true-)，标称值（nominal-），和误差状态值（error-state）三个量，真实值是有标称值和误差值组合而来。标称值是有非线性方程而来的"大信号"，误差值的线性方程而来满足线性高斯滤波的“小信号”。

2.1.1相关变量

• 这里的相关变量中，输入值是IMU的测量值，以及当地重力加速度。其他真值值变量的值都是由他们而来。其中normal值是一个抽象变量，表示理想值，是无法估计的，在融合中我们使用估计的真实值做为标称值来预测。
• 标称值是完全理想值，我们把各种因素产生的真实值和标称值的不确定性归于变量表示。
• 这里采用Hamilton 四元数的表述方法
• 这里的角速率使用的是locally的表述，这样可以直接使用机体系b下的传感器测量值
• 这里的角度误差使用的也是locally的表述，这是很多文献和算法采用的经典方式，不过有证明说采用globally的方式定义的角度误差有更好的性质。
• 这里的加速度定义的是globally，我的理解是，方便计算导航系下的速度和位置。

2.1.2真实状态运动方程(true-state kinematics equations )

• true-state 变量表示的运动方程.IMU bias的噪声定义为随机游走。

• 由于初始状态姿态未知，这种不确定即重力加速度向量的不确定，但是实际工程中我们是制定初始状态的，即重力加速度不确定不再存在，这里的方程也认为其为恒定值。

• .IMU误差来源分为两种，内部bias

• 这里的IMU真实值是没有bias和测量噪声的值,IMU加速度计测量加速度不包含重力加速度，即如果自由落体运动，不考虑噪声加速度计的读数是0。

利用IMU测量代替真实值的系统真实状态运动方程，该方程状态为,由IMU的含噪声测量值驱动，并受高斯白噪声干扰.