状态估计

运动学模型

IMU 测量

我们使用 6 轴惯性测量单元 (IMU) 来传播惯性导航系统 (INS)，它提供局部旋转速度 (角速率)${\mathit{\omega }}_m$和局部平移加速度的测量${\mathbf{a}}_m$:

$$\begin{array}{rl}& {}^I{\mathit{\omega }}_m(t)={}^I\mathit{\omega }(t)+{\mathbf{b}}_g(t)+{\mathbf{n}}_g(t)(1)\\ & {}^I{\mathbf{a}}_m(t)={}^I\mathbf{a}(t)+{}_W^I\mathbf{R}(t{)}^W\mathbf{g}+{\mathbf{b}}_a(t)+{\mathbf{n}}_a(t)(2)\end{array}$$

其中${}^I\mathit{\omega }$和${}^I\mathbf{a}$分别是IMU局部坐标系{$I$}中的真实旋转速度和平移加速度，和是陀螺仪和加速度计偏差，${\mathbf{n}}_g$和${\mathbf{n}}_a$是高斯白噪声，${}^W\mathbf{g}=[\begin{array}{ccc}0& 0& 9.81\end{array}{]}^{\mathrm{\top }}$是在全局坐标系{$W$}中表达的重力（注意，地球不同位置的重力略有不同），${}_W^I\mathbf{R}$是从全局坐标系到IMU局部坐标系的旋转矩阵。

惯性状态向量

我们将时间的INS状态向量${\mathbf{X}}_I$定义为：

$${\mathbf{x}}_I(t)=\left[\begin{array}{c}{}_W^I\overline{q}(t)\\ {}^W{\mathbf{p}}_I(t)\\ {}^W{\mathbf{v}}_I(t)\\ {\mathbf{b}}_g(t)\\ {\mathbf{b}}_a(t)\end{array}\right](3)$$

其中${}_W^I\overline{q}$是表示 IMU 坐标系全局旋转的单位四元数，${}^W{\mathbf{p}}_I$是IMU在全局坐标系中的位置，${}^W{\mathbf{v}}_I$是IMU在全局坐标系中的速度。为了便于表示，我们通常会将时间写为$I$的下标，以描述 IMU 当时的状态（例如，$\begin{array}{c}{I}_t\\ W\end{array}\overline{\mathit{q}}=\begin{array}{c}I\\ W\end{array}\overline{\mathit{q}}(t))$）。为了定义 IMU 误差状态，对位置、速度和偏差采用标准的加性误差定义，而我们使用具有左四元数乘性误差$\otimes$的四元数误差状态$\delta \overline{q}$：

$$\begin{array}{rl}{}_W^I\overline{q}& =\delta \overline{q}\otimes {}_W^I\hat{\overline{q}}(4)\\ \delta \overline{q}& =\left[\begin{array}{c}\hat{\mathbf{k}}\sin (\frac{1}{2}\tilde{\theta })\\ \cos (\frac{1}{2}\tilde{\theta })\end{array}\right]\mathbf{\simeq }\left[\begin{array}{c}\frac{1}{2}\tilde{\mathit{\theta }}\\ 1\end{array}\right](5)\end{array}$$

其中$\hat{\mathbf{k}}$是旋转轴，$\tilde{\theta }$是旋转角度。对于小旋转，误差角度向量近似为$\tilde{\mathit{\theta }}=\tilde{\theta }\hat{\mathbf{k}}$三个方向轴的误差向量。因此，总IMU误差状态定义为以下15x1（不是16x1）向量：

$${\tilde{\mathbf{x}}}_I(t)=\left[\begin{array}{c}{}_G^I\tilde{\mathit{\theta }}(t)\\ {}^W{\tilde{\mathbf{p}}}_I(t)\\ {}^W{\tilde{\mathbf{v}}}_I(t)\\ {\tilde{\mathbf{b}}}_g(t)\\ {\tilde{\mathbf{b}}}_a(t)\end{array}\right](6)$$

IMU 运动学方程

$$\begin{array}{rl}{}_W^I\dot{\overline{q}}(t)& =\frac{1}{2}\mathrm{\Omega }(\omega (t){)}_W^I\overline{q}(t)(7)\\ {}^W{\dot{\mathbf{p}}}_I(t)& ={}^W{\mathbf{v}}_I(t)(8)\\ {}^W{\dot{\mathbf{v}}}_I(t)& ={}^W\mathbf{a}(t)(9)\\ \dot{{\mathbf{b}}_g}(t)& ={\mathbf{n}}_{wg}(t)(10)\\ {\dot{\mathbf{b}}}_a(t)& ={\mathbf{n}}_{wa}(t)(11)\end{array}$$

其中，

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

我们将陀螺仪和加速度计的偏差建模为随机游走，因此它们的时间导数为白高斯分布。请注意，上述运动学是根据真实加速度和角速度定义的，它们可以根据传感器测量值和状态计算出来。

连续时间惯性积分

给定在时间间隔（$t\in \left[\begin{array}{c}{t}_k,t_{k+1}\end{array}\right]$）内连续时间${}^I\mathit{\omega }\left(\begin{array}{c}t\end{array}\right)$和 ${}^I\mathbf{a}\left(\begin{array}{c}t\end{array}\right)$，以及它们的估计值${}^I\hat{\mathit{\omega }}(t)$和${}^I\hat{\mathit{a}}(t)$，我们可以定义上述IMU运动学微分方程的解。

