VINS-Mono中重力优化
在初始化阶段我们估计出了$g^{c_0}$，但这个$g^{c_0}$是没有模长约束的，所以重力的估计不是很准确，进而会导致预积分的位置项不准确。我们知道重力的模长是9.8，VINS-Mono中利用这一点对重力矢量进行了第二次优化，实际上2012年第一篇提出预积分技术的论文中也指明了重力优化这一点。
1、优化重力向量
因为重力的模长固定，三维变量$g^{c_0}$的自由度只有2维，VINS-Mono中采用球面坐标进行参数化：
$$
g^{c_0} = ||g|| \cdot \bar{g}^{c_0}+w_1 \vec{b}_1 + w_2 \vec{b}_2 \tag{1.1}
$$
其中$\bar{g}^{c_0}$是$g^{c_0}$方向的单位矢量，$w_1,w_2$是待优化变量，$\vec{b}_1,\vec{b}_2$是切平面的基向量，如下选取：
$$
\begin{aligned}
&\vec{b}_{1}=
\left\{\begin{array}{ll}
\left(\bar{g}^{c_{0}} \times[1,0,0]\right), & \bar{g}^{c_{0}} \neq[1,0,0]^{\top} \\\\
\left(\bar{g}^{c_{0}} \times[0,0,1]\right), & \text { otherwise }
\end{array}\right.
\\\\
&\vec{b}_2 = \bar{g}^{c_0} \times \vec{b}_1
\end{aligned} \tag{1.2}
$$
这样
视觉惯导联合初始化中的公式（3.6）可做如下改写：
$$
\begin{aligned}
&\mathcal{X}_{I}^{k}=\left[{v}_{k}^{b_{k}}, {v}_{k+1}^{b_{k+1}},w_1,w_2, s\right]^{\top} \\\\
&H_{b_{k+1}}^{b_k} =
\left[\begin{array}{cccc}
-\mathbf{I} \Delta t & \mathbf{0} & \frac{1}{2} R_{c_0}^{b_k}\vec{b}_1\Delta t^{2} &\frac{1}{2} R_{c_0}^{b_k}\vec{b}_2\Delta t^{2} & R_{c_0}^{b_k}\left(\overline{\mathbf{p}}_{c_{k+1}}^{c_{0}}-\overline{\mathbf{p}}_{c_{k}}^{c_{0}}\right) \\\\
-\mathbf{I} & {R}_{c_0}^{b_k} R_{b_{k+1}}^{c_0} & R_{c_0}^{b_k}\vec{b}_1\Delta t & R_{c_0}^{b_k}\vec{b}_2\Delta t & \mathbf{0}
\end{array}\right]_{6 \times 9}
\end{aligned} \tag{1.3}
$$
观测项变为：
$$
\hat{z}_{b_{k+1}}^{b_k}=
\left[\begin{array}{c}
\hat{\alpha}_{b_{k+1}}^{b_{k}}- p_c^b + R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b-\frac{1}{2}R_{c_0}^{b_k}||g|| \bar{g}^{c_0}\Delta t^2 \\\\
\hat{\beta}_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}||g|| \bar{g}^{c_0}\Delta t
\end{array}\right]_{6\times 1}
=
H_{b_{k+1}}^{b_k}\mathcal{X}_{I}^{k}+\mathbf{n}_{b_{k+1}}^{b_{k}} \tag{1.4}
$$
采用与
视觉惯导联合初始化中同样的求解方法来解这个线性方程。
2、对齐导航系
求解出重力矢量在$c_0$帧中的投影后，可以找到$c_0$帧到导航坐标系的旋转矩阵$R_{c_0}^w$，这样得以将所有的轨迹转换到导航系（东北天系）下，使得系统的roll和pitch角可观。这一步是通过对齐$c_0$帧与导航坐标系中的重力矢量完成的：
轴角为：
$$
n = \frac{g^{c_0}\times g^w}{||g^{c_0}\times g^w||}，\quad \theta=atan\frac{||g^{c_0}\times g^w||}{g^{c_0}\cdot g^w} \tag{2.1}
$$
旋转矩阵可以通过罗德里格斯公式获得了。
