卡爾曼濾波器,這是一種使用噪聲傳感器測量(和貝葉斯規則)來生成未知量的可靠估計的算法(例如車輛可能在3秒內的位置)。
我們知道高斯方程包含兩個主要參數:
- 一個是平均數
- 一個是方差,通常寫爲平方值
。 一般來說,高斯方程是這樣的:
我們將該方程的第一部分稱爲 係數,第二部分稱爲 指數。第二部分在定義高斯的形狀時最爲重要,而係數是一個歸一化因子。
對於不確定的連續數量,例如自動駕駛汽車的預估位置,我們使用高斯方程來表示該數量的不確定性。方差越小,我們對數量越有把握。
# import math functions from math import * import matplotlib.pyplot as plt import numpy as np # gaussian function def 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
# an example Gaussian gauss_1 = f(10, 4, 8) print(gauss_1)
繪製高斯分佈圖
由於我們的函數僅返回x的特定值的值,因此我們可以通過遍歷x值範圍並創建高斯分佈值g的結果列表來繪製高斯分佈圖
# display a gaussian over a range of x values # define the parameters mu = 10 sigma2 = 4 # define a range of x values x_axis = np.arange(0, 20, 0.1) # create a corresponding list of gaussian values g = [] for x in x_axis: g.append(f(mu, sigma2, x)) # plot the result plt.plot(x_axis, g)
新的均值和方差
該程序要接收兩個均值和方差,併爲高斯方程返回一個全新的、已更新的均值和方差。此步驟稱爲參數或測量更新,因爲它是當初始置信度(由下面的藍色高斯表示)與新信息(具有一定不確定性的測量,即橙色高斯))合併時發生的更新。 已知兩個均值和方差:
更新之後的公式:
更新的高斯將是這兩個高斯的組合,其中新的均值介於兩者之間,並且方差小於兩個給定方差中的最小值。這意味着在測量之後,我們的新均值比初始置信度更加確定!
# import math functions from math import * import matplotlib.pyplot as plt import numpy as np # gaussian function def 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 # the update function def update(mean1, var1, mean2, var2): ''' This function takes in two means and two squared variance terms, and returns updated gaussian parameters.''' ## TODO: Calculate the new parameters new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2) new_var = 1 / (1 / var1 + 1 / var2) return [new_mean, new_var] # display a gaussian over a range of x values # define the parameters mu = new_params[0] sigma2 = new_params[1] # define a range of x values x_axis = np.arange(0, 20, 0.1) # create a corresponding list of gaussian values g = [] for x in x_axis: g.append(f(mu, sigma2, x)) # plot the result plt.plot(x_axis, g)
高斯函數移動
收集一些新測量之後,執行參數更新,然後,下一步是將運動合併到我們的高斯計算中。回想一下,在我們判斷機器人或自動駕駛汽車位置的時候:
- 測量更新增加了 我們的判斷確定性
- 運動更新/預測降低了我們的判斷確定性 這是因爲每次運動都有可能未達到或超越預期目的位置,並且由於運動的不準確性,我們最終會在每次運動後失去對確切位置的判斷確定性。
# import math functions from math import * import matplotlib.pyplot as plt import numpy as np # gaussian function def 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 # the update function 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) return [new_mean, new_var] # the motion update/predict function def predict(mean1, var1, mean2, var2): ''' This function takes in two means and two squared variance terms, and returns updated gaussian parameters, after motion.''' ## TODO: Calculate the new parameters new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var]
通過遍歷一系列x值並創建高斯值 g的結果列表來繪製一個高斯圖,如下所示。你可以嘗試更改mu和sigma2的值,看一看會發生什麼。
# display a gaussian over a range of x values # define the parameters mu = new_params[0] sigma2 = new_params[1] # define a range of x values x_axis = np.arange(0, 40, 0.1) # create a corresponding list of gaussian values g = [] for x in x_axis: g.append(f(mu, sigma2, x)) # plot the result plt.plot(x_axis, g)
1D 卡爾曼濾波器代碼
機器人在這個空間中移動時,它會通過執行以下循環來定位自己:
- 感測並執行測量更新任務
- 移動並執行動作更新任務 實現此濾波器後,你應該看到,一個非常不確定的位置高斯會變爲一個越來越確定的高斯,如下圖所示。
下面是常用的高斯方程和導入。
# import math functions from math import * import matplotlib.pyplot as plt import numpy as np # gaussian function def 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
以下是update
代碼,該代碼在合併初始置信度和新測量信息時執行參數更新。此外,完整的predict
代碼在合併一個運動後會對高斯值進行更新。
```python # the update function 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) return [new_mean, new_var]
# the motion update/predict function 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]
初始不確定性
初始參數,其中包括初始位置估計mu
和平方方差sig
。注意,初始估計設置爲位置0,方差非常大;這是一種高度混亂的狀態,就像我們在直方圖濾波器中使用的 均勻 分佈一樣。
# measurements for mu and motions, U measurements = [5., 6., 7., 9., 10.] motions = [1., 1., 2., 1., 1.] # initial parameters measurement_sig = 4. motion_sig = 2. mu = 0. sig = 10000. ## TODO: Loop through all measurements/motions ## Print out and display the resulting Gaussian # your code here ## TODO: Loop through all measurements/motions # this code assumes measurements and motions have the same length # so their updates can be performed in pairs for n in range(len(measurements)): # measurement update, with uncertainty mu, sig = update(mu, sig, measurements[n], measurement_sig) print('Update: [{}, {}]'.format(mu, sig)) # motion update, with uncertainty mu, sig = predict(mu, sig, motions[n], motion_sig) print('Predict: [{}, {}]'.format(mu, sig)) # print the final, resultant mu, sig print('\n') print('Final result: [{}, {}]'.format(mu, sig))
## Print out and display the final, resulting Gaussian # set the parameters equal to the output of the Kalman filter result mu = mu sigma2 = sig # define a range of x values x_axis = np.arange(-20, 20, 0.1) # create a corresponding list of gaussian values g = [] for x in x_axis: g.append(f(mu, sigma2, x)) # plot the result plt.plot(x_axis, g)