附录

MU误差状态传播详细推导

$$\begin{array}{rl}{}^I{\dot{\overline{q}}}_W(t)& =\frac{1}{2}\mathrm{\Omega }(\omega (t){)}^I{\overline{q}}_W(t)(1)\\ {}^W{\dot{\mathbf{p}}}_I(t)& ={}^W{\mathbf{v}}_I(t)(2)\\ {}^W{\dot{\mathbf{v}}}_I(t)& ={}^W\mathbf{a}(t)(3)\\ \dot{{\mathbf{b}}_g}(t)& ={\mathbf{n}}_{wg}(t)(4)\\ {\dot{\mathbf{b}}}_a(t)& ={\mathbf{n}}_{wa}(t)(5)\\ {\omega }_m(t)& =\omega (t)+{\mathbf{b}}_g(t)+{\mathbf{n}}_g(t)(6)\\ {\mathbf{a}}_m(t)& =\mathbf{C}({}^I{\overline{q}}_W(t))({}^W\mathbf{a}(t)+{}^W\mathbf{g})+{\mathbf{b}}_a(t)+{\mathbf{n}}_a(t)(7)\end{array}$$

其中，${\omega }_m$和${\mathbf{a}}_m$为IMU直接输出（包含偏置和噪声）的线性加速度和角速度，$\mathbf{C}({}^I{\overline{q}}_W(t))$为四元数对应的旋转矩阵

角度误差状态

公式(1)四元数对时间导数的详细推导过程：

$$\begin{array}{rl}{}_W^{L(t)}\dot{\overline{q}}(t)& =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{1}{\mathrm{\Delta }t}\left(\begin{array}{c}{}_W^{L(t+\mathrm{\Delta }t)}\overline{q}{-}_W^{L(t)}\overline{q})\end{array}\right)\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{1}{\mathrm{\Delta }t}(l_{L(t)}^{L(t+\mathrm{\Delta }t)}\overline{q}{\otimes }_W^{L(t)}\overline{q}-{\overline{q}}_0{\otimes }_W^{L(t)}\overline{q})\\ & \approx \underset{\mathrm{\Delta }t\to 0}{lim}\frac{1}{\mathrm{\Delta }t}([\begin{array}{c}\frac{1}{2}\cdot \tilde{\mathit{\theta }}\\ 1\end{array}]-\left[\begin{array}{c}\mathbf{0}\\ 1\end{array}\right]){\otimes }_W^{L(t)}\overline{q}\\ & =\frac{1}{2}[\begin{array}{c}\mathit{\omega }\\ 0\end{array}]{\otimes }_W^{L(t)}\overline{q}\\ & =\frac{1}{2}[\begin{array}{cc}-|\mathit{\omega }|\times & \mathit{\omega }\\ -{\mathit{\omega }}^{\mathrm{T}}& 0\end{array}]{}_W^{L(t)}\overline{q}\\ & =\frac{1}{2}\mathbf{\Omega }(\mathit{\omega }{)}_W^{L(t)}\overline{q}\end{array}$$

其中，

$$\mathrm{\Omega }(\omega )=[\begin{array}{cc}-|\omega {|}^{\times }& \omega \\ -{\omega }^{\mathrm{\top }}& 0\end{array}],|\omega {|}^{\times }\triangleq \left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$

连续状态误差方程:

$$\begin{array}{rl}\overline{q}& =\tilde{\overline{q}}\otimes \hat{\overline{q}}(8)\\ \dot{\overline{q}}& =\dot{\tilde{\overline{q}}}\otimes \hat{\overline{q}}+\tilde{\overline{q}}\otimes \dot{\hat{\overline{q}}}(9)\end{array}$$

其中，$\overline{q}$是JPL单位四元数，$\tilde{\overline{q}}$是四元数旋转误差，$\hat{\overline{q}}$是四元数估计值

带入之前(1)的求导结果改写为：

$$\begin{array}{rl}\frac{1}{2}[\begin{array}{c}\mathit{\omega }\\ 0\end{array}]\otimes \overline{q}& =\dot{\tilde{\overline{q}}}\otimes \hat{\overline{q}}+\tilde{\overline{q}}\otimes (\frac{1}{2}[\begin{array}{c}\hat{\mathit{\omega }}\\ 0\end{array}]\otimes \hat{\overline{q}})(10)\\ \dot{\tilde{\overline{q}}}\otimes \hat{\overline{q}}& =\frac{1}{2}([\begin{array}{c}\mathit{\omega }\\ 0\end{array}]\otimes \overline{q}-\tilde{\overline{q}}\otimes \left[\begin{array}{c}\hat{\mathit{\omega }}\\ 0\end{array}\right]\otimes \hat{\overline{q}})(11)\\ \dot{\tilde{\overline{q}}}& =\frac{1}{2}([\begin{array}{c}\mathit{\omega }\\ 0\end{array}]\otimes \tilde{\overline{q}}-\tilde{\overline{q}}\otimes \left[\begin{array}{c}\hat{\mathit{\omega }}\\ 0\end{array}\right])(12)\end{array}$$

关于陀螺仪的理想值，通过：

$$\begin{array}{c}{\mathit{\omega }}_m(t)=\mathit{\omega }(t)+{\mathbf{b}}_g(t)+{\mathbf{n}}_g(t)\\ \hat{\mathit{\omega }}={\mathit{\omega }}_m-{\hat{\mathbf{b}}}_g\\ {\tilde{\mathbf{b}}}_g={\mathbf{b}}_g-\widehat{{\mathbf{b}}_g}\end{array}$$

推出：

$$\mathit{\omega }=\hat{\mathit{\omega }}-{\tilde{\mathbf{b}}}_g-{\mathbf{n}}_{\mathrm{g}}$$

带入后：

$$\begin{array}{rl}& \dot{\tilde{\overline{q}}}=\frac{1}{2}([\begin{array}{c}\hat{\mathit{\omega }}\\ 0\end{array}]\otimes \tilde{\overline{q}}-\tilde{\overline{q}}\otimes \left[\begin{array}{c}\hat{\mathit{\omega }}\\ 0\end{array}\right])-\frac{1}{2}\left[\begin{array}{c}{\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathrm{g}}\\ 0\end{array}\right]\otimes \tilde{\overline{q}}\\ & =\frac{1}{2}[\begin{array}{cc}-2|\hat{\mathit{\omega }}{|}^{\times }& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 1}^{\mathrm{\top }}& 0\end{array}]\cdot \tilde{\overline{q}}-\frac{1}{2}\left[\begin{array}{c}{\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathrm{g}}\\ 0\end{array}\right]\otimes \tilde{\overline{q}}\\ & =\frac{1}{2}[\begin{array}{cc}-2|\hat{\mathit{\omega }}{|}^{\times }& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 1}^{\mathrm{\top }}& 0\end{array}]\cdot \tilde{\overline{q}}-\frac{1}{2}\left[\begin{array}{cc}-|{\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathrm{g}}{|}^{\times }& ({\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathrm{g}})\\ -({\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathrm{g}}{)}^{\mathrm{\top }}& 0\end{array}\right]\cdot \left[\begin{array}{c}\tilde{\mathbf{q}}\\ 1\end{array}\right]\\ & =\frac{1}{2}[\begin{array}{cc}-2|\hat{\mathit{\omega }}{|}^{\times }& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 1}^{\mathrm{T}}& 0\end{array}]\cdot \tilde{\overline{q}}-\frac{1}{2}\left[\begin{array}{c}({\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathrm{g}})\\ 0\end{array}\right]-O(|{\tilde{\mathbf{b}}}_g\parallel \tilde{\mathbf{q}}|,|{\mathbf{n}}_{\mathrm{g}}|\left|\tilde{\mathbf{q}}\right|)\end{array}$$

