摘要

本文提出了一种基于扩展卡尔曼滤波(EKF)的实时视觉辅助惯性导航算法。这项工作的主要贡献是推导出一个测量模型，该模型能够表达当从多个相机位姿观察到一个静态特征时产生的几何约束。该测量模型不需要将三维特征位置包含在EKF的状态向量中，并且在线性化误差范围内是最优的。我们提出的视觉辅助惯性导航算法的计算复杂度在特征数量上仅为线性，并且能够在大规模真实环境中进行高精度的位姿估计。该算法的性能在广泛的实验结果中得到了证明，包括在城市区域内定位的相机/IMU系统。

1 介绍

在过去的几年中，视觉辅助惯性导航的研究受到了学术界的广泛关注。基于MEMS的惯性传感器制造的最新进展使得制造小型、廉价和非常精确的惯性测量单元(IMU)成为可能，适用于移动机器人和无人机等小规模系统的位姿估计。这些系统通常在GPS信号不可靠的城市环境(“城市峡谷”)，以及室内、空间和其它一些无法进行全球定位测量的环境中运行。在无法依赖GPS测量的情况下，相机的低成本、重量轻和功耗小使其成为辅助惯性导航的理想选择。

视觉感知的一个重要优势是图像是高维测量，具有丰富的信息内容。特征提取方法通常可以检测和跟踪图像中的数百个特征，如果使用得当，可以得到良好的定位结果。然而，大量的数据也对估计算法的设计提出了重大挑战。在要求实时定位性能时，需要在算法的计算复杂度和估计精度之间进行权衡。

在本文中，我们提出了一种算法，能够优化地利用由视觉特征的多个测量提供的定位信息。我们的方法的动机是观察到，当一个静态特征从几个相机位姿观测到，它是可能定义涉及所有这些位姿的几何约束。我们工作的主要贡献是一个测量模型，该模型表达了这些约束，而不包括滤波器状态向量中的3D特征位置，导致计算复杂度仅与特征数量成线性关系。在对下一节的相关工作进行了简要讨论之后，第三节将介绍提出的估计器的细节。在第四节中，我们描述了在不受控制的城市环境中进行的大规模实验的结果，这表明所提出的估计器能够实现准确和实时的位姿估计。最后，在第五节得出了本工作的结论。

2 相关工作

一类融合惯性测量与视觉特征观测的算法遵循同时定位和建图(SLAM)范式。在这些方法中，当前IMU位姿以及所有视觉路标点的三维位置被联合估计。这些方法与基于SLAM的仅依靠相机定位的方法具有相同的基本原理，但不同的是，IMU测量用于状态传播，而不是统计运动模型。基于SLAM的算法的基本优势在于，它们可以解释相机的位姿和观测到的特征的3D位置之间存在的相关性。另一方面，SLAM的主要局限性是计算复杂度高；正确处理这些相关性的计算成本很高，因此在具有数千个特征的环境中执行基于视觉的SLAM仍然是一个具有挑战性的问题。

存在一些算法，与SLAM相反，仅估计相机的位姿(即不共同估计特征位置)，以实现实时操作。这些方法中计算效率最高的是利用特征测量来推导成对图像之间的约束。例如，在[7]中，一种基于图像的运动估计算法应用于连续的图像对，以获得随后与惯性测量融合的位移估计。类似地，在[8]和[9]中，当前图像和以前图像之间的约束使用极线几何定义，并在扩展卡尔曼滤波器(EKF)中与IMU测量相结合。在[10]和[11]中，采用极线几何与统计运动模型相结合，而在[12]中则融合了极线约束与飞机动力学模型。使用特征测量对图像之间施加约束在哲学上与本文提出的方法相似。然而，一个根本的区别是，我们的算法可以表达多个相机位姿之间的约束，因此，在同一特征在两幅以上的图像中可见的情况下，可以获得更高的估计精度。

在保持由多个相机位姿组成的状态向量的算法中也采用了成对约束。在[13]中，实现了一个增广状态卡尔曼滤波，其中机器人位姿的滑动窗口保持在滤波状态。另一方面，在[14]中，所有相机位姿都是同时估计的。在这两种算法中，成对的相对位姿测量是从图像中导出的，并用于状态更新。该方法的缺点是，当一个特征在多幅图像中出现时，多个位姿之间的附加约束被丢弃，从而导致信息丢失。此外，当对相同的图像测量值进行处理以计算多个位移估计时，它们在统计上并不是独立的，如[15]所示。

