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} \]