6.零偏的更新

IMU噪声的推导是假定零偏固定不变，在面对实际过程中，对于零偏的处理有一个技巧：假定零偏的变化是线性的，保留其一阶项。在原先的基础上进行修正。

数学模型并不一定是和真实世界完全对应的。有时，数学模型是对真实世界的一种简化，便于后续的算法计算。

对预积分的观测进行修正，将观测视为 $b_{g,i}$ 和 $b_{a,i}$ 的函数
$$\begin{array}{rl}\Delta {\tilde{R}}_{ij}(b_{g,i}+\delta b_{g,i})& =\Delta {\tilde{R}}_{ij}(b_{g,i})\text{Exp}\left(\frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}\right)\\ \Delta {\tilde{v}}_{ij}(b_{g,i}+\delta b_{g,i},b_{a,i}+\delta b_{a,i})& =\Delta {\tilde{v}}_{ij}(b_{g,i},b_{a,i})+\frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}+\frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}\delta b_{a,i}\\ \Delta {\tilde{p}}_{ij}(b_{g,i}+\delta b_{g,i},b_{a,i}+\delta b_{a,i})& =\Delta {\tilde{p}}_{ij}(b_{g,i},b_{a,i})+\frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}+\frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}\delta b_{a,i}\end{array}\text{\hspace{1em}(6.1)}$$

旋转观测为
$$\begin{array}{rl}\Delta {\tilde{R}}_{ij}(b_{g,i}+\delta b_{g,i})& =\prod _{k=i}^{j-1}\text{Exp}(({\tilde{\omega }}_k-(b_{g,i}+\delta b_{g,i})\Delta t)\\ & =\prod _{k=i}^{j-1}\text{Exp}(({\tilde{\omega }}_k-b_{g,i})\Delta t)\text{Exp}(-J_{r,k}\delta b_{g,i}\Delta t)\\ & =\underset{\Delta {\tilde{R}}_{i,i+1}}{\underbrace{\text{Exp}(({\tilde{\omega }}_i-b_{g,i})\Delta t)}}\text{Exp}(-J_{r,i}\delta b_{g,i}\Delta t)\underset{\Delta {\tilde{R}}_{i+1,i+2}}{\underbrace{\text{Exp}(({\tilde{\omega }}_{i+1}-b_{g,i})\Delta t)}}\text{Exp}(-J_{r,i+1}\delta b_{g,i}\Delta t)\cdots \\ & \cdots \\ & =\Delta {\tilde{R}}_{ij}\prod _{k=i}^{j-1}\text{Exp}(-\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}\delta b_{g,i}\Delta t)\\ & \approx \Delta {\tilde{R}}_{ij}\text{Exp}\left(\sum _{k=i}^{j-1}-\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}\Delta t\delta b_{g,i}\right)\end{array}\text{\hspace{1em}(6.2)}$$

根据式子(6.1)中的旋转部分，显然
$$\frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}=-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}\Delta t\text{\hspace{1em}(6.3)}$$

速度观测为
$$\begin{array}{rl}\Delta {\tilde{v}}_{ij}(b_i+\delta b_i)& =\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}(b_{g,i}+\delta b_{g,i})({\tilde{a}}_k-b_{a,i}-\delta b_{a,i})\Delta t\\ & =\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}\text{Exp}\left(\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\delta b_{g,i}\right)({\tilde{a}}_k-b_{a,i}-\delta b_{a,i})\Delta t\\ & \approx \sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}\left(I+{\left(\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\delta b_{g,i}\right)}^{\wedge }\right)({\tilde{a}}_k-b_{a,i}-\delta b_{a,i})\Delta t\\ & \approx \sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i})\Delta t-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}\delta b_{a,i}\Delta t+\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}{\left(\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\delta b_{g,i}\right)}^{\wedge }({\tilde{a}}_k-b_{a,i})\Delta t\\ & =\Delta {\tilde{v}}_{ij}-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}\Delta t\delta b_{a,i}-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\Delta t\delta b_{g,i}\\ & =\Delta {\tilde{v}}_{ij}+\frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}\delta b_{a,i}+\frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}\end{array}\text{\hspace{1em}(6.4)}$$

