slam kf

1、KF

1.從機率來理解機率估計
由於但願整個運動估計較長時間內最優，因此反而會用最新的知識去更新以前的狀態，就好比在作完當前幀的位姿估計的時候，修改局部地圖的路標點。
若是站在以前的狀態上來考慮，用的是將來的信息。
因此不只用過去的信息估計如今的狀態，也用將來的信息更新本身。由於本身站在一箇中間的時間點上。這是批量式 batch
漸進式：只考慮過去和前一個時刻的。incremental
1.1前言
運動方程：
xk=f(xk-1,uk)+wk;
因爲xk表示的位姿，它和上一時刻之間的位姿是運動關係。而帶觀測值就是觀測方程了。
觀測方程：
zk,j=h(yj,xk)+vk,j
yj是路標點，具體的zk,j能夠是觀測方向啊，或者位姿之類的。因爲路標點不少，因此觀測方程會比較多，遠遠多於運動方程。
若是沒有測量運動的裝置的時候，好比沒有里程計，這時候只能單純的經過圖像來求解位姿了。這時候能夠有幾種處理方式好比假設沒有運動方程，假設相機不動，假設相機勻速運動。不知道具體這種是怎麼作的。相似於sfm問題,就是structure from motion，經過一組圖像來恢復運動和結構，但slam圖像有時間順序。