忽略⼆阶项:

$$\dot{\tilde{\overline{q}}}=\left[\begin{array}{c}-{\mathbf{|}\hat{\mathit{\omega }}|}^{\times }\tilde{\mathbf{q}}-\frac{1}{2}({\tilde{\mathbf{b}}}_g+{\mathbf{n}}_{\mathbf{g}})\\ 0\end{array}\right]$$

其中，

$$\begin{gathered} \tilde{\overline{q}}=[\begin{array}{c}\tilde{\overline{q}}\\ {\tilde{\mathit{q}}}_4\end{array}]=[\begin{array}{c}\hat{\mathbf{k}}\sin (\tilde{\mathit{\theta }}/2)\\ \cos (\tilde{\mathit{\theta }}/2)\end{array}] \\ \approx [\begin{array}{c}\frac{1}{2}\tilde{\mathit{\theta }}\\ 1\end{array}] \end{gathered}$$

代入得：

$$\dot{\tilde{\mathit{\theta }}}=-|\hat{\mathit{\omega }}{|}^{\times }\tilde{\mathit{\theta }}-{\tilde{\mathbf{b}}}_g-{\mathbf{n}}_{\mathrm{g}}$$

速度误差状态

估计值：

$${}^W{\dot{\hat{\mathbf{v}}}}_I(t)=\mathbf{C}{({}^I{\hat{\overline{q}}}_W(t))}^{\mathrm{\top }}{}^I\hat{\mathbf{a}}(t)-{}^W\mathbf{g}$$

理想值：

$$\begin{array}{c}{}^W{\dot{\mathbf{v}}}_I(t)={}^W\mathbf{a}(t)\\ {\mathbf{a}}_m(t)=\mathbf{C}({}^I{\overline{q}}_W(t))({}^W\mathbf{a}(t)+{}^W\mathbf{g})+{\mathbf{b}}_a(t)+{\mathbf{n}}_a(t)\end{array}$$

误差：

$$\tilde{\mathbf{v}}=\mathbf{v}-\hat{\mathbf{v}}$$

两边对时间求导数：

$$\begin{array}{rl}\dot{\tilde{\mathbf{v}}}& =\mathbf{C}{\left({}^I{\overline{q}}_W\right)}^{\mathrm{\top }}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)-\mathbf{C}{\left({}^I{\hat{\overline{q}}}_W\right)}^{\mathrm{\top }}\hat{\mathbf{a}}\\ & =\mathbf{C}{(\tilde{\overline{q}}\otimes \hat{\overline{q}})}^{\mathrm{\top }}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}\hat{\mathbf{a}}\\ & =\mathbf{C}(\hat{\overline{q}}{)}^T\mathbf{C}{\left(\tilde{\overline{q}}\right)}^{\mathrm{\top }}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}\hat{\mathbf{a}}\end{array}$$

通过${}_W^I\mathbf{C}(\tilde{\overline{q}})\approx {\mathbf{I}}_{3\times 3}-|\tilde{\mathit{\theta }}{|}^{\times }$,${\tilde{\mathbf{b}}}_a={\mathbf{b}}_a-\widehat{{\mathbf{b}}_a}$,$\hat{\mathbf{a}}={\mathbf{a}}_m-\widehat{{\mathbf{b}}_a}$，求得：

$$\begin{array}{rl}\dot{\tilde{\mathbf{v}}}& =\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}({\mathbf{I}}_{3\times 3}+|\tilde{\mathit{\theta }}{|}^{\times })(\hat{\mathbf{a}}-{\tilde{\mathbf{b}}}_a-{\mathbf{n}}_a)-\mathbf{C}{\left(\hat{q}\right)}^{\mathrm{\top }}\hat{\mathbf{a}}\\ & =-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}{\tilde{\mathbf{b}}}_a-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}{\mathbf{n}}_a+\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}|\tilde{\mathit{\theta }}{|}^{\times }\hat{\mathbf{a}}\\ & =-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}{\tilde{\mathbf{b}}}_a-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}{\mathbf{n}}_a-\mathbf{C}{\left(\hat{\overline{q}}\right)}^{\mathrm{\top }}|\hat{\mathbf{a}}{|}^{\times }\tilde{\mathit{\theta }}\end{array}$$

位置误差状态

估计值：

$${}^W{\dot{\hat{\mathbf{p}}}}_I(t){=}^W{\hat{\mathbf{v}}}_I(t)+(\mathbf{C}({}^I{\hat{\overline{q}}}_W(t){)}^T{}^I\hat{\mathbf{a}}(t)-{}^W\mathbf{g})\mathrm{\Delta }t$$

理想值：

$$\begin{array}{c}{}^W{\dot{\mathbf{p}}}_I(t)={{}^W\mathbf{v}}_I(t)+{}^W\mathbf{a}(t)\mathrm{\Delta }t\\ {\mathbf{a}}_m(t)=\mathbf{C}({}^I{\overline{q}}_W(t))({}^W\mathbf{a}(t)+{}^W\mathbf{g})+{\mathbf{b}}_a(t)+{\mathbf{n}}_a(t)\end{array}$$

误差：

$$\tilde{\mathbf{p}}=\mathbf{p}-\hat{\mathbf{p}}$$

两边对时间求导数：

$$\begin{array}{rl}\dot{\tilde{\mathbf{p}}}& =\mathbf{v}-\hat{\mathbf{v}}+(\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}\mathbf{C}(\tilde{\overline{q}}{)}^{\mathrm{\top }}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)-\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}\hat{\mathbf{a}})\mathrm{\Delta }t\\ & =\hat{\mathbf{v}}+\tilde{\mathbf{v}}-\hat{\mathbf{v}}+(\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}\mathbf{C}(\tilde{\overline{q}}{)}^{\mathrm{\top }}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)-\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}\hat{\mathbf{a}})\mathrm{\Delta }t\\ & =\tilde{\mathbf{v}}+(\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}\mathbf{C}(\tilde{\overline{q}}{)}^{\mathrm{\top }}({\mathbf{a}}_m-{\mathbf{b}}_a-{\mathbf{n}}_a)-\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}\hat{\mathbf{a}})\mathrm{\Delta }t\end{array}$$

通过${}_G^L\mathbf{C}(\tilde{\overline{q}})\approx {\mathbf{I}}_{3\times 3}-|\tilde{\mathit{\theta }}{|}^{\times }$[13],${\tilde{\mathbf{b}}}_a={\mathbf{b}}_a-\widehat{{\mathbf{b}}_a}$,$\hat{a}=a_m-\widehat{{\mathbf{b}}_a}$，求得：

$$\dot{\tilde{\mathbf{p}}}=\tilde{\mathbf{v}}-\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}{\tilde{\mathbf{b}}}_a\mathrm{\Delta }t-\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}{\mathbf{n}}_a\mathrm{\Delta }t-\mathbf{C}(\hat{\overline{q}}{)}^{\mathrm{\top }}|\hat{\mathbf{a}}\mathrm{\Delta }t{|}^{\times }\tilde{\mathit{\theta }}$$

将所有误差状态写成矩阵的形式:

$$\mathbf{F}=\left[\begin{array}{ccccc}-|\hat{\mathit{\omega }}{|}^{\times }& {\mathbf{0}}_3& {\mathbf{0}}_3& -{\mathbf{I}}_3& {\mathbf{0}}_3\\ -{}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}|\hat{\mathbf{a}}\mathrm{\Delta }t{|}^{\times }& {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3& -{}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }t\\ {-}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}|\hat{\mathbf{a}}{|}^{\times }& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {-}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}\\ 0_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right](47)$$