[16]给出了一种算法，类似于本文提出的方法，直接使用路标测量值来施加多个相机位姿之间的约束。这是一个视觉里程计算法，它临时初始化路标，使用它们对连续相机位姿的窗口施加约束，然后丢弃它们。然而，这种方法不能用于协同惯性测量。此外，路标点估计和相机轨迹之间的相关性没有得到适当的解释，因此，该算法没有提供任何状态估计的协方差度量。

在可变状态维数滤波器(VSDF)中也保持一个相机位姿窗口。VSDF是一种混合批量/递归方法，(i)使用延迟线性化来提高对线性化准确性的鲁棒性，(ii)利用信息矩阵的稀疏性，这在没有使用动态运动模型时自然会出现。然而，在动态运动模型可用的情况下(如视觉辅助惯性导航)，VSDF的计算复杂度最多为特征数量的二次型。

与VSDF相比，我们在本文中提出的多状态约束滤波器能够利用延迟线性化的优点，而复杂度在特征数量上仅为线性。通过直接表示多相机之间的几何约束，避免了成对位移估计带来的计算负担和信息丢失。此外，与SLAM类型的方法相比，它不需要在滤波器状态向量中包含3D特征位置，但仍然获得了最优的位姿估计。由于这些特性，所述算法是非常有效的，并如第四节所示，能够实时高精度视觉辅助惯性导航。

3 估计器描述

提出的基于EKF的估计器的目标是跟踪IMU固定的坐标系{I}\{I\}{I}相对于全局参考坐标系{G}\{G\}{G}的3D位姿。为了简化处理地球自转对IMU测量结果的影响(参见公式(7) ~(8))，全局坐标系选择地心地固坐标系(ECEF)。算法的概述在算法1中给出。IMU测量值在可用时立即进行处理，以传播EKF状态和协方差(参见第3-B节)。另一方面，每次记录图像时，当前相机位姿估计被附加到状态向量(参见第3-C节)。状态增广对于处理特征测量是必要的，因为在EKF更新过程中，每个被跟踪特征的测量值被用于在所有相机位姿之间施加约束。因此，在任何时刻，EKF状态向量包括(i)不断变化的IMU状态，XIMUX_{IMU}XIMU，和(ii)一个历史到NmaxN_{max}Nmax过去的相机的位姿。下面，我们将详细描述算法的各个组成部分。

A EKF状态向量的结构

IMU状态的演变由向量描述：
XIMU=[GIqˉTbgTGvITbaTGpIT]T(1)X_{IMU}=[^I_G\bar{q}^T \ b_g^T \ ^Gv_I^T\ b_a^T \ ^Gp_I^T]^T \tag{1} XIMU=[GIqˉT bgT GvIT baT GpIT]T(1)
其中GIqˉT^I_G\bar{q}^TGIqˉT是单位四元数，用来描述从坐标系{G}\{G\}{G}到坐标系{I}\{I\}{I}的旋转；GpI^Gp_IGpI和GvI^Gv_IGvI是IMU相对于坐标系{G}\{G\}{G}的位置和速度；bgb_gbg和bab_aba是3×13\times13×1向量，它们分别用来描述陀螺仪和加速度计测量的偏差。将IMU偏差建模为随机游走过程，分别由高斯白噪声向量nwgn_{wg}nwg和nwan_{wa}nwa驱动。根据公式（1），IMU的误差状态定义如下：
X~IMU=[δθITb~gTGv~ITb~aTGp~IT]T(2)\tilde{X}_{IMU}=[\delta \theta_I^T \ \tilde{b}_g^T \ ^G\tilde{v}_I^T \ \tilde{b}_a^T \ ^G\tilde{p}_I^T]^T \tag{2} X~IMU=[δθIT b~gT Gv~IT b~aT Gp~IT]T(2)
对于位置、速度和偏差，使用标准加性误差定义(即变量xxx的估计x^\hat{x}x^的误差定义为x~=x−x^\tilde{x}=x-\hat{x}x~=x−x^)。然而，对于四元数则采用了不同的定义。具体而言，如果四元数qˉ\bar{q}qˉ的估计值为qˉ^\hat{\bar{q}}qˉ^，然后用误差四元数δqˉ\delta \bar{q}δqˉ描述姿态误差，这是由这个关系定义的qˉ=δqˉ⨂qˉ^\bar{q}=\delta \bar{q} \bigotimes \hat{\bar{q}}qˉ=δqˉ⨂qˉ^。在这个表达式中，符号⨂\bigotimes⨂表示四元数的乘积。误差四元数为，
δqˉ≃[12δθT1]T(3)\delta \bar{q} \simeq \bigg[\frac{1}{2} \delta \theta^T \ 1 \bigg]^T \tag{3} δqˉ≃[21δθT 1]T(3)
直观上，四元数δqˉ\delta \bar{q}δqˉ描述了导致真实和估计姿态重合的小旋转。由于姿态对应3个自由度，使用δθ\delta \thetaδθ来描述姿态误差是一种最小表示。

