如何理解SLAM中的First-Estimates Jacobian？

01-15

按自己的粗淺理解整理總結一下~

按小夥伴的說法，拋磚等大神。先回答個傻點的，就會有大神來反駁給出好答案了，哈哈~~歡迎指正。

1.Observability能觀性

對於一個系統，Observability性質（能觀性）[1,2]，決定了這個系統在進行狀態估計時，哪些自由度是可以被估計出來的。並且其能觀性是不受估計方法（Closed-form 方法、EKF、或者Nonlinear Optimization等等）改變的。

與自控理論中的可觀性一樣，能觀性通過Observability Matrix（能觀性矩陣）體現，系統Unobservable的狀態維數是這個矩陣零空間的維數。（對於非線性系統，相關性質和矩陣比較特別些，會用到Locally Weakly Observable的概念或者其他的定義，未深究，感興趣可以看[1]和它的參考文獻[8]）

比如，單目純視覺SLAM里，尺度和6DOF的絕對位姿——總共7DOF——無法被估計。（絕對位姿的6個自由度可以通過固定某一幀位姿來進行固定，而剩下的尺度卻無法通過純視覺固定下來，這在誤差累積下，會造成單目VSLAM的尺度漂移[3]）

再比如Visual-Inertial系統里，在運動激勵充分（足夠多軸向有足夠大的加速度/角速度）的情況下，尺度、滾轉/俯仰角是可以被估計的，只剩下絕對偏航角、絕對位置無法獲得[1]，也就是說對於Visual-Inertial系統在合適的運動模式下Unobservable的維數是4。（甚至相機-IMU外參和零偏也可以估計。）

Full observability requires the camera–IMU platform to undergo rotation about at least two IMU axes and acceleration along two IMU axes.

2.二維機器人EKF-SLAM中First Estimate Jacobian提出

Huang Guoquan老師在[4]中提出First Estimate Jacobian（之後簡寫成FEJ），論文是針對二維機器人3DOF的EKF-SLAM。

上述情況，離散化後EKF系統的傳播和測量方程如下

對於3DOF的系統，其能觀性矩陣如下

從M的形式中包含雅克比矩陣 $H_k/Phi_k$ 可以看出，不同的線性化點導致 $H_k/Phi_k$ 不同，那麼也會導致M不同。

接下來就是看不同的線性化點對M的影響。

在「理想」的EKF-SLAM情況下，把狀態 $x_k^*$ 選成真值，Huang老師了證明這時M的零空間維數是3，和真正的Unobservable維數相同。

$ext{dim}( ext{Null}(M_{ ext{ideal}}))=3$

但在實際的EKF-SLAM中，狀態真值是不知道的，所以一般做法是選擇最新的狀態值來進行 $H_k/Phi_k$ 的計算。那麼計算 $H_k/Phi_k$ 的狀態值就不同了，這種情況下，M的零空間維數是2，

$ext{dim}( ext{Null}(M_{ ext{std}}))=2$

也就是說這個做法把原本Unobservable的一個維度變成Observable的了（這個維度對應狀態yaw）。

這種Observability性質的改變帶來的影響，論文里稱為inconsistency。

Therefore, the filter gains 「nonexistent」 information about the robot』s global orientation. This leads to an unjustified reduction in the orientation uncertainty, which in turn, further reduces the uncertainty in all the state variables, causing inconsistency.

為了修正這個問題，提出了First Estimate Jacobian。Inconsistency的原因是， $H_k/Phi_k$ 計算時採用了不同的線性化狀態點。那麼，用相同的狀態點進行線性化計算 $H_k/Phi_k$ 就不會有這個問題了，具體來說，位姿採用propagated值而非updated值，landmark使用第一次估計值。First Estimate就是這個直觀含義，即採用estimation from the first time。

這樣，雖然線性化點離真值的誤差可能相對更大，但能保證能觀性矩陣的性質和「理想」系統一樣，即

$ext{dim}( ext{Null}(M_{ ext{FEJ}}))=3$

3.三維Visual-Inertial系統中FEJ的引入

Li Mingyang在MSCKF2.0[5]中採用FEJ來對MSCKF進行改進。

基本思路和上面一樣，作者對標準MSCKF的能觀性矩陣進行分析，發現矩陣的零空間維數是3。而前面[1]等已經指出，Visual-Inertial系統在一般情況下的Unobservable維數是4。

這裡和二維EKF-SLAM中的yaw一樣，對應於yaw的原本Unobservable的自由度，在標準MSCKF中錯誤的變成Observable了。