进⼀步做离散化：

$$\begin{array}{rl}\mathbf{\Phi }(t+\mathrm{\Delta }t,t)& =\exp \left(\mathbf{F}\mathrm{\Delta }t\right)\\ & ={\mathbf{I}}_{6\times 6}+\mathbf{F}\mathrm{\Delta }t+\frac{1}{2!}{\mathbf{F}}^2\mathrm{\Delta }{t}^2+\dots \end{array}$$$$\mathbf{\Phi }=\left[\begin{array}{ccccc}{\mathbf{I}}_3-|\hat{\mathit{\omega }}\mathrm{\Delta }t{|}^{\times }& {\mathbf{0}}_3& {\mathbf{0}}_3& -{\mathbf{I}}_3\mathrm{\Delta }t& {\mathbf{0}}_3\\ -{}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}|\hat{\mathbf{a}}\mathrm{\Delta }{t}^2{|}^{\times }& {\mathbf{I}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3& -{}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }{t}^2\\ {-}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}|\hat{\mathbf{a}}\mathrm{\Delta }t{|}^{\times }& {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3& {-}_W^I{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }t\\ 0_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3\end{array}\right](47)$$

车轮里程计观测误差状态传播详细推导

车轮里程计预积分

考虑在两个克隆时间点$t_i$和$t_{i+1}$之间对轮式里程计测量进行预积分。连续时间下的2D运动学模型在时间区间$t_{\tau }\in [t_i,t_{i+1}]$内给定为：

$$\left[\begin{array}{c}{}_{O_i}^{O_{\tau }}\dot{\theta }\\ {}^{O_i}{\dot{x}}_{O_{\tau }}\\ {}^{O_i}{\dot{y}}_{O_{\tau }}\end{array}\right]=\left[\begin{array}{c}{-}^{O_{\tau }}\omega \\ {}^{O_{\tau }}v\cos {(}_{\tau }^{O_i}\theta )\\ {}^{O_{\tau }}v\sin {(}_{\tau }^{O_i}\theta )\end{array}\right]=\left[\begin{array}{c}{-}^{O_{\tau }}\omega \\ {}^{O_{\tau }}v\cos {(}_i^{O_{\tau }}\theta )\\ {-}^{O_{\tau }}v\sin {(}_i^{O_{\tau }}\theta )\end{array}\right](23)$$

其中，$\begin{array}{c}{O}_{\tau }\\ O_i\end{array}\theta$是局部偏航角，${}^{O_i}{x}_{O_{\tau }}$和${}^{O_i}{y}_{O_{\tau }}$是$\{O_{\tau }\}$在起始积分坐标系$\left\{O_i\right\}$中的二维位置。请注意，使用${-}^{O\tau }\omega$和${-}^{O_{\tau }}v\sin {(}_{O_i}^{O_{\tau }}\theta )$是因为遵循全局到局部的方向表示。还请注意，该模型揭示了2D方向在积分周期内的演变事实。

为了在不访问状态估计（特别是方向）的情况下，将从时间步$i$到$\tau +1$的所有轮式里程计测量局部组合起来，可以执行以下测量的积分：

$$\begin{array}{rl}\begin{array}{c}{}_{O_i}^{O_{\tau +1}}\theta \end{array}& {=}_{O_i}^{O_{\tau }}\theta -{\int }_{t_{\tau }}^{t_{\tau +1}}{}^{O_t}\omega dt(24)\\ & \approx {}_{O_i}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega \mathrm{\Delta }t(25)\\ {}^{O_i}{x}_{O_{\tau +1}}& ={}^{O_i}{x}_{O_{\tau }}+{\int }_{t_{\tau }}^{t_{\tau +1}}{}^{O_t}v\cos {(}_{O_i}^{O_t}\theta )dt(26)\\ & \approx {{}^{O_i}x}_{O_{\tau }}+{\int }_{t_{\tau }}^{t_{\tau +1}}v\cos {(}_{O_i}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega (t-t_{\tau }))dt(27)\\ & ={}^{O_i}{x}_{O_{\tau }}-\frac{{}^{O_{\tau }}v(\sin {(}_{O_i}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega \mathrm{\Delta }t)-\sin {(}_{O_i}^{O_{\tau }}\theta ))}{{}^{O_{\tau }}\omega }(28)\\ {}^{O_i}{y}_{O_{\tau +1}}& ={}^{O_i}{y}_{O_{\tau }}-{\int }_{t_{\tau }}^{t_{\tau +1}}{}^{O_t}v\sin ({}^{O_t}\theta )dt(29)\\ & \approx {}^{O_i}{y}_{O_{\tau }}-{\int }_{t_{\tau }}^{t_{\tau +1}}{}^{O_{\tau }}v\sin ({}_{O_i}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega (t-t_{\tau }))dt(30)\\ & ={}^{O_i}{y}_{O_{\tau }}-\frac{{}^{O_{\tau }}v(\cos {(}_{O_i}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega \mathrm{\Delta }t)-\cos {(}_{O_i}^{O_{\tau }}\theta ))}{{}^{O_{\tau }}\omega }(31)\end{array}$$

其中，$\mathrm{\Delta }{t}_k=t_{\tau +1}-t_{\tau }$。请注意，假设${}^{O_{\tau }}\omega$和${}^{O_{\tau }}v$（离散传感器模型）是恒定的，但考虑了$t_{\tau }$和$t_{\tau +1}$之间的航向角变化，以便拥有比假设它恒定更准确的模型。

内参雅可比矩阵

轮式里程计的积分与内参${\mathbf{x}}_{WI}$​相互纠缠在一起，理想情况下，当有新的内参估计可用时，应该重新积分这些测量值，但这会使预积分失去计算效率。为了解决这个问题，对当前内参的预积分里程计测量进行线性化处理，同时适当考虑由于内参的线性化误差和噪声引起的测量不确定性：

$${\mathbf{z}}_{i+1}\simeq \mathbf{g}({\omega }_{ml(i:i+1)},{\omega }_{mr(i:i+1)},{\hat{\mathbf{x}}}_{WI})+\frac{\partial \mathbf{g}}{\partial {\tilde{\mathbf{x}}}_{WI}}{\tilde{\mathbf{x}}}_{WI}+\frac{\partial \mathbf{g}}{\partial {\mathbf{n}}_w}{\mathbf{n}}_w$$

其中，${\mathbf{n}}_w$是堆叠的噪声向量，其${\tau }_{-\mathrm{th}}$块对应于$t_{\tau }\in [t_i,t_{i+1}]$时间段内的编码器测量噪声（$\mathrm{i}.\mathrm{e}.,[n_{{\omega }_{l,\tau }}{n}_{{\omega }_{r,\tau }}{]}^{\mathrm{\top }}$）。

2D方向扰动的定义。从一般的3D方向${}_{O_i}^{O\tau }\mathbf{R}$扰动推导出这个定义：

$${}_{O_i}^{O\tau }\mathbf{R}=\exp (-{}_{O_i}^{O\tau }\mathit{\theta })=\exp ({}_{O_i}^{O\tau }\tilde{\mathit{\theta }})\exp (-{}_{O_i}^{O\tau }\hat{\mathit{\theta }})$$

