摘要
本文提出了一种基于扩展卡尔曼滤波（ekf）的实时视觉辅助惯性导航算法。这项工作的主要贡献是推导了一个测量模型，该模型能够表达从多个相机姿态观察静态特征时产生的几何约束。这种测量模型不需要在ekf的状态向量中包含3d特征位置，并且在线性化误差范围内是最优的。我们提出的视觉辅助惯性导航算法的计算复杂度仅在特征数量上是线性的，并且能够在大规模真实世界环境中进行高精度姿态估计。该算法的性能在大量的实验结果中得到了验证，包括一个城市区域内的摄像机/imu系统。
EKF状态向量构成与状态向量的传递
误差状态卡尔曼滤波 Error-State Kalman Filter (ErKF) 估计的是状态值的误差部分
IMU部分的协方差矩阵的微分方程是
P=PFT+PF+GQ