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]$$