其中，${}_{O_i}^{O_{\tau }}\mathit{\theta }={}_{O_i}^{O_{\tau }}\theta {\mathbf{e}}_3$，其中${\mathbf{e}}_i$是第$i$个标准单位基向量。需要注意的是，遵循JPL符号约定，使用$\exp {(}_{O_i}^{O_{\tau }}\mathit{\theta })=\exp ({-}_{O_i}^{O_{\tau }}\tilde{\mathit{\theta }})\exp {(}_{O_i}^{O_{\tau }}\hat{\mathit{\theta }})$。根据上述方程，得到：

$$\begin{array}{rl}\exp {(}_{O_i}^{O_{\tau }}\tilde{\mathit{\theta }})& =\exp ({-}_{O_i}^{O\tau }\mathit{\theta })\exp ({-}_{O_i}^{O\tau }\hat{\mathit{\theta }}{)}^{-1}\\ & =\left[\begin{array}{ccc}\cos {(}_{O_i}^{O_{\tau }}\theta )& \sin {(}_{O_i}^{O_{\tau }}\theta )& 0\\ -\sin {(}_{O_i}^{O_{\tau }}\theta )& \cos {(}_{O_i}^{O_{\tau }}\theta )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos {(}_{O_i}^{O_{\tau }}\hat{\theta })& -\sin {(}_{O_i}^{O_{\tau }}\hat{\theta })& 0\\ \sin {(}_{O_i}^{O_{\tau }}\hat{\theta })& \cos {(}_{O_i}^{O_{\tau }}\hat{\theta })& 0\\ 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos {(}_{O_i}^{O_{\tau }}\theta {-}_{O_i}^{O_{\tau }}\hat{\theta })& \sin {(}_{O_i}^{O_{\tau }}\theta {-}_{O_i}^{O_{\tau }}\hat{\theta })& 0\\ -\sin {(}_{O_i}^{O_{\tau }}\theta {-}_{O_i}^{O_{\tau }}\hat{\theta })& \cos {(}_{O_i}^{O_{\tau }}\theta {-}_{O_i}^{O_{\tau }}\hat{\theta })& 0\\ 0& 0& 1\end{array}\right]\\ & =\exp {(}_{O_i}^{O\tau }\hat{\mathit{\theta }}-{}_{O_i}^{O\tau }\mathit{\theta })\end{array}$$

计算结果可以通过去除指数函数将其简化为 ${}_{O_i}^{O\tau }\tilde{\mathit{\theta }}{=}_{O_i}^{O\tau }\hat{\mathit{\theta }}{-}_{O_i}^{O\tau }\mathit{\theta }$，并得到2D方向误差的定义为：

$${}_{O_i}^{O\tau }\tilde{\mathit{\theta }}{=}_{O_i}^{O\tau }\hat{\mathit{\theta }}{-}_{O_i}^{O\tau }\mathit{\theta }(42)$$

使用推导（42），可以从（25），（28）和（31）得到$t_{\tau }$步骤积分的雅可比矩阵：

