卡爾曼濾波器的特殊案例

卡阿爾曼濾波器為每個(gè)結(jié)果狀態(tài)找到最佳的平均因子。另外，以某種方式保存過(guò)去的狀態(tài)。它針對(duì)每個(gè)時(shí)間范圍對(duì)變量執(zhí)行聯(lián)合概率分布。該算法對(duì)每個(gè)步驟使用新的均值和新方差，以便計(jì)算結(jié)果的不確定性，并嘗試為測(cè)量更新（傳感/預(yù)測(cè)）和運(yùn)動(dòng)更新（運(yùn)動(dòng)）的每個(gè)時(shí)間范圍提供準(zhǔn)確的測(cè)量。該算法還使用其他誤差和統(tǒng)計(jì)噪聲來(lái)表示初始的不確定性。

? 將來(lái)自各種傳感器（如LiDAR和Radar跟蹤器）的數(shù)據(jù)輸入轉(zhuǎn)換為可用形式。計(jì)算和推斷速度。

? 減少目標(biāo)位置和速度的測(cè)量誤差（噪聲）。

? 使用先前的狀態(tài)估計(jì)和新數(shù)據(jù)預(yù)測(cè)目標(biāo)的未來(lái)狀態(tài)。

? 簡(jiǎn)單，實(shí)用和可移植的算法。

? 估計(jì)一個(gè)連續(xù)狀態(tài)和結(jié)果，卡爾曼濾波器給了我們一個(gè)單峰分布。

卡爾曼過(guò)濾為我們提供了一種數(shù)學(xué)方法，這種方法依據(jù)物體的初始位置和相關(guān)變量來(lái)推斷物體之后的運(yùn)動(dòng)速度和狀態(tài)。因此，在這里，我們將創(chuàng)建一個(gè)一維卡爾曼濾波器，設(shè)置初始位置，結(jié)合物體運(yùn)動(dòng)的不確定性，來(lái)估計(jì)物體未來(lái)的位置以及運(yùn)動(dòng)速度。此外，如果我們想了解卡爾曼濾波器的工作原理，我們首先需要了解一些有關(guān)高斯的知識(shí)，它代表卡爾曼濾波器中的單峰分布。

高斯是在位置空間上的連續(xù)函數(shù)，其下面的面積之和最多為1。高斯的特征在于兩個(gè)參數(shù)，平均值，經(jīng)?？s寫(xiě)為希臘字母μ，高斯的寬度通常被稱(chēng)為方差即σ 2。所以，我們常用平均值和方差來(lái)尋找對(duì)象的位置，獲得最佳估計(jì)。另外，寬度越大，不確定性越大。

一維高斯方程

上圖表示高斯的均值（μ）和方差（σ2）。平均值（μ）越高，物體在該位置出現(xiàn)的機(jī)會(huì)就越高。相反，如果方差（σ2）較大，即分布較寬，則該對(duì)象的不確定性較高；可以放置在高斯內(nèi)部的任何位置。

就公式而言，它是二次函數(shù)的指數(shù)，我們?nèi)”磉_(dá)式的指數(shù)。我們的查詢(xún)點(diǎn)x相對(duì)于平均值（μ）的二次方差，除以方差（σ2），乘以-（1/2）?，F(xiàn)在，如果x =μ，則分子變?yōu)?，如果x為0，即1。事實(shí)證明，我們必須在2πσ2的平方根上對(duì)常數(shù)進(jìn)行歸一化處理。

高斯是其特征在于通過(guò)給定的指數(shù)函數(shù)平均值（μ），其限定了高斯曲線(xiàn)的峰值的位置，和一個(gè)方差（σ 2）限定曲線(xiàn)的寬度/擴(kuò)散。所有高斯都是：對(duì)稱(chēng)的。它們具有一個(gè)峰（也稱(chēng)為“單峰”分布），并且在該峰的兩側(cè)均具有指數(shù)下降。

方差是高斯分布的度量；方差越大，對(duì)應(yīng)的高斯越短。差異也是確定性的度量；如果試圖找到最確定的位置，例如汽車(chē)的位置，則需要一個(gè)高斯函數(shù)，其均值是汽車(chē)的位置并且不確定性/傳播范圍最小。我們編寫(xiě)一個(gè)高斯函數(shù)：

