VINS-Mono-IMU预积分 （五：连续时间预积分误差状态传递）

以上是结论
接下来将推导上面的公式是如何来的
估计值的求导也符合真实值求导的规则，下面会用这个规则对一些变量进行消除

位置求导

$P$ 为估计值，$P^{{}^{\prime }}=V$，真值为 $P_t=P+\Delta P$ ，对其进行求导 $(P+\Delta P{)}^{{}^{\prime }}=V+\Delta V=>P^{{}^{\prime }}+\Delta P^{{}^{\prime }}=V+\Delta V=>\Delta P^{{}^{\prime }}=\Delta V$

零偏求导

$b_t^{{}^{\prime }}=0=>(b+\Delta b^{{}^{\prime }})=0=>b^{{}^{\prime }}+\Delta b^{{}^{\prime }}=0$，$b^{{}^{\prime }}$实际是一个高斯分布$n_b$，代入$则\Delta b^{{}^{\prime }}=-n_b$，正负都一样。
$b_t=b_b+\Delta b_b$

速度求导

速度是相对于 $b_k$ 时刻的速度 ,要变换到bk时刻下，真实值$\dot{V_t}=R_{b_k,t}\cdot (a-ba)$，按照真实值=估计值+误差值的推导。
对于速度和旋转变换是估计值+误差值的形式，而加速度则是$\hat{a}=a_t+b_a+n_a$ 意思是测量值等于真值+零偏+噪声
$(V+\Delta V{)}^{{}^{\prime }}=R\cdot \Delta R(a-n_a-b_a-\Delta b_a)$。
根据十四讲的内容，$\Delta R=exp({\Phi }^{\hat{}})$，Φ是旋转向量（李代数），对其进行一阶泰勒展开=$(I+\Delta \Phi \hat{})$，将其代入展开
$V^{{}^{\prime }}+\Delta V^{{}^{\prime }}=R(a-n_a-b_a-\Delta b_a)+R\cdot \Delta \Phi \hat{}\cdot (a-n_a-b_a-\Delta b_a)$ ②
由于真实值 $\dot{V_t}=R_{b_k,t}\cdot (a-ba)$，实际上估计值也是可以写成相同的形式，知识$a,b_a$是测量值而已，则根据上面的式子，估计值速度的导数 $\dot{V}=R\cdot (a-ba)$ ，代入上式 ②，则可以把 $\dot{V}$ 给抵消掉，变成
$\Delta V^{{}^{\prime }}=R(-n_a-\Delta b_a)+R\cdot \Delta \Phi \hat{}(a-b_a)$
说明: $\Delta \Phi \hat{}$实际上是一个一阶小量，而 $n_a,\Delta b_a$ 也是一阶小量，它们分别是相对于加速度和零偏的误差值，一阶小量乘一阶小量等于二阶，二阶是相对于一阶的无穷小，由于在这里只维护一阶小量，所以括号内忽略掉 $n_a,\Delta b_a$

利用 $a^{\hat{}}\cdot b=a\times b=-b\times a=-b^{\hat{}}\cdot a$ 的性质
$\Delta V^{{}^{\prime }}=R(-n_a-\Delta b_a)+R\cdot \Delta {\Phi }^{\hat{}}(a-b_a)$ =>$\Delta V^{{}^{\prime }}=R(-n_a-\Delta b_a)-R\cdot (a-b_a{)}^{\hat{}}\cdot \Delta \Phi$ ΔΦ表示李代数（旋转向量）
变换一下
$\Delta V^{{}^{\prime }}=-R\cdot (a-b_a{)}^{\hat{}}\cdot \Delta \Phi -R{n}_a-R\Delta b_a$
这样就和最上面论文中的公式对应上了

位姿求导

根据第一节的求导可得 $\dot{q}=\frac{1}{2}\Omega (w)\cdot q$ ，不过这里是矩阵的乘法，$\dot{q}=\frac{1}{2}[q(w){]}_R\cdot q$ ，这个 $q(w)$ 是一个是实部为 0，虚部为向量的一个四元数 ，变回四元数乘法则为 $=>\dot{q_t}=\frac{1}{2}q\oplus w=\frac{1}{2}{q}_t\oplus (w_t-b_{wt})$ , $w$ 为陀螺仪的角速度测量值，上面的 $\dot{q}$ 是当作真实值，现在写成真实值=估计值+误差值的形式

