VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差，及其对状态量的雅克比矩阵、协方差递推方程的推导)

1. 前言

之前看过崔华坤的《VINS论文推导及代码解析》还有深蓝学院的VIO课程，对VINS的后端非线性优化有了较为清晰的认识，但是一直没有时间整理写成笔记，最近看到Manni的博客VINS-Mono理论学习——后端非线性优化 概括得很不错，针对这三份资料还有自己的一些理解重新整理下，感谢优秀的大佬们提供的参考资料。

2. 非线性最小二乘

尽管非线性最小二乘是很常见的问题，可参考《SLAM14讲》第六章节，《多视图几何》附录6，乔治梅森大学Timothy Sauer的《数值分析》(忘记第几章了，书也没带回家，哭.jpg)，在我之前的博客非线性最小二乘也对Guass-Newton和LM进行了介绍，这里简单回顾一下，由于VINS-Mono中在视觉重投影误差添加了鲁棒核函数，因此也整理下对 残差(也可以说是误差，一个意思，以下内容不分残差和误差) 添加鲁棒核函数对需要优化的状态量增量方程的影响。

2.1 Guass-Newton 和 Levenberg-Marquardt

对于最常见的最小二乘问题有：$$\underset{x}{\min }F(x)=\underset{x}{\min }\frac{1}{2}{\Vert f(x)\Vert }_2^2$$ 将残差写成组合向量的形式：$f(x)=\left[\begin{array}{c}{f}_1(x)\\ \cdots \\ f_m(x)\end{array}\right]$, 则$$F(x)=\frac{1}{2}{\Vert f(x)\Vert }_2^2=\frac{1}{2}{f}^T(x)f(x)=\frac{1}{2}\sum _{i=1}^m(f_i(x){)}^2$$ 同理，如果记$J_i(x)=\frac{\partial f_i(x)}{\partial x}$, 则有：$$\frac{\partial f(x)}{\partial x}=J^{m\times n}=\left[\begin{array}{c}{J}_1(x)\\ \cdots \\ J_m(x)\end{array}\right],J_i(x{)}^{1\times n}=\left[\begin{array}{c}\frac{\partial f_i(x)}{\partial x_1}\frac{\partial f_i(x)}{\partial x_2}\cdots \frac{\partial f_i(x)}{\partial x_n}\end{array}\right]$$

对残差函数$f(x)$进行一阶泰勒展开：$f(x+\Delta x)=f(x)+J\Delta x$, 其中$J$是残差函数$f$的雅可比矩阵，代入损失函数(以下推导用$f$代替$f(x)$)：$$\begin{gathered} F(x+\Delta x)\approx L(\Delta x)=\frac{1}{2}(f+J\Delta x{)}^T(f+J\Delta x)=\frac{1}{2}{f}^{T}f+\Delta x^T{J}^{T}f+\frac{1}{2}\Delta x^T{J}^{T}J\Delta x \\ =F(x)+\Delta x^T{J}^{T}f+\frac{1}{2}\Delta x^T{J}^{T}J\Delta x\text{\hspace{1em}(1)} \end{gathered}$$ 这样损失函数就近似成了一个二次函数，如果雅可比矩阵是满秩的(并不一定满秩)，则$J^{T}J$正定，损失函数有最小值，且易得 $F^{\prime }(x)=(J^{T}f{)}^T,F^{\prime \prime }(x)=J^{T}J$

Gauss-Newton Method：
令公式(1)的一阶导数等于0，得到$$(J^{T}J)\Delta x_{gn}=-J^{T}f$$ 由于$H=J^{T}J$可能出现H矩阵奇异或者病态，此时高斯牛顿法增量的稳定性较差，导致算法不收敛。同时对于步长问题，若求出来的$\Delta x_{gn}$太长，则可能出现局部近似不够准确，无法保证迭代收敛。

Levenberg-Marquardt Method (LM):
在高斯牛顿法的基础上引入阻尼因子：$$(J^{T}J+\mu I)\Delta x_{lm}=-J^{T}f,s.t.\mu \ge 0$$ 其中$\mu$称为阻尼因子。

阻尼因子的作用： $\mu >0$保证$(J^{T}J+\mu I)$正定，迭代朝着下降方向进行。
阻尼因子的初始化： ${\mu }_0=\tau max\left\{\begin{array}{c}(J^{T}J{)}_{ij}\end{array}\right\}$, 按需设定 $\tau \in [10^{-8},1]$。
阻尼因子$\mu$的更新策略：

