IMU预积分推导（二）

IMU预积分推导系列文章
IMU预积分推导（一）
IMU预积分推导（二）
IMU预积分推导（三）

4、零偏更新对预积分观测量的影响

在前面的噪声分离和误差传播过程中，假设了第$i$时刻陀螺仪零偏${\mathbf{b}}_{g,i}$加速度计零偏${\mathbf{b}}_{a,i}$是固定的，但实际上零偏作为优化变量是不断更新的。第$i$时刻，假设更新前加速度计和陀螺仪零偏的值分别为${\mathbf{b}}_{g,i}$和${\mathbf{b}}_{a,i}$，更新小量分别用$\delta {\mathbf{b}}_{a,i}$和$\delta {\mathbf{b}}_{g,i}$表示，更新后的零偏仍用${\mathbf{b}}_{g,i}$和${\mathbf{b}}_{a,i}$表示
$$\begin{gathered} {\mathbf{b}}_{g,i}\leftarrow {\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i} \\ {\mathbf{b}}_{a,i}\leftarrow {\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i} \end{gathered}$$
理论上，零偏更新后，预积分观测量应该按$(3)$重新计算，才能得到预积分测量值的修正$\Delta {\tilde{\mathbf{R}}}_{ij},\Delta {\tilde{\mathbf{v}}}_{ij},\Delta {\tilde{\mathbf{p}}}_{ij}$。但这种方式计算代价过大，可以将预积分观测量看成零偏的函数，当零偏更新之后，通过一阶泰勒展开在原先预积分观测量的基础上进行修正，即
$$\begin{array}{rl}\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i})& =\mathrm{Exp}(\mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i})))\\ & \approx \mathrm{Exp}(\mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}))+{\frac{\partial \mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}))}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i})\\ & =\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i})\mathrm{Exp}({\mathbf{J}}_r(\mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}))){\frac{\partial \mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}))}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i})\\ & =\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i})\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)\end{array}\text{\hspace{1em}(17a)}$$
为简便起见，此处记$\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}={\mathbf{J}}_r(\mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}))){\frac{\partial \mathrm{Log}(\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}))}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i}}$
$$\begin{array}{rl}\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})& \approx \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+{\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{g,i}+{\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{a,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\\ & =\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\\ \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})& \approx \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+{\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{g,i}+{\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{a,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\\ & =\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\end{array}\text{\hspace{1em}(17b)(17c)}$$
为简便起见，此处记

$\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}={\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}},\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}={\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{a,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}}$，

$\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}={\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{g,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}},\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}={\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})}{\partial {\mathbf{b}}_{a,i}}|}_{{\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}}$。

$\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}},\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}},\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}},\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}},\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}$都称为雅可比矩阵，注意其中预积分旋转测量值对零偏的雅可比矩阵并不是由偏导数组成的数学上的雅可比矩阵。

下面通过展开并取对应项的方法求解上述各雅可比矩阵

4.1、预积分旋转测量值对零偏的雅可比矩阵
$$\begin{array}{rl}\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i})& =\prod _{k=i}^{j-1}\mathrm{Exp}(({\tilde{\mathbf{\omega }}}_k-({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i}))\Delta t)\\ & =\Delta {\tilde{\mathbf{R}}}_{ij}\prod _{k=i}^{j-1}\mathrm{Exp}(-\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}\delta {\mathbf{b}}_{g,i}\Delta t)\\ & \approx \Delta {\tilde{\mathbf{R}}}_{ij}\mathrm{Exp}(-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}\delta {\mathbf{b}}_{g,i}\Delta t)\end{array}\text{\hspace{1em}(18)}$$
其中第一行推导至第二行参考了$(5)$，第二行推导至第三行参考了$(11)$的推导过程

比较$(17)$和$(2i_0)$，可以得到
$$\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}=-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{k+1,j}^T{\mathbf{J}}_{r,k}\Delta t\text{\hspace{1em}(19)}$$
4.2、预积分速度测量值对零偏的雅可比矩阵
$$\begin{array}{rl}\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})& =\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{g,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-\delta {\mathbf{b}}_{a,i})\Delta t\\ & \approx \sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{a,i})\mathrm{Exp}(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-\delta {\mathbf{b}}_{a,i})\Delta t\\ & \approx \sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{a,i})\left(\mathbf{I}+{\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)}^{\wedge }\right)({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-\delta {\mathbf{b}}_{a,i})\Delta t\\ & =\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{a,i},{\mathbf{b}}_{a,i})-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{a,i})\delta {\mathbf{b}}_{a,i}\Delta t-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{a,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\Delta t\end{array}\text{\hspace{1em}(20)}$$
从第三行推导至第四行舍去了高阶小项${\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)}^{\wedge }\delta {\mathbf{b}}_{a,i}$

