VSLAM：IMU预积分公式推导 一、IMU预积分 传统的递推算法是根据上一时刻的IMU状态量，利用当前时刻测量得到的加速度与角速度，进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中，各个状态量都是估计值，并且会不断调整，每次调整都会重新进行积分，传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合，避免优化时重复进行积分。四元数的表示方法有两种：一种是Hamilton(右手系)表示，另一种是JPL(左手系)表示。读者对公式推导时一定注意。
常用性质：
1. 四元数的连续积分：
q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in[k, k+1]} \dot{q}_{t} d t
2. 四元数的左乘与右乘：
q_{a} \otimes q_{b}=\mathcal{R}\left(q_{b}\right) q_{a}=\left[\begin{array}{cccc}
s_{b} & z_{b} & -y_{b} & x_{b} \\
-z_{b} & s_{b} & x_{b} & y_{b} \\
y_{b} & -x_{b} & s_{b} & z_{b} \\
-x_{b} & -y_{b} & -z_{b} & s_{b}
\end{array}\right]\left[\begin{array}{c}
x_{a} \\
y_{a} \\
z_{a} \\
s_{a}
\end{array}\right] \\
=\mathcal{L}\left(q_{a}\right) q_{b}=\left[\begin{array}{cccc}
s_{a} & -z_{a} & y_{a} & x_{a} \\
z_{a} & s_{a} & -x_{a} & y_{a} \\
-y_{a} & x_{a} & s_{a} & z_{a} \\
-x_{a} & -y_{a} & -z_{a} & s_{a}
\end{array}\right]\left[\begin{array}{c}
x_{b} \\
y_{b} \\
z_{b} \\
s_{b}
\end{array}\right]
我们将四元数表示为：
q=\left[\begin{array}{llll}
x & y & z & s
\end{array}\right]=\left[\begin{array}{ll}
\omega & s
\end{array}\right]
，则左右乘可以表示为：
\mathcal{R}(q)=\Omega(\omega)+s I_{4 \times 4}=\left[\begin{array}{cc}
-\omega^{\wedge} & \omega \\
-\omega^{T} & 0
\end{array}\right]+s I_{4 \times 4}
\mathcal{L}(q)=\Psi(\omega)+s I_{4 \times 4}=\left[\begin{array}{cc}
\omega^{\wedge} & \omega \\
-\omega^{T} & 0
\end{array}\right]+s I_{4 \times 4}
3. 四元数导数如下：
\dot{q}_{t}=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t+\delta t}-q_{t}\right)
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t} \otimes q_{t+\delta t}^{t}-q_{t} \otimes\left[\begin{array}{l}
0 \\
1
\end{array}\right]\right)
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t} \otimes\left[\begin{array}{r}
\widehat{\boldsymbol{k}} \sin \frac{\theta}{2} \\
\cos \frac{\theta}{2}
\end{array}\right]-q_{t} \otimes\left[\begin{array}{l}
0 \\
1
\end{array}\right]\right)
\approx \lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left(q_{t} \otimes\left[\begin{array}{c}
\widehat{\boldsymbol{k}} \frac{\theta}{2} \\
1
\end{array}\right]-q_{t} \otimes\left[\begin{array}{l}
0 \\
1
\end{array}\right]\right)
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left[\mathcal{R}\left(\left[\begin{array}{c}
\widehat{\boldsymbol{k}} \frac{\theta}{2} \\
1
\end{array}\right]\right)-\mathcal{R}\left(\left[\begin{array}{l}
0 \\
1
\end{array}\right]\right)\right] q_{t}
=\lim _{\delta t \rightarrow 0} \frac{1}{\delta t}\left[\begin{array}{cc}
-\frac{\theta^{\wedge}}{2} & \frac{\theta}{2} \\
-\frac{\theta^{T}}{2} & 0
\end{array}\right] q_{t}
有角速度：
\omega=\lim _{\delta t \rightarrow 0} \frac{\theta}{\delta t} ，则导数可表示为：
\dot{q}_{t}=\frac{1}{2}\left[\begin{array}{cc}
-\omega^{\wedge} & \omega \\
-\omega^{T} & 0
\end{array}\right] q_{t}=\frac{1}{2} \Omega(\omega) q_{t}=\frac{1}{2} \mathcal{R}\left(\left[\begin{array}{l}
\omega \\
0
\end{array}\right]\right) q_{t}=\frac{1}{2} q_{t} \otimes\left[\begin{array}{l}
\omega \\
0
\end{array}\right]
1.1 当前时刻的位置，速度，旋转变量的连续表达式 我们将图像帧记作
k 及
k+1 ，body坐标系下记作
b_k 和
b_{k+1} ，我们将位置，速度和旋转在时间
t_k 到
t_{k+1} 内进行积分，其世界坐标系下的公式可以写为：
p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in[k, k+1]}\left[R_{t}^{w}\left(\hat{a}_{t}-b_{a_{t}}\right)-g^{w}\right] d t^{2}
v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{t \in[k, k+1]}\left[R_{t}^{w}\left(\hat{a}_{t}-b_{a_{t}}\right)-g^{w}\right] d t
q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in[k, k+1]} \frac{1}{2} \Omega\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right) q_{t}^{b_{k}} d t
1.2 当前时刻的位置，速度，旋转变量的离散表达式 我们以中值积分给出离散表示:
p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} \bar{a}_{l} \delta t^{2}
v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\bar{a}_{l} \delta t
q_{b_{k+1}}^w=q_{b_k}^w \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\bar{\hat w_l}\delta t\end{bmatrix}
\bar{a}_{l}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)-g^{w}+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)-g^{w}\right]
\overline{\widehat{\omega}_{l}}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}
1.3 两帧之间的位置，速度，旋转增量的连续表达式 基本思想就是将参考坐标系从
w 转到第
k 帧的body坐标系下，相当于两边同时乘
R_w^{b_k} ，我们直接用论文中的公式来表示：
R_{w}^{b_{k}} p_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)+\alpha_{b_{k+1}}^{b_{k}}
R_{w}^{b_{k}} v_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(v_{b_{k}}^{w}-g^{w} \Delta t_{k}\right)+\beta_{b_{k+1}}^{b_{k}}
q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}}
其中：
\alpha_{b_{k+1}}^{b_{k}}=\iint_{t \in[k, k+1]}\left[R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)\right] d t^{2}
\beta_{b_{k+1}}^{b_{k}}=\int_{t \in[k, k+1]}\left[R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)\right] d t
\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in[k, k+1]} \frac{1}{2} \Omega\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right) \gamma_{t}^{b_{k}} d t
上述可以理解为
b_{k+1} 对
b_k 的相对运动量，其中
b_k 状态的改变并不会对其产生影响，可以将其作为非线性优化变量，避免重复计算。实际到这里，只要求解出积分，我们就完成了预积分的计算，我们的目标也就是在此。实际当中随机游走也是发生改变的，所以我们将上述变量再次进行一阶近似，我们再次使用论文中的公式进行表示：
\alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\alpha} \delta b_{a}+J_{b_{\omega}}^{\alpha} \delta b_{\omega}
\beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\beta} \delta b_{a}+J_{b_{\omega}}^{\beta} \delta b_{\omega}
\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_{\omega}}^{\gamma} \delta b_{\omega}
\end{array}\right]
至此，IMU的预积分表达式我们就已经得到了。
1.4 两帧之间的位置，速度，旋转增量的离散表达式 \hat{\alpha}_{i+1}^{b_{k}}=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} \bar{a}_{l}^{\prime} \delta t^{2}
\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+\overline{\hat{a}}_{l}^{\prime} \delta t
\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \widehat{\omega_{l}}^{\prime} \delta t
\end{array}\right] \\
\overline{\hat{a}}_{i}^{\prime}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)\right]
\overline{\widehat{\omega}_{\imath}}^{\prime}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}
1.5 连续表达式下的位置、速度、旋转增量误差、协方差、Jacobian IMU在每一个时刻积分出来的变量都是有误差的，我们针对误差进行分析，分析误差的传递方程，下面为连续时间下的误差导数方程：
\left[\begin{array}{l}
\delta \dot{\alpha}_{t}^{b_{k}} \\
\delta \dot{\beta}_{t}^{b_{k}} \\
\delta \dot{\theta}_{t}^{b_{k}} \\
\delta \dot{b}_{a_{t}} \\
\delta \dot{b}_{w_{t}}
\end{array}\right]=\left[\begin{array}{ccccc}
0 & I & 0 & 0 & 0 \\
0 & 0-R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)^{\wedge} & -R_{t}^{b_{k}} & 0 & 0 \\
0 & 0 & -\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right)^{\wedge} & 0 & -I \\
0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0
\end{array}\right]\left[\begin{array}{c}
\delta \alpha_{t}^{b_{k}} \\
\delta \beta_{t}^{b_{k}} \\
\delta \theta_{t}^{b_{k}} \\
\delta b_{a_{t}} \\
\delta b_{w_{t}}
\end{array}\right]+\left[\begin{array}{llll}
0 & 0 & 0 & 0 \\
-R_{t}^{b_{k}} & 0 & 0 & 0 \\
0 & -I & 0 & 0 \\
0 & 0 & I & 0 \\
0 & 0 & 0 & I
\end{array}\right]\left[\begin{array}{c}
n_{a} \\
n_{w} \\
n_{b_{a}} \\
n_{b_{w}}
\end{array}\right]
=F_{t} \delta z_{t}^{b_{k}}+G_{t} n_{t}
我们对
\delta \dot{\beta}_{t}^{b_{k}} 进行推导，假设true表示真实测量值，含误差，nominal表示不含噪声的理论值，则有：
\delta \dot{\beta}=\dot{\beta}_{\text {true }}-\dot{\beta}_{\text {nominal }}
其中：
\dot{\beta}_{\text {true }}=R_{t_{\text {true }}}^{b_{k}}\left(\hat{a}_{t_{\text {true }}}-b_{a_{t} \text { true }}\right)=R_{t}^{b_{k}} \exp \left(\delta \theta^{\wedge}\right)\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}\right)\\
=R_{t}^{b_{k}}\left(I+\delta \theta^{\wedge}\right)\left(\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}\right)
=R_{t}^{b_{k}}\left[\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}+\delta \theta^{\wedge}\left(\hat{a}_{t}-b_{a_{t}}\right)\right]
=R_{t}^{b_{k}}\left[\hat{a}_{t}-n_{a}-b_{a_{t}}-\delta b_{a_{t}}-\left(\hat{a}_{t}-b_{a_{t}}\right)^{\wedge} \delta \theta\right]
\dot{\beta}_{\text {nominal }}=R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)
则：
\delta \dot{\beta}=\dot{\beta}_{\text {true }}-\dot{\beta}_{\text {nominal }}=-R_{t}^{b_{k}}\left(\hat{a}_{t}-b_{a_{t}}\right)^{\wedge} \delta \theta-R_{t}^{b_{k}} \delta b_{a_{t}}-R_{t}^{b_{k}} n_{a}
我们再对
\delta \dot{\theta}_{t}^{b_{k}} 进行推导：
\dot{q}_{t_{t r u e}}=\frac{1}{2} q_{t_{t r u e}} \otimes\left[\begin{array}{c}
\omega_{t r u e} \\
0
\end{array}\right]=\frac{1}{2} q_{t} \otimes \delta q \otimes\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\
0
\end{array}\right]
\dot{q}_{t_{\text {nominal }}}=\dot{q}_{t}=\frac{1}{2} q_{t} \otimes\left[\begin{array}{c}
\hat{\omega}_{t}-b_{\omega_{t}} \\
0
\end{array}\right]
根据四元数的导数性质：
\dot{q}_{t_{t r u e}}=\left(q_{t} \dot{\otimes} \delta q\right)=\dot{q}_{t} \otimes \delta q+q_{t} \otimes \dot{\delta q}
=\frac{1}{2} q_{t} \otimes\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}} \\
0
\end{array}\right] \otimes \delta q+q_{t} \otimes \dot{\delta q}
我们将上面的等式进行汇总：
\frac{1}{2} q_{t} \otimes \delta q \otimes\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\
0
\end{array}\right]=\frac{1}{2} q_{t} \otimes\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}} \\
0
\end{array}\right] \otimes \delta q+q_{t} \otimes \dot{\delta q} \\
\frac{1}{2} \delta q \otimes\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\
0
\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}} \\
0
\end{array}\right] \otimes \delta q+\dot{\delta q} \\
2 \dot{\delta q}=\delta q \otimes\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\
0
\end{array}\right]-\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}} \\
0
\end{array}\right] \otimes \delta q \\
2 \dot{\delta q}=\mathcal{R}\left(\left[\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}} \\
0
\end{array}\right]\right) \delta q-\mathcal{L}\left(\begin{array}{c}
\widehat{\omega}_{t}-b_{\omega_{t}} \\
0
\end{array}\right) \delta q \\
2 \dot{\delta} q=\left[\begin{array}{cc}
-\left(2 \widehat{\omega}_{t}-2 b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}}\right)^{\wedge} & -n_{\omega}-\delta b_{\omega_{t}} \\
\left(n_{\omega}+\delta b_{\omega_{t}}\right)^{T} & 0
\end{array}\right]\left[\begin{array}{l}
\frac{\delta \theta}{2} \\
1
\end{array}\right]
最终：
\dot{\delta} \theta=-\left(2 \widehat{\omega}_{t}-2 b_{\omega_{t}}-n_{\omega}-\delta b_{\omega_{t}}\right)^{\wedge} \frac{\delta \theta}{2}-n_{\omega}-\delta b_{\omega_{t}}
\approx-\left(\widehat{\omega}_{t}-b_{\omega_{t}}\right)^{\wedge} \delta \theta-n_{\omega}-\delta b_{\omega_{t}}
我们将导数的定义拿来：
\delta \dot{z}_{t}^{b_{k}}=\lim _{\delta t \rightarrow 0} \frac{\delta z_{t+\delta t}^{b_{k}}-\delta z_{t}^{b_{k}}}{\delta t}
则：
\delta z_{t+\delta t}^{b_{k}}=\delta z_{t}^{b_{k}}+\delta \dot{z}_{t}^{b_{k}} \delta t=\left(\mathrm{I}+F_{t} \delta t\right) \delta z_{t}^{b_{k}}+\left(G_{t} \delta t\right) n_{t}
对比扩展卡尔曼公式，我们发现其正好是非线性系统进行线性的表达式，据此，我们给出相似的协方差预测公式：
P_{t+\delta t}^{b_{k}}=\left(\mathrm{I}+F_{t} \delta t\right) P_{t}^{b_{k}}\left(\mathrm{I}+F_{t} \delta t\right)^{T}+\left(G_{t} \delta t\right) Q\left(G_{t} \delta t\right)^{T}
其中，协方差初始值为0，噪声协方差矩阵可以表示为：
Q^{12 \times 12}=\left[\begin{array}{cccc}
\sigma_{a}^{2} & 0 & 0 & 0 \\
0 & \sigma_{w}^{2} & 0 & 0 \\
0 & 0 & \sigma_{b_{a}}^{2} & 0 \\
0 & 0 & 0 & \sigma_{b_{w}}^{2}
\end{array}\right]
类似，我们也可以获得误差的Jacobian迭代公式：
J_{t+\delta t}=\left(\mathrm{I}+F_{t} \delta t\right) J_{t}
Jacobian的初始值为单位矩阵。
1.6 离散形式的增量分析 实际只需根据中值积分，将连续形式表达式进行离散化即可，推导过程省略，比较简单
\left[\begin{array}{l}
\delta \alpha_{k+1} \\
\delta \theta_{k+1} \\
\delta \beta_{k+1} \\
\delta b_{a_{k+1}} \\
\delta b_{w_{k+1}}
\end{array}\right]=\left[\begin{array}{ccccc}
I & f_{01} & \delta t & f_{03} & f_{04} \\
0 & f_{11} & 0 & 0 & -\delta t \\
0 & f_{21} & I & f_{23} & f_{24} \\
0 & 0 & 0 & I & 0 \\
0 & 0 & 0 & 0 & I
\end{array}\right]\left[\begin{array}{l}
\delta \alpha_{k} \\
\delta \theta_{k} \\
\delta \beta_{k} \\
\delta b_{a_{k}} \\
\delta b_{w_{k}}
\end{array}\right]
+\left[\begin{array}{cccccc}
v_{00} & v_{01} & v_{02} & v_{03} & 0 & 0 \\
0 & \frac{-\delta t}{2} & 0 & \frac{-\delta t}{2} & 0 & 0 \\
-\frac{R_{k} \delta t}{2} & v_{21} & -\frac{R_{k+1} \delta t}{2} & v_{23} & 0 & 0 \\
0 & 0 & 0 & 0 & \delta t & 0 \\
0 & 0 & 0 & 0 & 0 & \delta t
\end{array}\right]\left[\begin{array}{c}
n_{a_{k}} \\
n_{w_{k}} \\
n_{a_{k+1}} \\
n_{w_{k+1}} \\
n_{b_{a}} \\
n_{b_{w}}
\end{array}\right]
其中：
f_{01}=\frac{\delta t}{2} f_{21}=-\frac{1}{4} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t^{2}-\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t^{2}
f_{03}=-\frac{1}{4}\left(R_{k}+R_{k+1}\right) \delta t^{2}
f_{04}=\frac{\delta t}{2} f_{24}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{3}
f_{11}=I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t
f_{21}=-\frac{1}{2} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t-\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t
f_{23}=-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t
f_{23}=-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t
f_{24}=\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}
v_{00}=-\frac{1}{4} R_{k} \delta t^{2}
v_{01}=v_{03}=\frac{\delta t}{2} v_{21}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} \frac{\delta t}{2}
v_{02}=-\frac{1}{4} R_{k+1} \delta t^{2}
v_{21}=v_{23}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}
离散误差传递方程可以简写为：
\delta z_{k+1} 15 \times 1=F^{15 \times 15} \delta z_{k}^{15 \times 1}+V^{15 \times 18} Q^{18 \times 1}
则Jacobian的迭代公式为：
J_{k+1}{ }^{15 \times 15}=F J_{k}
Jacobian的初始值为：
J_{k}=I 。注意，我们在此计算Jacobian，仅仅是为了后端非线性优化过程对bias的计算提供帮助。
协方差的迭代公式：
P_{k+1}{ }^{15 \times 15}=F P_{k} F^{T}+V Q V^{T}
协方差矩阵初始值为0，噪声的协方差矩阵为:
Q^{18 \times 18}=\left[\begin{array}{cccccc}
\sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0 \\
0 & \sigma_{w}^{2} & 0 & 0 & 0 & 0 \\
0 & 0 & \sigma_{a}^{2} & 0 & 0 & 0 \\
0 & 0 & 0 & \sigma_{w}^{2} & 0 & 0 \\
0 & 0 & 0 & 0 & \sigma_{b_{a}}^{2} & 0 \\
0 & 0 & 0 & 0 & 0 & \sigma_{b_{w}}^{2}
\end{array}\right]