假设EKF在时间步长kkk时的状态向量中包含NNN个相机位姿，该向量的形式为：
X^k=[X^IMUkTGC1qˉ^TGp^C1T⋯GCNqˉ^TGp^CNT]T(4)\hat{X}_k=[\hat{X}_{IMU_k}^T\ \ \ \ ^{C_1}_G\hat{\bar{q}}^T\ \ \ ^G\hat{p}_{C_1}^T \ \ \ \cdots \ \ \ ^{C_N}_G\hat{\bar{q}}^T \ \ \ ^G\hat{p}_{C_N}^T ]^T \tag{4} X^k=[X^IMUkT GC1qˉ^T Gp^C1T ⋯ GCNqˉ^T Gp^CNT]T(4)
其中GCiqˉ^^{C_i}_G\hat{\bar{q}}GCiqˉ^和Gp^Ci,i=1⋯N^G\hat{p}_{C_i},i=1\cdots NGp^Ci,i=1⋯N分别是相机姿态和位置的估计。EKF误差状态向量相应地定义为如下：
X~k=[X~IMUkTδθC1TGp~C1T⋯δθCNTGp~CNT]T(5)\tilde{X}_k=[\tilde{X}_{IMU_k}^T \ \ \ \delta \theta^T_{C_1}\ \ \ ^G\tilde{p}_{C_1}^T \ \ \ \cdots \ \ \ \delta \theta_{C_N}^T \ \ \ ^G\tilde{p}_{C_N}^T]^T \tag{5} X~k=[X~IMUkT δθC1T Gp~C1T ⋯ δθCNT Gp~CNT]T(5)

B 传播

对连续时间IMU系统模型进行离散化，得到滤波器传播方程，具体如下：

1）连续时间系统建模：IMU状态的时间演化由[20]给出：
GIqˉ˙(t)=12Ω(ω(t))GIqˉ(t),b˙g(t)=nwg(t)Gv˙I(t)=Ga(t),b˙a(t)=nwa(t),Gp˙I(t)=GvI(t)(6)\begin{matrix} ^I_G\dot{\bar{q}}(t)=\frac{1}{2}\Omega(\omega(t))^I_G\bar{q}(t), \ \ \ \dot{b}_g(t)=n_{wg}(t) \\ \\ ^G\dot{v}_I(t)=^Ga(t), \ \ \ \dot{b}_a(t)=n_{wa}(t), \ \ \ ^G\dot{p}_I(t)=^Gv_I(t) \end{matrix} \tag{6} GIqˉ˙(t)=21Ω(ω(t))GIqˉ(t), b˙g(t)=nwg(t)Gv˙I(t)=Ga(t), b˙a(t)=nwa(t), Gp˙I(t)=GvI(t)(6)
在这些表达式中，Ga^GaGa是在全局坐标系中的机体加速度，ω=[ωxωyωz]T\omega=[\omega_x \ \omega_y \ \omega_z]^Tω=[ωx ωy ωz]T是在IMU坐标系下的旋转速度，
Ω(ω)=[−⌊ω×⌋ω−ωT0],⌊ω×⌋=[0−ωzωyωz0−ωx−ωyωx0]\Omega(\omega)=\begin{bmatrix} -\lfloor \omega \times \rfloor & \omega \\ -\omega^T & 0 \end{bmatrix}, \ \ \ \lfloor \omega \times \rfloor = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} Ω(ω)=[−⌊ω×⌋−ωTω0], ⌊ω×⌋=⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤
ωm\omega_mωm和ama_mam分别表示陀螺仪和加速度计测量，由[20]给出：
ωm=ω+C(GIqˉ)ωG+bg+ng(7)\omega_m=\omega+C(^I_G\bar{q})\omega_G+b_g+n_g \tag{7} ωm=ω+C(GIqˉ)ωG+bg+ng(7)
am=C(GIqˉ)(Ga−Gg+2⌊ωG×⌋GvI+⌊ωG×⌋2⋅GpI)+ba+na(8)a_m=C(^I_G\bar{q})(^Ga-{^Gg}+2\lfloor \omega_G \times \rfloor^Gv_I + {\lfloor \omega_G \times \rfloor}^2\cdot {^Gp_I}) + b_a + n_a \tag{8} am=C(GIqˉ)(Ga−Gg+2⌊ωG×⌋GvI+⌊ωG×⌋2⋅GpI)+ba+na(8)
其中C(⋅)C(\cdot)C(⋅)表示旋转矩阵，ngn_gng和nan_ana是零均值的高斯白噪声，用来建模测量噪声。值得注意的是，IMU的测量纳入了地球自转的影响，ωG\omega_GωG。此外，加速度计测量包括重力加速度，Gg^GgGg，在局部坐标系中表示。