平移观测为
$$\begin{array}{rl}\Delta {\tilde{p}}_{ij}(b_i+\delta b_i)=& \sum _{k=i}^{j-1}\left[\Delta {\tilde{v}}_{ik}(b_{a,i}+\delta b_{a,i},b_{g,i}+\delta b_{g,i})\Delta t+\frac{1}{2}\Delta {\tilde{R}}_{ik}(b_{g,i}+\delta b_{g,i})({\tilde{a}}_k-b_{a,i}-\delta b_{a,i})\Delta t^2\right]\\ \approx & \sum _{k=i}^{j-1}\left[\left(\Delta {\tilde{v}}_{ik}+\frac{\partial \Delta {\tilde{v}}_{ik}}{\partial b_{a,i}}\delta b_{a,i}+\frac{\partial \Delta {\tilde{v}}_{ik}}{\partial b_{g,i}}\delta b_{g,i}\right)\Delta t+\frac{1}{2}\Delta {\tilde{R}}_{ik}\left(I+{\left(\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\delta b_{g,i}\right)}^{\wedge }\right)({\tilde{a}}_k-b_{a,i}-\delta b_{a,i})\Delta t^2\right]\\ \approx & \Delta {\tilde{p}}_{ij}+\sum _{k=i}^{j-1}\left[\frac{\partial {\tilde{v}}_{ik}}{\partial b_{a,i}}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{ik}\Delta t^2\right]\delta b_{a,i}+\sum _{k=i}^{j-1}\left[\frac{\partial {\tilde{v}}_{ik}}{\partial b_{g,i}}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\Delta t^2\right]\delta b_{g,i}\\ =& \Delta {\tilde{p}}_{ij}+\frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}\delta b_{a,i}+\frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}\delta b_{g,i}\end{array}\text{\hspace{1em}(6.5)}$$

将雅可比矩阵汇总，有
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}& =-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}\Delta t\\ \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}& =-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}\Delta t\\ \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}& =-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\Delta t\\ \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}& =\sum _{k=i}^{j-1}\left[\frac{\partial {\tilde{v}}_{ik}}{\partial b_{a,i}}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{ik}\Delta t^2\right]\\ \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}& =\sum _{k=i}^{j-1}\left[\frac{\partial {\tilde{v}}_{ik}}{\partial b_{g,i}}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{ik}({\tilde{a}}_k-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{ik}}{\partial b_{g,i}}\Delta t^2\right]\end{array}\text{\hspace{1em}(6.6)}$$

为了方便计算，将(6.6)中的旋转零偏雅可比化为递推形式
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}& =-\sum _{k=i}^{j-1}\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}\Delta t\\ & =-\sum _{k=i}^{j-2}\Delta {\tilde{R}}_{k+1,j}^T{J}_{r,k}\Delta t-\Delta {\tilde{R}}_{j,j}^T{J}_{r,j-1}\Delta t\\ & =-\Delta {\tilde{R}}_{j-1,j}^T\sum _{k=i}^{j-2}\Delta {\tilde{R}}_{k+1,j-1}^T{J}_{r,k}\Delta t-J_{r,j-1}\Delta t\\ & =\Delta {\tilde{R}}_{j-1,j}^T\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}-J_{r,j-1}\Delta t\end{array}$$

同理，对公式(6.6)整理为递推形式
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{R}}_{ij}}{\partial b_{g,i}}& =\Delta {\tilde{R}}_{j-1,j}^T\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}-J_{r,j-1}\Delta t\\ \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{a,i}}& =\frac{\partial \Delta {\tilde{v}}_{i,j-1}}{\partial b_{a,i}}-\Delta {\tilde{R}}_{i,j-1}\Delta t\\ \frac{\partial \Delta {\tilde{v}}_{ij}}{\partial b_{g,i}}& =\frac{\partial \Delta {\tilde{v}}_{i,j-1}}{\partial b_{g,i}}-\Delta {\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}\Delta t\\ \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{a,i}}& =\frac{\partial \Delta {\tilde{p}}_{i,j-1}}{\partial b_{a,i}}+\frac{\partial {\tilde{v}}_{i,j-1}}{\partial b_{a,i}}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{i,j-1}\Delta t^2\\ \frac{\partial \Delta {\tilde{p}}_{ij}}{\partial b_{g,i}}& =\frac{\partial \Delta {\tilde{p}}_{i,j-1}}{\partial b_{g,i}}+\frac{\partial {\tilde{v}}_{i,j-1}}{\partial b_{g,i}}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{i,j-1}({\tilde{a}}_{j-1}-b_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{R}}_{i,j-1}}{\partial b_{g,i}}\Delta t^2\end{array}\text{\hspace{1em}(6.7)}$$

---

7.预积分更新

每个时刻的状态为 $[R,p,v,b_a,b_g]$。
通过定义残差的不同，对应的雅可比也有所不同。下面给出原论文中的做法
$$\begin{array}{rl}{r}_{\Delta R_{ij}}& =\text{Log}\left(\Delta {\tilde{R}}_{ij}^T(R_i^T{R}_j)\right)\\ r_{\Delta v_{ij}}& =R_i^T(v_j-v_i-g\Delta t_{ij})-\Delta {\tilde{v}}_{ij}\\ r_{\Delta p_{ij}}& =R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2)-\Delta {\tilde{p}}_{ij}\end{array}\text{\hspace{1em}(7.1)}$$

扰动模型的雅可比简单于导数模型，根据符号的统一，选择右扰动。

KaTeX parse error: Expected '\right', got '&' at position 24: …ligned} \left[1&̲1\\ &2\\ &3\rig…