比较$(18)$和$(22)$，可以得到
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& =-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{g,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\Delta t\\ \frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}& =-\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{g,i})\Delta t\end{array}\text{\hspace{1em}(21)(22)}$$
4.3、预积分位移测量值对零偏的雅可比矩阵
$$\begin{array}{rl}\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})& =\sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-\delta {\mathbf{b}}_{a,i})\Delta t^2\right]\\ & \approx \sum _{k=i}^{j-1}\left[\left(\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}\right)\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{g,i})\left(\mathbf{I}+{\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right)}^{\wedge }\right)({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}-\delta {\mathbf{b}}_{a,i})\Delta t^2\right]\\ & \approx \Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\right]\delta {\mathbf{b}}_{a,i}+\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{g,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\Delta t^2\right]\delta {\mathbf{b}}_{g,i}\end{array}\text{\hspace{1em}(23)}$$
其中，第二行推导至第三行与$(22)$推导过程类似，省略了高阶小项

比较$(19)$和$(25)$，可以得到
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& =\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\mathbf{b}}_{g,i})({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}_{g,i}}\Delta t^2\right]\\ \frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}& =\sum _{k=i}^{j-1}\left[\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\right]\end{array}\text{\hspace{1em}(24)(25)}$$
4.4、将雅可比矩阵归结至递推形式