同樣，這種能觀性的改變也會帶來inconsistency，而採用FEJ的方法可以修正這個影響。

4.Sliding Window方法中的FEJ

上面都是濾波類型方法的應用，對於優化的方法，OKVIS[6]使用了這一方式，DSO[7]參考OKVIS也採用了FEJ。

we employ first- estimate Jacobians, i.e. whenever linearization of a variable is employed, we fix the linearization point for any subsequent linearization involving that particular variable

對於Sliding window的方法，一般會採用Marginalization的方式把離開window的狀態所帶有的信息給保留下來。

（Marginalization可以看看賀一家博客和裡面的參考文獻 SLAM中的marginalization 和 Schur complement）

如上圖是OKVIS的window，在新增一個KeyFrame時，會把window中的較早的一個KeyFrame以及相關的一些landmark給marginalize掉。

（OKVIS為了保證稀疏性，不是所有的landmark都進行marg。OKVIS中old KeyFrame之間covisible的landmark——Landmarks visible in KF1 but not KF4——被marg，而能被最新的KF或者frame所看到的landmark直接drop，這樣只會導致KF與KF之間被fill-in。）

（上面博客中提到，「要marg那些不被其他幀觀測到的特徵點」，是保證KF與KF之間也不會有fill-in。）

進行Marg時會用到相關狀態的Jacobian，按照一般的思路，也是用最新的估計值作為線性化點來計算。以上圖對KF1進行Marg為例，KF2/KF3和其他沒有被Marg的狀態，在這次進行Marg時計算Jacobian的線性化點，和Marg之後進行優化計算、或者對KF2進行Marg時的線性化點，因為狀態更新之後就不同了。

****************以下為個人理解+跑火車部分****************

個人理解，能觀性是系統的固有特性，和採用EKF還是sliding window optimization的方法無關。

直觀上，Sliding window方式中Jacobian計算時線性化點的不同，和上面EKF-SLAM或者MSCKF中情況是類似的。在EKF/MSCKF中線性化點不同導致的能觀性的改變，同樣也會造成Sliding window方式中能觀性的改變。

（不負責任的猜測：如果嚴格按照[1]中的方式，通過Lie Derivative來計算Observability矩陣，也會用到Jacobian，相比用真值計算，不同的線性化點可能也會引起矩陣零空間的改變。暫時沒去找哪篇論文有證明，所以只是猜測。）

另一種理解，Marginalization的操作，把舊信息以prior的形式保留了下來，等價於一個EKF-SLAM系統，但是被marg的狀態固定並且不再更新。它們的線性化點也就固定了，並且計算prior的時候關於未被marg的狀態的Jacobian會有不同，也就造成了等價EKF-SLAM系統中Jacobian的不同。EKF-SLAM中採用FEJ來保證consistency，那麼Sliding window這種也可以。

****************以上為個人理解+跑火車部分****************

疑問：

1.對於類似ORBSLAM這種保存有全局地圖信息的系統是否需要？？？

2.線性化點的誤差較大，對精度的影響多大？（論文里有說inconsistency的修正帶來的好處比線性化點的誤差更大點，整體性能提升。）

參考文獻

[1] Kelly, J., Sukhatme, G. S. (2011). Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. The International Journal of Robotics Research, 30(1), 56-79.

[2] Martinelli, A. (2013). Resolvability of Visual-Inertial Structure from Motion in Closed-form.

[3] Strasdat, H., Montiel, J. M. M., Davison, A. J. (2010). Scale drift-aware large scale monocular SLAM. Robotics: Science and Systems VI.

[4] Huang, G. P., Mourikis, A. I., Roumeliotis, S. I. (2009). A first-estimates Jacobian EKF for improving SLAM consistency. In Experimental Robotics(pp. 373-382). Springer Berlin Heidelberg.

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

[6] Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., Furgale, P. (2015). Keyframe-based visual–inertial odometry using nonlinear optimization. The International Journal of Robotics Research, 34(3), 314-334.

[7] Engel, J., Koltun, V., Cremers, D. (2016). Direct sparse odometry. arXiv preprint arXiv:1607.02565.

[8] Hermann, R., Krener, A. J. (1977). Nonlinear controllability and observability. IEEE Transactions on automatic control, 22(5), 728-740.

First Jacobian 方法最早的時候時候出自 EKF-SLAM 2D from "A First-Estimates Jacobian EKF for Improving SLAM Consistency".