$$\begin{array}{rl}{}_W^{I_{k+1}}\mathbf{R}& =\exp (-{\int }_{t_k}^{t_{k+1}}{}^I\mathit{\omega }\left(t_{\tau }\right)d\tau ){}_W^{I_k}\mathbf{R}(12)\\ & \triangleq \mathrm{\Delta }{\mathbf{R}}_k{}_W^{I_k}\mathbf{R}={}_{I_k}^{I_{k+1}}\mathbf{R}{}_W^{I_k}\mathbf{R}\\ {}^W{\mathbf{p}}_{I_{k+1}}& ={}^W{\mathbf{p}}_{I_k}+{}^W{\mathbf{v}}_{I_k}\mathrm{\Delta }{t}_k+{}_W^{I_k}{\mathbf{R}}^{\mathrm{\top }}{\int }_{t_k}^{t_{k+1}}{\int }_{t_k}^{{}_{I_{\tau }}^s}{\mathbf{I}}^{I_k}{\mathbf{R}}^I\mathbf{a}\left(t_{\tau }\right)d\tau ds-\frac{1}{2}{}^W\mathbf{g}\mathrm{\Delta }{t}_k^2(13)\\ & \triangleq {}^W{\mathbf{p}}_{I_k}+{}^W{\mathbf{v}}_{I_k}\mathrm{\Delta }{t}_k+{}_W^{I_k}{\mathbf{R}}^{\mathrm{\top }}\mathrm{\Delta }{\mathbf{p}}_k-\frac{1}{2}{}_a^W\mathbf{g}\mathrm{\Delta }{t}_k^2\\ {}^W{\mathbf{v}}_{I_{k+1}}& ={}^W{\mathbf{v}}_{I_k}+{}_W^{I_k}{\mathbf{R}}^{\mathrm{\top }}{\int }_{t_k}^{t_{k+1}}{}_{I_{\tau }}^{I_{\tau }}{\mathbf{R}}^I\mathbf{a}\left(t_{\tau }\right)d\tau -{}^W\mathbf{g}\mathrm{\Delta }{t}_k(14)\\ & \triangleq {}^W{\mathbf{v}}_{I_k}+{}_W^{I_k}{\mathbf{R}}^{\mathrm{\top }}\mathrm{\Delta }{\mathbf{v}}_k-{}^W\mathbf{g}\mathrm{\Delta }{t}_k\\ {\mathbf{b}}_{g_{k+1}}& ={\mathbf{b}}_{g_k}+{\int }_{t_k}^{t_{k+1}}{\mathbf{n}}_{wg}\left(t_{\tau }\right)d\tau (15)\\ {\mathbf{b}}_{a_{k+1}}& ={\mathbf{b}}_{a_k}+{\int }_{t_k}^{t_{k+1}}{\mathbf{n}}_{wa}\left(t_{\tau }\right)d\tau (16)\end{array}$$

其中，$\mathrm{\Delta }{t}_k=t_{k+1}-t_k$，$\begin{array}{r}{}_{I_{\tau }}^{I_k}\mathbf{R}=\exp ({\int }_{t_k}^{t_{\tau }}{}^I\mathit{\omega }(t_u)du)\end{array}$，$\exp (\cdot )$是$SO(3)$的矩阵指数，向量在它们的下标时间步骤（例如，${}^W{\mathbf{v}}_{I_k}={}^W{\mathbf{v}}_I(t_k)$）处进行评估。偏差受到随机游走噪声${\mathbf{n}}_{wg}$和${\mathbf{n}}_{wa}$的影响，它们是零均值的白高斯噪声。我们有以下的积分组成部分：

$$\begin{array}{rl}& \mathrm{\Delta }{\mathbf{R}}_k\triangleq {}_{I_k}^{I_{k+1}}\mathbf{R}=\exp (-{\int }_{t_k}^{t_{k+1}}{}_I^I\mathit{\omega }\left(t_{\tau }\right)d\tau )(17)\\ & \mathrm{\Delta }{\mathbf{p}}_k\triangleq {\int }_{t_k}^{t_{k+1}}{\int }_{t_k}^s{}_I^{I_k}{\mathbf{R}}^I\mathbf{a}\left(t_{\tau }\right)d\tau ds(18)\\ & \mathrm{\Delta }{\mathbf{v}}_k\triangleq {\int }_{t_k}^{t_{k+1}}{}_{I\tau }^{I_k}{\mathbf{R}}^I\mathbf{a}\left(t_{\tau }\right)d\tau (19)\end{array}$$

离散时间IMU状态传播