将期望算子应用于状态传播方程(公式（6）)，我们得到了IMU状态估计的传播方程：
GIqˉ^˙=12Ω(ω^)GIqˉ^,b^˙g=03×1,Gv^˙I=Cq^Ta^−2⌊ωG×⌋Gv^I−⌊ωG×⌋2⋅Gp^I+Ggb^˙a=03×1,Gp^˙I=Gv^I(9)\begin{matrix} ^I_G\dot{\hat{\bar{q}}}=\frac{1}{2}\Omega(\hat{\omega})^I_G\hat{\bar{q}}, \ \ \ \dot{\hat{b}}_g=\pmb{0}_{3\times1}, \\ \\ ^G\dot{\hat{v}}_I=C^T_{\hat{q}}\hat{a}-2\lfloor\omega_G \times\rfloor ^G\hat{v}_I - \lfloor \omega_G \times \rfloor^2 \cdot {^G\hat{p}_I} + ^Gg \\ \\ \dot{\hat{b}}_a=\pmb{0}_{3\times1}, \ \ \ ^G\dot{\hat{p}}_I=^G\hat{v}_I \end{matrix} \tag{9} GIqˉ^˙=21Ω(ω^)GIqˉ^, b^˙g=0003×1,Gv^˙I=Cq^Ta^−2⌊ωG×⌋Gv^I−⌊ωG×⌋2⋅Gp^I+Ggb^˙a=0003×1, Gp^˙I=Gv^I(9)
其中为了简洁，我们把做如下记号规定：Cq^=C(GIqˉ^)C_{\hat{q}}=C(^I_G\hat{\bar{q}})Cq^=C(GIqˉ^)，a^=am−b^a\hat{a}=a_m-\hat{b}_aa^=am−b^a和ω^=ωm−b^g−Cq^ωG\hat{\omega}=\omega_m-\hat{b}_g-C_{\hat{q}}\omega_Gω^=ωm−b^g−Cq^ωG。IMU误差状态的线性化连续时间模型为：
X~˙IMU=FX~IMU+GnIMU(10)\dot{\tilde{X}}_{IMU}=F\tilde{X}_{IMU}+Gn_{IMU} \tag{10} X~˙IMU=FX~IMU+GnIMU(10)
其中nIMU=[ngTnwgTnaTnwaT]Tn_{IMU}=[n_g^T \ \ \ n_{wg}^T \ \ \ n_a^T \ \ \ n_{wa}^T]^TnIMU=[ngT nwgT naT nwaT]T是系统噪声。nIMUn_{IMU}nIMU的协方差矩阵为QIMUQ_{IMU}QIMU，它取决于IMU的噪声特性和在传感器标定时的离线计算。最后，公式（10）中的FFF和GGG矩阵，由下式给出：
F=[−⌊ω^×⌋−I303×303×303×303×303×303×303×303×3−Cq^T⌊a^×⌋03×3−2⌊ωG×⌋−Cq^T−⌊ωG×⌋203×303×303×303×303×303×303×3I303×303×3]F=\begin{bmatrix} -\lfloor \hat{\omega}\times \rfloor & -\pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3}\\ -C_{\hat{q}}^T\lfloor \hat{a}\times \rfloor & \pmb{0}_{3\times 3} & -2\lfloor \omega_G \times \rfloor & -C_{\hat{q}}^T & -\lfloor \omega_G \times \rfloor ^2 \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3}\\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \end{bmatrix} F=⎣⎢⎢⎢⎢⎡−⌊ω^×⌋0003×3−Cq^T⌊a^×⌋0003×30003×3−III30003×30003×30003×30003×30003×30003×3−2⌊ωG×⌋0003×3III30003×30003×3−Cq^T0003×30003×30003×30003×3−⌊ωG×⌋20003×30003×3⎦⎥⎥⎥⎥⎤
其中I3\pmb{I}_3III3是3×33\times 33×3的单位矩阵。
G=[−I303×303×303×303×3I303×303×303×303×3−Cq^T03×303×303×303×3I303×303×303×303×3]G=\begin{bmatrix} -\pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & -C_{\hat{q}}^T & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{I}_3 \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \end{bmatrix} G=⎣⎢⎢⎢⎢⎡−III30003×30003×30003×30003×30003×3III30003×30003×30003×30003×30003×3−Cq^T0003×30003×30003×30003×30003×3III30003×3⎦⎥⎥⎥⎥⎤