這裡我依然用可觀性（observability）來回答。但是角度會有不同。

對於 SLAM 來說，狀態 $mathbf{X}$ , 也就是Robot Pose和landmark，組成的系統是

$(1) mathbf{X}_{n+1}=f( mathbf{X}_{n}, mathbf{u}_{n}, mathbf{epsilon}_{n} ) , mathbf{z}_{n+1}=h_{n+1}(mathbf{X}_{n+1}, xi_{n+1} )$

其中 $mathbf{z}_{n+1}$ 是第n+1步時的觀測， $mathbf{u}_n$ 是第n+1步時的odometry， $(mathbf{epsilon}_{n}, xi_{n+1})$ 是已知協方差矩陣的高斯白雜訊。

對於SLAM系統來說 $f(cdot)$ 和 $h_{n+1}(cdot)$ 是非常特殊的。我們可以發現SLAM 系統(1) 是一個十分特殊的系統。

因為這個系統(1) 對一個帶隨機噪音的全局剛體變換 $T_{Theta}$ (將整個世界做旋轉平移)是不可觀的 (unobservable)，這裡的 ${Theta}$ 是隨機噪音。

也就是說

如果有兩個不同的狀態初始點 $mathbf{X}_0, mathbf{Y}_0$ 只要滿足 $mathbf{Y}_0=T_{Theta} (mathbf{X}_0)$

給定同樣的 odometry和噪音，那麼按照迭代方程

（1） $mathbf{X}_{k} = f(mathbf{X}_{k-1}, mathbf{u}_{k-1}, epsilon_{k-1})$ ， $mathbf{Y}_{k} = f(mathbf{Y}_{k-1}, mathbf{u}_{k-1}, epsilon_{k-1})$ for k=1,...n+1

生成的兩條軌跡, 居然會有同樣的輸出 $h_{n+1}(mathbf{X}_{n+1})=h_{n+1}(mathbf{Y}_{n+1})$ .

按照上面所描述，在任何隨機剛體變換 $T$ 下，系統(1)的輸出都是不變的，也就說（1）是具有隨機剛體不變性。這就實際上對應著兩個鐵一般的事實。 SLAM系統的輸出與坐標系的選取無關, 同時與對於選取的坐標系的uncertainty也是沒有關係的。換句話來說，系統（1）對於 隨機剛體變換 $T$ 來說，是不可觀的，這種不可觀這實際上是相對於線性系統的不可觀性質一個推廣。

而一個filter 狀態估計器，實際上是一個以 $hat{X}$ 為狀態的系統。這個系統的輸入是 $mathbf{u}_k,mathbf{z}_k$ ,輸出是

$hat{ mathbf{z} }_{n+1}= h_{n+1}(hat{mathbf{X}}_{n+1}, 0 )$ 。當我們設計一個filter 的時候，毫無疑問是想要這個新的虛擬的系統儘可能的模仿原來的系統 (1) ，包括種種性質。自然來說，我們也希望這個filter 滿足隨機剛體不變性。

那麼對於EKF來說，怎麼就說這個filter是或者不是滿足 隨機剛體變換不變性的呢？自然是，從兩個不同的「相對應」的初始點，給予同樣的輸入，生成的軌跡卻有相同的輸出。

那麼下面更嚴格一點說這個事。

EKF的狀態當然包括均值估計 $hat{mathbf{X}}$ 和協方差矩陣 $hat{mathbf{P}}$

對初始狀態 $(hat{mathbf{X}}_0, mathbf{P}_0)$ 施加一個隨機剛體變換我們得到另一個狀態點 $( hat{mathbf{Y}}_0, mathbf{P}y_0 )$ 。

這個狀態點是 $( hat{mathbf{Y}}_0, mathbf{P}y_0 ) =({T}_{Theta} ( hat{mathbf{X}}_0 ), ar{mathbf{Q}}_1mathbf{P}_0ar{mathbf{Q}}^intercal_1+ ar{mathbf{Q}}_2 ar{Sigma}ar{mathbf{Q}}^intercal_2)$ 這裡 $ar{mathbf{Q}}_1=left.frac{partial ( {T}_{0} (hat{mathbf{X}}_0 oplus mathbf{e} ) ominus {T}_0 (hat{mathbf{X}}_0 ) ) }{partial mathbf{e}} ight vert_{mathbf{e}=mathbf{0} }, \ ar{mathbf{Q}}_2=left.frac{partial ( {T}_{Theta} (hat{mathbf{X}}_0 ) ominus {T}_{0} (hat{mathbf{X}}_0 ) ) }{partial Theta } ight vert_{Theta=mathbf{0} },$

