本文主要推導高斯分佈(正態分佈)的乘積,以便能更清楚的明白Kalman濾波的最後矯正公式。
Kalman濾波主要分爲兩大步驟:
1.系統狀態轉移估計,2.系統測量矯正;
在第2步中的主要理論依據就是兩個獨立高斯分佈的乘積如何計算的問題,即如何融合 估計值 和 觀測值 得到系統狀態的最優估計。
高斯分佈的概率密度函數:
f(x)=2πδ1e−2δ2(x−u)2(1)
其本質問題可抽象爲:已知兩個獨立高斯分佈 N1∼(u1,δ12), N2∼(u2,δ22),求新的概率分佈 N=N1×N2∼(?,?)
在進行理論推導之前,我們先通過Matlab數值計算看看兩獨立概率分佈的乘積情況:
如圖所示綠色和紅色分別代表兩個獨立的高斯分佈函數,藍色爲兩個分佈的乘積,從藍色形狀可以粗略的看出乘積結果可能爲一個幅值被壓縮的高斯分佈,其期望在[u1,u2]之間,但是真實是否如此還需理論推導。
N1 的概率分佈函數爲f1(x), N2 的概率分佈函數爲f2(x), 則:
f1(x)f2(x)=2πδ11e−2δ12(x−u1)2⋅2πδ21e−2δ22(x−u2)2=2πδ1δ21e−(2δ12(x−u1)2+2δ22(x−u2)2)(2)
可以直接先單獨分析指數部分,設:
β構造新的正態分佈=2δ12(x−u1)2+2δ22(x−u2)2=2δ12δ22(δ12+δ22)x2−2(u2δ12+u1δ22)x+(u12δ22+u22δ12)=δ12+δ222δ12δ22x2−2δ12+δ22u2δ12+u1δ22x+δ12+δ22u12δ22+u22δ12=δ12+δ222δ12δ22(x−δ12+δ22u2δ12+u1δ22)2+δ12+δ22u12δ22+u22δ12−(δ12+δ22u2δ12+u1δ22)2=γδ12+δ222δ12δ22(x−δ12+δ22u2δ12+u1δ22)2+λδ12+δ222δ12δ22δ12+δ22u12δ22+u22δ12−(δ12+δ22u2δ12+u1δ22)2(3)
設 λ 如上所示,則 β=γ+λ ,其中 γ 爲一個 N∼(u,δ2) 的正態分佈,λ 爲一個常數值。繼續簡化 λ ,如下:
λ=δ12+δ222δ12δ22δ12+δ22u12δ22+u22δ12−(δ12+δ22u2δ12+u1δ22)2=2δ12δ22(δ12+δ22)(u12δ22+u22δ12)(δ12+δ22)−(u2δ12+u1δ22)2=2δ12δ22(δ12+δ22)(u12δ22δ12+u22δ14+u22δ22δ12+u12δ24)−(u22δ14+2u1u2δ12δ22+u12δ24)=2δ12δ22(δ12+δ22)δ12δ22(u12+u22−2u1u2)=2(δ12+δ22)(u1−u2)2(4)
則可得兩個高斯分佈相乘爲:
f1(x)f2(x)=2πδ1δ21e−β=2πδ1δ21e−(γ+λ)=2πδ1δ21e−γ⋅e−λ=2πδ1δ21e−2δ2(x−u)2⋅e−2(δ12+δ22)(u1−u2)2(5)
其中:
u=δ12+δ22u2δ12+u1δ22, δ2=δ12+δ22δ12δ22(6)
把常數項綜合爲 Sg 可得其直觀表達方式:
f1(x)f2(x)=Sg⋅2πδ1e−2δ2(x−u)2(7)
Sg=2π(δ12+δ22)1e−2(δ12+δ22)(u1−u2)2(8)
到此,兩個高斯分佈相乘的分佈函數即推導出來,即相乘後的分佈函數爲一個被壓縮或者放大的高斯分佈,Sg 爲縮放因子,相乘後的概率密度的積分不等於1,但其方差和均值性質不變,所以 N=N1×N2∼(u,δ2),也就是我們常說兩個高斯分佈相乘同樣服從高斯分佈。
在Kalman濾波中的系統矯正環節中,實際是融合估計值和觀測值,使用Kalman濾波都是假設這兩者的分佈服從高斯分佈,各有各的期望和方差。計算Kalman的最優估計的本質就是在計算合成的 u。
這裏我們再研究一下縮放因子 Sg。可以看出,
- ① 當 Sg<1 時,概率分佈被壓縮;
- ② 當 Sg>1 時,概率分佈被放大;
平時我們大多數情況下會看到情況①,那什麼情況下才能出現情況②呢???
進行如下分部分析:
Sg=2π(δ12+δ22)1e−2(δ12+δ22)(u1−u2)2=q(x)p(x)(9)
其中:
p(x)=exp(−2(δ12+δ22)(u1−u2)2), q(x)=2π(δ12+δ22)(10)
可以很容易的得到 p(x)>0 , q(x)>0 ,當 Sg<1 時, p(x)<q(x) , 當 Sg>1 時, p(x)>q(x) , 因此這裏問題轉化爲判斷 p(x),q(x) 的大小。
設自變量 x=δ12+δ22 , N=(u1−u2)2 ,則 x> ,N>0
p(x)=exp(−2xN), q(x)=2πx(11)
如下是仿真計算 :q(x) 是條固定的曲線,p(x) 是根據N移動的曲線蔟,大多數情況會出現 p(x)<q(x)(情況①)
可以看出當 p(x),q(x) 只有一個交點時,即是一個情況②可能出現的臨界,(這個臨界 Nk我沒有計算出來,但仿真差不多在0.06附近,暫且就這樣認爲吧),N 越小曲線越靠右,就會出現局部 p(x)>q(x) 的情況。
可以得出如下結論:
- 當 N>0.06 時,不可能出現情況②
- 當 0<N<0.06 時,且a<x<b(其中a,b爲兩交點,a>0,b<2π1),出現情況②
我們手動仿真了一些情況,如下
當可以通過 Sg 來判斷當前的融合概率是否是被增強還是削弱時,我們是否也可以同時用來判斷融合的有效性。在機器人定位中,利用Kalman融合後,可以利用 Sg 判斷定位融合是否有效。
- 預測和測量相差很遠時, Sg 就變小,融合分佈概率分散,真實位置概率變小;
- 預測和測量相差很近時,且方差很小時,融合分佈概率更集中,真實位置概率變大
如此我們可以設置一個閾值來判斷當前融合的有效性。