$(q\oplus \Delta q{)}^{{}^{\prime }}=\frac{1}{2}q\oplus \Delta q\oplus (w-n_w-b_w-\Delta b_w)$，根据四元数乘法求导的规则，只对左边进行变形 $=>\dot{q}\oplus \Delta q+q\oplus \dot{\Delta q}=...$
由于估计值的导数和真实值是一样的，只是下标不同
$=>\frac{1}{2}q\oplus (w-b_w)\oplus \Delta q+q\oplus \dot{\Delta q}=\frac{1}{2}q\oplus \Delta q\oplus (w-n_w-b_w-\Delta b_w)$ ，根据乘法结合律，约掉 $q$ ，四元数乘法不符合乘法交换律但是符合乘法结合律
$=>\frac{1}{2}(w-b_w)\oplus \Delta q+\dot{\Delta q}=\frac{1}{2}\Delta q\oplus (w-n_w-b_w-\Delta b_w)$
同时乘以2
$=>(w-b_w)\oplus \Delta q+2\cdot \dot{\Delta q}=\Delta q\oplus (w-n_w-b_w-\Delta b_w)$
把 $(w-b_w)->y,(w-n_w-b_w-\Delta b_w)->x$ 通过这样进行替换把式子变得更简洁
把导数放到一边
$2\cdot \dot{\Delta q}=\Delta q\oplus x-y\oplus \Delta q$，为了把 $\Delta q$ 提到一边，把四元数乘法变成矩阵乘法
$=>2\cdot \dot{\Delta q}=([x{]}_R-[y{]}_L)\cdot \Delta q$
注意 $x$ 和 $y$ 是一个实部为0的四元数

根据这个公式（第二节有讲），只需要考虑内部的相减，不用考虑实部的相减即可
$\Delta q=\left[\begin{array}{c}cos\frac{\theta }{2}\\ \vec{n}\cdot sin\frac{\theta }{2}\end{array}\right]=\left[\begin{array}{c}1\\ \frac{\Delta \theta }{2}\end{array}\right]$ 其中 $\Delta \theta 是矢量=w\cdot \Delta t$
$2\dot{\Delta q}=\left[\begin{array}{c}0\\ \Delta {\theta }^{{}^{\prime }}\end{array}\right]$ 这里面把 2 乘进去了
右边的$([x{]}_R-[y{]}_L)\cdot \Delta q$ 写成矩阵有$=(\left[\begin{array}{cc}0& -x^T\\ x& -x^{\hat{}}\end{array}\right]-\left[\begin{array}{cc}0& -y^T\\ y& -y^{\hat{}}\end{array}\right])\cdot \left[\begin{array}{c}1\\ \frac{\Delta \theta }{2}\end{array}\right]$

合并则有$\left[\begin{array}{cc}0& -x^T+y^T\\ x-y& -x^{\hat{}}-y^{\hat{}}\end{array}\right]\cdot \left[\begin{array}{c}1\\ \frac{\Delta \theta }{2}\end{array}\right]$，$\left[\begin{array}{cc}0& -x^T+y^T\\ x-y& -x^{\hat{}}-y^{\hat{}}\end{array}\right]$∈4×4，第一行的是1×4，第2行是3×4，由于 $2\dot{\Delta q}=\left[\begin{array}{c}0\\ \Delta {\theta }^{{}^{\prime }}\end{array}\right]\in 4\times 1$其中的实部为 0 ，所以只考虑虚部相乘的部分
$=>\Delta {\theta }^{{}^{\prime }}=x-y-(x+y{)}^{\hat{}}\cdot \frac{\theta }{2}$ 现在要把 $(w-b_w)->y,(w-n_w-b_w-\Delta b_w)->x$ 替换回去
$=>\Delta {\theta }^{{}^{\prime }}=-n_w-\Delta b_w-(2w-2b_w{)}^{\hat{}}\cdot \frac{\Delta \theta }{2}$由于 $\Delta \theta$ 是一阶无穷小，所以 $(x-y)$ 里面的 $n_w,\Delta b_w$ 是可以忽略的
约掉2 $=>\Delta {\theta }^{{}^{\prime }}=-n_w-\Delta b_w-(w-b_w{)}^{\hat{}}\cdot \Delta \theta$ 这样就和论文中的公式完全一致了

说明： 我们最后拿到IMU的值是离散的，我们要从连续推导离散的一个形式，这个只是一个中间形式，离散的形式才是最终的形式，将在下一节讲解