2）离散时间实现：IMU对周期为T的信号ωm\omega_mωm和ama_mam进行采样，这些测量结果用于EKF中的状态传播。每次接收到新的IMU测量值时，通过公式(9)的5阶龙格-库塔数值积分传播IMU状态估计。此外，EKF的协方差矩阵也被传播。为此，我们引入下面的协方差划分：
Pk∣k=[PIIk∣kPICk∣kPICk∣kTPCCk∣k](11)P_{k|k}=\begin{bmatrix} P_{II_{k|k}} & P_{IC_{k|k}} \\ P_{IC_{k|k}}^T & P_{CC_{k|k}} \end{bmatrix} \tag{11} Pk∣k=[PIIk∣kPICk∣kTPICk∣kPCCk∣k](11)
其中PIIk∣kP_{II_{k|k}}PIIk∣k为演化IMU状态的15×1515\times 1515×15维协方差矩阵，PCCk∣kP_{CC_{k|k}}PCCk∣k是相机位姿估计的6N×6N6N\times 6N6N×6N的协方差矩阵，PICk∣kP_{IC_{k|k}}PICk∣k是IMU状态下的误差与相机位姿估计之间的相关性。在此记号下，传播状态的协方差矩阵为：
Pk+1∣k=[PIIk+1∣kΦ(tk+T,tk)PICk∣kPICk∣kTΦ(tk+T,tk)TPCCk∣k]P_{k+1|k}=\begin{bmatrix} P_{II_{k+1|k}} & \Phi(t_k+T,t_k)P_{IC_{k|k}} \\ P_{IC_{k|k}}^T \Phi (t_k + T, t_k)^T & P_{CC_{k|k}} \end{bmatrix} Pk+1∣k=[PIIk+1∣kPICk∣kTΦ(tk+T,tk)TΦ(tk+T,tk)PICk∣kPCCk∣k]
其中PIIk+1∣kP_{II_{k+1|k}}PIIk+1∣k是通过李亚普诺夫方程的数值积分来计算的：
P˙II=FPII+PIIFT+GQIMUGT(12)\dot{P}_{II}=FP_{II}+P_{II}F^T+GQ_{IMU}G^T \tag{12} P˙II=FPII+PIIFT+GQIMUGT(12)
在初始条件PIIk∣kP_{II_{k|k}}PIIk∣k下，对时间区间(tk,tk+T)(t_k,t_k+T)(tk,tk+T)进行了数值积分。状态转移矩阵Φ(tk+T,tk)\Phi(t_k+T,t_k)Φ(tk+T,tk)也同样通过微分方程的数值积分得到
Φ˙(tk+τ,tk)=FΦ(tk+τ,tk),τ∈[0,T](13)\dot{\Phi}(t_k+\tau,t_k)=F\Phi(t_k+\tau,t_k), \ \ \ \ \ \tau\in [0, T] \tag{13} Φ˙(tk+τ,tk)=FΦ(tk+τ,tk), τ∈[0,T](13)
其中初始条件Φ(tk,tk)=I15\Phi(t_k,t_k)=\pmb{I}_{15}Φ(tk,tk)=III15。

C 状态增广

记录新图像后，由IMU位姿估计计算出相机位姿估计为：
GCqˉ^=ICqˉ⊗GIqˉ^Gp^C=Gp^I+Cq^TIpC(14)\begin{matrix} ^C_G\hat{\bar{q}}=\ ^C_I\bar{q} \ \otimes\ ^I_G\hat{\bar{q}} \\ \\ ^G\hat{p}_C =\ ^G\hat{p}_I+C_{\hat{q}}^T\ ^Ip_C \end{matrix} \tag{14} GCqˉ^= ICqˉ ⊗ GIqˉ^Gp^C= Gp^I+Cq^T IpC(14)
其中ICqˉ^C_I\bar{q}ICqˉ是四元数，表示IMU坐标系和相机坐标系之间的旋转，IpC^Ip_CIpC是相机坐标系的原点相对于坐标系{I}\{I\}{I}的位置，这两个都是已知的。将该相机位姿估计加到状态向量中，并相应增大EKF的协方差矩阵：
Pk∣k←[I6N+15J]Pk∣k[I6N+15J]T(15)P_{k|k} \leftarrow \begin{bmatrix} \pmb{I}_{6N+15} \\ \pmb{J} \end{bmatrix} P_{k|k} \begin{bmatrix} \pmb{I}_{6N+15} \\ \pmb{J} \end{bmatrix}^T \tag{15} Pk∣k←[III6N+15JJJ]Pk∣k[III6N+15JJJ]T(15)
其中J\pmb{J}JJJ可由公式（14）得出：
J=[C(ICqˉ)03×903×303×6N⌊Cq^TIpC×⌋03×9I303×6N](16)\pmb{J}=\begin{bmatrix} C(^C_I\bar{q}) & \pmb{0}_{3\times 9} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 6N} \\ \lfloor C_{\hat{q}}^T\ ^Ip_C \times \rfloor & \pmb{0}_{3\times 9} & \pmb{I}_3 & \pmb{0}_{3\times 6N} \end{bmatrix} \tag{16} JJJ=[C(ICqˉ)⌊Cq^T IpC×⌋0003×90003×90003×3III30003×6N0003×6N](16)