$ar{Sigma}=cov( Theta)$

EKF 作為一個系統，

以 $(hat{mathbf{X}}_0, mathbf{P}_0)$ 為初始點, $mathbf{u}_k,mathbf{z}_k$ (for k=1: n+1)為輸入，生成出來的終點是 $(hat{mathbf{X}}_{n}, mathbf{P}_{n})$

以 $( hat{mathbf{Y}}_0, mathbf{P}y_0 )$ 為初始點, $mathbf{u}_k,mathbf{z}_k$ (for k=1: n+1)為輸入，生成出來的終點是 $(hat{mathbf{Y}}_{n}, mathbf{Py}_{n})$

（EKF內部的機制，我們暫時先不關心）

如果滿足：

$h_{n+1}(hat{mathbf{X}}_{n+1}, 0 ) =h_{n+1}(hat{mathbf{Y}}_{n+1}, 0 )$ 那麼這個基於EKF的filter 我們就tm說是滿足隨機剛體變換不變性。

很遺憾， naive的EKF SLAM並不滿足 隨機剛體變換不變性，但是當 $ar{Sigma}=cov( Theta)=0$ ，居然這個不變性居然tm是有的。換句話來說， naive 的EKF 作為 $hat{mathbf{X}}$ 的系統，並沒有很好的模仿原本的系統 (1) (這裡注意 (1) 是針對 ${mathbf{X}}$ 的系統），但是卻有 deterministic剛體變換不變性，也就是navie-EKF確實是不依賴於坐標系的選取的，但是卻依賴於對這個坐標系的uncertainty。

First Estimate Jacobian -EKF，（FEJ-EKF），是基於 naive-EKF，但是在計算雅可比矩陣的時候， $mathbf{F}_n$ 在 $hat{mathbf{X}}_{n|n-1}$ 處而不是常見的 $hat{mathbf{X}}_{n|n}$ 處取值, $mathbf{H}_{n+1}$ 在 $(hat{mathbf{R}}_{n+1|n},hat{mathbf{p}}_{n+1|n},hat{mathbf{f}}_{0})$ 處而不是常見的 $(hat{mathbf{R}}_{n+1|n},hat{mathbf{p}}_{n+1|n},hat{mathbf{f}}_{n+1|n})$ 處取值。

這麼做的後果就是使得新的 filter 系統滿足 隨機identity變換不變性, 也就是使得估計的均值狀態與坐標系的不確定性不相關。

這樣一來 隨機identity變換不變性加上原有 deterministic剛體變換不變性， FEJ-EKF

最終巧妙的獲得了 隨機剛體變換不變性，成功的模仿了原來的系統（1）的隨機剛體變換不變性性質，當然性能就比naive-EKF的更好一些了，一般來說。

站在反面來說，如果一個filter 不滿足 隨機剛體變換不變性， 這實際上意味著這個filter 不太好，

參雜了太多私貨，使得估計的狀態依賴於坐標系的選取或者對於全局的uncertainty。這當然不是我們想要的。

以上就是我的一點淺見。

參考文獻 RomaTeng/EKF-SLAM-on-Manifold

http://ieeexplore.ieee.org/document/7812660/

First estimate jacobian用來解決 estimator 中不正確的可觀性的問題. 上面的幾位已經說了一些,我來說說我自己的理解以及First Estimate Jacobian 在Filter-based VIO 以及 Sliding Window Optimization VIO中的應用方法. 個人理解,歡迎交流拍磚.

使用First estimate jacobian 是為了解決在不正確的線性化點上做線性化(包括在系統模型,以及觀測模型上)給系統的客觀性帶來的不一致(inconsistency)的問題.這種不一致性表現在,它會使得estimator在狀態空間中沒有信息的方向上獲得信息,這就是一種"虛幻"的信息("artificial" information).最終導致系統不爭取的觀測性,導致系統的發散. 對於這一點, Guoquan Huang 老師在 A First-Estimates Jacobian EKF for Improving SLAM Consistency [1]中以一個2D的給予EKF的slam問題給予了證明. 其觀點是: 在[2]中已經證明在2d SLAM中,估計器的不可觀的維度是3(可觀矩陣的零空間nullspace的rank為3),也就是說,在2d Slam中機器人的空間2d坐標(x,y)以及它的角度 $heta$ 都是不可觀測的. 但是"標準"的EKF Slam 中估計器的可觀測矩陣的零空間的rank卻是2. 這就導致的EKF不爭正確的觀測性.造成估計器的發散. 文章提出一中簡單有效的方法來改進"標準"EKF來使得它具有正確的可觀測性,即 First estimate jacobian.
Huang 老師的First estimate jacobian的具體做法是改變系統方程(System Equ. )和觀測方程(Measurement Equ.) 的線性化點: a.系統迭代上, 在根據系統方程迭代的時候使用上一時刻的先驗(Prior)估計 $x_{R_{k|k-1}}$ (對k-1時刻狀態的第一次估計)來迭代得到這一時刻的系統先驗估計 $x_{R_{k+1|k}}$ ,而不是使用上一時刻的後驗估計(postprior) $x_{R_{k|k}}$ 來迭代 ; b. 在觀測上, 使用第一次對landmark的估計 $x_{L}$ 來計算觀測方程的線性化點,而不是使用這一時刻的對landmark的估計來求; 以上兩個改進,就是First estimate jacobian. 在文章後續的實驗中可以看出, 採用First estimate jacobian之後,系統的誤差要比"Standard"的方法好很多.

3. EKF 在應用到VIO中也存在相同的問題. Visual Inertial Odometry 已經被證明他有4個不可以觀測的唯度,即 3D空間中的位置 x, y,z 以及其繞重力方向旋轉的yaw角度不客觀. 這也與我們的直覺一致. 但是在使用標準的EKF方法時候,這個不可觀測的唯度變成了3, 更進一步地講,繞重力旋轉的角度yaw變得可觀測了! 這就導致的相似的inconsistency. Mingyang Li 在他的[3]中分析了這一問題.並同樣使用改變系統方程和觀測方程的線性化點的方法來克服這個問題. 修改的方式也是使用First estimate jacobian來改變, 形成的MSCKF2.0.

4. 以上都是在filter 方法上的應用和研究. 在給予優化方法上,同樣可以使用First estimate jacobian.

基於優化的問題的核心是求解 $HDelta x = -b$ 這件事. 為了使得這個問題的規模是控制在一定規模下的,那麼就要講old state Marginalize 出去. 在marginalize old state的時候,關於 old state的 jacobian 矩陣也好, hession 矩陣也好就永久的固定在了一個線性化點上, 這些Jacobian和Hession 不僅僅與Old state相關,也和與他直接相關連的狀態相關(我們就叫它們 $x_r$ 吧,r 表示剩下的). 如果在後續的優化中,我們使用每一步迭代更新得到的 $x_r$ 來線性化得到H的話,那麼我們相當於在同一個狀態的兩個點上線性化cost function(計算marginalize 相關的cost function的時候我們已經將 $x_r$ 固定),這就導致了錯誤的可觀性. 用SLAM中的marginalization 和 Schur complement 中的一張圖來說明在同一個cost function中使用同一個狀態的兩個線性化點給系統的nullspace帶來的變化

於是,Cuong Dong-Si在[4]中, 通過使用在marginalize時候的 $x_r$ 來固定之後的線性化點的辦法來客服不正確的可觀行. 即opt方法中的First estimate jacobian.

5. First estimate jacobian方法已經被應用在了很多需要從Sliding window中 marginalize out 裝填的VIO系統中, 包括OKVIS, DSO,以及Stefan Leutenegger在IROS2107 添加IMU的ElasticFusion的改進中.

[1] Huang, Guoquan P., Anastasios I. Mourikis, and Stergios I. Roumeliotis. "A first-estimates Jacobian EKF for improving SLAM consistency." Experimental Robotics. Springer, Berlin, Heidelberg, 2009.

[2] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot. Consistency of the

EKF-SLAM algorithm. In IEEE/RSJ International Conference on Intelligent

Robots and Systems, pages 3562{3568, Beijing, China, Oct. 2006.

[3] Li, Mingyang, and Anastasios I. Mourikis. "High-precision, consistent EKF-based visual-inertial odometry." The International Journal of Robotics Research 32.6 (2013): 690-711.

[4] Dong-Si, Tue-Cuong, and Anastasios I. Mourikis. "Motion tracking with fixed-lag smoothing: Algorithm and consistency analysis." Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011.

整理了自己學習SLAM時用到的開發資源與方法發布在Github上：GeekLiB/Lee-SLAM-source