$$\begin{array}{rl}\begin{array}{l}{}_{O_i}^{O_{\tau +1}}\tilde{\theta }\end{array}& {=}_{O_i}^{O_{\tau }}\tilde{\theta }+\mathrm{\Delta }{t}^{O_{\tau }}\tilde{\omega }(43)\\ & ={}_{O_i}^{O\tau }\tilde{\theta }+h_{\theta \omega }{}^{O_{\tau }}\tilde{\omega }(44)\\ & ={}_{O_i}^{O\tau }\tilde{\theta }+h_{\theta \omega }{\mathbf{H}}_{\omega \mathbf{x}}{\tilde{\mathbf{x}}}_{WI}+h_{\theta \omega }{\mathbf{H}}_{\omega \mathbf{n}}(45)\\ & ={}_{O_i}^{O_{\tau }}\tilde{\theta }+{\mathbf{H}}_{1,\tau }{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{2,\tau }{\mathbf{n}}_{\omega ,\tau }(46)\end{array}$$$$\begin{array}{rl}{}^{O_i}{\tilde{x}}_{O_{\tau +1}}& ={}^{O_i}{\tilde{x}}_{O_{\tau }}+\frac{{}^{O_{\tau }}\hat{v}(\cos {(}_{O_i}^{O_{\tau }}\hat{\theta }{-}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t)-\cos {(}_{O_i}^{O_{\tau }}\hat{\theta }))}{{}^{O_{\tau }}\hat{\omega }}{}^{O_i^{\tau }}\tilde{\theta }\\ & +\frac{{}^{O_{\tau }}\hat{v}(\sin {(}_{O_i}^{O_{\tau }}\hat{\theta }{-}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t)-\sin {(}_{O_i}^{O_{\tau }}\hat{\theta }){+}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t\cos {(}_{O_i}^{O_{\tau }}\hat{\theta }{-}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t))}{{}^{O_{\tau }}{\hat{\omega }}^2}{}^{O_{\tau }}\tilde{\omega }\\ & -\frac{\sin {(}_{O_i}^{O_{\tau }}\hat{\theta }-{}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t)-\sin ({}_{O_i}^{O_{\tau }}\hat{\theta })}{{}^{O_{\tau }}\hat{\omega }}{}^{O_{\tau }}\tilde{v}(47)\\ & ={}^{O_i}{\tilde{x}}_{O_{\tau }}+h_{x\theta }{}_{O_i}^{O_{\tau }}\tilde{\theta }+h_{x\omega }{}^{O_{\tau }}\tilde{\omega }+h_{xv}{}^{O_{\tau }}\tilde{v}(48)\\ & ={}^{O_i}{\tilde{x}}_{O_{\tau }}+h_{x\theta }{}_{O_i}^{O_{\tau }}\tilde{\theta }+h_{x\omega }({\mathbf{H}}_{\omega \mathbf{x}}{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{\omega \mathbf{n}}{\mathbf{n}}_{\omega ,\tau })+h_{xv}({\mathbf{H}}_{v\mathbf{x}}{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{v\mathbf{n}}{\mathbf{n}}_{\omega ,\tau })(49)\\ & ={}^{O_i}{\tilde{x}}_{O_{\tau }}+h_{x\theta }{}_{O_i}^{O_{\tau }}\tilde{\theta }+(h_{x\omega }{\mathbf{H}}_{\omega \mathbf{x}}+h_{xv}{\mathbf{H}}_{v\mathbf{x}}){\tilde{\mathbf{x}}}_{WI}+(h_{x\omega }{\mathbf{H}}_{\omega \mathbf{n}}+h_{xv}{\mathbf{H}}_{v\mathbf{n}}){\mathbf{n}}_{\omega ,\tau }(50)\\ & ={}^{O_i}{\tilde{x}}_{O_{\tau }}+{\mathbf{H}}_{3,\tau }{}_{O_i}^{O_{\tau }}\tilde{\theta }+{\mathbf{H}}_{4,\tau }{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{5,\tau }(51)\end{array}$$$$\begin{array}{rl}{}^{O_i}{\tilde{y}}_{O_{\tau +1}}& ={}^{O_i}{\tilde{y}}_{O_{\tau }}-\frac{{}^{O_{\tau }}\hat{v}(\sin {(}_{O_i}^{O_{\tau }}\hat{\theta }-{}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t)-\sin {(}_{O_i}^{O_{\tau }}\hat{\theta }))}{{}^{O_{\tau }}\hat{\omega }}{}_{O_i}^{O_{\tau }}\tilde{\theta }\\ & +\frac{{}^{O_{\tau }}\hat{v}(\cos {(}_{O_i}^{O_{\tau }}\hat{\theta }{-}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t)-\cos {(}_{O_i}^{O_{\tau }}\hat{\theta }){-}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t\sin {(}_{O_i}^{O_{\tau }}\hat{\theta }{-}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t))}{{}^{O_{\tau }}{\hat{\omega }}^2}{}^{O_{\tau }}\tilde{\omega }\\ & -\frac{\cos {(}_{O_i}^{O_{\tau }}\hat{\theta }-{}^{O_{\tau }}\hat{\omega }\mathrm{\Delta }t)-\cos ({}_{O_i}^{O_{\tau }}\hat{\theta })}{{}^{O_{\tau }}\hat{\omega }}{}^{O_{\tau }}\tilde{v}(52)\\ & ={}^{O_i}{\tilde{y}}_{O_{\tau }}+h_{y\theta }{}_{O_i}^{O_{\tau }}\tilde{\theta }+h_{y\omega }{}^{O_{\tau }}\tilde{\omega }+h_{yv}{}^{O_{\tau }}\tilde{v}(53)\\ & ={}^{O_i}{\tilde{y}}_{O_{\tau }}+h_{y\theta }{}_{O_i}^{O_{\tau }}\tilde{\theta }+h_{y\omega }({\mathbf{H}}_{\omega \mathbf{x}}{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{\omega \mathbf{n}}{\mathbf{n}}_{\omega ,\tau })+h_{yv}({\mathbf{H}}_{v\mathbf{x}}{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{v\mathbf{n}}{\mathbf{n}}_{\omega ,\tau })(54)\\ & ={}^{O_i}{\tilde{y}}_{O_{\tau }}+h_{y\theta }{}_{O_i}^{O_{\tau }}\tilde{\theta }+(h_{y\omega }{\mathbf{H}}_{\omega \mathbf{x}}+h_{yv}{\mathbf{H}}_{v\mathbf{x}}){\tilde{\mathbf{x}}}_{WI}+(h_{y\omega }{\mathbf{H}}_{\omega \mathbf{n}}+h_{yv}{\mathbf{H}}_{v\mathbf{n}}){\mathbf{n}}_{\omega ,\tau }(55)\\ & ={}^{O_i}{\tilde{y}}_{O_{\tau }}+{\mathbf{H}}_{6,\tau }{}_{O_i}^{O_{\tau }}\tilde{\theta }+{\mathbf{H}}_{7,\tau }{\tilde{\mathbf{x}}}_{WI}+{\mathbf{H}}_{8,\tau }{\mathbf{n}}_{\omega ,\tau }(56)\end{array}$$

其中，${\mathbf{H}}_{\omega \mathbf{x}},{\mathbf{H}}_{\omega \mathbf{n}},{\mathbf{H}}_{v\mathbf{x}}$和${\mathbf{H}}_{v\mathbf{n}}$是测量（22）关于内部参数和噪声的雅可比矩阵，给定估计值如下：

$$\begin{array}{rl}{}^O\tilde{\omega }& =\left[\begin{array}{ccc}\frac{-{\omega }_{ml}}{\hat{b}}& \frac{{\omega }_{mr}}{\hat{b}}& -\frac{{\omega }_{mr}{\hat{r}}_r-{\omega }_{ml}{\hat{r}}_l}{{\hat{b}}^2}\end{array}\right]\left[\begin{array}{c}{\tilde{r}}_l\\ {\tilde{r}}_r\\ \tilde{b}\end{array}\right]+\left[\begin{array}{cc}\frac{{\hat{r}}_l}{\hat{b}}& -\frac{{\hat{r}}_r}{\hat{b}}\end{array}\right]\left[\begin{array}{c}{n}_{{\omega }_l}\\ n_{{\omega }_r}\end{array}\right](57)\\ & ={\mathbf{H}}_{\omega \mathbf{x}}{\tilde{\mathbf{x}}}_{CI}+{\mathbf{H}}_{\omega \mathbf{n}}{\mathbf{n}}_{\omega }(58)\\ {}^O\tilde{v}& =\left[\begin{array}{ccc}\frac{{\omega }_{ml}}{2}& \frac{{\omega }_{mr}}{2}& 0\end{array}\right]\left[\begin{array}{c}{\tilde{r}}_l\\ {\tilde{r}}_r\\ \tilde{b}\end{array}\right]+\left[\begin{array}{cc}-\frac{{\hat{r}}_l}{2}& -\frac{{\hat{r}}_r}{2}\end{array}\right]\left[\begin{array}{c}{n}_{{\omega }_l}\\ n_{{\omega }_r}\end{array}\right](59)\\ & ={\mathbf{H}}_{v\mathbf{x}}{\stackrel{\mathbf{~}}{\mathbf{x}}}_{CI}+{\mathbf{H}}_{v\mathbf{n}}{\mathbf{n}}_{\omega }(60)\end{array}$$

可以发现，$\tau +1$步预积分的误差是$\tau$步预积分和测量误差的线性组合。利用上述方程，可以递归计算噪声协方差${\mathbf{P}}_{m,\tau +1}$和雅可比矩阵$\frac{\partial {\mathbf{g}}_{\tau +1}}{\partial {\tilde{\mathbf{x}}}_{WI}}$如下：

$$\begin{array}{rl}{\mathrm{\Phi }}_{tr,\tau }& =\left[\begin{array}{ccc}1& 0& 0\\ {\mathbf{H}}_{3,\tau }& 1& 0\\ {\mathbf{H}}_{6,\tau }& 0& 1\end{array}\right],{\mathrm{\Phi }}_{WI,\tau }=\left[\begin{array}{c}{\mathbf{H}}_{1,\tau }\\ {\mathbf{H}}_{4,\tau }\\ {\mathbf{H}}_{7,\tau }\end{array}\right],{\mathrm{\Phi }}_{n,\tau }=\left[\begin{array}{c}{\mathbf{H}}_{2,\tau }\\ {\mathbf{H}}_{5,\tau }\\ {\mathbf{H}}_{8,\tau }\end{array}\right](61)\\ {\mathbf{P}}_{m,\tau +1}& ={\mathrm{\Phi }}_{tr,\tau }{\mathbf{P}}_{m,\tau }{\mathbf{\Phi }}_{tr,\tau }^{\mathrm{\top }}+{\mathbf{\Phi }}_{n,\tau }{\mathbf{Q}}_{\tau }{\mathbf{\Phi }}_{n,\tau }^{\mathrm{\top }}(62)\\ \frac{\partial {\mathbf{g}}_{\tau +1}}{\partial {\tilde{\mathbf{x}}}_{WI}}& ={\mathrm{\Phi }}_{tr,\tau }\frac{\partial {\mathbf{g}}_{\tau }}{\partial {\tilde{\mathbf{x}}}_{WI}}+{\mathbf{\Phi }}_{WI,\tau }(63)\end{array}$$

其中，${\mathbf{Q}}_{\tau }$是$t_{\tau }$时刻轮式编码器测量的噪声协方差。这些方程展示了雅可比矩阵和噪声协方差在预积分间隔期间的演化过程。因此，可以根据零初始条件$(\mathrm{i}.\mathrm{e}.,{\mathbf{P}}_{m,0},\frac{\partial {\mathbf{g}}_0}{\partial {\tilde{\mathbf{x}}}_{WI}}={\mathbf{0}}_3)$在预积分结束时递归计算测量噪声协方差${\mathbf{P}}_m$和雅可比矩阵$\frac{\partial \mathbf{g}}{\partial {\stackrel{\mathbf{~}}{\mathbf{x}}}_{WI}}$的值。假设使用$n$个测量值进行预积分，可以推导出$\frac{\partial \mathbf{g}}{\partial {\stackrel{\mathbf{~}}{\mathbf{x}}}_{WI}}$的闭式表达式：

$$\begin{array}{rl}{}_{O_i}^{O_{i+1}}\tilde{\theta }& =\sum _{k=1}^n{h}_{\theta \omega ,k}{\mathbf{H}}_{\omega \mathbf{x},k}{\tilde{\mathbf{x}}}_{CI}(64)\\ & =\sum _{k=1}^n\mathrm{\Delta }{t}_k\left[\begin{array}{ccc}\frac{-{\omega }_{ml,k}}{\hat{b}}& \frac{{\omega }_{mr,k}}{\hat{b}}& -\frac{{\omega }_{mr,k}{\hat{r}}_r-{\omega }_{ml,k}{\hat{r}}_l}{{\hat{b}}^2}\end{array}\right]{\tilde{\mathbf{x}}}_{CI}(65)\\ & =\left[\begin{array}{ccc}{\mathrm{\Gamma }}_{\mathit{\theta }1}& {\mathrm{\Gamma }}_{\mathit{\theta }2}& {\mathrm{\Gamma }}_{\mathit{\theta }3}\end{array}\right]{\tilde{\mathbf{x}}}_{CI}(66)\\ {}^{O_i}{\tilde{x}}_{O_{i+1}}& =\sum _{k=1}^n{h}_{x\theta ,k}{}_{O_i}^{O_k}\tilde{\theta }+(h_{x\omega ,k}{\mathbf{H}}_{\omega \mathbf{x},k}+h_{xv,k}{\mathbf{H}}_{v\mathbf{x},k}){\tilde{\mathbf{x}}}_{CI}(67)\\ & =\sum _{k=1}^n{h}_{x\theta ,k}\{\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j[\frac{-{\omega }_{ml,j}}{\hat{b}}\frac{{\omega }_{mr,j}}{\hat{b}}-\frac{{\omega }_{mr,j}{\hat{r}}_r-{\omega }_{ml,j}{\hat{r}}_l}{{\hat{b}}^2}]{\tilde{\mathbf{x}}}_{CI}\}\\ & +(h_{x\omega ,k}\left[\begin{array}{ccc}\frac{-{\omega }_{ml,k}}{\hat{b}}& \frac{{\omega }_{mr,k}}{\hat{b}}& -\frac{{\omega }_{mr,k}{\hat{r}}_r-{\omega }_{ml,k}{\hat{r}}_l}{{\hat{b}}^2}\end{array}\right]+h_{xv,k}\left[\begin{array}{ccc}\frac{{\omega }_{ml,k}}{2}& \frac{{\omega }_{mr,k}}{2}& 0\end{array}\right]){\tilde{\mathbf{x}}}_{CI}(68)\\ & =\left[\begin{array}{ccc}{\mathrm{\Gamma }}_{\mathbf{x}1}& {\mathrm{\Gamma }}_{\mathbf{x}2}& {\mathrm{\Gamma }}_{\mathbf{x}3}\end{array}\right]{\tilde{\mathbf{x}}}_{CI}(69)\\ {}^{O_i}{\tilde{y}}_{O_{i+1}}& =\sum _{k=1}^n{h}_{y\theta ,k}{}_{O_i}^{O_k}\tilde{\theta }+(h_{y\omega ,k}{\mathbf{H}}_{\omega \mathbf{x},k}+h_{yv,k}{\mathbf{H}}_{v\mathbf{x},k}){\tilde{\mathbf{x}}}_{CI}(70)\\ & =\sum _{k=1}^n{h}_{y\theta ,k}\{\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\left[\begin{array}{ccc}\frac{-{\omega }_{ml,j}}{\hat{b}}& \frac{{\omega }_{mr,j}}{\hat{b}}& -\frac{{\omega }_{mr,j}{\hat{r}}_r-{\omega }_{ml,j}{\hat{r}}_l}{{\hat{b}}^2}]{\tilde{\mathbf{x}}}_{CI}\}\end{array}\right](71)\\ & +(h_{y\omega ,k}\left[\begin{array}{ccc}\frac{-{\omega }_{ml,k}}{\hat{b}}& \frac{{\omega }_{mr,k}}{\hat{b}}& -\frac{{\omega }_{mr,k}{\hat{r}}_r-{\omega }_{ml,k}{\hat{r}}_l}{{\hat{b}}^2}\end{array}\right]+h_{yv,k}\left[\begin{array}{ccc}\frac{{\omega }_{ml,k}}{2}& \frac{{\omega }_{mr,k}}{2}& 0\end{array}\right]){\tilde{\mathbf{x}}}_{CI}(72)\\ & =\left[\begin{array}{ccc}{\mathrm{\Gamma }}_{\mathbf{y}1}& {\mathrm{\Gamma }}_{\mathbf{y}2}& {\mathrm{\Gamma }}_{\mathbf{y}3}\end{array}\right]{\tilde{\mathbf{x}}}_{CI}(73)\end{array}$$

其中

$$\begin{array}{rl}& {\mathbf{\Gamma }}_{\mathit{\theta }1}=\sum _{k=1}^n-\mathrm{\Delta }{t}_k\frac{{\omega }_{ml,k}}{\hat{b}}\\ & {\mathbf{\Gamma }}_{\mathit{\theta }2}=\sum _{k=1}^n\mathrm{\Delta }{t}_k\frac{{\omega }_{mr,k}}{\hat{b}}\\ & {\mathbf{\Gamma }}_{\mathit{\theta }3}=\sum _{k=1}^n-\mathrm{\Delta }{t}_k\frac{{\omega }_{mr,k}{\hat{r}}_r-{\omega }_{ml,k}{\hat{r}}_l}{{\hat{b}}^2}\\ & {\mathbf{\Gamma }}_{\mathbf{x}1}=\sum _{k=1}^n\{-h_{x\omega ,k}\frac{{\omega }_{ml,k}}{\hat{b}}+h_{xv,k}\frac{{\omega }_{ml,k}}{2}-h_{x\theta ,k}\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\frac{{\omega }_{ml,j}}{\hat{b}}\}\\ & {\mathbf{\Gamma }}_{\mathbf{x}2}=\sum _{k=1}^n\{h_{x\omega ,k}\frac{{\omega }_{mr,k}}{\hat{b}}+h_{xv,k}\frac{{\omega }_{mr,k}}{2}+h_{x\theta ,k}\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\frac{{\omega }_{mr,j}}{\hat{b}}\}\\ & {\mathbf{\Gamma }}_{\mathbf{x}3}=\sum _{k=1}^n\{-h_{x\omega ,k}\frac{{\omega }_{mr,k}{\hat{r}}_r-{\omega }_{ml,k}{\hat{r}}_l}{{\hat{b}}^2}-h_{x\theta ,k}\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\frac{{\omega }_{mr,j}{\hat{r}}_r-{\omega }_{ml,j}{\hat{r}}_l}{{\hat{b}}^2}\}\\ & {\mathbf{\Gamma }}_{\mathbf{y}1}=\sum _{k=1}^n\{-h_{y\omega ,k}\frac{{\omega }_{ml,k}}{\hat{b}}+h_{yv,k}\frac{{\omega }_{ml,k}}{2}-h_{y\theta ,k}\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\frac{{\omega }_{ml,j}}{\hat{b}}\}\\ & {\mathbf{\Gamma }}_{\mathbf{y}2}=\sum _{k=1}^n\{h_{y\omega ,k}\frac{{\omega }_{mr,k}}{\hat{b}}+h_{yv,k}\frac{{\omega }_{mr,k}}{2}+h_{y\theta ,k}\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\frac{{\omega }_{mr,j}}{\hat{b}}\}\\ & {\mathbf{\Gamma }}_{\mathbf{y}3}=\sum _{k=1}^n\{-h_{y\omega ,k}\frac{{\omega }_{mr,k}{\hat{r}}_r-{\omega }_{ml,k}{\hat{r}}_l}{{\hat{b}}^2}-h_{y\theta ,k}\sum _{j=1}^{k-1}\mathrm{\Delta }{t}_j\frac{{\omega }_{mr,j}{\hat{r}}_r-{\omega }_{ml,j}{\hat{r}}_l}{{\hat{b}}^2}\}\end{array}$$

得到内参状态的雅可比行列式为：

$$\frac{\partial \mathbf{g}}{\partial {\tilde{\mathbf{x}}}_{WI}}=\left[\begin{array}{ccc}{\mathbf{\Gamma }}_{\mathit{\theta }\mathbf{1}}& {\mathbf{\Gamma }}_{\mathit{\theta }\mathbf{2}}& {\mathbf{\Gamma }}_{\mathit{\theta }\mathbf{3}}\\ {\mathbf{\Gamma }}_{\mathit{x}\mathbf{1}}& {\mathbf{\Gamma }}_{\mathit{x}\mathbf{2}}& {\mathbf{\Gamma }}_{\mathit{x}\mathbf{3}}\\ {\mathbf{\Gamma }}_{\mathit{y}\mathbf{1}}& {\mathbf{\Gamma }}_{\mathit{y}\mathbf{2}}& {\mathbf{\Gamma }}_{\mathit{y}\mathbf{3}}\end{array}\right]$$

外参雅可比矩阵

请注意，预积分的轮式测量（33）仅提供里程计平面上的2D相对运动，而VIWO状态向量（34）包含3D IMU/相机姿态。为了建立预积分里程计与状态之间的联系，需要明确IMU和里程计之间的相对变换（外部校准）[参见（32）]：

$${\mathbf{z}}_{i+1}=\left[\begin{array}{c}{}_{O_i}^{O_{i+1}}\theta \\ {}^{O_i}{\mathbf{d}}_{O_{i+1}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{e}}_3^{\mathrm{\top }}\mathrm{Log}{(}_I^O{\mathbf{R}}_G^{I_{i+1}}{\mathbf{R}}_G^{I_i}{\mathbf{R}}^{\mathrm{\top }}{{}_I^O\mathbf{R}}^{\mathrm{\top }})\\ {\mathrm{\Lambda }}_I^O{\mathbf{R}}_G^{I_i}\mathbf{R}{(}^G{\mathbf{p}}_{I_{i+1}}+{}_G^{I_{i+1}}{\mathbf{R}}^{\mathrm{\top }}{{}^I\mathbf{p}}_O-{{}^G\mathbf{p}}_{I_i}-{}_G^{I_i}{\mathbf{R}}^{\mathrm{\top }}{{}^I\mathbf{p}}_O)\end{array}\right]$$

其中，$\mathrm{\Lambda }=[{\mathbf{e}}_1{\mathbf{e}}_2{]}^{\mathrm{\top }}$，$\mathrm{Log}(\cdot )$是$SO(3)$矩阵对数函数[8]。由于这个测量依赖于连续的两个姿态以及里程计/IMU的外部参数，当在MSCKF中使用它进行更新时，需要计算测量残差和相应的测量雅可比矩阵，计算方法如下。

每次测量的残差可以定义为：

$$\begin{array}{rl}& {\mathbf{r}}_{\theta }={\mathbf{e}}_3^{\mathrm{\top }}\mathrm{Log}{(}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }})-{}_{O_i}^{O_{i+1}}\theta \\ & {\mathbf{r}}_{\mathbf{d}}={}^{O_i}{\mathbf{d}}_{O_{i+1}}-{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}\hat{\mathbf{R}}({}^G{\hat{\mathbf{p}}}_{I_{i+1}}+{}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }I}{\hat{\mathbf{p}}}_O-{}^G{\hat{\mathbf{p}}}_{I_i}-{}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }I}{\hat{\mathbf{p}}}_O)\end{array}$$

