一种集成惯性测量和视觉特征观测的算法遵循同时定位和构图(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特征位置，但仍有最佳位置估计。由于这些特性，算法非常有效，如第四节所示，实时高精度视觉辅助惯性导航。

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

IMU状态的演变由向量描述：
$$X_{IMU}={[}_G^I{\bar{q}}^T{b}_g^T{}^G{v}_I^T{b}_a^T{}^G{p}_I^T{]}^T\text{\hspace{1em}(1)}$$
其中${}_G^I{\bar{q}}^T$是单位四元数，用来描述从坐标系 { G } \{G\} { G}到坐标系 { I } \{I\} { I}的旋转；${}^G{p}_I$和${}^G{v}_I$是IMU相对于坐标系 { G } \{G\} { G}的位置和速度；$b_g$和$b_a$是$3\times 1$向量，它们分别用来描述陀螺仪和加速度计测量的偏差。将IMU偏差建模为随机游走过程，分别由高斯白噪声向量$n_{wg}$和$n_{wa}$驱动。根据公式（1），IMU的误差状态定义如下：
$${\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\text{\hspace{1em}(2)}$$
对于位置、速度和偏差，使用标准加性误差定义(即变量$x$的估计$\hat{x}$的误差定义为$\tilde{x}=x-\hat{x}$)。然而，对于四元数则采用了不同的定义。具体而言，如果四元数$\bar{q}$的估计值为$\hat{\bar{q}}$，然后用误差四元数$\delta \bar{q}$描述姿态误差，这是由这个关系定义的$\bar{q}=\delta \bar{q}⨂\hat{\bar{q}}$。在这个表达式中，符号$⨂$表示四元数的乘积。误差四元数为，
$$\delta \bar{q}\simeq [\frac{1}{2}\delta {\theta }^{T}1{]}^T\text{\hspace{1em}(3)}$$
直观上，四元数$\delta \bar{q}$描述了导致真实和估计姿态重合的小旋转。由于姿态对应3个自由度，使用$\delta \theta$来描述姿态误差是一种最小表示。

假设EKF在时间步长$k$时的状态向量中包含$N$个相机位姿，该向量的形式为：
X ^ k = [ X ^ I M U k T       G C 1 q ˉ ^ T      G p ^ C 1 T     ⋯      G C N q ˉ ^ T      G p ^ C N T ] 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^IMUk​T​    GC1​​qˉ​^​T   Gp^​C1​T​   ⋯