$$\begin{array}{rl}{}_W^{I_{k+1}}\hat{\overline{q}}& =\exp (\frac{1}{2}\mathbf{\Omega }({\mathit{\omega }}_{m,k}-{\mathbf{b}}_{g,k}-{\mathbf{n}}_{g,k})\mathrm{\Delta }{t}_k){}_W^{I_k}\hat{\overline{q}}\\ & =\cos \left(\frac{|\mathit{\omega }|}{2}\mathrm{\Delta }{t}_k\right)\cdot {\mathbf{I}}_4+\frac{1}{|\mathit{\omega }|}\sin \left(\frac{|\mathit{\omega }|}{2}\mathrm{\Delta }{t}_k\right)\cdot \mathbf{\Omega }(\mathit{\omega })\\ & \simeq {\mathbf{I}}_4+\frac{\mathrm{\Delta }{t}_k}{2}\mathbf{\Omega }(\mathit{\omega })(20)\\ {}^W{\mathbf{p}}_{I_{k+1}}& ={}^W{\mathbf{p}}_{I_k}+{}^W{\mathbf{v}}_{I_k}\mathrm{\Delta }{t}_k-\frac{1}{2}{}^W\mathbf{g}\mathrm{\Delta }{t}_k^2+\frac{1}{2}{{}_W^{I_k}\mathbf{R}}^{\mathrm{\top }}({\mathbf{a}}_{m,k}-{\mathbf{b}}_{a,k}-{\mathbf{n}}_{a,k})\mathrm{\Delta }{t}_k^2(21)\\ {}^W{\mathbf{v}}_{k+1}& {=}^W{\mathbf{v}}_{I_k}{-}^W\mathbf{g}\mathrm{\Delta }{t}_k+{{}_W^{I_k}\mathbf{R}}^{\mathrm{\top }}({\mathbf{a}}_{m,k}-{\mathbf{b}}_{a,k}-{\mathbf{n}}_{a,k})\mathrm{\Delta }{t}_k(22)\end{array}$$

离散时间IMU状态误差传播(详细推导过程见附录):

$$\begin{array}{rl}& {}_W^{I_{k+1}}\tilde{\mathit{\theta }}={}_{I_k}^{I_{k+1}}{\hat{\mathbf{R}}}_W^{I_k}\tilde{\mathit{\theta }}-{}_{I_k}^{I_{k+1}}\hat{\mathbf{R}}{\mathbf{J}}_r({}_{I_k}^{I_{k+1}}\hat{\mathit{\theta }})({\tilde{\mathbf{b}}}_{g,k}+{\mathbf{n}}_{g,k})\mathrm{\Delta }{t}_k(23)\\ & {}^W{\tilde{\mathbf{p}}}_{I_{k+1}}={}^W{\tilde{\mathbf{p}}}_{I_k}{+}^W{\tilde{\mathbf{v}}}_{I_k}\mathrm{\Delta }{t}_k-\frac{1}{2}{{}_W^{I_k}\hat{\mathbf{R}}}^{\mathrm{\top }}|{\hat{\mathbf{a}}}_k\mathrm{\Delta }{t}_i^2{|}^{\times }{}_G^{I_k}\tilde{\mathit{\theta }}-\frac{1}{2}{{}_W^{I_k}\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }{t}_k^2({\tilde{\mathbf{b}}}_{a,k}+{\mathbf{n}}_{a,k})(24)\\ & {}^W{\tilde{\mathbf{v}}}_{k+1}={}^W{\tilde{\mathbf{v}}}_{I_k}-{}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}|{\hat{\mathbf{a}}}_k\mathrm{\Delta }{t}_k{|}^{\times }{}_W^{I_k}\tilde{\mathit{\theta }}-{}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}({\tilde{\mathbf{b}}}_{a,k}+{\mathbf{n}}_{a,k})\mathrm{\Delta }{t}_k(25)\\ & {\tilde{\mathbf{b}}}_{g,k+1}={\tilde{\mathbf{b}}}_{g,k}+{\mathbf{n}}_{wg}(26)\\ & {\tilde{\mathbf{b}}}_{a,k+1}={\tilde{\mathbf{b}}}_{a,k}+{\mathbf{n}}_{wa}(27)\end{array}$$

其中，${}_{I_k}^{I_{k+1}}\hat{\mathit{\theta }}={-}^{I_k}\hat{\mathit{\omega }}\mathrm{\Delta }{t}_k,{\mathbf{J}}_r$为右雅可比矩阵

离散IMU误差状态传播矩阵：

$$\mathbf{\Phi }(t_{k+1},t_k)=\left[\begin{array}{ccccc}{}_{I_k}^{I_{k+1}}\hat{\mathbf{R}}& {\mathbf{0}}_3& {\mathbf{0}}_3& {-}_{I_k}^{I_{k+1}}\hat{\mathbf{R}}{\mathbf{J}}_r{(}_{I_k}^{I_{k+1}}\hat{\mathit{\theta }})\mathrm{\Delta }{t}_k& {\mathbf{0}}_3\\ -\frac{1}{2}{}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}|{\hat{\mathbf{a}}}_k\mathrm{\Delta }{t}_k^2{|}^{\times }& {\mathbf{I}}_3& \mathrm{\Delta }{t}_k{\mathbf{I}}_3& {\mathbf{0}}_3& -\frac{1}{2}{}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }{t}_k^2\\ {-}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}|{\hat{\mathbf{a}}}_k\mathrm{\Delta }{t}_k{|}^{\times }& {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3& {-}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }{t}_k\\ 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](28)$$

离散噪声传播矩阵：

$${\mathbf{G}}_k=\left[\begin{array}{cccc}{-}_{I_k}^{I_{k+1}}\hat{\mathbf{R}}{\mathbf{J}}_r{(}_{I_k}^{I_{k+1}}\hat{\mathit{\theta }})\mathrm{\Delta }{t}_k& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& -{\frac{1}{2}}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }{t}_k^2& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {-}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}\mathrm{\Delta }{t}_k& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{I}}_3\end{array}\right](29)$$

