如何理解EKF中的consistency？

卡爾曼濾波，SLAM

這個和答過的FEJ(First Estimate Jacobian)一樣可以從同一套基礎理論來理解，之前關於consistency寫的不是很明白，這裡分享下直觀理解順便給自己在這方面做個總結。公式推導和深入理解，還是要靠自己看相關文獻。

----------------------------

EKF-SLAM中的consistency的定義[1]：

a state estimator is consistent if the estimation errors (i) are zero-mean, and (ii) have covariance matrix smaller or equal to the one calculated by the filter.

翻譯成中文的解釋，即要求：1.狀態估計結果的誤差是零均值的（無偏？）；2.狀態估計結果的誤差的協方差，比estimator估計的協方差要更小或相等（也就是說估計的協方差不能比真實誤差的協方差更小）。

這其中有兩個概念：狀態估計結果的誤差的協方差（真正的這個協方差需要有真值才能算出），以及estimator估計出來的協方差（比如EKF中就有協方差的估計和傳遞）。

[1]中對上述的第2點進行了分析。已有的工作表明，對於[1]中考慮的平面上三自由度（二維平移+一維旋轉）空間中運動的機器人，估計出的旋轉自由度的協方差總是（趨向於）比真實的協方差小。這就不滿足consistency的第2點要求了。

進一步，[1]從observability（能觀性）的角度對破壞consistency的來源進行了理論分析：

a) 上述三自由度機器人系統，其本質上的不能觀的自由度是三個；

b) 但按照標準EKF-SLAM的方式，不能觀自由度是兩個，少掉了一個自由度；

c) b與a相比，少掉一個自由度的原因在於，在EKF中進行線性化時，狀態更新導致線性化的狀態點不一樣，這個不同會使系統的能觀性矩陣性質發生改變（即[1]中對standard EKF的分析）；

d) 對於不能觀的自由度，其真實方差將會不斷(unbounded)增長（比方說VO中，世界坐標系中的位姿估計誤差可能會一直累加），不能觀自由度減小一維導致的直觀後果是，等效於引入了錯誤的觀測信息，使估計的方差在這個維度（錯誤地）出現reduction，因此不滿足consistency的第2點。

e) [1]中為了應對這個inconsistency問題，採用FEJ的方式，固定線性化點，不再導致能觀性變化。（另外還有OC-EKF的方式[2]）

--------------------------------

從上文看出，consistency和observability（能觀性）的分析密切相關。

[1]中最初是針對平面情況分析的，後來擴展到了三維空間情況，特別是視覺慣性融合領域。如FEJ的答案中所說，在運動充分時，視覺慣性系統的不能觀自由度是4維，對應三維絕對位置和一維yaw。（相比而言，單目VSLAM的不能觀自由度是7維：三軸位置、三軸旋轉、尺度。）

和平面情況類似，線性化點不同造成的影響同樣使得視覺慣性系統的能觀性變化，yaw對應的自由度錯誤地變得能觀了，所以同樣可以用FEJ的方式來提高consistency。MSCKF 2.0中就進行了這樣的處理。

--------------------------------

（接下來這部分的理解仍欠火候。。期待補充……）

除了EKF-SLAM，基於sliding window優化的SLAM方法中如果用到邊緣化（Marginalization）的操作，也可能會有上述問題。

直觀的說明是：邊緣化的目的是獲得某些變數的分布，作為下次優化時的先驗分布。邊緣化對正規方程Hx=b的H矩陣操作，而H矩陣的計算涉及到非線性觀測函數的線性化，也需要選取線性化狀態點。

對於一組狀態[x,y]，將x邊緣化計算y的分布時，需要選擇y的狀態點進行線性化，假設是在y0處。之後對狀態進行擴充變為[y,z]並進行了狀態更新（觀測到新的landmark，同時有新的相機pose），這時重新構建正規方程Hx=b時還要繼續進行線性化。但y的狀態有可能從y0已經更新到y1，導致線性化點的不同。

這就造成：先驗分布對應y0，對H貢獻部分稱H0；新的[y,z]進行線性化時對應y1，對H貢獻部分稱H1。正規方程實際是(H0+H1)x=b。這同樣會造成inconsistency，來源同樣是能觀性的改變。借用 @楊楠 所給DSO作者的圖（見最後附圖），對同一個非線性函數在不同狀態點線性化時，H零空間會退化，而H零空間維數與能觀自由度個數有關（這裡參考VSLAM或視覺慣性導航系統能觀性相關的文獻），這種退化導致能觀性改變，進而會影響consistency。

因此在sliding window優化的SLAM方法中，現在也有進行FEJ來考慮consistency的做法，比如視覺慣性導航的OKVIS和純視覺直接法的DSO。

但是，基於非線性優化的方法可以多次線性化，使線性化點更接近真值。所以到底是固定一個不準確的初始線性化點用FEJ來保持consistency，還是不採用FEJ而用最新的最接近真值的狀態點進行線性化來估計，兩者精度孰優孰劣，暫時還沒有看到比較分析。

-------------------------------

參考文獻（不全，其他相關的可以看FEJ的回答）

[1] Huang G P, Mourikis A I, Roumeliotis S I. Analysis and improvement of the consistency of extended kalman filter based slam. ICRA 2008.

[2] Hesch J A, Kottas D G, Bowman S L, et al. Camera-imu-based localization: Observability analysis and consistency improvement. The International Journal of Robotics Research, 2014, 33(1):182–201.

------------------------

附圖