$(21),(23),(24),(26),(27)$中的雅可比矩阵是累加形式的，若$j>i$，可以参考把它们归结至递推形式
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& =\Delta {\tilde{\mathbf{R}}}_{j-1,j}^T\frac{\partial \Delta {\tilde{\mathbf{R}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}-{\mathbf{J}}_{r,j-1}\Delta t\\ \frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& =\frac{\partial \Delta {\tilde{\mathbf{v}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}-\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}\Delta t\\ \frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}& =\frac{\partial \Delta {\tilde{\mathbf{v}}}_{i,j-1}}{\partial {\mathbf{b}}_{a,i}}-\Delta {\tilde{\mathbf{R}}}_{i,j-1}\Delta t\\ \frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}& =\frac{\partial \Delta {\tilde{\mathbf{p}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{i,j-1}}{\partial {\mathbf{b}}_{a,i}}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_{a,i}{)}^{\wedge }\frac{\partial \Delta {\tilde{\mathbf{R}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}\Delta t^2\\ \frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}& =\frac{\partial \Delta {\tilde{\mathbf{p}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}-\frac{\partial \Delta {\tilde{\mathbf{v}}}_{i,j-1}}{\partial {\mathbf{b}}_{g,i}}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{i,j-1}\Delta t^2\end{array}\text{\hspace{1em}(26a)(26b)(26c)(26d)(26e)}$$

5、预积分的优化目标

设$X_k$表示直到$k$时刻的全部关键帧，即
X k = { x i } i ∈ K k X_{k}=\{\mathbf{x}_{i}\}_{i\in{K}_{k}} Xk​={xi​}i∈Kk​​
其中 K k = { i 0 , i 1 , ⋯   } K_{k}=\{i_{0},i_{1},\cdots\} Kk​={i0​,i1​,⋯}表示直到$k$时刻包含关键帧的全部时刻

$I_{ij}$表示两个连续的关键帧，即第$i$时刻关键帧和第$j$时刻关键帧之间的IMU测量，$O_i$表示GNSS等低频传感器第$i$时刻的观测，故直到$k$时刻的全部测量值组成的集合为
Z k = { I i j , O i } i , j ∈ K k Z_{k}=\{I_{ij},O_{i}\}_{i,j\in{K}_{k}} Zk​={Iij​,Oi​}i,j∈Kk​​
给定$Z_k$和先验概率$p(X_{i_0})$的条件下，$X_k$的后验概率可以表示为
$$\begin{array}{rl}p(X_k|Z_K)& \propto p(X_k)p(Z_k|X_k)\\ & =p({\mathbf{x}}_{i_0})\prod _{i,j\in K_k}p({\mathbf{b}}_j|{\mathbf{b}}_i)\prod _{i,j\in K_k}p(I_{ij}|{\mathbf{x}}_i,{\mathbf{x}}_j)\prod _{i\in K_k}p(O_i|{\mathbf{x}}_i)\end{array}\text{\hspace{1em}(27)}$$
优化目标是最大化$(27)$式的后验概率
$$X_k^{\ast }=\underset{X_k}{\mathrm{argmax}}p(X_k|Z_k)\text{\hspace{1em}(28)}$$
最大化后验概率等价于最小化后验概率的负对数，因此可将$(28)$写成
$$X_k^{\ast }=\underset{X_k}{\mathrm{argmin}}-\log p(X_k|Z_k)\text{\hspace{1em}(29)}$$
不妨$(27)$中各概率服从高斯分布，$p({\mathbf{x}}_{i_0})\propto \exp \left\{-\frac{1}{2}\Vert {\mathbf{r}}_{{\mathbf{x}}_{i_0}}{\Vert }_{\mathbf{\Sigma }({\mathbf{x}}_{i_0})}^2\right\}$，$p({\mathbf{b}}_j|{\mathbf{b}}_i)\propto \exp \left\{-\frac{1}{2}\Vert {\mathbf{r}}_{{\mathbf{b}}_j}{\Vert }_{\mathbf{\Sigma }({\mathbf{b}}_j)}^2\right\}$，$p(I_{ij}|{\mathbf{x}}_i,{\mathbf{x}}_j)\propto \exp \left\{-\frac{1}{2}\Vert {\mathbf{r}}_{\Delta {\mathbf{x}}_{ij}}{\Vert }_{\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{ii}))}^2\right\}$，$p(O_i|{\mathbf{x}}_i)\propto \exp \left\{-\frac{1}{2}\Vert {\mathbf{r}}_{{\mathbf{z}}_i}{\Vert }_{\mathbf{\Sigma }({\mathbf{z}}_i)}^2\right\}$，此时$(29)$可写成
$$\begin{array}{r}{X}_k^{\ast }=\underset{X_k}{\mathrm{argmin}}\left(\Vert {\mathbf{r}}_{{\mathbf{x}}_{i_0}}{\Vert }_{\mathbf{\Sigma }({\mathbf{x}}_{i_0})}^2+\sum _{i,j\in K_k}\Vert {\mathbf{r}}_{{\mathbf{b}}_j}{\Vert }_{\mathbf{\Sigma }({\mathbf{b}}_j)}^2+\sum _{i,j\in K_k}\Vert {\mathbf{r}}_{\Delta {\mathbf{x}}_{ij}}{\Vert }_{\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{ii}))}^2+\sum _{i\in K_k}\Vert {\mathbf{r}}_{{\mathbf{z}}_i}{\Vert }_{\mathbf{\Sigma }({\mathbf{z}}_i)}^2\right)\end{array}\text{\hspace{1em}(30)}$$
其中${\mathbf{r}}_{{\mathbf{x}}_{i_0}},{\mathbf{r}}_{{\mathbf{b}}_j},{\mathbf{r}}_{\Delta {\mathbf{x}}_{ij}},{\mathbf{r}}_{{\mathbf{z}}_i}$分别称为先验残差，递推残差，预积分残差，第$i$时刻的观测残差

下面讨论各残差的具体形式：

先验残差${\mathbf{r}}_{{\mathbf{x}}_{i_0}}=[{\mathbf{r}}_{{\mathbf{R}}_{i_0}}^T,{\mathbf{r}}_{{\mathbf{v}}_{i_0}}^T,{\mathbf{r}}_{{\mathbf{p}}_0}^T,{\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}^T,{\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}^T{]}^T$，其中
$$\begin{array}{rl}{\mathbf{r}}_{{\mathbf{R}}_{i_0}}& =\mathrm{Log}({\check{\mathbf{R}}}_{i_0}^T{\mathbf{R}}_{i_0})\\ {\mathbf{r}}_{{\mathbf{v}}_{i_0}}& ={\mathbf{v}}_{i_0}-{\check{\mathbf{v}}}_{i_0}\\ {\mathbf{r}}_{{\mathbf{p}}_{i_0}}& ={\mathbf{p}}_{i_0}-{\check{\mathbf{p}}}_{i_0}\\ {\mathbf{r}}_{{\mathbf{b}}_{g,i_0}}& ={\mathbf{b}}_{g,i_0}-{\check{\mathbf{b}}}_{g,i_0}\\ {\mathbf{r}}_{{\mathbf{b}}_{a,i_0}}& ={\mathbf{b}}_{a,i_0}-{\check{\mathbf{b}}}_{a,i_0}\end{array}\text{\hspace{1em}(31a)(31b)(31c)(31d)(31e)}$$
分别称为先验旋转残差，先验速度残差，先验位移残差，先验陀螺仪零偏残差，先验加速度计零偏残差。$[{\check{\mathbf{R}}}_{i_0},{\check{\mathbf{v}}}_{i_0},{\check{\mathbf{p}}}_{i_0},{\check{\mathbf{b}}}_{g,i_0},{\check{\mathbf{b}}}_{a,i_0}]$为先验，是已知的或上次优化的结果。暂不讨论$\mathbf{\Sigma }({\mathbf{x}}_{i_0})$求解。

递推残差可写为${\mathbf{r}}_{{\mathbf{b}}_j}=[{\mathbf{r}}_{{\mathbf{b}}_{g,j}}^T,{\mathbf{r}}_{{\mathbf{b}}_{a,j}}^T{]}^T$，其中
$$\begin{array}{r}{\mathbf{r}}_{{\mathbf{b}}_{g,j}}={\mathbf{b}}_{g,j}-{\mathbf{b}}_{g,i}\\ {\mathbf{r}}_{{\mathbf{b}}_{a,j}}={\mathbf{b}}_{a,j}-{\mathbf{b}}_{a,i}\end{array}\text{\hspace{1em}(32a)(32b)}$$
分别称为递推陀螺仪零偏残差和递推加速度计零偏残差。暂不讨论$\mathbf{\Sigma }({\mathbf{b}}_j)$求解。

预积分残差${\mathbf{r}}_{\Delta {\mathbf{x}}_{ij}}=[{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}^T,{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}^T,{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}^T{]}^T$，其中
$$\begin{array}{rl}{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}& \stackrel{\mathrm{def}}{=}\mathrm{Log}\left[(\Delta {\tilde{\mathbf{R}}}_{ij}{)}^T\Delta {\mathbf{R}}_{ij}\right]\\ {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}& \stackrel{\mathrm{def}}{=}\Delta {\mathbf{v}}_{ij}-\Delta {\tilde{\mathbf{v}}}_{ij}\\ {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}& \stackrel{\mathrm{def}}{=}\Delta {\mathbf{p}}_{ij}-\Delta {\tilde{\mathbf{p}}}_{ij}\end{array}\text{\hspace{1em}(33a)(33b)(33c)}$$
分别称为预积分旋转残差，预积分速度残差，预积分位移残差。对应的协方差矩阵为通过迭代$(16)$求出的$\mathbf{\Sigma }(\delta (\Delta {\mathbf{x}}_{ii}))$

观测残差${\mathbf{r}}_{{\mathbf{z}}_i}$和$\mathbf{\Sigma }({\mathbf{z}}_i)$取决于传感器，在实验部分再进行讨论。

通过增量$\delta \mathbf{\varphi },\delta \mathbf{p},\delta \mathbf{v},\delta {\mathbf{b}}_g,\delta {\mathbf{b}}_a$可以更新系统状态$\mathbf{x}$，增量作用如下
$$\begin{array}{ll}& \mathbf{R}\leftarrow \mathbf{R}\mathrm{Exp}(\delta \mathbf{\varphi })\\ & \mathbf{p}\leftarrow \mathbf{p}+\mathbf{R}\delta \mathbf{p}\\ & \mathbf{v}\leftarrow \mathbf{v}+\delta \mathbf{v}\\ & {\mathbf{b}}_g\leftarrow {\mathbf{b}}_g+\delta {\mathbf{b}}_g\\ & {\mathbf{b}}_a\leftarrow \mathbf{b}+\delta {\mathbf{b}}_a\end{array}$$
在不考虑相关性的前提下，可以假设每个增量单独作用，即其产生作用时其他增量为零，将残差视为该增量的函数，下面通过展开求解残差相对于每个状态增量的雅可比矩阵