VINS中的重力-尺度-速度初始化（2）
细化重力
(quad)上一篇文章中得到的 (g) 一般是存在误差的。因为在实际应用中，当地的重力向量的模一般是已知固定大小的（所以只有两个自由度未知），而我们在前面求解时并没有利用这个条件，因此最后计算出来的重力向量很难刚好满足这个条件。于是，在vins的初始化中，还会对得到的重力向量进行修正。
(quad)首先，作者对重力向量进行参数化：
]
其中，(overline{hat{g}})是上一步中估计得到的重力向量方向的单位向量，(b_1)与(b_2)是另外两个单位向量，是(overline{hat{g}})切平面上的两个互相垂直的单位向量，获得方式如下：
此时我们可以从上面的式子知道，原本三维的优化变量 (g^{c 0}) 已经可以使用两维的 (w^{2 times 1}) 替代。参考上一节的推导，待优化变量变成了
v_{b_{k}}^{b_{k}}
v_{b_{k+1}}^{b_{k+1}}
w^{2 times 1}
s
end{array}right]
]
最后得到的观测方程也变为了
-I Delta t_{k} & 0 & frac{1}{2} R_{c_{0}}^{b_{k}} Delta t_{k}^{2} vec{b} & R_{c_{0}}^{b_{k}}left(p_{c_{k+1}}^{c_{0}}-p_{c_{k}}^{c_{0}}right)
-I & R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} & R_{c_{0}}^{b_{k}} Delta t_{k} vec{b} & 0
end{array}right]left[begin{array}{c}
v_{b_{k}}^{b_{k}}
v_{b_{k+1}}^{b_{k+1}}
w
s
end{array}right]=left[begin{array}{c}
alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}-frac{1}{2} R_{c_{0}}^{b_{c}} Delta t_{k}^{2}|g| hat{hat{g}}
beta_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}} Delta t_{k}|g| hat{g}
end{array}right]
]
基于新得到的观测方程（可以使用LDLT分解求解），我们可以通过重力约束不断地对初始化结果进行修正，迭代对其 进行求解（vins中迭代了4次），最后得到一个修正后的初始化结果。
与世界坐标系对齐
这一步一般是最后一步，一般世界坐标系选择的是东北天坐标系。则这个对齐操作就是得到将重力向量旋转到Z轴上的旋转矩阵，这个旋转矩阵就是将原本坐标变换到世界坐标系（东北天坐标系）的变换矩阵。
找到这个变换矩阵后，接下来就是使用这个变换矩阵将位姿，速度等状态信息都变换到世界坐标系下。
加速度计bias
通过上面的阅读，我们会发现在vins的服务器托管网初始化中并没有对加速度计的bias进行估计，感觉对于超定的方程，多计算一个加速度计的bias好像也不是什么难事，，而且建模应该会更准确。在这个初始化工作的论文中其实有解释原因：
- 通过仿真实验证明，如果运动不剧烈，加速度计的bias很难被观测出来；
- 忽略加速计的bias对估计其他初始值的影响不大；
- 加速度计的bias是可以通过之后的VIO紧耦合优化，慢慢估计出来的；
参考资料
- [1] https://github.com/HKUST-Aerial-Robotics/VINS-Mono
- [2] VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen, IEEE Transactions on Robotics
- [3] https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw
- [4] https://xhy3054.github.io/
服务器托管，北京服务器托管，服务器租用 http://www.fwqtg.net
机房租用，北京机房租用，IDC机房托管， http://www.fwqtg.net
鲁棒主成分分析(Robust Principal Component Analysis, RobustPCA)是一种将时间序列矩阵分解为低秩分量和稀疏分量的技术。这种分解能够识别潜在的趋势，以及检测异常和异常值。在本中我们将研究RobustPCA的数学基础，介…