from math import *import matplotlib.pyplot as pltimport numpy as np# gaussian functiondef f(mu, sigma2, x):    ''' f takes in a mean and squared variance, and an input x       and returns the gaussian value.'''    coefficient = 1.0 / sqrt(2.0 * pi *sigma2)    exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)    return coefficient * exponential

在卡爾曼濾波器中，我們迭代使用貝葉斯規(guī)則的測(cè)量（測(cè)量更新），貝葉斯規(guī)則僅是乘積或乘法，而運(yùn)動(dòng)更新（預(yù)測(cè)）則使用總概率（卷積或加法）進(jìn)行迭代。

為了理解周期，我們假設(shè)我們正在對(duì)車(chē)輛進(jìn)行定位并且我們有一個(gè)先驗(yàn)分布（藍(lán)色高斯）；這是一個(gè)非常寬泛的高斯平均值?，F(xiàn)在，我們得到一個(gè)測(cè)量值（橙色高斯），它告訴我們有關(guān)車(chē)輛定位的信息。這是我們之前的一個(gè)例子，我們對(duì)位置不確定，但測(cè)量結(jié)果告訴我們有關(guān)車(chē)輛的位置信息。

注意：在上圖中，Mu（μ）是先前的均值，Nu（v） 是新的測(cè)量均值。最終均值在兩個(gè)舊均值，先驗(yàn)均值和測(cè)量均值之間移動(dòng)。在測(cè)量方面，它要稍遠(yuǎn)一些，因?yàn)榕c以前相比，該測(cè)量可以更確定地確定車(chē)輛的位置。我們?cè)酱_定，就越會(huì)在確定答案的方向上拉均值。

所得的高斯比兩個(gè)分量的高斯更確定，即協(xié)方差小于設(shè)備中兩個(gè)協(xié)方差中的任一個(gè)。直觀上來(lái)說(shuō)，是因?yàn)槲覀儗?shí)際上獲得了位置信息。在任一高斯裝置中，這兩個(gè)高斯都具有較高的信息量。

1.假設(shè)我們按照貝葉斯規(guī)則將兩個(gè)高斯乘以一個(gè)先驗(yàn)概率和一個(gè)測(cè)量概率。先驗(yàn)的均值為Mu （μ），且為σ2，方差為Nu （v），協(xié)方差為r-square （r2）。

2. 然后，新的均值Mu prime （μ'）是舊均值的加權(quán)和。Mu （μ）由r平方（r2）加權(quán)，Nu （v）由Sigma平方（σ2）加權(quán)，并由加權(quán)因子之和標(biāo)準(zhǔn)化。新的方差項(xiàng)將是Sigma平方素?cái)?shù)（σ2'）。

3. 顯然，先驗(yàn)的高斯不確定性要高得多，因此σ2更大，這意味著Nu（v）的權(quán)重比Mu （μ）大得多。需要注意的是，方差項(xiàng)不受實(shí)際方法的影響，它僅使用以前的方差。

def update(mean1, var1, mean2, var2):   ''' This function takes in two means and two squared variance terms,       and returns updated gaussian parameters.'''    #Calculate the new parameters   new_mean = (var2*mean1 + var1*mean2)/(var2+var1)   new_var = 1/(1/var2 + 1/var1)

高斯運(yùn)動(dòng)：

新的均值（μ'）是舊均值Mu （μ）加上u的運(yùn)動(dòng)。因此，如果在x方向上移動(dòng)了10米，那么它就是10米，并且知道σ2'是舊σ2加上運(yùn)動(dòng)高斯的方差（r2）。這就是我們所需要知道的，僅僅是補(bǔ)充。在預(yù)測(cè)步驟中得到的高斯只是將這兩件事加起來(lái)，即mu（μ）加u和σ2加方差（r2）。

def predict(mean1, var1, mean2, var2):   ''' This function takes in two means and two squared variance terms, and returns updated gaussian parameters, after motion.''' #Calculate the new parameters new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var]

過(guò)濾器管道：

