VINS-Mono-IMU预积分 （七：预积分零偏建模方式）

由于里面的零偏 $b_a,b_w$ 也是待优化变量，每次求解一次之后 $b_a,b_w$ 都会被改变以满足残差最小，那这样整个积分就得重新积分，这样会很耗时
由上节给出的更新公式 $\Delta x_{k+1}=F\Delta x_k+Gn$ ，$F\in 15\times 15$ ，求协方差矩阵的时候 $=F\cdot P\cdot F^T$，此时是3个15×15的矩阵相乘（一个IMU数据），如果每迭代一次就要把所有的IMU数据都这样重新相乘的话计算量会非常大。
问题: $\Delta x$的方差P怎么来的
可以根据一阶泰勒展开来近似，来进行更新，避免重新预积分
$f(x+\Delta b)=f(x)+J\cdot \Delta b$
预积分原本的公式如下
在离散时间下的 k 到 k+1的状态更新看上一节，实际也是用上一节的方式来进行更新，这里只是为了方便理解，因为雅可比 $J$ 的计算还是用连续时间预积分的公式来计算的，现在这里是对零偏 $b$ 进行求导，上一节是对时间 $\Delta t$ 进行求导，只是在变成离散时间下会用中值积分进行近似而已

当零偏 $b_a,b_w$ 经过优化发生变化后的更新方式如下
下面$J_{b_a}^{\alpha },J_{b_a}^{\beta },J_{b_w}^{\alpha },J_{b_w}^{\beta },J_{b_w}^{\gamma }$分别是 $\alpha ,\beta ,\gamma$ 对零偏 $b$ 的求导
用一阶线性近似的方式来进行更新

现在关键在于如何求出上面列写的雅可比矩阵 $J$
$\Delta b=b_k-b_{k+1}$ 这个比较好计算出来，用优化前后的两个零偏相减即可
定义一下：
$\Delta \alpha =J_{b_a}^{\alpha }\cdot \Delta b_a$ ，这个 $\alpha$ 对 $b_a$ 的雅可比乘上 $b_a$ 的变化量等于 $\alpha$ 的变化量
这里的雅可比 $J$ 不进行一个个的求导，而是自己对自己求导
定义 x = [ α β γ b a b w ] x=\begin{bmatrix} α\\ β\\ γ\\ b_{a}\\ b_{w}\\ \end{bmatrix} x= ​αβγba​bw​​ ​，对自己求导 $\frac{\sigma x}{\sigma x}$ = J = [ σ α σ α σ α σ β σ α σ γ σ α σ b a σ α σ b w σ β σ α σ β σ β σ β σ γ σ β σ b a σ β σ b w σ γ σ α . . . σ γ σ γ . . . . . . σ b a σ α . . . . . . σ b a σ b a . . . σ b w σ α . . . . . . . . . σ b w σ b w ] =J=\begin{bmatrix} \frac{σα}{σα}&\frac{σα}{σβ}&\frac{σα}{σγ}&\frac{σα}{σb_{a}}&\frac{σα}{σb_{w}}\\ \frac{σβ}{σα}& \frac{σβ}{σβ}& \frac{σβ}{σγ}& \frac{σβ}{σb_{a}}& \frac{σβ}{σb_{w}}\\ \frac{σγ}{σα}&...&\frac{σγ}{σγ}&...&...\\ \frac{σb_{a}}{σα}&...&...&\frac{σb_{a}}{σb_{a}}&...\\ \frac{σb_{w}}{σα}&...&...&...&\frac{σb_{w}}{σb_{w}}\\ \end{bmatrix} =J= ​σασα​σασβ​σασγ​σασba​​σασbw​​​σβσα​σβσβ​.........​σγσα​σγσβ​σγσγ​......​σba​σα​σba​σβ​...σba​σba​​...​σbw​σα​σbw​σβ​......σbw​σbw​​​ ​
在什么积分都没有做的情况下这个雅可比矩阵 $J$ 为一个单位矩阵，对角线上自己对自己求导就是单位矩阵，在IMU数据还没有来之前， $x$ 中的变量两两间是没有联系的，则求导为0。
求这个雅可比就是为了获得 $\alpha ，\beta ，\gamma$ 对 $b_a,b_w$ 的导数。
接下来开始推导：
$x_k$ 是 k 时刻的状态值，也是预积分量，$\frac{\sigma x_k}{\sigma x}$ 可以求出第 $k$ 时刻预积分量对自己的求导，但是现在需要将零偏从第 $k$ 时刻转移到第 $k+1$时刻，根据上一节的更新公式 $\Delta x_{k+1}=F\Delta x_k+Gn$ ，这个公式描述的是状态量从 $k$ 到 $k+1$ 时刻的转移，这里面的 $F$ 相当于是 $\frac{\sigma x_{k+1}}{\sigma x_k}$ ，相乘得到 $J_{k+1}=F\cdot J_k=\frac{\sigma x_{k+1}}{\sigma x_k}\cdot \frac{\sigma x_k}{\sigma x}=\frac{\sigma x_{k+1}}{\sigma x}$，这个矩阵里面就包含了各个状态量关于 $b_a,b_w$ 的导数。这个 $J_{k+1}$ 是一个k+1时刻自己对自己的雅可比
为什么要算 k+1 时刻的 J：
根据一阶泰勒近似 $f(x+\Delta b)=f(x)+J_b^x\cdot \Delta b$ ，这样当零偏经过优化发生变化后，只需要将变化量乘上导数加进去就能近似下一时刻的状态值，也是为了进行一阶近似才需要计算上面那个雅可比矩阵。
因为经过优化后，预积分量和零偏都会得到优化，但是预积分量还是用上一时刻的零偏进行计算的，获得新的零偏后，此时的预积分量应该根据新的零偏进行更新才对，但是进行重新积分很耗时，又由于零偏的变化量不大，所以用一阶近似，所以要计算雅可比矩阵。

协方差矩阵的传递

方差传递基本性质
$a$ ~ $A$
$Ka$ ~ $K^{2}A$
$a$ ~ $A$
$Fa$ ~ $F\cdot A\cdot F^T$，$F$ 是一个矩阵
$a$~$A$ $b$ ~ $B$
$a+b$ ~$A+B$
所以
$\Delta x_{k+1}：$$P_{k+1}$
$\Delta x_k：$$P_k$
$n_k：$$V_k$
状态变换公式：$\Delta x_{k+1}=F\Delta x_k+Gn$
协方差更新公式：$P_{k+1}=F\cdot P_k\cdot F^T+G\cdot V_k\cdot G^T$