VINS-Mono-IMU预积分 （六：离散时间预积分误差传递）

为了求出连续时间的变化量，所以要求出连续时间误差量的变化量的导数，因为是误差卡尔曼滤波，所以是对误差量进行求导。
而实际系统的数据肯定是离散的。
现在要根据前面连续时间的方程推导出离散下的变化
上一节的方程的简洁表达式为 $\dot{\Delta x}=F_t\cdot \Delta x+G_t\cdot n_t$ ， $\Delta x\in 15\times 1,F_t\in 15\times 15,G_t\in 15\times 12,n_t\in 12\times 1$
只有连续的才可以求导，离散的是无法求导的，现在要求出离散时间下的变化关系是什么， 即 $\Delta x_k$ 与 $\Delta x_{k+1}$ 之间的变化关系是什么，在已知 $\Delta x_k$ 的情况下推导出 $\Delta x_{k+1}$

导数的定义 $f(x+\Delta x)=f(x)+J(x)\cdot \Delta x$
$\Delta x$ 同样适用$=>\Delta x_{k+1}=\Delta x_k+\dot{\Delta x}\cdot \Delta t$，把$\dot{\Delta x}=F_t\cdot \Delta x+G_t\cdot n_t$ 代入得
$=>\Delta x_{k+1}=\Delta x_k+(F_t\cdot \Delta x+G_t\cdot n_t)\cdot \Delta t$ 这里的 $\dot{\Delta x}$仍然是连续时间的导数
$=>\Delta x_{k+1}=(I+F_t\Delta t)\cdot \Delta x_k+G_t\cdot \Delta t\cdot n_t$ 这里面把 $\Delta x$ 近似成了 $\Delta x_k$ 后面会详细推导这个近似的过程
由于中值积分的原因，$n_t\in \left[\begin{array}{c}{n}_{ak}\\ n_{wk}\\ n_{ak+1}\\ n_{wk+1}\\ n_{ba}\\ n_{bw}\end{array}\right]\in 18\times 1$ ，这里面认为零偏是不会变化的，所以没有k+1时刻的零偏的噪声，零偏的导数就是高斯噪声。
其中$\Delta x_{k+1}\in 15\times 1，(I+F_t\Delta t)\in 15\times 15，\Delta x_k\in 15\times 1，G_t\cdot \Delta t\in 15\times 18$

上述方程的矩阵具体形式如下：

上面可以表示为
$\Delta x_{k+1}=F\cdot \Delta x_k+G\cdot n_k$
表示为从$\Delta x_k$ 时刻的误差转移到 $\Delta x_{k+1}$ 时刻的误差，代码中也是构造 F ，G矩阵

接下来将对每一项进行推导

离散时间角度误差推导

先列出连续时间积分的角度的导数
$\dot{\Delta \theta }=-({\hat{w}}_t-b_{wt}{)}^{\hat{}}\Delta \theta -n_w-\Delta b_{wt}$
根据导数的定义
$\Delta {\theta }_{k+1}=\Delta {\theta }_k+\dot{\Delta \theta }\cdot \Delta t$
然后把 $\dot{\Delta \theta }$ 代入
$=>\Delta {\theta }_{k+1}=\Delta {\theta }_k-[({\hat{w}}_t-b_{wt}{)}^{\hat{}}\Delta \theta +n_w+\Delta b_{wt}]\cdot \Delta t$
通过中值积分来近似表达 $w_t$ 和 $n_w$
$=>\Delta {\theta }_{k+1}=\Delta {\theta }_k-[((\frac{w_k+w_{k+1}}{2})-b_{wt}{)}^{\hat{}}\Delta {\theta }_k+n_w+\Delta b_{wt}]\cdot \Delta t$
整理一下
$=>[I-(\frac{w_k+w_{k+1}}{2}-b_w{)}^{\hat{}}\cdot \Delta t]\cdot \Delta {\theta }_k-\Delta t\cdot \frac{n_{wk}+n_{wk+1}}{2}-\Delta t\cdot \Delta b_w$ 这个对应 $f_{11}$

离散时间速度误差推导

先列出连续时间积分的速度的导数
$\dot{\Delta \beta }=-R_t(\hat{a}-b_{at}{)}^{\hat{}}\Delta \theta -R_t\Delta b_{at}-R_t{n}_a$
根据导数的定义
$\Delta {\beta }_{k+1}=\Delta {\beta }_k+\dot{\Delta \beta }\cdot \Delta t$
把 $\dot{\Delta \beta }$ 代入
$=>\Delta {\beta }_{k+1}=\Delta {\beta }_k-[R_t(\hat{a}-b_{at}{)}^{\hat{}}\Delta \theta +R_t\Delta b_{at}+R_t{n}_a]\cdot \Delta t$
然后用中值积分近似 $R_t,a_k,{\theta }_k$
$=>\Delta {\beta }_{k+1}=\Delta {\beta }_k-[\frac{1}{2}[R_k(a_k-b_a{)}^{\hat{}}\Delta {\theta }_k+R_{k+1}(a_{k+1}-b_a{)}^{\hat{}}\Delta {\theta }_{k+1}]+\frac{1}{2}(R_k+R_{k+1})\cdot \Delta b_a+\frac{1}{2}(R_k{n}_k+R_{k+1}{n}_{k+1})]\cdot \Delta t$

上面包含了 $\Delta {\theta }_{k+1}$ ，然后把上面刚刚推导的 $\Delta {\theta }_{k+1}=[I-(\frac{w_k+w_{k+1}}{2}-b_w{)}^{\hat{}}\cdot \Delta t]\cdot \Delta {\theta }_k-\Delta t\cdot \frac{n_{wk}+n_{wk+1}}{2}-\Delta t\cdot \Delta b_w$ 代入进去，剩下的就是整理归纳系数了，自己归纳一下就能得到上面对应的值了

离散时间位置误差推导

连续时间下位置的导数
$\dot{\Delta \alpha }=\Delta \beta$
根据导数的定义
$\Delta {\alpha }_{k+1}=\Delta {\alpha }_k+\dot{\Delta \alpha }\cdot \Delta t$
代入 $\dot{\Delta \alpha }=\Delta \beta$
$=>\Delta {\alpha }_{k+1}=\Delta {\alpha }_k+\Delta \beta \cdot \Delta t$
根据中值积分
$=>\Delta {\alpha }_{k+1}=\Delta {\alpha }_k+\frac{\Delta {\beta }_k+\Delta {\beta }_{k+1}}{2}\cdot \Delta t$
然后把上面的$\Delta {\beta }_{k+1}=\Delta {\beta }_k-[\frac{1}{2}[R_k(a_k-b_a{)}^{\hat{}}\Delta {\theta }_k+R_{k+1}(a_{k+1}-b_a{)}^{\hat{}}\Delta {\theta }_{k+1}]+\frac{1}{2}(R_k+R_{k+1})\cdot \Delta b_a+\frac{1}{2}(R_k{n}_k+R_{k+1}{n}_{k+1})]\cdot \Delta t$ 代入进去
里面把 $\Delta {\beta }_k,\Delta {\theta }_k$ 提取出来 ，然后还是基础的系数归纳

离散时间零偏误差推导

连续时间零偏的求导
$\dot{\Delta b_a}=n_{ba}$
根据导数的定义
$\Delta b_{ak+1}=\Delta b_{ak}+n_{ba}\Delta t$
陀螺仪的也是一样的
$\Delta b_{wk+1}=\Delta b_{wk}+n_{bw}\Delta t$