协方差更新：

$$\begin{array}{rl}{\mathbf{P}}_{k+1|k}& =\mathbf{\Phi }(t_{k+1},t_k){\mathbf{P}}_{k|k}\mathbf{\Phi }(t_{k+1},t_k{)}^{\mathrm{\top }}+{\mathbf{G}}_k{\mathbf{Q}}_d{\mathbf{G}}_k^{\mathrm{\top }}\end{array}(30)$$

线性观测更新

线性观测模型：

$${\mathbf{z}}_{m,k}={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{n}}_k(31)$$

卡尔曼增益计算：

$$K_i=P_{k|k-1}{H}_k^{\mathrm{\top }}{(H_k{P}_{k|k-1}{H}_k^{\mathrm{\top }}+R_k)}^{-1}(32)$$

误差状态更新：

$${\tilde{\mathbf{x}}}_{I_k}=K_k{\tilde{\mathbf{z}}}_{m,k}(33)$$

协方差矩阵更新:

$$P_{k|k}=({\mathbf{I}}_3-K_k{H}_k)P_{k|k-1}({\mathbf{I}}_3-K_{k}H{)}^{\mathrm{\top }}+K_k{R}_k{K}_k^{\mathrm{\top }}(34)$$

卡方检验：

$${\tilde{\mathbf{z}}}_{m,k}^{\mathrm{\top }}{(H_k{P}_{k|k-1}{H}_k^{\mathrm{\top }}+\alpha R_k)}^{-1}{\tilde{\mathbf{z}}}_{m,k}<{\chi }^2(48)$$

其中，$\alpha \in [50,100]$为噪声放大系数。

GPS观测

在GPS数据到来时我们首先对其进行坐标系转换，将原始的WGS-84坐标系转换到ENU（东北天）坐标系当中。并且我们还对GPS数据进行处理使用两克隆帧之间的相对GPS位置变换作为观测：

$${}^{G_k}{\mathbf{d}}_{G_{k+1}}=\sum _{i=0}^{N-2}{(}^E{\mathbf{p}}_{G_{k+1}}{-}^E{\mathbf{p}}_{G_k})$$

这是因为我们在实验中，发现将GPS数据直接作为观测当轨迹发生漂移时，后续的GPS观测难以进行更新。

GPS误差状态定义：

$${\tilde{\mathbf{x}}}_k={\left[\begin{array}{ccc}{}^W{\tilde{\mathbf{p}}}_{I_k}^{\mathrm{\top }}& {}^W{\tilde{\mathbf{p}}}_{I_{k-1}}^{\mathrm{\top }}& {}_E^W{\tilde{\mathit{\theta }}}^{\mathrm{\top }}\end{array}\right]}^{\mathrm{\top }}(35)$$

观测模型：

$$\begin{array}{r}{}^{G_k}{\mathbf{d}}_{G_{k+1}}={}_W^E\hat{\mathbf{R}}({}^W{\hat{\mathbf{p}}}_{I_k}-{}^W{\hat{\mathbf{p}}}_{I_{k-1}})(36)\end{array}$$

其中，${}^{G_k}{\mathbf{d}}_{G_{k+1}}$为在时间间隔$t\in \left[\begin{array}{c}{t}_k,t_{k+1}\end{array}\right]$内的GPS相对位置变换。

将（35）式对状态${\tilde{\mathbf{x}}}_k$进行求导可计算出雅可比矩阵：

$$H_k=\left[\begin{array}{ccc}{}_W^E\hat{\mathbf{R}}& -{}_W^E\hat{\mathbf{R}}& -{}_W^E\hat{\mathbf{R}}|{}^W{\hat{\mathbf{p}}}_{I_k}-{}^W{\hat{\mathbf{p}}}_{I_{k-1}}{|}^{\times }\end{array}\right](37)$$

轮速观测

我们获取的轮速数据是由轮式编码器发出的原始数据，因此我们要对原始数据进行结算，并且找到两个克隆帧之间的数据进行积分，获得时间内的相对位置和旋转变换作为观测。

轮速状态定义：

$${\mathbf{x}}_k={\left[\begin{array}{cccc}{\mathbf{x}}_{I_k}^{\mathrm{\top }}& {\mathbf{x}}_{C_k}^{\mathrm{\top }}& {\mathbf{x}}_{WE}^{\mathrm{\top }}& {\mathbf{x}}_{WI}^{\mathrm{\top }}\end{array}\right]}^{\mathrm{\top }}(38)$$

其中，${\mathbf{x}}_{WI}={\left[\begin{array}{ccc}{r}_l& r_r& b\end{array}\right]}^{\mathrm{\top }}$ 分别表示左右轮的半径和基准连接点的长度，${\mathbf{x}}_{WE}={\left[\begin{array}{cc}{}_I^O{\overline{q}}^{\mathrm{\top }}& {}^O{\mathbf{p}}_I^{\mathrm{\top }}\end{array}\right]}^{\mathrm{\top }}$，表示IMU和轮速的外参。

车轮编码器测量模型:

地面车辆由两个差动轮驱动，安装在共同的轴上，每个轮子都配备有编码器，提供局部角速率读数：