若是假設狀態量和噪聲都服從高斯分佈，那麼只要在程序中存儲均值和協方差陣就能夠了。
均值：變量最優值的估計，協方差陣：度量偏差的，也就是不肯定性的。
只有運動方程不行，由於偏差會累積。沒有講只有觀測值會怎樣。
1.2改記號
xk={xk,y1,y2,...,ym}用它來表示k時刻的因此未知量，就是位姿和m個路標點，路標點是未知的嗎？
zk,j=zk
因此原來的方程變成了
xk=f(xk-1,uk)+wk
zk=h(xk)+vk
比以前難理解，就是把路標點給隱匿了，必定程度上會讓人難以理解。
雖然是zk=什麼什麼，但其實zk是已知的，可是yj是未知的。
由於xk包含了全部的未知量，因此要估計xk的狀態分佈。用的是0到k的數據，用機率表示就是
P(xk|x0,u1:k,z1:k)已知量是uk,zk,因此是u1:k,z1:k,當初始化的時候是x0.而後加u1,z1纔有x1,u是從時刻1開始輸入的。
因此是P(xk|x0,u1:k,z1:k)
按照貝葉斯估計，把這個機率變成
先展開
P(xk|x0,u1:k,z1:k)=P(xkx0u1:kz1:k)/P(x0,u1:k,z1:k)
分子分母提出zk來
=P(zk|xk,x0,u1:k,z1:k-1)*P(xk,x0,u1:k,z1:k-1)/（P(zk|x0,u1:k,z1:k-1)*P(x0,u1:k,z1:k-1))
分子的後一項和分母的後一項相除
=P(zk|xk,x0,u1:k,z1:k-1)*P(xk|x0,u1:k,z1:k-1)/P(zk|x0,u1:k,z1:K-1)
不知道爲何把P(zk|xk,x0,u1:k,z1:k-1)/P(zk|x0,u1:k,z1:k-1)類比於P(zk|xk)
因此整個式子類比於
P(zk|xk)*P(xk|x0,u1:k,z1:k-1)
P(xk|x0,u1:k,z1:k)是後驗來着，由於後驗執果索因，根據觀測方程，zk=h(xk)+vk.zk是由xk獲得的，而如今求zk已經發生了以後xk的機率，這是後驗。
而P(xk|x0,u1:k,z1:k-1),z1:k-1是根據以前的x1:k-1獲得的，u1:k是以前的輸入，因此這些都是xk以前的數據，根據以前的數據估計k時刻xk發生的機率，這是先驗。
後驗等於先驗*似然。
P(zk|xk)是似然來着。
先驗按xk-1展開，就是把機率變成積分形式
P(xk|x0,u1:k,z1:k-1)=(P(xk|xk-1,x0,u1:k,z1:k-1)P(xk-1|x0,u1:k,z1:k-1)dxk-1)的積分形式
這裏是假設k時刻只和k-1時刻狀態有關，但若是考慮更早時刻的，還能夠展開。
一階馬爾科夫性：k只和k-1有關，濾波方法採用了這個假設。以EKF爲主。
非線性優化是認爲跟全部時刻有關。slam主流使用非線性優化。
2.線性系統,kf
kf來時候，k時刻狀態只和k-1時刻有關。積分公式裏面的兩個機率
P(xk|xk-1,x0,u1:k,z1:k-1)=P(xk|xk-1,uk),這裏的uk能夠拿掉，它其實就是k-1時刻的狀態分佈做爲輸入來計算xk.
P(xk-1|x0,u1:k,z1:k-1)=P(xk-1|x0,u1:k-1,z1:k-1)其實也沒有簡化，不明白爲何不把x0省掉，只去掉uk.
只須要把k-1時刻推導到k時刻就能夠了。
線性高斯
xk=Akxk-1+uk+wk;
zk=Ckxk+vk
假設全部的狀態和噪聲都服從於高斯分佈，wk~N(0,R),vk~N(0,Q)，R,Q省略了下標。
假設知道xk-1的預測值xk-1.hat(),Pk-1.hat
那麼根據運動方程先驗P(xk|x0,u1:k,z1:k-1)=N(Akxk-1.hat+uk,Ak.*Pk-1.hat()*Ak.t+R)
計算的時候要加上常數uk.
均值是常數都要加，協方差才忽略常數。
這樣先驗xk.g,pk.g就都知道了。
P(zk|xk)比較困惑的是，這裏xk的狀態分佈不知道，這裏寫的是
P(zk|xk)=N(Ckxk,R)
即便把ckxk當作常數，後面的協方差陣也應該是Q，而不是R，不明白爲何這麼設。
後驗服從於N(xk.hat,Pk.hat)
因此N(xk.hat,Pk.hat)=N(Ckxk,R)*N(xk.g,Pk.g)
N(xk.hat,Pk.hat)都是xk服從的，N(Ckxk,R)是zk服從的。因此
(xk-xk.hat).t*Pk.hat.n(xk-xk.hat)=(zk-Ckxk).t*R.n*(zk-Ckxk)+(xk-xk.g).t*Pk.g.n*(xk-xk.g)
中間乘的是協方差的逆。
比較xk的二次項係數
Pk.hat.n=Ck.t*R.n*Ck+Pk.g.n
等式同乘Pk.hat,得
I=Pk.hat*Ck.t*R.n*Ck+Pk.hat*Pk.g.n
爲了寫着方便，把等式右邊第一項的前三個定義爲K.即K=Pk.hat*Ck.t*R.n
I=K*Ck+Pk.hat*Pk.g.n
因此Pk.hat=(I-KCk).Pk.g,後驗協方差就求出來了。
也就是說K跟後驗，觀察方程係數Ck,觀察方程噪聲偏差R有關。後驗就是跟z有關嘛。
比較xk的一次項係數
Pk.hat.n*(-xk.hat)-xk.hat.t*Pk.hat.n=zk.t*R.n*(-Ck)-Ck.t*R.n*zk-xk.g.t*Pk.g.n-Pk.g.n*xk.g
Pk.hat.n*xk.hat=Ck.t*R.n*zk+Pk.g.n*xk.g
等式兩邊同乘Pk.hat
xk.hat=Pk.hat*Ck.t*R.n*zk+Pk.hat*Pk.g.n*xk.g
=K*zk+PK.hat*Pk.g.n*xk.g
而I=K*CK+Pk.hat*Pk.g.n
因此Pk.hat*Pk.g.n=I-K*Ck
因此
xk.hat=K*zk+(I-K*Ck)*xk.g
提取K得=xk.g+K(zk-Ck*xk.g)
因此xk.hat=xk.g+K(zk-Ckxk.g)
Pk.hat=(I-KCk)*Pk.g
因此迭代能夠這麼來寫
1.預測
求出xk.g,pk.g
xk.g=Akxk-1.hat+uk,Pk.g=AkPk-1.hat*Ak.t+R
爲了形成循環，這裏用k-1的後驗來求k的先驗。Ak.t要放在後面
2.更新，先計算K
K=Pk.g*Ck.t(CKPk.gCk.t+RK).n
記住公式就好了，能夠用其餘方式推導出來的。K 能夠不用Pk.hat獲得。
而後計算xk.hat,Pk.hat,用於下一次的迭代。
xk.hat=xk.g+K(zk-Ckxk.g)
Pk.hat=(I-KCk)*Pk.g
對，更新以後去求下一次的值。
這就是kf了，kf是線性系統的最優無偏估計。
2、EKF
slam非線性系統，由於李代數。高斯分佈通過非線性變換，再也不是高斯分佈。取近似，把非高斯分佈近似於一個高斯分佈。
EKF，這個擴展主要指的是到非線性系統的擴展。在某個點附近，作一階線性近似
原方程
xk=f(xk-1,uk)+wk
zk=h(xk)+vk
近似後
f(xk-1,uk)=f(xk-1.hat,uk)+f'(xk-1.hat,uk)*(xk-1-xk-1.hat)+wk
h(xk)=h(xk.g)+h'(xk.g)(xk-xk.g)+sk
記f'(xk-1.hat,uk)=F,h'(xk.g)=H
也就是xk=Fxk-1-F*xk-1.hat+f(xk-1.hat,uk)+wk
zk=Hxk-Hxk.g+h(xk.g)+vk
這樣就相似於以前的線性高斯系統
xk=Axk-1+uk+wk
zk=Ckxk+vk
那麼這樣計算下來
xk.g=f(xk-1.hat,uk),Pk.g=FPk-1.hat*F.t+Rk
更新的時候
Kk=Pk.g*H.t*(HPk.g*H.t+Qk).n