D 测量模型

我们现在提出了用于更新状态估计的测量模型，这是本文的主要贡献。由于EKF用于状态估计，对于构建测量模型，它足以定义一个线性依赖于状态误差X~\tilde{X}X~的残差rrr，根据一般形式：
r=HX~+noise(17)r = H\tilde{X} + noise \tag{17} r=HX~+noise(17)
在这个表达式中，HHH是测量雅克比矩阵，为了应用EKF框架，噪声项必须是零均值，白色的，并且与状态误差不相关。

为了推导出我们的测量模型，我们的动机是这样一个事实：从多个相机位姿观测一个静态特征会导致涉及所有这些位姿约束。在我们的工作中，相机观察按跟踪特征分组，而不是按测量记录的相机位姿分组(后者是这种情况，例如，在计算位姿之间的成对约束的方法)。对同一个3D点的所有测量都用来定义一个约束方程(参见公式(24))，它涉及测量发生的所有相机位姿。这是在不包括滤波器状态向量中的特征位置的情况下实现的。我们提出了一个测量模型，考虑一个单一的特征，fjf_jfj，已观察到一组MjM_jMj相机位姿(GCiqˉ,GpCi),i∈Sj(^{C_i}_G\bar{q}, \ ^Gp_{C_i}),\ i \in S_j(GCiqˉ, GpCi), i∈Sj。该模型描述了该特征的每个MjM_jMj观测值：
zi(j)=1CiZj[CiXjCiYj]+ni(j),i∈Sj(18)z_i^{(j)}=\frac{1}{^{C_i}Z_j}\begin{bmatrix} ^{C_i}X_j \\ ^{C_i}Y_j \end{bmatrix} + n_i^{(j)}, \ \ i \in S_j \tag{18} zi(j)=CiZj1[CiXjCiYj]+ni(j), i∈Sj(18)
其中ni(j)n_{i}^{(j)}ni(j)是2×12\times 12×1的图像噪声向量，其协方差矩阵为Ri(j)=σim2I2\pmb{R}_i^{(j)}=\sigma_{im}^2\pmb{I_2}RRRi(j)=σim2I2I2I2。表示在相机坐标系中的特征位置，Cipfj^{C_i}\pmb{p}_{f_j}Cipppfj由下式给出：
Cipfj=[CiXjCiYjCiZj]=C(GCiqˉ)(Gpfj−GpCi)(19)^{C_i}\pmb{p}_{f_j}=\begin{bmatrix} ^{C_i}X_j \\ ^{C_i}Y_j \\ ^{C_i}Z_j \end{bmatrix} = C(^{C_i}_G\bar{q})(^Gp_{f_j}-^Gp_{C_i}) \tag{19} Cipppfj=⎣⎡CiXjCiYjCiZj⎦⎤=C(GCiqˉ)(Gpfj−GpCi)(19)
其中Gpfj^Gp_{f_j}Gpfj为在全局坐标系中的3D特征位置。由于这是未知的，在我们的算法的第一步，我们使用最小二乘最小化来获得特征位置的估计，Gp^fj^G\hat{p}_{f_j}Gp^fj。这是在相应的时刻使用测量zi(j),i∈Sjz_i^{(j)}, i \in S_jzi(j),i∈Sj和滤波器相机位姿的估计值来实现的（参见附录）。

