MSCKF2.0(Mingyang Li-IJRR2013) 論文要點總結

論文：Li M, Mourikis A I. High-precision, consistent EKF-based visual-inertial odometry[J]. The International Journal of Robotics Research, 2013, 32(6): 690-711.

摘要

本篇論文的主要工作包括：

1. 分析和比較MSCKF和幾種傳統EKF-SLAM方法的理論基礎和性能精度；
2. 證明了現有的MSCKF和EKF-SLAM的狀態估計過程均是不一致（inconsistent），並給出原因是量測雅各比計算方式不對；
3. 提出一種新的、實時的、能夠實現一致估計的EKF-VIO算法（MSCKF2.0），主要貢獻包括：1）確保了建立的線性系統模型正確的觀測性；2）對相機-IMU的外參進行在線估計校正。

引言

文中主要對以下三類VIO估計器進行分析和比較：
1）EKF-SLAM：濾波狀態裏包含當前的IMU pose、相機觀測到的特徵點 pose；
2）滑動窗口算法：濾波狀態裏只包含一個不同時刻pose構成的滑動窗口，相機觀測到的特徵點被用來在pose間構造概率約束；
3）MSCKF：濾波狀態裏只包含多個IMU的pose，此被證明是一種線性化系統的最大後驗估計。

在文中，經過蒙特卡洛仿真和實際環境測試表明，MSCKF方法在精度、一致性和計算效率方面均大大優於EKF-SLAM方法。作者認爲會產生這種優勢的原因是在於MSCKF方法未對特徵點的位置進行高斯假設，但是在EKF-SLAM中對特徵點位置進行了高斯假設。

此外，經過分析總結了MSCKF產生不一致的兩個因素：
1）IMU相對重力方向的旋轉（yaw角）在VIO中是不可觀測的，但是在MSCKF和EKF-SLAM的線性化系統模型裏表現出來確實可觀測的。因此在實際中，估計器錯誤的認爲它有更多的觀測信息，故而導致低估狀態的協方差矩陣，所以此時實際輸出的狀態協方差並不能真正反映狀態量的協方差。
2）相機和IMU間的外參（旋轉和平移）被認爲是一個預知的較優參數。但是，並且通常，這些標定參數被估計器誤認爲是精確的，所以導致標定模型中未建模的不確定因素導致狀態協方差的再次低估。

爲了提高MSCKF的的一致性，文中對上述兩個問題進行了分析和解決：
1）在濾波器雅各比計算過程中修復了濾波器線性化系統模型的觀測性。此外，還推導了一個閉合的IMU誤差狀態轉移矩陣；
2）將camera-IMU間的外參數包含在了MSCKF的狀態向量裏，隨着IMU狀態進行一起在線估計。

作者將更改後的MSCKF算法稱爲MSCKF2.0，與EKF-SLAM相比，其具有更高的一致性、精度、計算效率。甚至可以與使用滑動窗口迭代最小化的算法進行比較，但是計算效率更高。通過分析和論證，作者認爲，對於非線性測量模型，使建立的線性系統模型的觀測性更加匹配可能比重新線性化更加重要。

相關工作

松耦合VIO：將IMU和圖像測量進行單獨處理。
優點：計算消耗少，特別適合於計算資源有限的系統；
缺點：會引起信息損失，比如在圖像對間使用特徵測量進行位姿估計忽視了連續時間間的相關性，以及單獨處理IMU測量並不能確保傳感器零偏的最優估計。

文中主要貫注於緊耦合方法，即直接融合視覺和慣性數據，從而實現更高的精度。經過分析表明，一個設計適當EKF估計器可以得到比迭代最小化更高的性能，並且計算資源消耗更小。

最常見的緊耦合EKF估計器是EKF-SLAM，在這種方法裏當前相機的pose和特徵點位置被進行聯合估計。缺點是計算量大，與狀態向量裏的特徵點數量的三次方成正比。甚至在某些時候，比迭代最小化的計算消耗更大。爲了解決計算量大的問題，MSCKF應運而生，其在狀態向量裏維護一組pose的滑動窗口，使用相機測量的特徵點在這些pose間建立約束，這樣算法的複雜度只與特徵點數量成線性關係。而且經過作者的分析和比較，證明MSCKF比EKF-SLAM更快、精度更高，一致性更好。

文中的重點貢獻工作是分析和提高基於EKF的視覺慣性系統的一致性。過去在3D視覺定位的一致性中重點關注於特徵點位置的參數化。（Civera et al., 2008）已經表明笛卡爾座標系（XYZ）參數會對特徵點的估計導致嚴重非高斯屬性，從而降低精度和一致性，所以提出了一種逆深度特徵參數化方法，其可以更好的適配相機的測量模型，並提高性能。（Sola, 2010）提出一種固定齊次特徵參數化方法，表明可以進一步提升濾波器一致性。文中對以上三種特徵參數化方法進行了測試，儘管固定齊次特徵參數化優於其他方法，但是其性能依舊弱於MSCKF算法（MSCKF中使用的是齊次座標系下的逆深度特徵參數化）。

與之前其他的論文相比，作者有以下方面的不同：
1）比較了MSCKF和其他EKF-SLAM方法的性能；
2）發現了影響EKF-SLAM方法的相同的，錯誤的觀測性問題；
3）解決了由於預知的不精確的相機-IMU外參標定參數導致的不一致問題；
3）進行了大規模的仿真和實驗進行分析和比較。