$${\omega }_{ml}={\omega }_l+n_{{\omega }_l},{\omega }_{mr}={\omega }_r+n_{{\omega }_r}(39)$$

其中，${\omega }_l$和${\omega }_r$分别表示每个轮子的真实角速度，$n_{{\omega }_l}$和$n_{{\omega }_r}$表示对应的白噪声。编码器读数组合起来可以提供车辆主体坐标系$\left\{O\right\}$的2D线性和角速度，坐标系$\left\{O\right\}$位于基准连接点的中心：

$${}^O\omega =({\omega }_r{r}_r-{\omega }_l{r}_l)/b,{}^{O}v=({\omega }_r{r}_r+{\omega }_l{r}_l)/2(40)$$

车轮里程计预积分:

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

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

其中，$\begin{array}{c}{O}_{\tau }\\ O_k\end{array}\theta$是局部偏航角，${}^{O_k}{x}_{O_{\tau }}$和${}^{O_k}{y}_{O_{\tau }}$是$\{O_{\tau }\}$在起始积分坐标系$\left\{O_k\right\}$中的二维位置。请注意，使用${-}^{O\tau }\omega$和${-}^{O_{\tau }}v\sin {(}_{O_k}^{O_{\tau }}\theta )$是因为遵循全局到局部的方向表示。

将从时间步$i$到$\tau +1$的所有轮式里程计测量局部组合，执行以下测量的积分(详细推导过程见附录)：

$$\begin{array}{rl}\begin{array}{c}{}_{O_k}^{O_{\tau +1}}\theta \end{array}& ={}_{O_k}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega \mathrm{\Delta }t(42)\\ {}^{O_k}{x}_{O_{\tau +1}}& ={}^{O_k}{x}_{O_{\tau }}-\frac{{}^{O_{\tau }}v(\sin {(}_{O_k}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega \mathrm{\Delta }t)-\sin {(}_{O_k}^{O_{\tau }}\theta ))}{{}^{O_{\tau }}\omega }(43)\\ {}^{O_k}{y}_{O_{\tau +1}}& ={}^{O_k}{y}_{O_{\tau }}-\frac{{}^{O_{\tau }}v(\cos {(}_{O_k}^{O_{\tau }}\theta -{}^{O_{\tau }}\omega \mathrm{\Delta }t)-\cos {(}_{O_k}^{O_{\tau }}\theta ))}{{}^{O_{\tau }}\omega }(44)\end{array}$$

其中，$\mathrm{\Delta }{t}_k=t_{\tau +1}-t_{\tau }$。请注意，假设${}^{O_{\tau }}\omega$和${}^{O_{\tau }}v$是恒定的，但考虑了$t_{\tau }$和$t_{\tau +1}$之间的航向角变化。

从$t_k$积分到$t_{k+1}$，并得到如下的2D相对姿态测量：

$$\begin{array}{rl}{\mathbf{z}}_{k+1}& =\left[\begin{array}{c}{}_{O_k}^{O_{k+1}}\theta \\ {}^{O_k}{\mathbf{d}}_{O_{k+1}}\end{array}\right]=\left[\begin{array}{c}{\int }_{t_k}^{t_{k+1}}{}^{O_t}\omega dt\\ {\int }_{t_k}^{t_{k+1}}{}^{O_t}v\cos {(}_{O_k}^{O_t}\theta )dt\\ {\int }_{t_k}^{t_{k+1}{O}_t}v\sin {(}_{O_k}^{O_t}\theta )dt\end{array}\right]\\ & =:\mathbf{g}({\omega }_{l(k:k+1)},{\omega }_{r(k:k+1)},{\mathbf{x}}_{WI})\end{array}(45)$$

其中，${\omega }_{(k:k+1)}$表示从$t_k$到$t_{k+1}$所有轮式测量值的积分。

观测模型：

$$\begin{array}{rl}{}_{O_k}^{O_{k+1}}\theta & ={\mathbf{e}}_3^{\mathrm{\top }}\mathrm{Log}{(}_I^O{\mathbf{R}}_W^{I_{i+1}}{\mathbf{R}}_W^{I_k}{\mathbf{R}}^{\mathrm{\top }}{{}_I^O\mathbf{R}}^{\mathrm{\top }})(46)\\ {}^{O_k}{\mathbf{d}}_{O_{k+1}}& ={\mathrm{\Lambda }}_I^O{\mathbf{R}}_W^{I_k}\mathbf{R}{(}^W{\mathbf{p}}_{I_{k+1}}+{}_W^{I_{k+1}}{\mathbf{R}}^{\mathrm{\top }}{{}^I\mathbf{p}}_O-{{}^W\mathbf{p}}_{I_k}-{}_G^{I_k}{\mathbf{R}}^{\mathrm{\top }}{{}^I\mathbf{p}}_O)(47)\end{array}$$

雅可比矩阵(详细推导过程见附录)：

$$H_{k+1}=[\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}}](48)$$

其中，

