極品巧克力
前言
通過前兩篇文章，《D-LG-EKF詳細解讀》和《誤差狀態四元數詳細解讀》，已經把相機和IMU融合的理論全部都推導一遍了。而且《誤差狀態四元數》還對實際操作中的可能遇到的一些情況，進行指導。
這些理論都已經比較完整了，那么，該如何在實際當中操作呢？該如何用到實際產品中呢？誤差狀態四元數，是有開源的程序的，但是它是集成在rtslam（ https://www.openrobots.org/wiki/rtslam/ ）里面的，不方便提取出來使用。
但還有另外一個開源的程序，ETH的MSF（https://github.com/ethz-asl/ethzasl_msf），它是獨立的一個開源程序，可以比較方便地用在自己的工程里面，並且它的理論與誤差狀態四元數很接近，稍微有點不同，所以MSF開源程序就成了一個不錯的選擇。
所以，我把MSF的程序全部都通讀一遍之后，結合程序來推導MSF的理論，總結成本文，包括MSF的實驗，與各位分享。
1.基本模型
基本模型如下圖所示。
MSF的可擴展性很好，程序里可以接入新的傳感器，比如GPS，激光雷達，碼盤等。
主要的理論還是誤差狀態四元數里面的理論。
相比於《卡爾曼四元數帶外參融合方法》，它的方法更好。
首先，它的核心狀態里面沒有重力在世界坐標系下的表示。因為它用的就是《誤差狀態四元數》里面的5.3.1的方法，直接就是以水平坐標系為世界坐標系。只是初始位置根據當前加速度計的測量值和理論重力計算出來，初始位置
帶着一個協方差而已。因為反正最后都是要轉換到水平坐標系下的，所以這種方法反而更加方便。
而且，IMU的世界坐標系和相機的世界坐標系，也不用綁在一起，同時建立。可以在IMU開啟一小段時間以后，相機再獲取它的世界坐標系。但相隔時間應該還是要盡量短點，因為位移單靠IMU的加速度計的積分，時間久了會很不准確，如果有輪子碼盤的話，則又可以好一些。調整
的目的，可能是因為世界坐標系和相機的世界坐標系，在時間戳上並不對應得很好，所以需要微調。
或者，為了避免這樣的麻煩，直接把IMU的世界坐標系和相機的世界坐標系，綁在一起，同時建立。這樣子，
。
也應該用ETH的手眼標定方法，借用棋盤格先標定好，再固定住。
就直接用尺子量好，固定住。如果用雙目相機的話，就不用考慮
了。
（如果加上輪速計的話，怎么更新呢？它的作用與IMU是相同的，都是相對值，而不是絕對值。所以，它應該與IMU實時融合，在主狀態與相機融合后，IMU預測出一個位姿態，輪速計預測出一個位姿，然后兩者融合出主狀態。只有主狀態才能與相機位姿融合。融合的話，可以用誤差狀態的思想來融合。）
所以，參考《誤差狀態四元數詳細解讀》，一個簡單魯棒的IMU與相機融合的模型應該是這樣的。
核心狀態為，
。
核心狀態上的擾動為，
。
運動模型跟《誤差狀態四元數》里面的一樣，
則相機位姿的觀測方程為，
使用《誤差狀態四元數》里面的公式，
其中，
首先，要計算第一項關於
的雅克比，
則用
表示上式，則雅克比計算如下，
然后，第二項，
要轉成三個元素的形式，即角軸李代數的形式。
用matlab程序，解出上面的式子，以及關於
的雅克比。
syms qicw qicx qicy qicz thetax thetay thetaz ...
qwiw qwix qwiy qwiz qzocw qzocx qzocy qzocz real
q1=[qicw,-qicx,-qicy,-qicz]';
q2=[1,-1/2*thetax,-1/2*thetay,-1/2*thetaz]';
q3=[qwiw,-qwix,-qwiy,-qwiz]';
q4=[qicw,qicx,qicy,qicz]';
q5=[qzocw,qzocx,qzocy,qzocz]';
temp = quaternion_mul(q1,q2);
temp = quaternion_mul(temp,q3);
temp = quaternion_mul(temp,q4);
temp = quaternion_mul(temp,q5);
temp
J=jacobian(2*temp(2:4,:),[thetax,thetay,thetaz])
simplify(J)
雅克比可以是，把temp轉換成角軸，再關於
求導。或者，角軸直接近似等於temp向量部分的2倍，再關於
求導，像上面的matlab程序這樣。
這樣子計算雖然准確，但是太麻煩了。論文里還用了近似的方法。為了方便地求雅克比，認為測量值近似為預測值直接轉換出來，即，
。
這時候，上式轉換成角軸，就是，向量部分的2倍，即，
。所以，雅克比為，
（這種用近似的方法，來算雅克比，雖然不如從原始公式上推導准確，但是可以極大地簡化計算，也許可以給D-LG-EKF里面計算H矩陣時借鑒。）
所以，就得到了雅克比矩陣
。
然后，
。就可以計算了。其余的流程，就跟《誤差狀態四元數》里面一樣了。
2.對延時的處理
前提條件是，各個傳感器的時間戳得是同一個時間源的，或者，時間戳很穩定，可以通過一些方法把它們時間戳之間的對應關系找到。不同傳感器的時間戳能准確對應上。
然后，因為測量值有時候會過一段時間才處理完，所以，把濾波中的狀態都記錄起來，然后，當有測量值過來的時候，更新對應時刻的狀態。然后繼續往后預測。如果有多個不同傳感器的測量值，也是如此操作。
3.計算相對測量
如果要融合單目相機。考慮到單目尺度的情況，怕有時候尺度會突然發生變化。為了應對這種情況，就都計算相對測量，就是兩個時刻之間的相對位姿態，這樣子，這一段的尺度
就可以濾得比較准確。然后，再把優化后的值，加入到原來的狀態中，方法跟《誤差狀態四元數》中的傳播差不多，就是把新濾出來的這段位姿的均值和擾動，加到原先狀態的均值和擾動中去，整合出新的均值和擾動。
而如果要融合的是輪子碼盤的話，則不必用這樣的方法。因為雖然輪子碼盤也有尺度問題，但是尺度是較穩定的。
如果是雙目相機的話，也不必考慮這種尺度突然變化的情況。
4.准確地計算每幀的協方差
為了更好地與GPS融合，就需要當前狀態需要有准確的協方差。
而以前計算出來的相機位姿的協方差是不准確的，沒有考慮到累積誤差造成的影響。造成了與IMU融合后的狀態協方差也是不准確的。得准確地計算出要融合的每幀圖像的協方差。這協方差，就是通過全局BA的方法，計算出來的。
但其實為了簡化。如果真的要與GPS融合的話。
在即時建圖的情況下，用上一段的方法。但是視覺的誤差累積還是很可觀的，所以如果是遠距離的話，應該以GPS為外界絕對測量，視覺只是用來計算相對測量的。只能用短距離的視覺相對測量。在主狀態之后，以此為起點，視覺的相對測量值與IMU的相對測量值融合，融合出相對測量狀態，再把相對狀態的均值和擾動合並進主狀態以及主狀態擾動，主狀態再與GPS融合，融合出新的主狀態。融合的話，可以用誤差狀態的思想來融合。
如果已經提前建圖，回環檢測都做了，地圖點固定且准確了，則視覺協方差就是當前幀的協方差，不必通過全局BA算出。
5. MSF方法總結
MSF的方法考慮得很全面，這個理論框架，可以用來融合多種傳感器。
我上面思考出來的方法，還考慮了與輪速計碼盤，GPS融合的具體操作情況，以后需要時再用。
6.實驗與改進
MSF的安裝與跑例程，可以參考這篇文章，
然后，我自己用ORBSLAM2跑Euroc的V201數據集，生成軌跡數據，和IMU數據一起送入到MSF中運行。為什么要跑標准數據集呢？因為標准數據集提供了IMU噪聲的真實參數，可以直接拿來使用，而且有真實的軌跡，groundtruth，可以用來評價融合結果的好壞。
可是運行結果卻總是發散，融合后的軌跡鋸齒非常嚴重。可是在理論上來講，它應該能取得比較好的效果的呀？所以猜測應該是程序與理論沒有對應上。
只好從程序里去查問題的原因。將程序里的所有的計算過程與算法公式一一對應起來之后，最終發現，是由程序里的2個地方導致的。
1.MSF程序有個隱含的假設，即圖像的世界坐標系是水平的。而我送的是以第一幀為世界坐標系的，而V201的第一幀並不是水平的。
2. 經過仔細推導程序的計算過程，發現MSF程序中的qwv，本質上是qvw，這個導致參數的初值給錯了。
將以上兩個問題改過來之后，MSF就可以正常運行我自己提供的數據了。
其中，在y軸上的結果如下。
以上，黑色的線是真實值，綠色的線是ORBSLAM通過圖像計算出來的位姿，紅色的線是圖像位姿和IMU融合后的結果，線上的每一個點都代表一個輸出數據。可以看出，msf融合后的結果，不僅可以把位姿的輸出頻率提高到和IMU一樣的頻率，還可以讓軌跡更加接近真實值。
但是，msf有一個缺點，那就是IMU的bias收斂得很慢，猜測是由於
近似造成的。如下圖所示。
上圖是加速度計的bias的收斂情況，和陀螺儀的bias的收斂情況。也許可以通過修改這部分的公式，讓bias收斂得更快。
7.參考文獻
-
Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to MAV navigation[C]// Ieee/rsj International Conference on Intelligent Robots and Systems. IEEE, 2013:3923-3929.