请注意，由于2D方位扰动（42）的定义，在方向残差中采用了预测-测量形式。

首先，扰动姿态以获取相对于姿态的雅可比矩阵（83）：

$$\begin{array}{rl}(\mathbf{I}-|{\mathbf{e}}_3{}_{O_i}^{O_{i+1}}\tilde{\theta }{|}^{\times }{)}_{O_i}^{O_{i+1}}\hat{\mathbf{R}}& ={}_I^O\hat{\mathbf{R}}(\mathbf{I}-{|}_G^{I_{i+1}}\tilde{\mathit{\theta }}{|}^{\times }{)}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}(\mathbf{I}+{|}_G^{I_i}\tilde{\mathit{\theta }}{|}^{\times }{)}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}\\ \begin{array}{c}{}_{O_i}^{O_{i+1}}\tilde{\theta }\end{array}& \approx {\mathbf{e}}_3^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}\tilde{\mathit{\theta }}-{\mathbf{e}}_3^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_G^{I_i}\tilde{\mathit{\theta }}\\ {}^{O_i}{\hat{\mathbf{d}}}_{O_{i+1}}+{}^{O_i}{\tilde{\mathbf{d}}}_{O_{i+1}}& ={\mathrm{\Lambda }}_I^O\hat{\mathbf{R}}(\mathbf{I}-{|}_G^{I_i}\tilde{\mathit{\theta }}{|}^{\times }){}_G^{I_i}\hat{\mathbf{R}}{(}^G{\hat{\mathbf{p}}}_{I_{i+1}}+{}^G{\tilde{\mathbf{p}}}_{I_{i+1}})\\ & +{\mathrm{\Lambda }}_I^O\hat{\mathbf{R}}(\mathbf{I}-{|}_G^{I_i}\tilde{\mathit{\theta }}{|}^{\times }{)}_G^{I_i}{\mathbf{R}}_G^{I_{i+1}}{\mathbf{R}}^{\mathrm{\top }}(\mathbf{I}+{|}_G^{I_{i+1}}\tilde{\mathit{\theta }}{|}^{\times }{)}^I{\hat{\mathbf{p}}}_O\\ & -{\mathrm{\Lambda }}_I^O\hat{\mathbf{R}}(\mathbf{I}-{|}_G^{I_i}\tilde{\mathit{\theta }}{|}^{\times }{)}_G^{I_i}\hat{\mathbf{R}}{(}^G{\hat{\mathbf{p}}}_{I_i}+{}^G{\tilde{\mathbf{p}}}_{I_i})-{}_I^O{\hat{\mathbf{R}}}^I{\hat{\mathbf{p}}}_O\\ {}^{O_i}{\tilde{\mathbf{d}}}_{O_{i+1}}& \approx {\mathrm{\Lambda }}_I^O\hat{\mathbf{R}}{|}_G^{I_i}\hat{\mathbf{R}}{(}^G{\hat{\mathbf{p}}}_{I_{i+1}}{+}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }I}{\hat{\mathbf{p}}}_O{-}^G{\hat{\mathbf{p}}}_{I_i}){|}^{\times }{}_G^{I_i}\hat{\mathit{\theta }}\\ & -{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^G{\tilde{\mathbf{p}}}_{I_i}-{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}|{}^I{\hat{\mathbf{p}}}_O{|}^{\times }{}_G^{I_{i+1}}\tilde{\mathit{\theta }}+{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^G{\tilde{\mathbf{p}}}_{I_{i+1}}\end{array}$$

此外，空间外部参数的扰动为：

$$\begin{array}{rl}& (\mathbf{I}-{\left|{\mathbf{e}}_3{}_{O_i}^{O_{i+1}}\tilde{\theta }\right|}^{\times }){}_{O_i}^{O_{i+1}}\hat{\mathbf{R}}=(\mathbf{I}-{\left|{}_O^I\tilde{\mathit{\theta }}\right|}^{\times }){}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}{{}_I^O\hat{\mathbf{R}}}^{\mathrm{\top }}(\mathbf{I}+{\left|{}_O^I\tilde{\mathit{\theta }}\right|}^{\times })\\ & {}_{Oi}^{O_{i+1}}\tilde{\theta }\approx {\mathbf{e}}_3^{\mathrm{\top }}(\mathbf{I}-{}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}){}_O^I\tilde{\mathit{\theta }}\\ & {}^{O_i}{\hat{\mathbf{d}}}_{O_{i+1}}+{}^{O_i}{\tilde{\mathbf{d}}}_{O_{i+1}}=\mathrm{\Lambda }(\mathbf{I}-{\left|{}_I^O\tilde{\mathit{\theta }}\right|}^{\times }){}_I^O\hat{\mathbf{R}}{}_G^{I_i}\hat{\mathbf{R}}{}^G{\hat{\mathbf{p}}}_{I_{i+1}}\\ & -\mathrm{\Lambda }(\mathbf{I}-{\left|{}_I^O\tilde{\mathit{\theta }}\right|}^{\times }){}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}(\mathbf{I}+{\left|{}_I^O\tilde{\mathit{\theta }}\right|}^{\times })({}^O{\hat{\mathbf{p}}}_I+{}^O{\tilde{\mathbf{p}}}_I)\\ & -\mathrm{\Lambda }(\mathbf{I}-{\left|{}_I^O\tilde{\mathit{\theta }}\right|}^{\times }){}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^G{\hat{\mathbf{p}}}_{I_i}+\mathrm{\Lambda }({}^O{\hat{\mathbf{p}}}_I+{}^O{\tilde{\mathbf{p}}}_I)\\ & {}^{O_i}{\tilde{\mathbf{d}}}_{O_{i+1}}\approx \mathrm{\Lambda }({\left|{}_I^O{\hat{\mathbf{R}}}_G^{I_i}\hat{\mathbf{R}}({}^G{\hat{\mathbf{p}}}_{I_{i+1}}+{}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }I}{\hat{\mathbf{p}}}_O-{}^G{\hat{\mathbf{p}}}_{I_i})\right|}^{\times }+{}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}{\left|{}^O{\hat{\mathbf{p}}}_I\right|}^{\times }){}_I^O\tilde{\mathit{\theta }}\\ & +\mathrm{\Lambda }(\mathbf{I}-{}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}){}^O{\tilde{\mathbf{p}}}_I\\ & \end{array}$$

