摘要
本文提出了一種基於擴展卡爾曼濾波（ekf）的實時視覺輔助慣性導航算法。這項工作的主要貢獻是推導了一個測量模型，該模型能夠表達從多個相機姿態觀察靜態特徵時產生的幾何約束。這種測量模型不需要在ekf的狀態向量中包含3d特徵位置，並且在線性化誤差範圍內是最優的。我們提出的視覺輔助慣性導航算法的計算複雜度僅在特徵數量上是線性的，並且能夠在大規模真實世界環境中進行高精度姿態估計。該算法的性能在大量的實驗結果中得到了驗證，包括一個城市區域內的攝像機/imu系統。
EKF狀態向量構成與狀態向量的傳遞
誤差狀態卡爾曼濾波 Error-State Kalman Filter (ErKF) 估計的是狀態值的誤差部分
IMU部分的協方差矩陣的微分方程是
P=PFT+PF+GQ