一旦得到特征位置的估计，我们计算测量残差：
ri(j)=zi(j)−z^i(j)(20)r_i^{(j)}=z_i^{(j)}-\hat{z}_i^{(j)} \tag{20} ri(j)=zi(j)−z^i(j)(20)
其中，
z^i(j)=1CiZ^j[CiX^jCiY^j],[CiX^jCiY^jCiZ^j]=C(GCiqˉ^)(Gp^fj−Gp^Ci)\hat{z}_i^{(j)}=\frac{1}{^{C_i}\hat{Z}_j}\begin{bmatrix} ^{C_i}\hat{X}_j \\ ^{C_i}\hat{Y}_j \end{bmatrix}, \ \ \ \begin{bmatrix} ^{C_i}\hat{X}_j \\ ^{C_i}\hat{Y}_j \\ ^{C_i}\hat{Z}_j \end{bmatrix} = C(^{C_i}_G\hat{\bar{q}})(^G\hat{p}_{f_j}-^G\hat{p}_{C_i}) z^i(j)=CiZ^j1[CiX^jCiY^j], ⎣⎡CiX^jCiY^jCiZ^j⎦⎤=C(GCiqˉ^)(Gp^fj−Gp^Ci)
对相机位姿估计和特征位置估计进行线性化，公式(20)的残差近似为：
ri(j)≃HXi(j)X~+Hfi(j)Gp~fj+ni(j)(21)r_i^{(j)} \simeq H_{X_i}^{(j)}\tilde{X}+H_{f_i}^{(j)}\ ^G\tilde{p}_{f_j}+n_i^{(j)} \tag{21} ri(j)≃HXi(j)X~+Hfi(j) Gp~fj+ni(j)(21)
在前面的表达式中HXi(j)H_{X_i}^{(j)}HXi(j)和Hfi(j)H_{f_i}^{(j)}Hfi(j)分别表示测量zi(j)z_i^{(j)}zi(j)相对于状态和特征位置的雅克比；Gp~fj^G\tilde{p}_{f_j}Gp~fj是fjf_jfj位置估计的误差。这个表达式中雅可比矩阵的确切值在[21]中提供。通过堆叠该特征的所有MjM_jMj测量值的残差，我们得到：
r(j)≃Hx(j)X~+Hf(j)Gp~fj+n(j)(22)r^{(j)} \simeq H_x^{(j)}\tilde{X} + H_f^{(j)}\ ^G\tilde{p}_{f_j} + n^{(j)} \tag{22} r(j)≃Hx(j)X~+Hf(j) Gp~fj+n(j)(22)
其中r(j)r^{(j)}r(j)、HX(j)H_X^{(j)}HX(j)、Hf(j)H_f^{(j)}Hf(j)和n(j)n^{(j)}n(j)是块向量或矩阵，分别带有元素ri(j)r_i^{(j)}ri(j)、HXi(j)H_{X_i}^{(j)}HXi(j)、Hfi(j)H_{f_i}^{(j)}Hfi(j)和ni(j)n_i^{(j)}ni(j)，其中i∈Sji\in S_ji∈Sj。由于不同图像的特征观测是相互独立的，因此n(j)n^{(j)}n(j)的协方差矩阵为R(j)=σim2I2MjR^{(j)}=\sigma_{im}^2\pmb{I}_{2M_j}R(j)=σim2III2Mj。

请注意由于状态估计，XXX，被用来计算特征位置的估计（参见附录），公式（22）中的误差Gp~fj^G\tilde{p}_{f_j}Gp~fj和误差X~\tilde{X}X~相关。因此，残差r(j)r^{(j)}r(j)不是公式（17）的形式，它不能直接应用于EKF中的测量更新。为了克服这个问题，我们定义残差ro(j)r_o^{(j)}ro(j)，通过将r(j)r^{(j)}r(j)投影至矩阵Hf(j)H_f^{(j)}Hf(j)的左零空间。具体而言，如果我们让AAA表示酉矩阵，它的列构成HfH_fHf的左零空间的基，我们得到：
ro(j)=AT(z(j)−z^(j))≃ATHX(j)X~+ATn(j)(23)r_o^{(j)}=A^T(z^{(j)}-\hat{z}^{(j)})\simeq A^TH_X^{(j)}\tilde{X} + A^Tn^{(j)} \tag{23} ro(j)=AT(z(j)−z^(j))≃ATHX(j)X~+ATn(j)(23)
=Ho(j)X~(j)+no(j)(24)=H_o^{(j)}\tilde{X}^{(j)}+n_o^{(j)} \tag{24} =Ho(j)X~(j)+no(j)(24)
由于2Mj×32M_j\times 32Mj×3维矩阵Hf(j)H_f^{(j)}Hf(j)是列满秩的，它的左零空间的维数是2Mj−32M_j-32Mj−3。因此，ro(j)r_o^{(j)}ro(j)是(2Mj−3)×1(2M_j-3)\times 1(2Mj−3)×1维向量。该残差与特征坐标中的误差无关，因此可以基于该残差进行EKF更新。公式（24）定义了观察特征fjf_jfj的所有相机位姿之间的线性化约束。这表达了测量zi(j)z_i^{(j)}zi(j)为MjM_jMj个状态提供的所有可用信息，因此得到的EKF更新是最优的，除了线性化引起的不准确性。需要指出的是，为了计算残差ro(j)r_o^{(j)}ro(j)和测量矩阵Ho(j)H_o^{(j)}Ho(j)，不需要显式地计算酉矩阵AAA。相反，向量rrr和矩阵HX(j)H_X^{(j)}HX(j)在Hf(j)H_f^{(j)}Hf(j)的零空间上的投影可以使用Givens旋转[22]，在O(Mj2)O(M_j^2)O(Mj2)运算中非常有效地计算。此外，由于矩阵AAA是酉矩阵，那么噪声向量no(j)n_o^{(j)}no(j)的协方差矩阵可以表示为：
E{no(j)no(j)T}=σim2ATA=σim2I2Mj−3E\{n_o^{(j)} {n_o^{(j)}}^T \}=\sigma_{im}^2 A^TA=\sigma_{im}^2I_{2M_j-3} E{no(j)no(j)T}=σim2ATA=σim2I2Mj−3

