kalman濾波（二）---擴展kalman濾波[EKF]的推導

 一.狀態估計的解釋

我們知道每個方程都受噪聲的影響，這里把位姿x和路標y看成服從某種概率分布的隨機變量。因此我們關心的問題就變成了：當我們已知某些運動數據u和觀測數據z時，如何確定狀態量x，y的分布？比較常見且合理的情況下，我們假設狀態量和噪聲項服從高斯分布---這意味着在程序中只需存儲它們的均值和協方差即可。均值可看作是對變量最優值的估計，而協方差矩陣度量了它的不確定性。如果認為k時刻狀態只與k-1時刻狀態有關，而與再之前無關，我們就會得到以卡爾曼濾波（EKF）為代表的濾波器方法，在濾波方法綜合那個，我們會將某時刻的狀態估計，推導到下一時刻；另一種方法考慮k時刻狀態與之前所有狀態的關系，將得到以非線性優化為主體的優化框架。目前SLAM的主流是非線性優化方法。

二.EKF

[1] 卡爾曼濾波 -- 從推導到應用(一)

 卡爾曼濾波 -- 從應用(一) 到 (二)

高斯分布，又稱為正態分布：

 

正態曲線呈鍾型，兩頭低，中間高，左右對稱因其曲線呈鍾形，因此人們又經常稱之為鍾形曲線。若隨機變量X服從一個數學期望為μ、方差為σ^2的正態分布，記為N(μ，σ^2)。其概率密度函數為正態分布的期望值μ決定了其位置，其標准差σ決定了分布的幅度。當μ = 0,σ = 1時的正態分布是標准正態分布。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

1.協方差的幾何意義

樣本方差的無偏估計可以通過以下方式獲得（μ為期望，即均值）：

方差只能用來解釋數據在與特征空間軸線平行的方向上的傳播。圖2所示的2d特征空間：

協方差：

協方差矩陣：，其中

二維正態分布數據完全由均值和2×2協方差矩陣來解釋。類似地，一個3×3的協方差矩陣用於捕捉三維數據的傳播，而n×n次協方差矩陣捕捉n維數據的傳播。

 

 

前兩個：協方差對稱，最大方差方向與軸不重合，並且數據被旋轉。

后兩個：協方差為0，最大方差方向是軸對齊

 

2.特征值分解Eigendecomposition of a covariance matrix

3.Covariance matrix as a linear transformation

4.協方差矩陣的特征向量

     協方差矩陣的特征向量的最直觀的解釋之一是，它們是數據變化最多的方向。（更准確地說，第一特征向量是數據變化最多的方向，第二特征向量是那些正交（垂直）到第一特征向量的最大方差的方向，第三個特征向量是與前兩個正交的最大方差的方向，等等）。下面是2個維度的示例[ 1 ]：

每個數據樣本是一個2維點，坐標x，y。這些數據樣本的協方差矩陣的特征向量是向量u和v；u，長箭頭，是第一特征向量，v，短箭頭，是第二個。（特征值是箭頭的長度），正如你所看到的，第一個特征向量點（從數據的平均值）到數據在歐氏空間中變化最大的方向，第二特征向量是正交的（垂直的）到第一個。


在3個維度中可視化有點困難，但這里有一個嘗試[ 2 ]：

在這種情況下，假設所有的數據點都位於橢球內。V1，方向的數據變化最大，是第一特征向量（λ1是相應的特征值）。V2是在那些與V1正交的方向上數據變化最大的方向。V3是那些方向與V1和V2正交的最大方差方向（雖然只有一個這樣的正交方向）。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

[2]卡爾曼濾波原理及實現---介紹的詳細，適合初學者；卡爾曼增益K解釋的較好

http://www.tina-vision.net/docs/memos/2003-003.pdf

在求解卡爾曼增益的過程中：

對兩個雲團進行重疊，找到重疊最亮的點（實際上我們能夠把雲團看做一幀圖像，這幀圖像上面的每一個像素具有一個灰度值，灰度值大小代表的是該事件發生在這個點的概率密度），最亮的點，從直覺上面講，就是以上兩種預測准確的最大化可能性，也就是得到了最終的結果。非常神奇的事情是，對兩個高斯分布進行乘法運算，得到新的概率分布規律仍然符合高斯分布，然后就取下圖當中藍色曲線峰值對應的橫坐標不就是結果了嘛。證明如下：

σ2

N(x,μ,σ)=1σ2π−−√e−(x−μ)22σ2(4
如果存在兩個這樣的高斯分布，只不過期望和方差不同，當兩個分布相乘，得到什么結果？

是不是下式成立：

注意：我這里鉛筆書寫的標注和上面博客中的是反的，只要按步驟推導還是容易理解的。

N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ′,σ′)(5