• 如果$\Delta x\to F(x)\uparrow$，则 $\mu \uparrow \to \Delta x\downarrow$，增大阻尼减小步长，拒绝本次迭代。
• 如果$\Delta x\to F(x)\downarrow$，则 $\mu \downarrow \to \Delta x\uparrow$，减小阻尼增加步长，减少迭代次数。

$\Delta x$和阻尼因子$\mu$的关系：
阻尼因子的更新由比例因子来确定：
$$\rho =\frac{F(x)-F(x+\Delta x_{lm})}{L(0)-L(\Delta x_{lm})}=\frac{实际下降}{近似下降}$$ 其中 $$\begin{gathered} L(0)-L(\Delta x_{lm})=-\Delta x_{lm}^T{J}^{T}f-\frac{1}{2}\Delta x_{lm}^T{J}^{T}J\Delta x \\ =-\frac{1}{2}\Delta x_{lm}^T(2J^{T}f+(J^{T}J+\mu I-\mu I)\Delta x_{lm}) \\ =-\frac{1}{2}\Delta x_{lm}^T(J^{T}f-\mu I\Delta x_{lm}) \\ =\frac{1}{2}\Delta x_{lm}^T(\mu \Delta x_{lm}-J^{T}f) \end{gathered}$$ 首先比例因子分母始终大于$0$，因为是沿负梯度方向进行的$\Delta x_{lm}$的调整，故$L(0)>L(\Delta x_{lm})$，如果：

• $\rho <0$，则$F(x)\uparrow$，应该$\mu \uparrow \to \Delta x\downarrow$，增大阻尼减小步长。
• 如果$\rho >0$且比较大，减小$\mu$，让LM接近Gauss-Newton使得系统更快收敛。
• 反之，如果是比较小的正数，则增大阻尼$\mu$，缩小迭代步长。

2.2 鲁棒核函数下状态量增量方程的构建

由上得 $$F(x)=\frac{1}{2}{\Vert f(x)\Vert }_2^2=\frac{1}{2}{f}^T(x)f(x)=\frac{1}{2}\sum _{i=1}^m(f_i(x){)}^2$$ 鲁棒核函数直接作用于残差$f_i(x)$上，则有
$$\underset{x}{\min }\frac{1}{2}\sum _i\rho ({\Vert f_i(x)\Vert }_2^2)=\underset{x}{\min }\frac{1}{2}\sum _i\rho (f_i(x{)}^2)$$ 将误差的平方项记作$s_i={\Vert f_i(x)\Vert }_2^2$，对鲁棒核误差函数进行二阶泰勒展开有：
$$\rho (s+\Delta s)\approx \rho (s)+{\rho }^{\prime }(s)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(s){\Delta }^{2}s\text{\hspace{1em}(2)}$$

---

对于$\Delta s$的计算，我们有：
$$\begin{gathered} \Delta s={\Vert f_i(x+\Delta x)\Vert }_2^2-{\Vert f_i(x)\Vert }_2^2\approx {\Vert f_i(x)+J_i\Delta x\Vert }_2^2-{\Vert f_i(x)\Vert }_2^2 \\ =(f_i(x)+J_i\Delta x{)}^2-f_i^2(x)=2f_i^T(x)J_i\Delta x+(\Delta x{)}^T{J}_i^T{J}_i\Delta x\text{\hspace{1em}(3.1)} \end{gathered}$$
若误差项为$\Sigma$范数(表征为误差项的协方差矩阵，协方差为对称矩阵，故协方差的逆(称为信息矩阵)亦为对称矩阵)而非二范数，我们有：
$$\begin{gathered} \Delta s={\Vert f_i(x+\Delta x)\Vert }_{{\Sigma }_i}^2-{\Vert f_i(x)\Vert }_{{\Sigma }_i}^2\approx {\Vert f_i(x)+J_i\Delta x\Vert }_{{\Sigma }_i}^2-{\Vert f_i(x)\Vert }_{{\Sigma }_i}^2 \\ =(f_i(x)+J_i\Delta x{)}^T{\Sigma }_i^{-1}(f_i(x)+J_i\Delta x)-f_i^T(x){\Sigma }_i^{-1}{f}_i(x) \\ =({\Sigma }_i^{-T}{f}_i(x){)}^T{J}_i\Delta x+(J_i\Delta x{)}^T{\Sigma }_i^{-1}{f}_i(x)+\Delta x^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x \\ =2f_i^T(x){\Sigma }_i^{-1}{J}_i\Delta x+\Delta x^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x\text{\hspace{1em}(3.2)} \end{gathered}$$