公式（23）中定义的残差并不是通过观察MjM_jMj图像中的静态特征而产生的几何约束的唯一可能表达。另一种方法是，例如，使用为每个Mj(Mj−1)/2M_j(M_j-1)/2Mj(Mj−1)/2图像对定义的极线约束。然而，得到的Mj(Mj−1)/2M_j(M_j-1)/2Mj(Mj−1)/2个方程仍然只对应2Mj−32M_j-32Mj−3独立约束，因为每次测量都被多次使用，使得方程在统计上具有相关性。我们的实验表明，相比于上述方法，使用线性化的极线约束导致一个明显更复杂的实现，并产生较差的结果。

论文阅读《A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation》1相关推荐

论文阅读《Characterizing BDS signal-in-space performance from integrity perspective》3
目录 6 结论附录A 附录B 附录C 6 结论本文从完好性的角度对北斗二号和北斗三号系统的SIS性能进行了描述.利用新提出的SISRE评价方案,分析了2016年至2020年年中4.5年的北斗二代数 ...
论文阅读《Characterizing BDS signal-in-space performance from integrity perspective》1
目录摘要 1 介绍 2 数据集和SISRE评价方法 2.1 数据集和数据清理 2.2 时间,坐标和修正 2.3 星历状态:数据年限.URA索引和健康标志 2.4 空间信号测距误差(SISRE)计算 ...
论文阅读 Learning Motion in Feature Space: Locally-Consistent Deformable Convolution Networks
Learning Motion in Feature Space: Locally-Consistent Deformable Convolution Networks for Fine-Graine ...
论文阅读《Revisiting Stereo Depth Estimation From a Sequence-to-Sequence Perspective with Transformers》
论文地址:https://ieeexplore.ieee.org/document/9711118 源码地址:https://github.com/mli0603/stereo-transformer ...
论文阅读《Pedestrian Dead Reckoning-Assisted Visual Inertial Odometry Integrity Monitoring》
目录摘要 1 介绍 2 背景 3 视觉误差分析和自主完好性监测 3.1 视觉误差分析 3.1.1 不足的特征 3.1.2 光照导致特征跟踪失败 3.1.3 特征分布不均匀 3.1.4 运动特征 3. ...
Harmonzing Performance and Isolation in Microkernels论文阅读
Harmonizing Performance and Isolation in Microkernels with Efficient Intra-kernel Isolation and Comm ...
【论文阅读】Parametrized Deep Q-Networks Learning: RL with Discrete-Continuous Hybrid Action Space
[论文阅读-深度强化学习打王者荣耀]Parametrized Deep Q-Networks Learning: Reinforcement Learning with Discrete-Contin ...
[论文阅读] (13)英文论文模型设计（Model Design）如何撰写及精句摘抄——以入侵检测系统(IDS)为例
<娜璋带你读论文>系列主要是督促自己阅读优秀论文及听取学术讲座,并分享给大家,希望您喜欢.由于作者的英文水平和学术能力不高,需要不断提升,所以还请大家批评指正,非常欢迎大家给我留言评论,学 ...
SR研究（1）RCAN论文阅读上
SR研究(1)RCAN论文阅读上阅读论文:Image Super-Resolution Using Very Deep Residual Channel Attention Networks RCA ...
深度学习论文阅读图像分类篇（五）：ResNet《Deep Residual Learning for Image Recognition》
深度学习论文阅读图像分类篇(五):ResNet<Deep Residual Learning for Image Recognition> Abstract 摘要 1. Introduct ...

论文阅读《A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation》1

目录

摘要