章节一：视觉惯性导航系统

1.1 介绍

这篇博客的目的与意义在于：

1. 帮助理解VINS（Visual Inertial Navigation System）的相关概念。
2. 从应用IMU的角度进行解释。

1.2 传感器模型

首先将文中涉及的主要符号进行说明：$(o)^X_Y$表示Y坐标系下的性质o，相对于坐标系X；其余列举如下：

1.2.1 相机模型

所谓的相机模型，就是将状态向量中的xyz变量，变成观测（uv）的过程。主要过程是：世界坐标系下的三维点xyz–》相机坐标系下的三维点$x^w_c,y^w_c,y^w_c$–》相机平面的观测uv。该式$[u,v]^T = Proj. f^c_w([x,y,z]^T)$即可完成上述过程。其中$f^c_w$将点从全局坐标系转换到局部坐标系，（注意这里与原文不太一样，我们采用机器人中的Pose的表示方式）,设当前帧的Pose为$R^w_c,T^w_c$

当$R^w_c$为单位阵，$T^w_c$等于0时，$f^c_w$就没有发生变化。接着当把点转换到局部坐标系下后，使用内方位元素K即可将点投影到相机平面。$d[u,v,1]=Kp^c$，假设K是单位阵，那么可以得出其之间的关系$u=x/z,v=y/z$。到此，就将状态向量中的三维点，与二维的观测uv完成了转换，即观测模型$(z-h(x))$中的$h(x)$

1.2.2 IMU模型

与上述模型一样，也是将状态向量中的变量，通过变换转换到观测上来。涉及的变量是两个相邻的pose、速度，观测是加速度与角速度。那这里的变换的目的就是将pose和速度，变换成加速度与角速度。

观测的不确定性

在视觉中，观测的不确定性一般就是给一个像素的误差，而IMU的观测不确定性更加复杂。

转换过程

涉及到的导航变量为$x_i = [p_i,v_i,q_i,a_i,w_i]$,以及$x_{i+1} = [p_{i+1},v_{i+1},q_{i+1},a_{i+1},w_{i+1}]$，通过牛顿第二定律，我们可以得到位置与加速度的关系（$s=v_0t+1/2at^2$)，速度与加速度的关系（$v=v_0+at$）,朝向与角速度的关系。这里值得注意的是加速度不是直接的测量值，而是要减去重力加速度（$q_ta_t-g$）；具体到这里的场景有如下等式成立

上式参考[1]中的1.33-35，从上式可以看出，初始的朝向（全局）的信息是必须的，以为只有知道该信息，才能对重力加速度对测量值的加速度进行分解，这里预积分就发挥作用了，后面单独说预积分。接着引进了三个中间量$\alpha$，这三个中间量加上两个加速度，五个量作为测量值进行误差函数的定义

相关知识点

1. 基于滤波的方法不好的原因是过早地固定住了前面状态的线性化点，导致陷入局部最优。滤波方法速度快的原因是因为把前面的状态都给边缘化掉了，如机器人状态只有当前的。
2. 后端优化中，改进优化计算量的策略主要有两种，一种是边缘化，一种是Conditioning（见P3）。
3. 一阶优化和二阶优化的区别，在参考1的p7中，提到了使用二阶优化方法，而在之前组会中，T提到manOpt使用的是一阶优化。此处有一个比较好的解释What is the definition of a first order method。根据海森矩阵的计算方式，高斯牛顿中使用的本质是一阶优化，那参考1中的p7表述应该是不太严谨的。

参考

[1] 参考Zhenfei的笔记，链接：vins note。