舒爾補在Slam中的應用

還是多元變量x服從高斯分佈,且由兩部分組成 x=[a b], 那麼變量之間構成的協方差矩陣爲

 

其中A = COV(a,a), D = cov(b,b), C= cov(a,b).  由此變量x的概率分佈爲

 

 

中間的協方差矩陣就可以用舒爾補進行分解,具體的分解過程是:

 

這樣就可以從多元高斯分佈中直接分解出邊際概率和條件概率;

以example1爲例:

設x2爲室外的溫度, x1, x3 分別爲房間1和房間3的室內溫度,三者之間的關係如下:

 

 

其中v1v2,v3 爲服從高斯分佈的均值爲0, 方差爲σ12, σ22, σ32 , 根據多元高斯分佈規律,三者的聯合概率分佈公式爲

   其中協方差中每一項爲對應兩項變量的期望;

求得

爲協方差矩陣;

 那麼信息矩陣的求解方式可以通過求聯合概率分佈的方式獲得:
 

 



                                  

從上述推導公式中可以獲得信息矩陣爲:

 

其中元素爲0 的位置表示這兩個變量關於其他變量條件獨立;也就是x1,x3之間的關係是關於x2 獨立,也就是在x2確定時,x1,x3之間獨立;

確定了協方差矩陣和信息矩陣之後,接下來在例子中,如果要將變量x3去除,那麼信息矩陣與協方差矩陣要怎麼變化呢?針對這個例子我們有兩種方法求解,

第一種是按照上面求解步驟再求解一次,就可以得到新的協方差和信息矩陣如下:

    

但是這種方法在變量比較少時可以直接求解,其實就是從上面的公式中去掉對應顏色的部分(x3對應爲藍色的部分),

但是在slam中觀測量很多的情況下,我們需要的是在已知矩陣(16)和(13) 的情況下直接推到出21, 23,也就是slam中邊緣化時,如何從N個觀測決定的狀態中推到出去除某一個關鍵幀對應的狀態,也就是N-1個觀測所對應的狀態;

這裏就用到前面舒爾補的公式,列出如下:

 

 

對應到該例子中,

原協方差矩陣爲:     信息矩陣爲: 

 

則根據中式32的計算,從聯合概率的協方差矩陣推導邊際概率的協防差矩陣即爲原協方差矩陣中的A部分;條件概率的協方差矩陣即爲A的舒爾補 , DeltaA;

那麼其對應的信息矩陣也就是A.inv() 和 DeltaA.inv() ,

但是我們需要的不是從協方差中計算信息矩陣,而是要從原來的信息矩陣中推算出新的信息矩陣,所以接下來就需要知道A.inv 和 deltaA.inv與原信息矩陣各個分塊

之間的關係,; 可以從公式36中進行推算求解,結果爲

 

 

這樣就分別求得了邊際概率的信息矩陣 和 條件概率的信息矩陣 與聯合概率的信息矩陣之間的關係;

所以在回到例子1, 求新的邊際概率的協方差矩陣和信息矩陣 就可以用聯合概率協方差矩陣和信息矩陣求出:
=   -----> A =

= ----> =

 

 

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章