这里是求预积分对约束的参数块进行求导，有这个雅可比矩阵才能进行优化步长的计算，这个是预积分这个约束因子对各个优化变量的求导，后面还有相机的观测

残差块中的 $\theta$ 是3维的，但是参数块中的四元数是4维的，因为相减后残差只剩虚部了，但是参数是从4个参数变过来的

预积分的残差具体如下，总共有15维的自由度，即y有15维

而参数块 $x$ ， 维护的是$k$ 和 $k+1$ 时刻的 $P,Q,V,Ba,Bg$
P是3维，Q是四元数有4维，因为是过参数化的形式，而 $V,Ba,Bg$ 总共是9维的参数块
所以整个参数块 $x$ 的大小为 7+9

残差对残差参数块的求导
$\left[\begin{array}{cccc}\frac{\partial e}{\partial P_k}& \frac{\partial e}{\partial V_k}& \frac{\partial e}{\partial P_{k+1}}& \frac{\partial e}{\partial V_{k+1}}\\ \frac{\partial e_1}{\partial P_k}& \frac{\partial e_1}{\partial V_k}& \frac{\partial e_1}{\partial P_{k+1}}& \frac{\partial e_1}{\partial V_{k+1}}\\ ⋮& & & \end{array}\right]$
这个矩阵有15行，因为误差矩阵 $e$ 是15维的(残差分别是$\alpha ,\beta ,\theta ,B_a,B_g构成，各自都是3个维度$)，参数块 $P$ 是7维，参数块 $V$ 是9维
所以把这个雅可比矩阵分块成了 $15\times 7$ ， $15\times 9$ ， $15\times 7$ ， $15\times 9$ 的形式
误差矩阵的维度和参数是不同的，求导就是对构成这个误差函数的里面的全部变量进行求导

由于我们维护的是 $R_{wc}$ ，所以我们的扰动是右乘的，十四讲里面维护的是 $R_{cw}$ 所以才使用左乘

对这个误差矩阵进行求导的时候，也可以按照误差参数块进行分别求导的，15=3*5，前三行雅可比使用位移的函数对 $P_k,V_k,P_{k+1},V_{k+1}$ 进行求导

对位置 $\delta \alpha$ 进行求导

以下示例都是对 $k$ 时刻的状态量进行求导，$k+1$ 时刻的同理
即使用 $\delta {\alpha }_{b_{k+1}}^{b_k}=\dots$ 分别对 $P,Q,V,Ba,Bg$ 进行求导

位置误差 $\delta \alpha$ 对平移 $P_{b_k}^w$ 的求导

代码中的 $Q_i$ 是$R_{b_k}^w$ ，所以代码中要取逆

$\frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial P_{b_k}^w}=-R_w^{b_k}$

位置 $\delta \alpha$ 对旋转 $R_w^{b_k}$ 进行求导

接下来是对旋转 $R_w^{b_k}$ 进行求导，由于代码中维护的是 $R_{b_k}^w$ ，所以这里的公式推导要取逆，方便代码的维护，这样是一个旋转方向的问题，如果直接左乘的话旋转方向就是反过来的了，这样操作的话旋转方向就是按照代码中维护的量的方向来进行操作

后面一串相乘后就是一个向量，当作向量 $a$
$\frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial R_w^{b_k}}=li{m}_{\varphi \to 0}\frac{(R_{b_k}^{w}exp({\varphi }^{\wedge }){)}^{-1}\cdot a-R_w^{b_k}\cdot a}{\varphi }$

有公式 $(A\cdot B{)}^{-1}=B^{-1}\cdot A^{-1}$
对旋转向量 $\varphi$ 取逆，相当于是换了一个旋转方向，所以 ${\varphi }^{-1}=-\varphi$
$=(I-{\varphi }^{\wedge })R_w^{b_k}\cdot a-R_w^{b_k}\cdot a$
$=-{\varphi }^{\wedge }\cdot R_w^{b_k}\cdot a$