$$\begin{array}{rl}& \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]\\ & \frac{\partial \mathbf{h}}{\partial {\tilde{\mathbf{x}}}_{I_{k+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}}}_W^{I_k}{\hat{\mathbf{R}}}_W^{I_{k+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{\left|{}^I{\hat{\mathbf{p}}}_O\right|}^{\times }& {\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_W^{I_k}\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}}}_W^{I_{k+1}}{\hat{\mathbf{R}}}_W^{I_k}{\hat{\mathbf{R}}}^{\mathrm{\top }}& {\mathbf{0}}_{1\times 3}\\ {\mathrm{\Lambda }}_I^O\hat{\mathbf{R}}{\left|{}_W^{I_k}\hat{\mathbf{R}}({}^W{\hat{\mathbf{p}}}_{I_{k+1}}+{}_W^{{}^{I_{k+1}}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}^I{\hat{\mathbf{p}}}_O-{}^W{\hat{\mathbf{p}}}_{I_k})\right|}^{\times }& -{\mathrm{\Lambda }}_I^O{\hat{\mathbf{R}}}_W^{I_k}\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}}}_W^{I_{k+1}}{\hat{\mathbf{R}}}_W^{I_k}{\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}}}_W^{I_k}{\hat{\mathbf{R}}}_W^{I_{k+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}}}_W^{I_k}\hat{\mathbf{R}}({}^W{\hat{\mathbf{p}}}_{I_{k+1}}+{}_W^{I_{k+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }I}{\hat{\mathbf{p}}}_O-{}^W{\hat{\mathbf{p}}}_{I_k})\right|}^{\times }+{}_I^O{\hat{\mathbf{R}}}_W^{I_k}{\hat{\mathbf{R}}}_W^{I_{k+1}}{\hat{\mathbf{R}}}^{\mathrm{\top }}{}_I^O{\hat{\mathbf{R}}}^{\mathrm{\top }}{\left|{}^O{\hat{\mathbf{p}}}_I\right|}^{\times })\end{array}$$

视觉观测

我们对前端已经成功匹配的特征点进行三角化，投影到全局坐标系{$W$}中生成$landmark$作为观测，我们将测量模型概括为一系列嵌套函数，以涵盖不同的特征参数化，如 3D 位置和逆深度等。假设一个视觉特征已经在随机克隆的滑动窗口上被追踪 ，我们可以将视觉承载测量值（即像素坐标）写成以下一系列嵌套函数：

观测模型：

$$\begin{array}{rl}{\mathbf{z}}_{m,k}& =\mathbf{h}({\mathbf{x}}_k)+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{z}}_{n,k},\mathit{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p{(}^{C_k}{\mathbf{p}}_f),\mathit{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t{(}^W{\mathbf{p}}_f{,}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k})),\mathit{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t({\mathbf{h}}_r(\mathit{\lambda },\cdots ){,}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k})),\mathit{\zeta })+{\mathbf{n}}_k\end{array}$$

${\mathbf{z}}_k={\mathbf{h}}_d({\mathbf{z}}_{n,k},\mathit{\zeta })$：将归一化坐标映射到畸变的UV坐标的畸变函数

${\mathbf{z}}_{n,k}={\mathbf{h}}_p{(}^{C_k}{\mathbf{p}}_f)$：将图像中的3D点投影到归一化UV坐标的投影函数

${}^{C_k}{\mathbf{p}}_f={\mathbf{h}}_t{(}^W{\mathbf{p}}_f{,}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k})$：将全局坐标系中的特征位置转换为当前相机坐标系

${}^W{\mathbf{p}}_f={\mathbf{h}}_r(\mathit{\lambda },\cdots )$：将特征的表示形式转换为全局坐标系中的三维特征

其中，${\mathbf{z}}_{m,k}$是原始UV像素坐标；${\mathbf{n}}_k$是原始像素噪声，通常假设为零均值白色高斯；${\mathbf{z}}_{n,k}$是归一化的未失真UV测量值；${}^{C_k}{\mathbf{p}}_f$是当前相机帧中的地标位置；$\mathit{\lambda }$是特征$landmark$的表示;并且{${}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k}$}表示全局坐标系中的当前相机姿势（位置和方向）。

雅可比矩阵：

考虑到上述的嵌套函数，我们可以利用链式法则来找到总状态雅可比矩阵。由于我们的特征表示函数${\mathbf{h}}_r(\cdots )$可能还依赖于状态，即一个锚定姿势，我们需要仔细考虑它的额外导数。请考虑以下关于状态$\mathbf{x}$雅可比矩阵的测量示例:

$$\begin{array}{rl}\frac{\partial {\mathbf{z}}_k}{\partial \mathbf{x}}& =\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial {\mathbf{z}}_{n,k}}\frac{\partial {\mathbf{h}}_p(\cdot )}{{\partial }^{C_k}{\mathbf{p}}_f}\frac{\partial {\mathbf{h}}_t(\cdot )}{\partial \mathbf{x}}+\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial {\mathbf{z}}_{n,k}}\frac{\partial {\mathbf{h}}_p(\cdot )}{{\partial }^{C_k}{\mathbf{p}}_f}\frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }^W{\mathbf{p}}_f}\frac{\partial {\mathbf{h}}_r(\cdot )}{\partial \mathbf{x}}\end{array}$$

在全局特征表示中，参见点特征表示部分，第二项将为零，而对于锚定表示，需要进行计算。

其中，

