ORB-SLAM3中IMU理论知识总结
常见的imu为MEMS，使用硅微加工技术制造，使用科氏力。
误差模型
\[m(t)=m_t(t)+bias(t)+\epsilon(t) \\
\dot{bias}(t)=n_b(t)
\]
- 固定偏差bias，作为状态量估计
- 白噪声\(\epsilon(t)\)积分成角度随机游走，连续时间标准差\(\sigma\)（标定结果）除以\(sqrt(\delta(t))\)得到离散标准差
- bias随机游走，连续时间随机游走（标定结果）乘以\(sqrt(\delta(t))\)得到离散随机游走
预积分
第i时刻到第j时刻，根据imu的离散读数，推出角度、速度和位置的变化。
噪声分离
根据imu的噪声，推出预积分的噪声，进而得到预积分的信息矩阵（协方差的逆）
预积分真值 = 测量值 - 误差
预积分误差
imu的噪声->预积分噪声
噪声递推
根据上一时刻的预积分误差递推下一时刻预积分误差，从而获取协方差递推
imu误差
预积分误差
预积分递推
协方差矩阵递推
bais更新->预积分更新
新的bias：\(\hat{\mathbf{b}}_{i}^{g}\) \(\hat{\mathbf{b}}_{a}^{a}\) 由旧的bias：\(\overline{\mathbf{b}}_{i}^{g}\) \(\overline{\mathbf{b}}_{i}^{a}\)和变化量\(\delta\mathbf{b}_{i}^{g}\)和\(\delta\mathbf{b}_{i}^{a}\)相加得到：即
\[\hat{\mathbf{b}}_{i}^{g}\leftarrow\overline{\mathbf{b}}_{i}^{g}+\delta\mathbf{b}_{i}^{g}\\
\hat{\mathbf{b}}_{i}^{a}\leftarrow\overline{\mathbf{b}}_{i}^{a}+\delta\mathbf{b}_{i}^{a}
\]
预积分关于bais变化的一阶近似更新公式
\[\tilde{\mathbf{R}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}\right) \approx \Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \\
\tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \approx \Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \\
\tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \approx \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}
\]
偏导项
\[\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_{k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \Delta t\right) \\
\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}=-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \\
\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}}=-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \Delta t\right) \\
\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \\
\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}}=\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right]\\
其中\mathbf{J}_{r}^{k}=\mathbf{J}_{r}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right)
\]
残差构建
状态量
\[\mathbf{R}_{i}, \mathbf{p}_{i}, \mathbf{v}_{i}, \mathbf{R}_{j}, \mathbf{p}_{j}, \mathbf{v}_{j}, \delta \mathbf{b}_{i}^{g}, \delta \mathbf{b}_{i}^{a}
\]
残差
\[\mathbf{r}_{\Delta R_{i j}} \triangleq \log \left\{\left[\tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{wi}^{T} \mathbf{R}_{wj}\right\} \\
\triangleq \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \Delta \mathbf{R}_{i j}\right] \\
\mathbf{r}_{\Delta v_{i j}} \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\
\triangleq \Delta \mathbf{v}_{i j}-\Delta \hat{\mathbf{v}}_{i j} \\
\mathbf{r}_{\Delta \mathbf{p}_{i j}} \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\
\triangleq \Delta \mathbf{p}_{i j}-\Delta \hat{\mathbf{p}}_{i j}
\]
状态更新
\[\mathbf{R}_{wi} \leftarrow \mathbf{R}_{wi} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right) \\
\mathbf{p}_{wi} \leftarrow \mathbf{p}_{wi}+\mathbf{R}_{wi} \cdot \delta \mathbf{p}_{wi}\\
\mathbf{v}_{i} \leftarrow \mathbf{v}_{i}+\delta \mathbf{v}_{i} \\
\mathbf{R}_{wj} \leftarrow \mathbf{R}_{wj} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\\
\mathbf{p}_{wj} \leftarrow \mathbf{p}_{wj}+\mathbf{R}_{wj} \cdot \delta \mathbf{p}_{wj}\\
\mathbf{v}_{j} \leftarrow \mathbf{v}_{j}+\delta \mathbf{v}_{j} \\
\delta \mathbf{b}_{i}^{g} \leftarrow \delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}}\\
\delta \mathbf{b}_{i}^{a} \leftarrow \delta \mathbf{b}_{i}^{a}+\widetilde{\delta \mathbf{b}_{i}^{a}}
\]
雅可比
旋转残差
对于\(\mathbf{R}_{wi}\)
\[\frac{\partial \mathbf{r}_{\Delta \vec{\phi}_{i j}}}{\partial \delta \vec{\phi}_{i}}=-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \vec{\phi}_{i j}}\right) \mathbf{R}_{w j}^{T} \mathbf{R}_{w i}
\]
对于\(\mathbf{R}_{wj}\)
\[\frac{\partial \mathbf{r}_{\Delta \vec{\phi}_{i j}}}{\partial \delta \vec{\phi}_{j}}=\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \vec{\phi}_{i j}}\right)
\]
对于\(\tilde{\mathbf{b}}_{i}^{g}\)
\[\frac{\partial \mathbf{r}_{\Delta \vec{\varphi}_{i j}}}{\partial \delta \tilde{\mathbf{b}}_{i}^{g}}=-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \vec{\phi}_{i j}}\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_{\Delta \vec{\phi}_{i j}}\right) \cdot \mathbf{J}_{r}\left(\frac{\partial \Delta \widetilde{\mathbf{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}_{i}^{g}\right) \cdot \frac{\partial \Delta \tilde{\mathbf{R}}_{i j}}{\partial \mathbf{b}^{g}}
\]
速度残差
对于\(\delta \tilde{\mathbf{b}}_{i}^{g} \delta \tilde{\mathbf{b}}_{i}^{a}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \tilde{\mathbf{b}}_{i}^{g}}=-\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}}\\
\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \tilde{\mathbf{b}}_{i}^{a}}=-\frac{\partial \Delta \tilde{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}}
\]
对于\(\mathbf{v}_{i}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{v}_{i}}=-\mathbf{R}_{w i}^{T}
\]
对于\(\mathbf{v}_{j}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{v}_{j}}=\mathbf{R}_{w i}^{T}
\]
对于\(\mathbf{R}_{w i}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \vec{\phi}_{i}}=\left[\mathbf{R}_{w i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]
\]
位置残差
对于\(\delta \tilde{\mathbf{b}}_{i}^{g} \delta \tilde{\mathbf{b}}_{i}^{a}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \tilde{\mathbf{b}}_{i}^{g}}=-\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}}\\
\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \tilde{\mathbf{b}}_{i}^{a}}=-\frac{\partial \Delta \tilde{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{a}}
\]
对于\(\mathbf{p}_{wi}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{p}_{i}}=-I
\]
对于\(\mathbf{p}_{wj}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{p}_{j}}=\mathbf{R}_{w i}^{T} \mathbf{R}_{w j}
\]
对于\(\mathbf{v}_{i}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{v}_{i}}=-\mathbf{R}_{w i}^{T} \Delta t_{ij}
\]
对于\(\mathbf{R}_{w i}\)
\[\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \vec{\phi}_{i}}=\left[\mathbf{R}_{w i}^{T} \cdot\left(\mathbf{p}_{wj}-\mathbf{p}_{wi}- \mathbf{v}_{i} \cdot \Delta t_{i j} - \frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)\right]
\]