叉乘有一个性质，$\vec{a}\times \vec{b}=-\vec{b}\times \vec{a}$
$\vec{a}\times \vec{b}=a^{\wedge }\cdot b$
$-\vec{b}\times \vec{a}=-b^{\wedge }\cdot a$
$a^{\wedge }\cdot b=-b^{\wedge }\cdot a$

则上面有
$=(R_w^{b_k}\cdot a{)}^{\wedge }\cdot \varphi$

然后约掉分母上的 $\varphi$
$\frac{\partial \delta {\alpha }_{b_{k+1}}^{b_k}}{\partial R_w^{b_k}}=(R_w^{b_k}\cdot a{)}^{\wedge }$

对速度 $\delta \beta$ 进行求导

也是使用 $\delta {\beta }_{b_{k+1}}^{b_k}=\dots$ 分别对 $P,Q,V,Ba,Bw$ 进行求导

速度 $\delta \beta$ 对位置 $P_{b_k}^w$ 求导

由于公式里面不含位置，所以这块导数 = 0

速度 $\delta \beta$ 对旋转 $R_w^{b_k}$ 求导

整体公式结构和上面的位移对旋转求导一致，所以求导结果也是 $(R_w^{b_k}\cdot a{)}^{\wedge }$

对旋转 $\delta \theta$ 进行求导

分别对 $P,Q,V,Ba,Bg$ 进行求导