$$\begin{array}{r}\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial {\mathbf{z}}_{n,k}}=\left[\begin{array}{cc}{f}_x\ast ((1+k_1{r}^2+k_2{r}^4)+(2k_1{x}_n^2+4k_2{x}_n^2(x_n^2+y_n^2))+2p_1{y}_n+(2p_2{x}_n+4p_2{x}_n))& f_x\ast (2k_1{x}_n{y}_n+4k_2{x}_n{y}_n(x_n^2+y_n^2)+2p_1{x}_n+2p_2{y}_n)\\ f_y\ast (2k_1{x}_n{y}_n+4k_2{x}_n{y}_n(x_n^2+y_n^2)+2p_1{x}_n+2p_2{y}_n)& f_y\ast ((1+k_1{r}^2+k_2{r}^4)+(2k_1{y}_n^2+4k_2{y}_n^2(x_n^2+y_n^2))+\begin{array}{r}(2p_1{y}_n+4p_1{y}_n)+2p_2{x}_n)\end{array}\end{array}\right]\end{array}$$$$\begin{array}{rl}\frac{\partial {\mathbf{h}}_p(\cdot )}{{\partial }^{C_k}{\mathbf{p}}_f}& =\left[\begin{array}{ccc}\frac{1}{C_z}& 0& \frac{{-}^{C}x}{{(}^{C}z{)}^2}\\ 0& \frac{1}{C_z}& \frac{{-}^{C}y}{{(}^{C}z{)}^2}\end{array}\right]\\ \frac{\partial {\mathbf{h}}_t(\cdot )}{\partial \mathbf{x}}& =\left[\begin{array}{ccc}{}_I^C\mathbf{R}{|}_W^{I_k}\mathbf{R}({}^W{\mathbf{p}}_f-{}^W{\mathbf{p}}_{I_k}){|}^{\times }& {-}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}& {}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}\end{array}\right]\\ \frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }^W{\mathbf{p}}_f}& ={}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}\end{array}$$

此外，如果进行在线外部标定，还需要相对于IMU-相机外部参数的雅可比矩阵:

$$\begin{array}{rl}\frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }_I^C\mathbf{R}}& =|{}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}({}^W{\mathbf{p}}_f-{}^W{\mathbf{p}}_{I_k}){|}^{\times }\\ \frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }^C{\mathbf{p}}_I}& ={\mathbf{I}}_{3\times 3}\end{array}$$

点特征表示:

3D点特征有两种主要的参数化方式：3D位置（xyz）和具有方向的逆深度。这两种方式都可以在全局坐标系中表示，也可以在参考锚定坐标系中表示，这样就需要依赖于观测到特征的“锚定”姿势。为了在我们的代码库中统一处理不同的特征参数化$\mathit{\lambda }$，我们详细推导了通用函数${}^G{\mathbf{p}}_f=\mathbf{f}(\cdot )$(详细推导过程见附录)，将不同的表示形式映射到全局位置。

全局XYZ

作为标准参数化，3D点特征的全局位置简单地由其在全局参考坐标系中的xyz坐标给出:

$$\begin{array}{rl}{}^W{\mathbf{p}}_f& =\mathbf{f}(\mathit{\lambda })\\ & =\left[\begin{array}{c}{}^{W}x\\ {}^{W}y\\ {}^{W}z\end{array}\right]\end{array}$$

其中，

$$\mathit{\lambda }={}^W{\mathbf{p}}_f={\left[\begin{array}{ccc}{}^{W}x& {}^{W}y& {}^{W}z\end{array}\right]}^{\mathrm{\top }}$$

很明显，相对于特征参数的雅可比矩阵是：

$$\frac{\partial \mathbf{f}(\cdot )}{\partial \mathit{\lambda }}={\mathbf{I}}_{3\times 3}$$

全局逆深度

3D点特征的全局逆深度表示形式类似于球坐标系，具体表达为：

$$\begin{array}{rl}{}^W{\mathbf{p}}_f& =\mathbf{f}(\mathit{\lambda })\\ & =\frac{1}{\rho }\left[\begin{array}{c}\cos (\theta )\sin (\varphi )\\ \sin (\theta )\sin (\varphi )\\ \cos (\varphi )\end{array}\right]\end{array}$$

其中：

$$\mathit{\lambda }={\left[\begin{array}{ccc}\theta & \varphi & \rho \end{array}\right]}^{\mathrm{\top }}$$

相对于特征参数的雅可比矩阵可以计算为：

