UVIP-SLAM公式推导
UVIP-SLAM公式推导
预备知识
相关内容过多，本文只摘录部分，详细预备知识及IMU预积分推导参阅这篇论文
李群和李代数
特殊正交群\(SO(3)\)，即李群，描述了三维空间中的旋转，它是一个三维黎曼流形，被定义为\(\mathrm{SO}(3) \doteq \lbrace \mathtt{R}\in
\mathbb {R}^{3\times 3} \; {:} \; \mathtt{R}^{\mathsf {T}}\mathtt{R}=
{\mathbf {I}}, \det(\mathtt{R})=1 \rbrace\)。
李群的切空间在恒等元（即单位矩阵）处被称为李代数，\(\mathfrak{so}(3)\)，它是三维向量空间，表示旋转矩阵的切空间，与3×3的反对称矩阵空间相符。可以使用帽子运算符将每个反对称矩阵与R3中的一个向量相对应。
反对称矩阵是一种特殊的矩阵，它的转置等于其负矩阵。在三维空间中，任何反对称矩阵都可以与一个向量一一对应，这个对应关系通过帽子运算符实现。
帽子运算符是一种从向量到反对称矩阵的映射。
对于一个三维向量\(\mathbf{\omega} = [\omega_1, \omega_2, \omega_3]^T\)，其帽子运算符\(\hat{\mathbf{v}}\)定义为：
\[ \begin{equation} \boldsymbol{\omega }^\wedge = \left[\begin{array}{c}\omega _1 \\ \omega _2 \\ \omega _3 \end{array}\right]^\wedge = \left[\begin{array}{ccc}0 & -\omega _3 & \omega _2 \\ \omega _3 & 0 & -\omega _1 \\ -\omega _2 & \omega _1 & 0 \end{array}\right] \in \mathfrak {so}(3). \end{equation} \]
类似地，vee运算符是帽子运算符的逆运算（\((\cdot)^\vee\)）将一个反对称矩阵映射到R3中的一个向量。对于一个反对称矩阵\(S=\omega^\wedge\)，vee运算符满足\(S^\vee=\omega\)。即：
\[ S = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix} ， S^\vee = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix}={\omega} \]
反对称矩阵的一个有用的性质是\[\begin{equation} \mathbf {a}^\wedge \; \mathbf {b} = -\mathbf {b}^\wedge \; \mathbf {a} \quad \forall \; \mathbf {a},\mathbf {b}\in \mathbb {R}^{3}. \end{equation}\]
指数映射
指数映射（在恒等元处）\(\exp: \mathfrak{so}(3) \rightarrow SO(3)\)，它将李代数的元素与旋转关联起来，并与标准矩阵指数（罗德里格斯公式）相符。
在李群理论中，指数映射是从李代数到李群的映射。对于旋转群\(SO(3)\)，指数映射将李代数\(\mathfrak{so}(3)\)（即3x3的反对称矩阵空间）中的元素映射到\(SO(3)\)（即3x3的正交矩阵空间）中的元素，也就是旋转矩阵。
指数映射的定义如下：
\[ \begin{equation} \exp(\boldsymbol{\phi}^\wedge) = \mathbf{I} + \frac{\sin(\|\boldsymbol{\phi}\|)}{\|\boldsymbol{\phi}\|} \boldsymbol{\phi}^\wedge + \frac{1-\cos(\|\boldsymbol{\phi}\|)}{\|\boldsymbol{\phi}\|^2} (\boldsymbol{\phi}^\wedge)^2 \end{equation} \]
其中，\(\boldsymbol{\phi}^\wedge\)是一个反对称矩阵，\(\|\boldsymbol{\phi}\|\)是对应的向量的模，\(\mathbf{I}\)是单位矩阵，\((\boldsymbol{\phi}^\wedge)^2\)是反对称矩阵的平方。
这个公式也被称为罗德里格斯公式，它提供了一种从旋转向量到旋转矩阵的转换方法。
指数映射的一阶近似是： \[ \begin{equation} \exp(\boldsymbol{\phi}^\wedge) \approx \mathbf{I} + \boldsymbol{\phi}^\wedge \end{equation} \]
在这个近似中，忽略了罗德里格斯公式中的高阶项。这个近似在\(\|\boldsymbol{\phi}\|\)较小（即旋转角度较小）时效果较好。这是因为当旋转角度较小时，\(\sin(\|\boldsymbol{\phi}\|) / \|\boldsymbol{\phi}\|\)和\((1-\cos(\|\boldsymbol{\phi}\|)) / \|\boldsymbol{\phi}\|^2\)的值接近于1和0，因此可以忽略罗德里格斯公式中的高阶项。
这个近似提供了一种简单且高效的方法，用于将旋转向量近似转换为旋转矩阵。
指数映射的另一个重要性质： \[ \begin{align} \mathtt{R}\ \mathrm{Exp}(\boldsymbol{\phi})\ \mathtt{R}^{\mathsf {T}}&= \exp (\mathtt{R}\boldsymbol{\phi}^{\wedge} \mathtt{R}^{\mathsf {T}}) = \mathrm{Exp}(\mathtt{R}\boldsymbol{\phi}) \\ \Leftrightarrow \quad \mathrm{Exp}(\boldsymbol{\phi})\ \mathtt{R}&= \mathtt{R}\ \mathrm{Exp}(\mathtt{R}^{\mathsf {T}}\boldsymbol{\phi}). \end{align} \]
对数映射
对数映射（在恒等元处）\(\log: SO(3) \rightarrow \mathfrak{so}(3)\)，它将\(SO(3)\)中的非单位矩阵\(R \neq I\)与一个反对称矩阵关联起来。对于\(SO(3)\)中的旋转矩阵\(R\)，其对数映射定义为： \[ \begin{equation} \log(R) = \frac{\varphi}{2\sin(\varphi)} (R - R^T) \end{equation} \]
其中，\(\varphi\)是\(R\)的旋转角度，可以通过\(R\)的迹（即对角线元素之和）计算得出：\[\varphi = \cos^{-1}\left(\frac{\text{tr}(R) - 1}{2}\right)\]
有：\(\log(R)^\vee = a\varphi\)，其中\(a\)和\(\varphi\)分别是\(R\)的旋转轴和旋转角度。如果\(R=I\)，那么\(\varphi=0\)，\(a\)是未确定的，因此可以任意选择。
这里，\(\log(R)^\vee\)是将旋转矩阵\(R\)通过对数映射转换为反对称矩阵，然后通过vee运算符转换为向量的过程。这个向量的方向与旋转轴相同，长度等于旋转角度。 当\(R=I\)时，旋转角度\(\varphi=0\)，这意味着没有旋转。在这种情况下，旋转轴\(a\)是未确定的，可以任意选择。这是因为任何方向的轴都可以表示零旋转。
为了方便表示，通常采用指数映射和对数映射的“向量化”版本： \[ \begin{equation} \begin{array}{cccccccc} \mathrm{Exp}:& \mathbb {R}^{3} &\rightarrow & \mathrm{SO}(3) &;& \boldsymbol{\phi }&\mapsto & \exp (\boldsymbol{\phi }^\wedge)\\ \mathrm{Log}:& \mathrm{SO}(3) &\rightarrow & \mathbb {R}^{3} &;& \mathtt{R}&\mapsto & \log (\mathtt{R})^\vee \end{array} \end{equation} \]
这样，就可以直接在向量和旋转矩阵之间进行转换，而无需显式处理反对称矩阵，使得表示和计算更加简洁和高效。
与指数映射类似，对数映射也有一阶近似的形式： \[ \begin{equation} \mathrm{Log}\big (\; \mathrm{Exp}(\boldsymbol{\phi }) \; \mathrm{Exp}(\delta \boldsymbol{\phi }) \;\big)\approx \boldsymbol{\phi }+\mathtt{J}_{r}^{-1}(\boldsymbol{\phi })\delta \boldsymbol{\phi } \end{equation} \]
其中，\(\mathtt{J}_{r}^{-1}(\boldsymbol{\phi })\)是\(\boldsymbol{\phi }\)处的右雅可比矩阵的逆矩阵。 \[\mathtt{J}_{r}^{-1}(\boldsymbol{\phi }) = \mathbf {I}+ \frac{1}{2}\boldsymbol{\phi }^\wedge + \left(\frac{1}{\Vert\boldsymbol{\phi }\Vert ^2} +\frac{1+\cos (\Vert \boldsymbol{\phi }\Vert)}{2\Vert \boldsymbol{\phi }\Vert \sin (\Vert\boldsymbol{\phi }\Vert)} \right)(\boldsymbol{\phi }^\wedge)^2. \; \nonumber\] 在李群理论中，右雅可比矩阵用于在李代数的元素（即旋转向量）上引入一个微小扰动。对于旋转群\(SO(3)\)，右雅可比矩阵的定义如下：\[J_r(\phi) = \frac{\partial \exp(\phi + \delta \phi)}{\partial \delta \phi} \bigg|_{\delta \phi = 0}\]
当\(\|\phi\|=0\)时，即旋转向量为零向量时，右雅可比矩阵\(J_r(\phi)\)及其逆矩阵\(J_r^{-1}(\phi)\)简化为单位矩阵。因为在这种情况下，旋转向量上的微小扰动等于扰动本身，因此雅可比矩阵为单位矩阵。
右雅可比矩阵将李代数中的加法扰动关联到李群上的乘法扰动，如上图所示。
\(SO(3)\)中的不确定性描述
在 \(SO(3)\) 中描述不确定性的一种自然方式是在切空间中定义一个分布，然后通过指数映射将其映射到 \(SO(3)\) 中。 \[ \begin{equation} \tilde{\mathtt{R}}=\mathtt{R}\,\, {\rm Exp}(\varepsilon),\quad \varepsilon \sim \mathcal{N} (0, \Sigma) \end{equation} \]
\(R\) 是一个给定的无噪声旋转（也就是均值），\(\epsilon\) 是一个小的、均值为零、协方差为 \(\Sigma\) 的正态分布扰动。 在切空间（也就是李代数 \(so(3)\)）中，将这个扰动 \(\epsilon\) 添加到旋转向量上，然后通过指数映射将结果映射到 \(SO(3)\) 中。这样，就得到了一个在 \(SO(3)\) 中的不确定性描述。
\(\tilde{R}\) 的分布
首先从 \(\mathbb{R}^3\) 中的高斯分布积分开始，以获得 \(\tilde{R}\) 的分布的显式表达式。 这个过程可以表示为以下的积分：
\[ \begin{equation} \int _{\mathbb {R}^{3}} p\,(\epsilon) d\epsilon = \int _{\mathbb {R}^{3}} \alpha e^{-\frac{1}{2} \left\Vert \epsilon \right\Vert ^2_{\Sigma } } \text{d}\epsilon = 1 \end{equation} \]
\(p(\epsilon)\) 是一个三维高斯分布，它的均值为零，协方差矩阵为 \(\Sigma\)。\(\epsilon\) 是一个三维向量，它表示旋转向量的扰动。\(\alpha\) 是高斯分布的归一化常数，它确保了高斯分布的总概率等于 1。\(\|\epsilon\|_{\Sigma}^2 = \epsilon^T \Sigma^{-1} \epsilon\) 是协方差为 \(\Sigma\) 的平方马氏距离。
然后，应用坐标变换 \(\epsilon = \mathrm{Log}(R^{-1}\tilde{R})\)。 \(\|\epsilon\| < \pi\) 时公式 (10) 的逆变换。这样，积分 (11) 变为：
\[ \begin{equation} \int _{\mathrm{SO}(3)} \beta (\tilde{\mathtt{R}}) \; e^{-\frac{1}{2} \left\Vert \mathrm{Log}(\mathtt{R}^{-1}\tilde{\mathtt{R}})\right\Vert ^2_{\Sigma } } \; \text{d}\tilde{\mathtt{R}}= 1 \end{equation} \]
\(\beta(\tilde{R})\) 是一个归一化因子，它确保了在 \(SO(3)\) 中的分布的总概率等于 1。这个归一化因子的形式是 \(\beta(\tilde{R}) = \alpha / |\det(J(\tilde{R}))|\)，其中 \(J(\tilde{R}) = J_r(\mathrm{Log}(R^{-1}\tilde{R}))\)，\(J_r(\cdot)\) 是右雅可比矩阵。
于是，可以直接将\(\mathbb{R}^3\)中的高斯分布表示为： \[ \begin{equation} p\,(\tilde{\mathtt{R}}) = \beta (\tilde{\mathtt{R}}) \; e^{-\frac{1}{2} \left\Vert \mathrm{Log}(\mathtt{R}^{-1}\tilde{\mathtt{R}})\right\Vert ^2_{\Sigma } }. \end{equation} \]
对于小协方差，可以近似地将 \(\beta\) 看作 \(\alpha\)，因为当 \(\tilde{R}\) 接近 \(R\) 时，\(J_r(\mathrm{Log}(R^{-1}\tilde{R}))\) 可以很好地近似为单位矩阵。注意，公式 (12) 已经假设了相对小的协方差 \(\Sigma\)，因为它在半径为 \(\pi\) 的开球外“剪切”了概率尾部（这是由于重新参数化 \(\epsilon = \mathrm{Log}(R^{-1}\tilde{R})\)，它限制了 \(\epsilon\) 使得 \(\|\epsilon\| < \pi\)）。将 \(\beta\) 近似为常数，给定一个按照公式 (13) 分布的测量值 \(\tilde{R}\)，旋转 \(R\) 的负对数似然函数是：
\[ \begin{equation} \mathcal{L}(R) = \frac{1}{2} \left\Vert \mathrm{Log}(R^{-1}\tilde{R})\right\Vert ^2_{\Sigma } + \text{const} = \frac{1}{2} \left\Vert \mathrm{Log}(\tilde{R}^{-1}R)\right\Vert ^2_{\Sigma } + \text{const} \end{equation} \]
这在几何上可以解释为 \(\tilde{R}\) 和 \(R\) 之间的平方角度（在 \(SO(3)\) 中的测地距离）乘以逆不确定性 \(\Sigma^{-1}\)。意味着，在度量两个旋转之间的距离时，考虑了不确定性的影响。如果不确定性大（即 \(\Sigma\) 的值大），那么这个距离就会变小；反之，如果不确定性小（即 \(\Sigma\) 的值小），那么这个距离就会变大。这是一个很直观的结果，因为当一个旋转的不确定性更大时，应该对这个旋转的测量值给予更小的信任度。
VIO
相机
定义投影函数为\(\pi: \mathbb {R}^3
\rightarrow \Omega\)，它将相机参考系下的3D点\(\mathbf {X_\mathtt {C}}\in \mathbb
{R}^3\)转换为图像平面上的2D点\(\mathbf {x_\mathtt{C}}\in \Omega \subset \mathbb
{R}^2\)，即\(\pi: \mathbb{R}^3
\rightarrow \mathbb{R}^2\)，投影函数\(\pi\)的定义如下： \[
\begin{equation}
\pi (\mathbf {X_\mathtt {C}}) = {\left[\begin{array}{c}f_u
\dfrac{X_\mathtt {C}}{Z_\mathtt {C}} + c_u \\
f_v \dfrac{Y_\mathtt {C}}{Z_\mathtt {C}} + c_v \end{array}\right]},
\quad \mathbf {X_\mathtt {C}}=\left[X_\mathtt
{C}\,\,Y_\mathtt {C}\,\,Z_\mathtt {C}\right]^T
\end{equation}
\] 其中，\(f_u\)和\(f_v\)是相机的焦距，\((c_u, c_v)\)是主点坐标，\(X_C, Y_C,
Z_C\)是相机坐标系下的3D点坐标。
IMU
用B表示IMU坐标系，即随体坐标系，用W表示世界坐标系，即固定坐标系。
IMU以固定时间间隔\(\Delta
t\)测量传感器的加速度\(\mathbf
{a}_\mathtt{B}\)和角速度\(\boldsymbol
{\omega}_\mathtt{B}\)，除了传感器噪声外，\(\mathbf {a}_\mathtt{B}\)和\(\boldsymbol
{\omega}_\mathtt{B}\)的结果还分别受到加速度计\(\mathbf {b}_\mathtt{a}\)和陀螺仪\(\boldsymbol
{b}_\omega\)缓慢变化的偏差(零偏)影响。此外，加速度计受到重力加速度\(\mathbf
{g}_\mathtt{w}\)的影响，需要在测量中减去。
在W坐标系下，IMU的旋转\(\mathbf
{R}_\mathtt{WB}\)、位置\(\mathbf
{p}_\mathtt{WB}\)和速度\(\mathbf
{v}_\mathtt{WB}\)的离散形式如下： \[
\begin{align}
\mathbf {R}^{k+1}_\mathtt {WB} = & \mathbf {R}^{k}_\mathtt {WB} \,
\text{Exp}\left(\left(\boldsymbol {\omega
}^k_\mathtt {B} - \boldsymbol {b}^k_g\right)\Delta t\right) \nonumber\\
_\mathtt {W}\mathbf {v}^{k+1}_\mathtt {B} = & {_\mathtt {W}\mathbf
{v}^{k}_\mathtt {B}} + \mathbf {g}_\mathtt {W}
\Delta t + \mathbf {R}^{k}_\mathtt {WB} \left(\boldsymbol {a}^k_\mathtt
{B} - \boldsymbol {b}^k_a\right)\Delta t
\nonumber\\
_\mathtt {W}\mathbf {p}^{k+1}_\mathtt {B} = & {_\mathtt {W}\mathbf
{p}^{k}_\mathtt {B}} + {_\mathtt {W}\mathbf
{v}^{k}_\mathtt {B}} \Delta t + \frac{1}{2}\mathbf {g}_\mathtt {W}
\Delta t^2 + \frac{1}{2} \mathbf {R}^{k}_\mathtt
{WB} \left(\boldsymbol {a}^k_\mathtt {B} - \boldsymbol
{b}^k_a\right)\Delta t^2
\end{align}
\]
两个连续关键帧之间的运动可以根据期间测量的所有预积分\(\Delta \mathbf {R}\)、\(\Delta \mathbf {v}\)、\(\Delta \mathbf {p}\)的值来定义： \[ \begin{align} \mathbf {R}^{i+1}_\mathtt {WB} = & \mathbf {R}^{i}_\mathtt {WB} \Delta \mathbf {R}_{i,i+1} \text{Exp}\left(\left(\mathbf {J}^g_{\Delta R}\mathbf {b}^i_g\right)\right) \nonumber\\ _\mathtt {W}\mathbf {v}^{i+1}_\mathtt {B} = & {_\mathtt {W}\mathbf {v}^{i}_\mathtt {B}} + \mathbf {g}_\mathtt {W} \Delta t_{i,i+1} \nonumber + \mathbf {R}^{i}_\mathtt {WB} \left(\Delta \mathbf {v}_{i,i+1} + \mathbf {J}^g_{\Delta v} \mathbf {b}^i_g + \mathbf {J}^a_{\Delta v} \mathbf {b}^i_a\right) \nonumber\\ _\mathtt {W}\mathbf {p}^{i+1}_\mathtt {B} = & {_\mathtt {W}\mathbf {p}^{i}_\mathtt {B}} + {_\mathtt {W}\mathbf {v}^{i}_\mathtt {B}} \Delta t_{i,i+1} + \frac{1}{2}\mathbf {g}_\mathtt {W} \Delta t^2_{i,i+1} \nonumber + \mathbf {R}^{i}_\mathtt {WB} \left(\Delta \mathbf {p}_{i,i+1} + \mathbf {J}^g_{\Delta p} \mathbf {b}^i_g + \mathbf {J}^a_{\Delta p} \mathbf {b}^i_a\right) \end{align} \]
雅可比矩阵\(J_a(\cdot)\)和\(J_g(\cdot)\)用于对改变偏差的效果进行一阶近似，而无需显式重新计算预积分。预积分和雅可比矩阵都可以在IMU测量值到达时有效地迭代计算。
在上述公式中，\(\mathbf {J}^g_{\Delta R}\)、\(\mathbf {J}^g_{\Delta v}\)、\(\mathbf {J}^g_{\Delta p}\)、\(\mathbf {J}^a_{\Delta v}\)和\(\mathbf {J}^a_{\Delta p}\)是雅可比矩阵，它们描述了陀螺仪偏差\(\mathbf {b}^i_g\)和加速度计偏差\(\mathbf {b}^i_a\)对预积分的影响。
- 对于\(\mathbf {J}^g_{\Delta R}\)，它是旋转预积分\(\Delta \mathbf {R}_{i,i+1}\)关于陀螺仪偏差\(\mathbf {b}^i_g\)的雅可比矩阵。
- 对于\(\mathbf {J}^g_{\Delta v}\)和\(\mathbf {J}^g_{\Delta p}\)，它们是速度预积分\(\Delta \mathbf {v}_{i,i+1}\)和位置预积分\(\Delta \mathbf {p}_{i,i+1}\)关于陀螺仪偏差\(\mathbf {b}^i_g\)的雅可比矩阵。
- 对于\(\mathbf {J}^a_{\Delta v}\)和\(\mathbf {J}^a_{\Delta p}\)，它们是速度预积分\(\Delta \mathbf {v}_{i,i+1}\)和位置预积分\(\Delta \mathbf {p}_{i,i+1}\)关于加速度计偏差\(\mathbf {b}^i_a\)的雅可比矩阵。
这些雅可比矩阵可以在IMU测量值到达时迭代计算，可以有效地处理偏差的变化，而无需重新计算所有的预积分。
相机和IMU被认为是刚性连接的，无论IMU如何移动或旋转，相机和IMU之间的相对位置和方向始终保持不变。它们的参考系统之间的转换\(T_{CB}=[R_{CB}|c{P_B}]\)是已知的，可以通过校准得到。
\(T_{CB}\)表示从相机坐标系(C)到IMU坐标系(B)的转换矩阵，\(R_{CB}\)表示从相机坐标系到IMU坐标系的旋转矩阵，\(c{P_B}\)表示从相机坐标系到IMU坐标系的平移向量。
DVL
DVL 因子的残差项和协方差相关的推导
IMU在B坐标系内的角速度和线性加速度的测量值分别表示为： \[ \begin{align*} \mathtt {_{B}}\tilde{\boldsymbol{\omega }}_\mathtt {WB}(t) = & \mathtt {_{B}}\boldsymbol{\omega }_\mathtt {WB}(t) + \mathbf {b}_{}^{\omega }(t) + \boldsymbol{\eta }_{}^{\omega }(t) \tag{3.1} \\ \mathtt {_{B}}\tilde{\mathbf {a}}(t) = & \mathbf {R}_{\mathtt {WB}}^{\boldsymbol{\top }}(t)(\mathtt {_{W}}\mathbf {a}(t)-\mathtt {_{W}}\mathbf {g}) + \mathbf {b}_{}^{a}(t) + \boldsymbol{\eta }_{}^{a}(t) \tag{3.2} \end{align*} \] > 其中，\(\mathtt {_{B}}\boldsymbol{\omega }_\mathtt {WB}(t)\)和\(\mathtt {_{B}}\mathbf {a}(t)\)是IMU在B坐标系内的角速度和线性加速度的真实值，\(\mathbf {b}_{}^{\omega }(t)\)和\(\mathbf {b}_{}^{a}(t)\)是角速度和线性加速度的偏差，\(\boldsymbol{\eta }_{}^{\omega }(t)\)和\(\boldsymbol{\eta }_{}^{a}(t)\)是角速度和线性加速度的噪声，\(\mathbf {R}_{\mathtt {WB}}^{\boldsymbol{\top }}(t)\)是IMU在W坐标系内的旋转矩阵的转置，\(\mathtt {_{W}}\mathbf {a}(t)\)是IMU在W坐标系内的线性加速度的真实值，\(\mathtt {_{W}}\mathbf {g}\)是重力加速度。
DVL在帧\(B_0\)内的线速度测量值定义为：
\[ \begin{align*} \mathtt {_{B_{o}}}\tilde{\mathbf {v}}_{}(t) = s_{d} \left(\mathtt {_{B_{o}}}\mathbf {v}_{}(t) \right) + \mathbf {b}_{}^{v}(t) + \boldsymbol{\eta }_{}^{v}(t) \tag{8} \end{align*} \tag{3.3} \]
其中，\(s_{d}(\cdot)\)是考虑环境因素的比例因子，例如水和水生地形条件下声速的变化，\(\mathbf {b}_{}^{v}(t)\)是线速度的偏差，\(\boldsymbol{\eta }_{}^{v}(t)\)是线速度的噪声 \(\mathbf {b}_{}^{v}(t)\)在时间间隔\([t_{i},t_{j}]\)内满足： \[\begin{align*}\mathbf {b}_{j}^{\nu } = \mathbf {b}_{i}^{\nu } + \boldsymbol{\eta }_{}^{b\nu d} \end{align*}\] 这里，$b^{}_i $ 是在时间 \(t_i\) 的线速度偏差，\(\eta^{b\nu d}\) 是具有协方差 \(\Sigma_{b\nu ij} = \Delta t_{ij} \text{Cov}(\eta_{b\nu d})\) 的离散高斯白噪声。\(\Delta t_{ij}\) 是时间间隔，\(\text{Cov}(\eta^{b\nu d})\) 是噪声 \(\eta^{b\nu d}\) 的协方差。
离散化
由于IMU和DVL的测量实际上是在离散时间内获得的，因此需要将连续时间的运动模型离散化。 \[ \begin{align*} \tilde{\boldsymbol{\omega }}_{k} = & \boldsymbol{\omega }_{k} + \mathbf {b}_{k}^{\omega } + \boldsymbol{\eta }_{k}^{\omega } \tag{3.4}\\ \\ \tilde{\mathbf {v}}_{k} = & s_{d} \left(\mathbf {v}_{k} \right) + \mathbf {b}_{k}^{v} + \boldsymbol{\eta }_{k}^{v}. \tag{3.5} \end{align*} \]
预积分位置测量值\(\Delta \mathbf {p}_{ij}\)和噪声\(\delta \mathbf {p}_{ij}\)的定义如下： \[ \begin{align*} \Delta \tilde{\mathbf {p}}_{ij} \triangleq & \sum _{k=i}^{j-1} \left[ \Delta \tilde{\mathbf {R}}_{ik}^{} \left(\frac{ \tilde{\mathbf {v}}_{k} - \mathbf {b}_{i}^{v} }{ s_{d} } \right) \Delta t \right] \tag{3.6} \\ \delta \mathbf {p}_{ij} \triangleq & \sum _{k=i}^{j-1} \left[ \Delta \tilde{\mathbf {R}}_{ik}^{} \frac{\boldsymbol{\eta }_{k}^{v}}{s_{d}} \Delta t - \Delta \tilde{\mathbf {R}}_{ik}^{} \left(\frac{ \tilde{\mathbf {v}}_{k} - \mathbf {b}_{i}^{v} }{ s_{d} } \right)^{\wedge } \delta \boldsymbol{\phi }_{ik} \Delta t \right] \tag{3.7} \end{align*} \] >详细见VILENS: Visual, Inertial, Lidar, and Leg Odometry for All-Terrain Legged Robots SECTION IV B-3
增量预积分旋转测量值\(\Delta \tilde{\mathbf {R}}_{ij}\)和噪声\(\delta \boldsymbol{\phi }_{ik}\)的定义如下： \[ \begin{align*} \Delta \tilde{\mathbf {R}}_{ik}^{} \triangleq & \prod _{k=i}^{k-1} \text{Exp} \left( \left(\tilde{\boldsymbol{\omega }}_{k} - \mathbf {b}_{i}^{\omega }\right) \Delta t\right) \tag{3.8} \\ \delta \boldsymbol{\phi }_{ik} \triangleq & \sum _{k=i}^{k-1} \Delta \tilde{\mathbf {R}}_{k+1,k}^{\boldsymbol{\top }} \mathbf {J}_{r}^{k} \boldsymbol{\eta }_{k}^{\omega } \Delta t \tag{3.9} \end{align*} \]
这里，\(J^{k}_r\) 是 \(SO(3)\) 的右雅可比矩阵，满足 \(J_{kr} \triangleq J_{kr}((\tilde{\omega}_k - b^{\omega}_i)\Delta t)\)。相对位置的残差误差及其相关协方差分别由公式 (3.10)和公式（3.11）给出。使用公式 (3.10)，DVL因子残差被形式化为 \(r_{Dij} \triangleq [r_{\Delta \tilde{p}_{ij}}^T]\)，其中因子中排除了旋转残差，以避免对 IMU 信号的重复计数。注意，\(r_{Dij}\) 是在 \(b_{\omega}\) 和 \(b_v\) 上评估的，并且可以使用第一阶近似 \(\Delta \tilde{p}_{ij}(b_{\omega i}, b_{v i})\) 更新。
\[ \begin{align*} \mathbf {r}_{\Delta \tilde{\mathbf {p}}_{ij}} = & \mathbf {R}_{i}^{\boldsymbol{\top }}(\mathbf {p}_{j}-\mathbf {p}_{i}) - \Delta \tilde{\mathbf {p}}_{ij} \left(\mathbf {b}_{i}^{\omega }, \mathbf {b}_{i}^{v}\right) \tag{3.10} \\ \boldsymbol{\Sigma }_{i,k+1}^{\mathcal {D}_{}} = & \mathbf {A} \boldsymbol{\Sigma }_{i,k}^{\mathcal {D}_{}} \mathbf {A}^{\boldsymbol{\top }} + \mathbf {B} \boldsymbol{\Sigma }_{\boldsymbol{\eta }_{}^{}}^{\mathcal {D}_{}} \mathbf {B}^{\boldsymbol{\top }} \tag{3.11} \end{align*} \]
矩阵\(\mathbf {A}\)和\(\mathbf {B}\)在下文推导
### 小偏差更新下相对位置的一阶近似
给定偏差更新\(\mathbf {b}_{}^{} \xleftarrow {} \bar{\mathbf {b}}_{}^{} + \delta \mathbf {b}_{}^{}\)，其中\(\delta \mathbf {b}_{}^{}\)相对较小，通过取一阶泰勒级数展开式: \[ \begin{align*} \Delta \tilde{\mathbf {p}}_{ij}\left(\mathbf {b}_{i}^{\omega }, \mathbf {b}_{i}^{v}\right) \!\simeq\! \Delta \tilde{\mathbf {p}}_{ij}\left(\bar{\mathbf {b}}_{i}^{\omega }, \bar{\mathbf {b}}_{i}^{v}\right) \!+\! \frac{\partial \Delta \bar{\mathbf {p}}_{ij}}{\partial \mathbf {b}_{}^{\omega }} \delta \mathbf {b}_{i}^{\omega } \!+\! \frac{\partial \Delta \bar{\mathbf {p}}_{ij}}{\partial \mathbf {b}_{}^{v}} \delta \mathbf {b}_{i}^{v} \tag{3.12} \end{align*} \]
对于\(\Delta \tilde{\mathbf {p}}_{ij}\left(\mathbf {b}_{i}^{\omega }, \mathbf {b}_{i}^{v}\right)\)的一阶近似，由公式 (3.6)通过以下推导得到： \[ \begin{align*} \Delta \tilde{\mathbf {p}}_{ij} (\hat{\mathbf {b}}_{i}^{}) = & \sum _{k=i}^{j-1} \left[ \Delta \tilde{\mathbf {R}}_{ik}^{} (\hat{\mathbf {b}}_{i}^{}) \left(\frac{ \tilde{\mathbf {v}}_{k} - \bar{\mathbf {b}}_{i}^{v} - \delta \mathbf {b}_{i}^{v} }{s_{d}} \right) \Delta t \right] \\ = & \sum _{k=i}^{j-1} \left[ \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \text{Exp} \left(\frac{\partial \Delta \bar{\mathbf {R}}_{ik}^{}}{\partial \mathbf {b}_{}^{\omega }} \delta \mathbf {b}_{i}^{\omega } \right) \times \left(\frac{ \tilde{\mathbf {v}}_{k} - \bar{\mathbf {b}}_{i}^{v} - \delta \mathbf {b}_{i}^{v} }{s_{d}} \right) \Delta t \right] \\ \simeq & \sum _{k=i}^{j-1} \left[ \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \left(\mathbf {I} + \left(\frac{\partial \Delta \bar{\mathbf {R}}_{ik}^{}}{\partial \mathbf {b}_{}^{\omega }} \delta \mathbf {b}_{i}^{\omega } \right)^{\wedge } \right) \times \left(\frac{ \tilde{\mathbf {v}}_{k} - \bar{\mathbf {b}}_{i}^{v} - \delta \mathbf {b}_{i}^{v} }{s_{d}} \right) \Delta t \right] \\ = & \Delta \tilde{\mathbf {p}}_{ij} (\bar{\mathbf {b}}_{i}^{}) +\: \sum _{k=i}^{j-1} \left[ - \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \frac{ \delta \mathbf {b}_{i}^{v} }{s_{d}} \Delta t \right] +\: \sum _{k=i}^{j-1} \left[ \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \left(\frac{\partial \Delta \bar{\mathbf {R}}_{ik}^{}}{\partial \mathbf {b}_{}^{\omega }} \delta \mathbf {b}_{i}^{\omega } \right)^{\wedge } \times \left(\frac{ \tilde{\mathbf {v}}_{k} - \bar{\mathbf {b}}_{i}^{v} }{s_{d}} \right) \Delta t \right] \\ = & \Delta \tilde{\mathbf {p}}_{ij} (\bar{\mathbf {b}}_{i}^{}) +\: \sum _{k=i}^{j-1} \left[ - \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \frac{ \Delta t }{s_{d}} \delta \mathbf {b}_{i}^{v} \right] +\: \sum _{k=i}^{j-1} \left[ - \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \left(\frac{ \tilde{\mathbf {v}}_{k} - \bar{\mathbf {b}}_{i}^{v} }{s_{d}} \right)^{\wedge } \times \frac{\partial \Delta \bar{\mathbf {R}}_{ik}^{}}{\partial \mathbf {b}_{}^{\omega }} \Delta t \delta \mathbf {b}_{i}^{\omega } \right] \tag{3.13} \end{align*} \] 因此，可以得到： \[ \begin{align*} \frac{\partial \Delta \bar{\mathbf {p}}_{ij}}{\partial \mathbf {b}_{}^{\omega }} = & - \sum _{k=i}^{j-1} \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \left(\frac{ \tilde{\mathbf {v}}_{k} - \bar{\mathbf {b}}_{i}^{v} }{ s_{d} } \right)^{\wedge } \frac{\partial \Delta \bar{\mathbf {R}}_{ik}^{}}{\partial \mathbf {b}_{}^{\omega }} \Delta t \\ \frac{\partial \Delta \bar{\mathbf {p}}_{ij}}{\partial \mathbf {b}_{}^{v}} = & - \sum _{k=i}^{j-1} \Delta \tilde{\mathbf {R}}_{ik}^{} (\bar{\mathbf {b}}_{i}^{}) \frac{ \Delta t }{ s_{d} } \end{align*} \]
由公式 (3.7)，考虑从\(i\)到\(k+1\)区间，可以得到： \[ \begin{align*} \delta \mathbf {p}_{i,k+1} \simeq & \delta \mathbf {p}_{ik} + \Delta \tilde{\mathbf {R}}_{ik}^{} \boldsymbol{\eta }_{k}^{v} \Delta t -\: \tilde{\mathbf {R}}_{ik}^{} \left(\frac{ \tilde{\mathbf {v}}_{k} - \mathbf {b}_{i}^{v} }{ s_{d} } \right)^{\wedge } \delta \boldsymbol{\phi }_{ik} \Delta t. \tag{3.14} \end{align*} \] 进一步用矩阵形式表示： \[ \begin{align*} \begin{bmatrix}\delta \boldsymbol{\phi }_{i,k+1} \\ \delta \mathbf {p}_{i,k+1} \end{bmatrix} = & \begin{bmatrix}\Delta \tilde{\mathbf {R}}_{k,k+1}^{\boldsymbol{\top }} & \boldsymbol{O}_{3 \times 3} \\ -\tilde{\mathbf {R}}_{ik}^{} (\frac{1}{ s_{d} } (\tilde{\mathbf {v}}_{k} - \mathbf {b}_{i}^{v}))^{\wedge } \Delta t & \boldsymbol{I}_{3 \times 3} \end{bmatrix} \begin{bmatrix}\delta \boldsymbol{\phi }_{ik} \\ \delta \mathbf {p}_{ik} \end{bmatrix} +\: \begin{bmatrix}\mathbf {J}_{r}^{k} \Delta t & \boldsymbol{O}_{3 \times 3} \\ \boldsymbol{O}_{3 \times 3} & \Delta \tilde{\mathbf {R}}_{ik}^{} \Delta t \end{bmatrix} \begin{bmatrix}\boldsymbol{\eta }_{k}^{\omega } \\ \boldsymbol{\eta }_{k}^{v} \end{bmatrix} \\ = & \mathbf {A}\ \begin{bmatrix}\delta \boldsymbol{\phi }_{ik} \\ \delta \mathbf {p}_{ik} \end{bmatrix} + \mathbf {B} \begin{bmatrix}\boldsymbol{\eta }_{k}^{\omega } \\ \boldsymbol{\eta }_{k}^{v} \end{bmatrix} \end{align*} \]