卡爾曼濾波器(Kalman Filters)

卡爾曼濾波器，這是一種使用噪聲感測器測量（和貝葉斯規則）來生成未知量的可靠估計的演算法（例如車輛可能在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 卡爾曼濾波器程式碼

機器人在這個空間中移動時，它會通過執行以下迴圈來定位自己：

1. 感測並執行測量更新任務

2. 移動並執行動作更新任務
   實現此濾波器後，你應該看到，一個非常不確定的位置高斯會變為一個越來越確定的高斯，如下圖所示。

   
   
   
   
   

   下面是常用的高斯方程和匯入。

# 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)