$$\begin{array}{r}\frac{\partial \mathbf{f}(\cdot )}{\partial \lambda }\end{array}=\left[\begin{array}{ccc}-\frac{1}{\rho }\sin (\theta )\sin (\varphi )& \frac{1}{\rho }\cos (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\cos (\theta )\sin (\varphi )\\ \frac{1}{\rho }\cos (\theta )\sin (\varphi )& \frac{1}{\rho }\sin (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\sin (\theta )\sin (\varphi )\\ 0& -\frac{1}{\rho }\sin (\varphi )& -\frac{1}{{\rho }^2}\cos (\varphi )\end{array}\right]$$

锚定XYZ

我们可以将3D点特征表示为某个“锚定”框架中（比如某个IMU本地框架，$\{\begin{array}{c}{}_W^{I_a}\end{array}\mathbf{\text{R}},{}^W{\mathbf{\text{p}}}_{I_a}\}$），通常这个框架对应于检测到该特征的第一个相机帧对应的IMU姿势。

$$\begin{array}{rl}{}^W{\mathbf{p}}_f& =\mathbf{f}(\mathit{\lambda }{,}_W^{I_a}\mathbf{R},{}^W{\mathbf{p}}_{I_a},{}_I^C\mathbf{R},{}^C{\mathbf{p}}_I)\\ & {\mathbf{=}}_W^{I_a}{\mathbf{R}}_I^{\mathrm{\top }C}{\mathbf{R}}^{\mathrm{\top }}(\mathit{\lambda }-{}^C{\mathbf{p}}_I)+{}^W{\mathbf{p}}_{I_a}\end{array}$$

其中，

$$\mathit{\lambda }={}^{C_a}{\mathbf{p}}_f={\left[\begin{array}{ccc}{}^{C_a}x& {}^{C_a}y& {}^{C_a}z\end{array}\right]}^{\mathrm{\top }}$$

相对于特征状态的雅可比矩阵为：

$$\frac{\partial \mathbf{f}(\cdot )}{\partial \mathit{\lambda }}={}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}$$

由于锚定姿态涉及到这种表示，其雅可比矩阵被计算为：

$$\begin{array}{rl}\frac{\partial \mathbf{f}(\cdot )}{{\partial }_W^{I_a}\mathbf{R}}& ={-}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{|{}_I^C{\mathbf{R}}^{\mathrm{\top }}{(}^{C_a}{\mathbf{p}}_f{-}^C{\mathbf{p}}_I)|}^{\times }\\ \frac{\partial \mathbf{f}(\cdot )}{{\partial }^W{\mathbf{p}}_{I_a}}& ={\mathbf{I}}_{3\times 3}\end{array}$$

此外，如果进行外部标定，还需要计算相对于IMU-相机外参数的以下雅可比矩阵：

$$\begin{array}{rl}& \frac{\partial \mathbf{f}(\cdot )}{{\partial }_I^C\mathbf{R}}={-}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}{|{(}^{C_a}{\mathbf{p}}_f{-}^C{\mathbf{p}}_I)|}^{\times }\\ & \frac{\partial \mathbf{f}(\cdot )}{{\partial }^C{\mathbf{p}}_I}={-}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}\end{array}$$

锚定逆深度

类似于全局逆深度情况，我们可以在锚定框架中，$\{\begin{array}{c}{}_W^{I_a}\end{array}\mathbf{\text{R}},{}^W{\mathbf{\text{p}}}_{I_a}\}$，使用具有方位角的逆深度（类似于球面坐标），来表示一个3D点特征：

$$\begin{array}{rl}{}^G{\mathbf{p}}_f& =\mathbf{f}(\mathit{\lambda },{}_W^{I_a}\mathbf{R},{}^W{\mathbf{p}}_{I_a},{}_I^C\mathbf{R},{}^C{\mathbf{p}}_I)\\ & ={}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}({}^{C_a}{\mathbf{p}}_f-{}^C{\mathbf{p}}_I)+{}^W{\mathbf{p}}_{I_a}\\ & ={}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}(\frac{1}{\rho }\left[\begin{array}{c}\cos (\theta )\sin (\varphi )\\ \sin (\theta )\sin (\varphi )\\ \cos (\varphi )\end{array}\right]-{}^C{\mathbf{p}}_I)+{}^W{\mathbf{p}}_{I_a}\end{array}$$

其中：

$$\mathit{\lambda }={\left[\begin{array}{ccc}\theta & \varphi & \rho \end{array}\right]}^{\mathrm{\top }}$$

对于特征状态的雅可比矩阵为：

$$\frac{\partial \mathbf{f}(\cdot )}{\partial \mathit{\lambda }}{=}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}\left[\begin{array}{ccc}-\frac{1}{\rho }\sin (\theta )\sin (\varphi )& \frac{1}{\rho }\cos (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\cos (\theta )\sin (\varphi )\\ \frac{1}{\rho }\cos (\theta )\sin (\varphi )& \frac{1}{\rho }\sin (\theta )\cos (\varphi )& -\frac{1}{{\rho }^2}\sin (\theta )\sin (\varphi )\\ 0& -\frac{1}{\rho }\sin (\varphi )& -\frac{1}{{\rho }^2}\cos (\varphi )\end{array}\right]$$

相对于锚定姿态的雅可比矩阵为：

$$\begin{array}{rl}\frac{\partial \mathbf{f}(\cdot )}{{\partial }_W^{I_a}\mathbf{R}}& ={-}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{|{}_I^C{\mathbf{R}}^{\mathrm{\top }}{(}^{C_a}{\mathbf{p}}_f{-}^C{\mathbf{p}}_I)|}^{\times }\\ \frac{\partial \mathbf{f}(\cdot )}{{\partial }^W{\mathbf{p}}_{I_a}}& ={\mathbf{I}}_{3\times 3}\end{array}$$

相对于IMU-相机外部参数的雅可比矩阵为：

$$\begin{array}{rl}& \frac{\partial \mathbf{f}(\cdot )}{{\partial }_I^C\mathbf{R}}={-}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}{|{(}^{C_a}{\mathbf{p}}_f{-}^C{\mathbf{p}}_I)|}^{\times }\\ & \frac{\partial \mathbf{f}(\cdot )}{{\partial }^C{\mathbf{p}}_I}={-}_W^{I_a}{\mathbf{R}}^{\mathrm{\top }}{}_I^C{\mathbf{R}}^{\mathrm{\top }}\end{array}$$