由于公式中不包含平移和速度量，所以对应的雅可比也为0
$\delta \theta =2\cdot ({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w$
k对应代码中的 i ，k+1对应代码中的 j

旋转 $\delta \theta$ 对 $q_{b_k}^w$ 进行求导

就是在右边加一个扰动，扰动为 $[1\frac{\theta }{2}{]}^T$
$\frac{\partial \delta \theta }{\partial q_{b_k}^w}=\frac{({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w\otimes \left[\begin{array}{c}1\\ \frac{\theta }{2}\end{array}\right]{)}^{-1}\otimes q_{b_{k+1}}^w-({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w}{\theta }$

把逆乘进去，对于扰动那里其实就是把虚部$\vec{n}$ 变一个旋转方向，所以是取个负号
$=({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes \left[\begin{array}{c}1\\ -\frac{\theta }{2}\end{array}\right]\otimes q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w-({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w$

这里会用一个四元数的性质
$a\otimes b=[a{]}_L\cdot b=[b{]}_R\cdot a$
具体看这篇文章讲解VINS-Mono-IMU预积分 （二：连续时间的PVQ积分+四元数求导）

公式变成
$=2[q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w{]}_R\cdot \left[\begin{array}{c}[({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}{]}_L\cdot \left[\begin{array}{c}1\\ -\frac{\theta }{2}\end{array}\right]\end{array}\right]-[q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w{]}_R\cdot \left[\begin{array}{c}[({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}{]}_L\cdot \left[\begin{array}{c}1\\ 0\\ ⋮\end{array}\right]\end{array}\right]$
$=2[q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w{]}_R\cdot [({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}{]}_L\cdot \left[\begin{array}{c}\left[\begin{array}{c}1\\ -\frac{\theta }{2}\end{array}\right]-\left[\begin{array}{c}1\\ 0\\ ⋮\end{array}\right]\end{array}\right]$
$=\frac{[q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w{]}_R\cdot [({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}{]}_L\cdot {\left[\begin{array}{c}0\\ -\theta \end{array}\right]}_{xyz}}{\theta }$

把 $\theta$ 约掉

$=-[q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w{]}_R\cdot [({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}{]}_L$

这里乘完后的结果只有四元数的虚部 $xyz$
所以把四元数进行矩阵化的公式会有点变动
原本的
$[q{]}_L=q_w\cdot I+\left[\begin{array}{cc}0& -q^T\\ q& q_{\times }\end{array}\right]$
$[q{]}_R=q_w\cdot I+\left[\begin{array}{cc}0& -q^T\\ q& -q_{\times }\end{array}\right]$
当只取虚部的时候
$[q{]}_L=q_w\cdot I+q_{\times }$
$[q{]}_R=q_w\cdot I-q_{\times }$

对于一个四元数取逆的时候
$q=\left[\begin{array}{c}cos\frac{\theta }{2}\\ \vec{n}\cdot sin\frac{\theta }{2}\end{array}\right]$
其实就是把旋转轴的方向换成反方向，实部是不变的，只有虚部会反方向
$q^{-1}=\left[\begin{array}{c}cos\frac{\theta }{2}\\ -\vec{n}\cdot sin\frac{\theta }{2}\end{array}\right]$
则
$[q^{-1}{]}_L=q_w\cdot I-q_{\times }=[q{]}_R$
$[q^{-1}{]}_R=q_w\cdot I+q_{\times }=[q{]}_L$

我们推导的公式是这样的
$=-[q_{b_k}^{w-1}\otimes q_{b_{k+1}}^w{]}_R\cdot [({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}{]}_L$

代码中用了上面的变换关系，代码中的公式是这样的
其实就是对四元数取个逆就可以进行左右乘矩阵的变换

$=-[q_{b_{k+1}}^{w-1}\otimes q_{b_k}^w{]}_L\cdot [({\gamma }_{b_{k+1}}^{b_k}){]}_R$

$\gamma$ 是预积分量来的
在计算这个雅可比前也还是会用实时修正的零偏对预积分量进行调整，调整后才进入雅可比的计算

对零偏进行求导

上面的公式中没有包含零偏的项，这里要用到预积分一阶近似更新公式

用这个近似公式代替前面的预积分，再对零偏进行求导

平移$\delta \alpha$ 对 k/i 时刻的 $b_a、b_w$的扰动

其实就是对预积分量进行一个扰动，此时预积分量前面的参数都等于是常数直接为0
公式为
$\frac{-[({\hat{\alpha }}_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha }\Delta b_a)-{\alpha }_{b_{k+1}}^{b_k}]}{\Delta b_a}=-J_{b_a}^{\alpha }$

对 $b_g$ 求导也是同理的，$\alpha 和\beta 都是一样的建模方式$，结果也是一样的
上面是对 i / k 时刻的零偏的求导，这里的零偏也是第 i 时刻的

$\delta b_a$ 对 i时刻ba的求导就是 $-I$，$b_g$ 同理

由于预积分量中零偏的建模中都是假设零偏与 $k$ 时刻有关，与 $k+1$ 时刻无关的，因为假设预积分过程中零偏是不会变的，虽然有联合优化零偏，但是零偏是通过一阶近似的方式加入到第 $k$ 时刻的零偏中，所以 $\alpha ,\beta ,\theta$ 对于 $k+1$ 时刻的零偏求导都是 0

$\theta$ 对陀螺仪零偏求导

$2({\gamma }_{b_{k+1}}^{b_k}\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_w}^{\gamma }\delta b_{w_k}\end{array}\right]{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w-2({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w$

解开逆
$=2\left[\begin{array}{c}1\\ -\frac{1}{2}{J}_{b_w}^{\gamma }\delta b_{w_k}\end{array}\right]\otimes ({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w-2({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w$

$=\left[\begin{array}{c}0\\ -J_{b_w}^{\gamma }\delta b_{w_k}\end{array}\right]\otimes ({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w$

$=\frac{{\left[\begin{array}{c}({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w\end{array}\right]}_R\cdot \left[\begin{array}{c}0\\ -J_{b_w}^{\gamma }\delta b_{w_k}\end{array}\right]}{\partial b_{w_k}}$

约掉 $b_{w_k}$
$={\left[\begin{array}{c}({\gamma }_{b_{k+1}}^{b_k}{)}^{-1}\otimes (q_{b_k}^w{)}^{-1}\otimes q_{b_{k+1}}^w\end{array}\right]}_R\cdot -J_{b_w}^{\gamma }$

$=-{\left[\begin{array}{c}(\otimes q_{b_{k+1}}^w{)}^{-1}\otimes q_{b_k}^w\otimes {\gamma }_{b_{k+1}}^{b_k}\end{array}\right]}_L\cdot J_{b_w}^{\gamma }$

最后这样的形式就和代码中的公式一致了