# measurements for mu and motions, Umeasurements = [5., 6., 7., 9., 10.]motions = [1., 1., 2., 1., 1.] # initial parametersmeasurement_sig = 4.motion_sig = 2.mu = 0.sig = 10000.  #0000000001 ## TODO: Loop through allmeasurements/motions## Print out and display the resultingGaussian# your code herefor i in range(len(measurements)):    #measurement update, with uncertainty   mu, sig = update(mu, sig, measurements[i], measurement_sig)   print('Update: [{}, {}]'.format(mu, sig))    #motion update, with uncertainty   mu, sig = predict(mu, sig, motions[i], motion_sig)   print('Predict: [{}, {}]'.format(mu, sig))   # print the final, resultant mu, sigprint('\n')print('Final result: [{}, {}]'.format(mu,sig))

?現(xiàn)在，我們將所有內(nèi)容放在一起編寫(xiě)一個(gè)具有這兩個(gè)功能的主程序，進(jìn)行更新和預(yù)測(cè)，并饋入一系列測(cè)量和運(yùn)動(dòng)。選擇的示例中，測(cè)量值= 5，6，7，9，10，運(yùn)動(dòng)是1，1，2，1，1。如果初始估算為5，但我們將其設(shè)置為0，不確定性為10,000。

?假設(shè)測(cè)量不確定度為常數(shù)4，運(yùn)動(dòng)不確定度為常數(shù)2。運(yùn)行該變量時(shí)，位置的第一個(gè)估算值基本上應(yīng)為5–4.99，原因是初始不確定性太大，因此估算值占主導(dǎo)地位通過(guò)第一次測(cè)量。不確定性減少到3.99，這比測(cè)量不確定性要好一些。然后，預(yù)測(cè)將增加1，但是不確定性增加到5.99，這是運(yùn)動(dòng)不確定性2。

?再次基于度量6更新，得到的估計(jì)值為5.99，幾乎是6。再次移動(dòng)1。測(cè)量了7。移動(dòng)了2.測(cè)量了9.移動(dòng)了1.測(cè)量了10，然后移動(dòng)了最終1。結(jié)果作為最終結(jié)果，該位置的預(yù)測(cè)為10.99，即10位置移動(dòng)了1 ，以及不確定性（殘余不確定性）4。

繪制高斯：

## Print out and display the final,resulting Gaussian# set the parameters equal to the output ofthe Kalman filter resultmu = musigma2 = sig # define a range of x valuesx_axis = np.arange(-20, 20, 0.1)# create a corresponding list of gaussianvaluesg = []for x in x_axis:g.append(f(mu, sigma2, x))# plot the resultplt.plot(x_axis, g)

請(qǐng)記住，在測(cè)量更新中，measurement_sig（var2或σ2）= 4，在運(yùn)動(dòng)更新（預(yù)測(cè)）中，motion_sig（var2或σ2）= 2將保持恒定。除此之外，代碼中的每個(gè)變量都會(huì)在每個(gè)時(shí)間步更新。例如，在過(guò)濾器管道的for循環(huán)中，mu和sig值將在測(cè)量更新中得到更新，然后將新的更新的mu和sig值輸入到運(yùn)動(dòng)更新（預(yù)測(cè)）功能中。然后再次通過(guò)運(yùn)動(dòng)更新（預(yù)測(cè)）功能生成新值時(shí)，將這些更新后的值輸入到測(cè)量更新（更新）功能中，循環(huán)繼續(xù)進(jìn)行，直到為車(chē)輛/物體的每個(gè)時(shí)間步長(zhǎng)計(jì)算不確定性為止。

        

         個(gè)人微信（如果沒(méi)有備注不拉群！）
        
        

         請(qǐng)注明：地區(qū)+學(xué)校/企業(yè)+研究方向+昵稱(chēng)
        
        

         

        


下載1：何愷明頂會(huì)分享

在「AI算法與圖像處理」公眾號(hào)后臺(tái)回復(fù)：何愷明，即可下載。總共有6份PDF，涉及 ResNet、Mask RCNN等經(jīng)典工作的總結(jié)分析

下載2：終身受益的編程指南：Google編程風(fēng)格指南

在「AI算法與圖像處理」公眾號(hào)后臺(tái)回復(fù)：c++，即可下載。歷經(jīng)十年考驗(yàn)，最權(quán)威的編程規(guī)范！


      

       

      
      

       下載3 CVPR2020
      
      

       

      
      

       在「AI算法與圖像處理」公眾號(hào)后臺(tái)回復(fù)：CVPR2020，即可下載1467篇CVPR 2020論文
      


     

      

       

        

         
覺(jué)得不錯(cuò)就點(diǎn)亮在看吧