---

将公式$(3)$代入公式$(2)$，令$f_i(x)=f_i$ ，$\rho =\rho (s)$，可得
$$\begin{gathered} \frac{1}{2}\rho (s+\Delta s)\approx \frac{1}{2}(\rho (s)+{\rho }^{\prime }(s)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(s){\Delta }^{2}s) \\ =\frac{1}{2}(\rho +{\rho }^{\prime }[2f_i^T{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{J}_i\Delta x]+\frac{1}{2}{\rho }^{\prime \prime }[2f_i^T{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{J}_i\Delta x{]}^2) \\ \approx \rho +{\rho }^{\prime }{f}_i^T{J}_i\Delta x+\frac{1}{2}{\rho }^{\prime }(\Delta x{)}^T{J}_i^T{J}_i^T\Delta x+{\rho }^{\prime \prime }(\Delta x{)}^T{J}_i^T{f}_i{f}_i^T{J}_i\Delta x \\ =\rho +{\rho }^{\prime }{f}_i^T{J}_i\Delta x+\frac{1}{2}(\Delta x{)}^T{J}_i^T({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_i{f}_i^T)J_i\Delta x\text{\hspace{1em}(4.1)} \end{gathered}$$
若误差项为$\Sigma$范数：
$$\begin{gathered} \frac{1}{2}\rho (s+\Delta s)\approx \frac{1}{2}(\rho (s)+{\rho }^{\prime }(s)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(s){\Delta }^{2}s) \\ =\frac{1}{2}(\rho +{\rho }^{\prime }[2f_i^T{\Sigma }_i^{-1}{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x]+\frac{1}{2}{\rho }^{\prime \prime }[2f_i^T{\Sigma }_i^{-1}{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x{]}^2) \\ \approx \rho +{\rho }^{\prime }{f}_i^T{\Sigma }_i^{-1}{J}_i\Delta x+\frac{1}{2}{\rho }^{\prime }(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x+{\rho }^{\prime \prime }(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1}{J}_i\Delta x \\ =\rho +{\rho }^{\prime }{f}_i^T{\Sigma }_i^{-1}{J}_i\Delta x+\frac{1}{2}(\Delta x{)}^T{J}_i^T({\rho }^{\prime }{\Sigma }_i^{-1}+2{\rho }^{\prime \prime }{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1})J_i\Delta x\text{\hspace{1em}(4.2)} \end{gathered}$$

---

由于$\Delta x$是一个极小量，故其三阶及以上的项数值很小，近似忽略$\Delta x$三阶及其以上的项。
对公式$(4)$中的$\Delta x$进行求导后，令其等于0，得到： $$\sum _i{J}_i^T({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_i{f}_i^T)J_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{f}_i$$ 化简得：$$\sum _i{J}_i^{T}W{J}_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{f}_i$$ 其中 $W={\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_i{f}_i^T$

若误差项为$\Sigma$范数：
$$\sum _i{J}_i^T({\rho }^{\prime }{\Sigma }_i^{-1}+2{\rho }^{\prime \prime }{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1})J_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{\Sigma }_i^{-1}{f}_i$$ 化简得：$$\sum _i{J}_i^{T}W{J}_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{\Sigma }_i^{-1}{f}_i$$ 其中 $W={\rho }^{\prime }{\Sigma }_i^{-1}+2{\rho }^{\prime \prime }{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1}$

---

3. 局部Bundle Adjustment代价函数的构建

对于需要优化的状态向量，包括滑动窗口内的所有IMU状态${\mathbf{x}}_k$(位姿$P$、速度$V$、旋转$Q$、加速度偏置$b_a$，陀螺仪偏置$b_w$)、相机到IMU的外参 ${\mathbf{x}}_c^b$、所有3D点的逆深度 ${\lambda }_m$：【论文公式(21)】$$\mathcal{X}=[{\mathbf{x}}_0,{\mathbf{x}}_1,\cdots ,{\mathbf{x}}_n,{\mathbf{x}}_c^b,{\lambda }_0,{\lambda }_1,\cdots ,{\lambda }_m]$$ $${\mathbf{x}}_k=[{\mathbf{p}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{b}}_a,{\mathbf{b}}_g],k\in [0,n]$$ $${\mathbf{x}}_c^b=[{\mathbf{p}}_c^b,{\mathbf{q}}_c^b]$$ 其中${\mathbf{x}}_k$代表相机获取图片时对应时刻$k$的IMU状态，$n$为滑动窗口内的关键帧的数量，$m$为滑动窗口内3D视觉特征(空间点)的数量，${\lambda }_m$为第$m$个3D视觉特征的逆深度(第一次被观察时计算得到的值)。
目标函数为 【论文公式(22)(23)】：$$\underset{\mathcal{X}}{\min }\left\{\begin{array}{c}{\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathcal{X}\Vert }^2+\sum _{k\in \mathcal{B}}{\Vert {\mathbf{r}}_{\mathcal{B}}({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathcal{X})\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in \mathcal{C}}\rho ({\Vert {\mathbf{r}}_{\mathcal{C}}({\hat{\mathbf{z}}}_l^{c_j},\mathcal{X})\Vert }_{{\mathbf{P}}_l^{c_j}}^2)\end{array}\right\}\text{\hspace{1em}(5)}$$ $$\rho (s)=\{\begin{array}{c}1,s\ge 1\\ 2\sqrt{s}-1,s<1\end{array}$$ 其中这三项分别为边缘化的先验信息 (由滑动窗口产生的关键帧边缘化)、IMU的测量误差、视觉的重投影误差，三种残差都是用马氏距离表示。${\mathbf{P}}_{b_{k+1}}^{b_k}$代表IMU残差的协方差，${\mathbf{P}}_l^{c_j}$代表视觉重投影误差的协方差，通过协方差的逆 (即信息矩阵) 对各个变量计算残差时进行加权。$\rho (s)$为Huber核函数。

4. IMU测量残差 (增量误差) 及其对状态量雅克比矩阵、协方差递推方程的推导

4.1 IMU测量残差 (增量误差)

在IMU预积分中我们已经得到了：
$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}(p_{b_k}^w+v_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w\Delta t_k^2)+{\alpha }_{b_{k+1}}^{b_k}$$ $$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w-g^w\Delta t_k)+{\beta }_{b_{k+1}}^{b_k}$$ $$q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}$$
因此俩帧之间的PVQ和bias的变化量(增量)误差为：【论文公式[24]】
$$r_{\mathcal{B}}^{15\times 1}({\hat{z}}_{b_{k+1}}^{b_k},\mathcal{X})=\left[\begin{array}{c}\delta {\alpha }_{b_{k+1}}^{b_k}\\ \delta {\beta }_{b_{k+1}}^{b_k}\\ \delta {\theta }_{b_{k+1}}^{b_k}\\ \delta b_a\\ \delta b_w\end{array}\right]=\left[\begin{array}{c}{R}_w^{b_k}(p_{b_{k+1}}^w-p_{b_k}^w-v_k^w\Delta t_k+\frac{1}{2}{g}^w\Delta t_k^2)-{\alpha }_{b_{k+1}}^{b_k}\\ R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w+g^w\Delta t_k)-{\beta }_{b_{k+1}}^{b_k}\\ 2[({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w{]}_{xyz}\\ b_{a{b}_{k+1}}-b_{a{b}_k}\\ b_{w{b}_{k+1}}-b_{w{b}_k}\end{array}\right]\text{\hspace{1em}(6)}$$
其中$[\cdot {]}_{xyz}$代表取四元素虚部部分来表示状态误差，$[{\alpha }_{b_{k+1}}^{b_k},{\beta }_{b_{k+1}}^{b_k},{\gamma }_{b_{k+1}}^{b_k}{]}^T$为IMU的预积分量，具体推导可见上一篇博客VINS-Mono之IMU预积分，预积分对应的误差、协方差及雅克比矩阵递推方程的推导，同时，加速度计和陀螺仪的偏置同样包含在残差中以在线校正。

4.2 IMU优化变量

优化变量主要包含第$k$时刻、第$k+1$时刻(其中$k$和$k+1$为前后捕获俩帧图像对应的时间)的PVQ和加速度计的偏置$b_a$、陀螺仪的偏置$b_w$:
$$[p_{b_k}^w,q_{b_k}],[v_{b_k}^w,b_{a_k},b_{w_k}],[p_{b_{k+1}}^w,q_{b_{k+1}}^w],[v_{b_{k+1}}^w,b_{a_{k+1}},b_{w_{k+1}}]$$

4.3 IMU增量残差对状态量的雅克比矩阵

这部分在imu_factor.h中的class IMUFactor:public ceres::SizedCostFunction<15,7,9,7,9>的函数virtual bool Evaluate()中实现，其中parameters[0~3]分别对应了以上4组优化变量的参数块，4组参数的长度依次是7,9,7,9。

代码IMUFactor::Evaluate()中redidual还乘以一个信息矩阵sqrt_info，这是由于在IMU测量误差和视觉的重投影误差通过协方差的逆进行了加权，因此实际优化的是$min(d)=min(r_k^T{P}^{-1}{r}_k)$, 这里$r_k$代表$k$时刻时候的残差，$P$代表协方差。而Ceres只接受诸如$min(r_k^T{r}_k)$的优化，因此在代码里，需要将信息矩阵$P^{-1}$做$LLT$分解，即$L{L}^T=P^{-1}$ ,代码中矩阵L对应sqrt_info,这样就可以将优化进行转换：$$min(d)=min(r_k^T{P}^{-1}{r}_k)=min((L^T{r}_k{)}^T(L^T{r}_k))=min(r_k^{\prime T}{r}_k)$$

计算雅克比时，残差对应的求偏差对象为上面的优化变量，但是计算时采用扰动方式计算，即扰动为$[\delta p_{b_k}^w,\delta {\theta }_{b_k}^w],[v_{b_k}^w,\delta b_{a_k},\delta b_{w_k}],[\delta p_{b_{k+1}}^w,\delta {\theta }_{b_{k+1}}^w],[\delta v_{k+1}^w,\delta a_{k+1},\delta b_{w_{k+1}}]$

这里给出先给出雅克比$J$的结果(对应上面的parameters[3])
$$J[0{]}^{15\times 7}=[\frac{\partial r_{\mathcal{B}}}{\partial p_{b_k}^w},\frac{\partial r_{\mathcal{B}}}{\partial q_{b_k}^w}]=\left[\begin{array}{cc}-R_w^{b_k}& [R_w^{b_k}(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Delta t_k+\frac{1}{2}{g}^w\Delta t_k^2){]}_{\times }\\ 0& [R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w+g^w\Delta t_k){]}_{\times }\\ 0& -{\mathcal{L}}_{3\times 3}[(q_{k+1}^w{)}^{-1}\otimes q_{b_k}^w]{\mathcal{R}}_{3\times 3}[{\gamma }_{b_{k+1}}^{b_k}]\\ 0& 0\\ 0& 0\end{array}\right]\text{\hspace{1em}(7)}$$ $$J[1{]}^{15\times 9}=[\frac{\partial r_{\mathcal{B}}}{\partial v_{b_k}^w},\frac{\partial r_{\mathcal{B}}}{\partial b_{a_k}},\frac{\partial r_{\mathcal{B}}}{\partial b_{w_k}}]=\left[\begin{array}{ccc}-R_w^{b_k}\Delta t_k& -J_{b_a}^{\alpha }& -J_{b_w}^{\alpha }\\ -R_w^{b_k}& -J_{b_a}^{\beta }& -J_{b_w}^{\beta }\\ 0& 0& -{\mathcal{L}}_{3\times 3}[(q_{b_{k+1}}^w{)}^{-1}\otimes q_{b_k}^w\otimes {\gamma }_{b_{k+1}}^{b_k}]J_{b_w}^{\gamma }\\ 0& -I& 0\\ 0& 0& -I\end{array}\right]\text{\hspace{1em}(8)}$$
$$J[2{]}^{15\times 7}=[\frac{\partial r_{\mathcal{B}}}{\partial p_{b_{k+1}}^w},\frac{\partial r_{\mathcal{B}}}{\partial q_{b_{k+1}}^w}]=\left[\begin{array}{cc}{R}_w^{b_k}& 0\\ 0& 0\\ 0& {\mathcal{L}}_{3\times 3}[({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w]\\ 0& 0\\ 0& 0\end{array}\right]\text{\hspace{1em}(9)}$$
$$J[3{]}^{15\times 9}=[\frac{\partial r_{\mathcal{B}}}{\partial v_{b_{k+1}}^w},\frac{\partial r_{\mathcal{B}}}{\partial b_{a_{k+1}}},\frac{\partial r_{\mathcal{B}}}{\partial b_{w_{k+1}}}]=\left[\begin{array}{ccc}0& 0& 0\\ R_w^{b_k}& 0& 0\\ 0& 0& 0\\ 0& I& 0\\ 0& 0& I\end{array}\right]\text{\hspace{1em}(10)}$$

对于上式，PVQ的求导可以直接采用对误差增量进行计算，而对$b_a$,$b_g$的求导，因为$k$时刻的bias相关的预计分计算是通过迭代一步一步递推的，直接求导太复杂，这里直接对预积分量在$k$时刻的bias附近用一阶泰勒展开来近似，而不是真的取迭代计算:
$${\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha }\delta b_a+J_{b_w}^{\alpha }\delta b_w$$ $${\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+J_{b_a}^{\beta }\delta b_a+J_{b_w}^{\beta }\delta b_w$$ $${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }\delta b_w\end{array}\right]$$
其中$J_{b_a}^{\alpha }=\frac{\partial {\alpha }_{b_{k+1}}^{b_k}}{\partial \delta b_a},J_{b_w}^{\alpha }=\frac{\partial {\alpha }_{b_{k+1}}^{b_k}}{\partial \delta b_w},J_{b_a}^{\beta }=\frac{\partial {\beta }_{b_{k+1}}^{b_k}}{\partial \delta b_a},J_{b_w}^{\beta }=\frac{\partial {\beta }_{b_{k+1}}^{b_k}}{\partial \delta b_w},J_{b_w}^{\gamma }=\frac{\partial {\gamma }_{b_{k+1}}^{b_k}}{\partial \delta b_w}$表示预积分量对$k$时刻的偏置的求导。关于偏置的雅克比矩阵可以根据IMU预积分讨论(VINS-Mono之IMU预积分，预积分对应的误差、协方差及雅克比矩阵递推方程的推导)的协方差递推公式，一步步递推获得。递推公式为：$$J_{k+1}=F{J}_k,J_0=I$$

---

以下是对公式$(7)(8)(9)(10)$的推导：

由于$\delta {\alpha }_{b_{k+1}}^{b_k}$(预积分Pose误差)和$\delta {\beta }_{b_{k+1}}^{b_k}$(预积分Velocity误差)形式很接近，因此其对各个状态量求导的雅克比形式也很相似。

首先明确下反对称符号的性质：有$$R[\delta \theta {]}_{\times }=-[\delta \theta {]}_{\times }R$$ $$[\delta \theta {]}_{\times }(Rp)=-[Rp{]}_{\times }\delta \theta$$ $$(Rexp([\delta \theta {]}_{\times }){)}^{-1}=exp(-[\delta \theta {]}_{\times })R^{-1}$$ $$(exp([\delta \theta {]}_{\times })R{)}^{-1}=R^{-1}exp(-[\delta \theta {]}_{\times })$$

1) 预积分Pose误差 $\delta {\alpha }_{b_{k+1}}^{b_k}$ 对$k$ 时刻状态量雅克比进行推导 ($k+1$时刻的雅克比同理)：

• 对$k$时刻Pose $p_{b_k}^w$的导数：$$\frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial p_{b_k}^w}=-R_w^{b_k}$$
• 对$k$时刻Velocity $v_{b_k}^w$的导数：$$\frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial v_{b_k}^w}=-R_w^{b_k}\Delta t_k$$
• 对$k$时刻旋转 $q_{b_k}^w$的导数：
  $$\begin{gathered} \frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial q_{b_k}^w}=\frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial \delta {\theta }_{b_k}^w}=\frac{\partial q_w^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }_{b_k}^w\end{array}\right](p_{b_{k+1}}^w-p_{b_k}^w-v_k^w\Delta t_k+\frac{1}{2}{g}^w\Delta t_k^2)}{\partial \delta {\theta }_{b_k}^w} \\ =\frac{\partial R_w^{b_k}exp([\delta {\theta }_{b_k}^w{]}_{\times })(p_{b_{k+1}}^w-p_{b_k}^w-v_k^w\Delta t_k+\frac{1}{2}{g}^w\Delta t_k^2)}{\partial \delta {\theta }_{b_k}^w} \\ \approx \frac{\partial R_w^{b_k}(I+[\delta {\theta }_{b_k}^w{]}_{\times })(p_{b_{k+1}}^w-p_{b_k}^w-v_k^w\Delta t_k+\frac{1}{2}{g}^w\Delta t_k^2)}{\partial \delta {\theta }_{b_k}^w} \\ =\frac{\partial -[\delta {\theta }_{b_k}^w{]}_{\times }{R}_w^b}{\partial \delta {\theta }_{b_k}^w} \end{gathered}$$