xk.hat=xk.g+Kk(zk-h(xk.g))
Pk.hat=(I-Kk*H)*Pk.g
以前線性高斯系統xk.hat,Pk.hat能夠說是無偏最優估計，這裏是單次線性近似的最大後驗估計(MAP)
1.討論
想要在某段時間內估計某個不肯定量，ekf.早期slam用ekf.
IF（信息濾波器），IKF（iterated KF),UKF(unscented KF)，粒子濾波器，SWF（sliding window Filter),或者使用分治法來提升EKF的效率。
優點：計算量受限，待估計量比較簡單的場合。
缺點：（1）馬爾科夫性，不能使用全部歷史數據，出現迴環不能處理。
（2）若是運動模型和觀測模型有強烈的非線性，那麼線性近似只能在很小的範圍內成立，在很遠的地方則不成。
非線性優化：每迭代一次，狀態改變以後，從新泰勒展開，濾波只在固定點上一次。因此狀態變化很是大依然適用。至於ekf爲何在固定點上只一次，不懂。

 

 

 

 

 

 

10.1.3非線性系統 ,Ekf
slam的運動方程和觀測方程一般是非線性函數，尤爲是slam中的相機模型，須要使用內參和李代數，exp（李代數）更不多是一個線性系統。一個高斯分佈，通過非線性變換，每每再也不是一個高斯分佈，取必定的近似，將以非高斯分佈近似成一個高斯分佈。
卡爾曼濾波擴展到非線性系統，ekf.一般的作法是，在某個點附近考慮運動方程和觀測方程的一階泰勒展開，只保留一階項，即線性的部分，而後按照線性系統進行推導。k-1時刻,xk-1.hat,Pk-1.hat
在k時刻，運動方程，觀測方程在xk-1.hat,pk-1.hat爲何不是xk-1,pk-1是由於其實沒有真值，只有估計值。由於都不能給出實際位姿吧。
一階泰勒展開。
xk約等於f(xk-1.hat,uk)+f對於xk-1的求導代入xk-1.hat後求得的值*(xk-1-xk-1.hat)+wk
其實就是xk=f(xk-1.hat,uk)+f'(xk-1.hat,uk)*(xk-1-xk-1.hat)+wk
奧，懂了，xk=f(xk-1,uk)它展開後就是f(xk-1.hat,uk)+f'(xk-1.hat,uk)*(Xk-1-Xk-1.hat)+wk
記這裏的偏導數爲F.
則xk約等於f(xk-1.hat,uk)+F(Xk-1-Xk-1.hat)
一樣對於觀測方程，也有
zk約等於h(xk.g)+h'(xk.g)(xk-xk.g)+nk
這裏的偏導數設成H.
爲何是偏導，zk除了xk未知量，沒有其餘的了。
那麼，在預測步驟中，根據運動方程有
P(xk|x0,u1:K,z0:k-1)=N(f(xk-1.hat,uk),FPk-1.hatF.t+Rk)
奧，這裏F就是Ak,H就是Ck.
這些推導和kf是十分類似的，爲方便記述，記這裏的先驗和協方差的均值爲
xk.g=f(xk-1.hat,uk),Pk.g=FPk-1.hat*F.t+Rk
不能徹底按公式推斷。xk=f(xk-1,uk)+wk
因此xk.g=f(xk-1.hat,uk)就能夠了。
而後，考慮在觀測中，咱們有
P(zk|xk)=N(h(xk.g)+H(xk-xk.g),Qk)
也就是xk已經存在要保留xk.可是方差爲何是QK.
EKF的預測與更新
Kk=Pk.g*H.t(HPk.g*H.t+Qk).n
xk.hat=xk.g+Kk(zk-h(xk.g)),pk.hat=(I-KkH).Pk.g
其實跟以前的還差很少，只不過H代替Ck,F梯度Ak.
slam非線性的狀況下，它給出了單次線性近似下的最大後驗估計(MAP)
10.1.1ekf的討論
ekf以形式簡潔，應用普遍著稱。當想要在某段時間內估計某不肯定量的時候，首先想到ekf.在早期的slam中，ekf佔據了很長一段時間的主導地位。研究者們討論了各類各樣的濾波器在slam中的應用，好比if(信息濾波器。IKF iterated kf,UKF(unscented KF)無氣味，例子濾波器，SWF（sliding window filter滑動窗口濾波器，或者採用分治法等思路改進ekf的效率。直至今日，儘管咱們認識到非線性優化比濾波器佔有明顯的優點，可是在計算資源受限，或者待估計量比較簡單的場合，仍是會用ekf.
侷限：
1.馬爾科夫性的侷限，若是當前幀確實跟好久以前的數據有關，好比迴環的狀況，那麼濾波器就很難處理了。非線性優化，full slam.
2.ekf濾波器僅在xk-1.hat處作了一次線性化，而後就根據此次線性結果，算後驗機率。這至關於在說，咱們認爲該點處的線性化近似在後驗機率處還是有效的。而實際上，當咱們離開工做點較遠時，一階泰勒展開並不能近似整個函數。這取決於運動模型和觀測模型的非線性狀況。若是它們有強烈的非線性，那麼線性近似就只在很小範圍內成立。不能認爲在很遠的地方仍能用線性來近似。這就是ekf的非線性偏差，也是它的主要問題所在。
在優化問題中，儘管咱們也作一階（最速降低）或二階（高斯牛頓法或列溫伯格-馬夸爾特方法）的近似，但每迭代一次，狀態估計發生變化以後，咱們會從新對新的估計點作泰勒展開，而不像ekf那樣只在固定點上作一次泰勒展開，這就使得優化方法適用範圍更廣，在狀態變化較大時亦能適用。
3.從程序實現上來講，efk須要存儲均值和方差，並對它們進行維護和更新。若是把路標量也放進狀態，因爲視覺slam中路標數量很大，這個存儲量是至關可觀的，且與狀態量呈平方增加，由於要存儲協方差矩陣。所以ekf slam被廣泛認爲不適用於大型場景。
因爲ekf存在這些明顯的缺點，同等計算量的狀況下，非線性優化能取得更好的效果。以非線性優化爲主的後端。主要介紹圖優化，並用g2o和ceres體驗一下後端的例子。
粒子濾波器的原理與卡爾曼濾波有很大不一樣。