将上述方程整理成矩阵形式，得到相对于每个状态的以下雅可比矩阵：

$$\begin{array}{rl}& \frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{I_{i+1}}}=\left[\begin{array}{ccc}{\mathbf{e}}_3^{\mathrm{\top }}{}_I^O\hat{\mathbf{R}}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 9}\\ -{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{\left|{}^I{\hat{\mathbf{p}}}_O\right|}^{\times }& {\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}\hat{\mathbf{R}}& {\mathbf{0}}_{2\times 9}\end{array}\right]\\ & \frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{C_{i+1}}}=\left[\begin{array}{cc}-{\mathbf{e}}_3^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}& {\mathbf{0}}_{1\times 3}\\ {\mathrm{\Lambda }}_I^O\hat{\mathbf{R}}{\left|{}_G^{I_i}\hat{\mathbf{R}}({}^G{\hat{\mathbf{p}}}_{I_{i+1}}+{}_G^{{}^{I_{i+1}}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}^I{\hat{\mathbf{p}}}_O-{}^G{\hat{\mathbf{p}}}_{I_i})\right|}^{\times }& -{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_G^{I_i}\hat{\mathbf{R}}\end{array}\right]\\ & \frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{WE}}=\left[\begin{array}{cc}{\mathbf{e}}_3^{\mathrm{\top }}(\mathbf{I}-{}_I^O{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }})& {\mathbf{0}}_{1\times 3}\\ {\mathbf{H}}_{WE1}& \mathrm{\Lambda }(\mathbf{I}-{}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }})\end{array}\right]\\ & {\mathbf{H}}_{WE1}=\mathrm{\Lambda }({\left|{}_I^O{\hat{\mathbf{R}}}_G^{I_i}\hat{\mathbf{R}}({}^G{\hat{\mathbf{p}}}_{I_{i+1}}+{}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }I}{\hat{\mathbf{p}}}_O-{}^G{\hat{\mathbf{p}}}_{I_i})\right|}^{\times }+{}_I^O{\hat{\mathbf{R}}}_G^{I_i}{\hat{\mathbf{R}}}_G^{I_{i+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}{\left|{}^O{\hat{\mathbf{p}}}_I\right|}^{\times })\end{array}$$