iii 基於EKF的視覺慣性里程計

本部分主要描述了文中使用的IMU狀態的參數化過程，並討論了兩種主流的緊耦合EKF-VIO公式，同時比較了它們的性能。

A. IMU狀態的參數化

• 估計的global系下的位移誤差
  
• 估計的global繫到IMU系的旋轉誤差

上式中$\delta{q}$表示需要將估計的global座標系旋轉到真實global座標系的旋轉小量。
上式中$\delta\theta$是一個3*1的表示global系下三個座標軸的姿態誤差。

• IMU誤差狀態描述

上式表示IMU的15*1維的誤差狀態向量。文中姿態誤差的選擇方式基於（Li and Mourikis, 2012a）論文，此分析指出如上將姿態誤差定義爲真實的global系和估計的global系間的誤差要優於定義爲真實的IMU系和估計的IMU系間的誤差。需要指出的是，後者（比如在Mourikis and Roumeliotis 2007中所用）在EKF的線性系統的觀測模型中引入了額外的誤差項 ？。

B. EKF-SLAM

• 濾波器的狀態包含當前IMU狀態$x_{Il}$, 及多個特徵點的位置
  
  本文中，特徵點的位置會考慮以下三種參數化方式：1）傳統的XYZ座標參數化；2）逆深度參數化（Civera et al., 2008）; 3）固定齊次參數化（Sola. 2010）。這是在實際中比較常用幾種特徵點位置參數化形式，後兩種的目的主要是爲了提升濾波器的一致性和精度。

• 濾波器的狀態更新方程、狀態協方差矩陣計算與常規EKF算法一致
  
  

• 特徵點 i 在時刻 $l$ 的觀測模型
  
  在EKF-SLAM中，特徵點觀測被直接用來進行狀態估計，此過程使用實際特徵點位置和期望的特徵點位置間的殘差建立, 線性化估計形式爲：
  
  上式中的$H_{il}$是$h$相對於濾波狀態的雅各比矩陣在狀態估計$\hat{x}_{l|l-1}$處的值。這是一個稀疏矩陣，只在IMU狀態處和第i個特徵點處包含非零塊，其他位置均爲0。
  
  一旦$r_{il}$和$H_{il}$計算完成, 會進行一個馬氏門限測試？，如果通過，則進行標準EKF等式的更新（Maybeck, 1982）。

  C. MSCKF

  與EKF-SLAM相比，MSCKF維護一個poses的滑動窗口作爲狀態向量，使用特徵點觀測在這些poses之前構造概率約束。在時間l下的狀態向量爲：
  
  上式中$\pi_i =[{q_G}^{I_i} p_i^G]^T$， i= l-N, …, l-1，此爲過去N個圖像被記錄時刻的imu的poses。
  
  算法流程：

• 狀態傳播：基於IMU輸出進行狀態向量和協方差更新；

• 測量更新：當一次圖像被記錄時進行更新；
  1.狀態增廣：基於記錄圖像時刻的imu位置和姿態對滑動窗口內的狀態向量和協方差變量進行增廣；
  2.圖像處理：角點特徵提取，特徵匹配；
  3.更新：對每個特徵進行跟蹤，並計算$r_i^o和H_i^o$，以及進行馬氏門限測試。使用通過測試的所有特徵進行一次EKF更新；
  4.狀態管理：移除所維護狀態量滑動窗口裏與所有已經處理過的特徵相關的imu的pose狀態。

D EKF-SLAM與MSCKF對比

• 所有的EKF-SLAM都假設imu狀態和特徵點的誤差在每個時間都是聯合高斯分佈。但是，由於相機測量模型的非線形，此估計不是很好，尤其是XYZ參數話特徵點的時候。通過優化視覺特徵參數化形式，比如使用逆深度參數化IDP和固定齊次參數化可以提高EKF-SLAM的精度和一致性，但是這些方法仍舊顯著表現差於MSCKF。這是因爲在MSCKF中特徵點不包括在狀態量裏，所以不存在特徵點的誤差假設，能夠避免一部分較大的估計不確定性誤差源；
• 在EKF-SLAM中，每一次都進行特徵測量線性化和處理。然而，在MSCKF中，使用了一個延遲線性化的方法：其只有當所有測量都可用時才處理一次特徵（多狀態約束）。這意外着在計算雅各比時能夠實現更加精確的特徵估計，從而得到更加精確的卡爾曼增益，並提高修正精度，達到更好的性能。

MSCKF 2.0

A 增強不可觀測子空間的修正維度

在MSCKF中相同狀態的不同估計量被用來進行計算雅各比，導致了yaw角融合時引進了虛假的信息。修正這個誤差的方法是，保證在涉及雅各比計算的時候，只使用imu位置和姿態的一組估計量。此處總是使用每一個狀態的第一次可用的估計值進行雅各比計算。

B 進行在線camera-imu標定

將camera-imu間的轉換矩陣包含在濾波器狀態裏，進行在線估計修正。

測試結果

1. 與msckf比較

2. 與迭代優化slam比較

3. camera-imu在線標定性能

4. 計算性能

MSCKF2.0在i7-2.66GHz的處理器上運行單線程c++代碼，每次更新（包含圖像處理和估計）是10ms。