请注意，雅可比行列式 (94) 和 (95) 与[3]相同。

观测误差状态传播

$$\begin{array}{rl}{\mathbf{z}}_{k+1}& :=\mathbf{g}({\omega }_{l(k:k+1)},{\omega }_{r(k:k+1)},{\mathbf{x}}_{WI})=\mathbf{h}({\mathbf{x}}_{I_{k+1}},{\mathbf{x}}_{C_{k+1}},{\mathbf{x}}_{WE})\\ & \approx \mathbf{g}({\omega }_{ml(k:k+1)},{\omega }_{mr(k:k+1)},{\hat{\mathbf{x}}}_{WI})+\frac{\partial \mathbf{g}}{\partial {\tilde{\mathbf{x}}}_{WI}}{\tilde{\mathbf{x}}}_{WI}+\frac{\partial \mathbf{g}}{\partial {\mathbf{n}}_{\omega }}{\mathbf{n}}_{\omega }\\ & \approx \mathbf{h}({\hat{\mathbf{x}}}_{I_{k+1}},{\hat{\mathbf{x}}}_{C_{k+1}},{\hat{\mathbf{x}}}_{WE})+\frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{I_{k+1}}}{\tilde{\mathbf{x}}}_{I_{k+1}}+\frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{C_{k+1}}}{\tilde{\mathbf{x}}}_{C_{k+1}}+\frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{WE}}{\tilde{\mathbf{x}}}_{WE}\\ {\tilde{\mathbf{z}}}_{k+1}& :=\mathbf{g}({\omega }_{ml(k:k+1)},{\omega }_{mr(k:k+1)},{\hat{\mathbf{x}}}_{WI})-\mathbf{h}({\hat{\mathbf{x}}}_{I_{k+1}},{\hat{\mathbf{x}}}_{C_{k+1}},{\hat{\mathbf{x}}}_{WE})\\ & \approx \underset{{\mathbf{H}}_{k+1}}{\underbrace{[\frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{I_{k+1}}}\frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{C_{k+1}}}\frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{WE}}-\frac{\partial \mathbf{g}}{\partial {\tilde{\mathbf{x}}}_{WI}}]}}{\tilde{\mathbf{x}}}_{k+1}-\frac{\partial \mathbf{g}}{\partial {\mathbf{n}}_{\omega }}{\mathbf{n}}_{\omega }\end{array}$$