一、高斯函數
1.1 介紹
一維高斯函數的表達式為 \(f\left ( x \right )=\frac{1}{\sqrt{2\pi \delta ^{2}}}exp^{-\frac{1}{2}\frac{\left ( x - \mu \right )^{2}}{\delta ^2}}\)，其中\(\mu\)為函數尖峰值，\(\delta\)為標准方差。
def f(mu, sigma2, x):
return 1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)
1.2 狀態更新
假設像貝葉斯定理一樣，兩個高斯函數相乘，\(f_1\left ( x \right )=\frac{1}{\sqrt{2\pi \delta ^{2}}}exp^{-\frac{1}{2}\frac{\left ( x - \mu \right )^{2}}{\delta ^2}}\) 和 \(f_2\left ( x \right )=\frac{1}{\sqrt{2\pi \gamma ^{2}}}exp^{-\frac{1}{2}\frac{\left ( x - \nu \right )^2}{\gamma ^2}}\)，相乘后獲得新的高斯函數\(f_3\left ( x \right )=\frac{1}{\sqrt{2\pi \delta ^{2'}}}exp^{-\frac{1}{2}\frac{\left ( x - {\mu}' \right )^{2}}{\delta ^{2'}}}\)，其中\(f_3\left ( x \right )\) 的波峰處於\(f_1\left ( x \right )\)，\(f_2\left ( x \right )\)兩個尖峰之間，且更細高，一個相對確定的分布，計算公式為\({\mu}'= \frac{1}{\delta ^2+\gamma ^2}[\gamma ^2\mu + \delta ^2\nu ]\)，\(\delta^{2'}= \frac{1}{\frac{1}{\delta ^2}+\frac{1}{\gamma ^2}}\)。
圖1. 高斯函數
def update(mean1, var1, mean2, var2):
new_mean = 1/(var1+var2)*(var1*mean2+var2*mean1)
new_var = 1/(1/var1 + 1/var2)
return [new_mean, new_var]
1.3 高斯運動預測
預測函數是兩個函數的相加，\(\mu '= \mu + \nu\)，\(\delta^{2'}= \delta ^2+\gamma ^2\)。
def predict(mean1, var1, mean2, var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
二、卡爾曼濾波
2.1 介紹
卡爾曼濾波器本質上是一個預測和更新的估算問題，根據之前一維高斯函數的觀測狀態\(f_2\)分布，與預測狀態\(f_1\)分布，獲得當前狀態\(f_3\)分布，這種可以由兩種相對不確定分布推導相對確定分布，是卡爾曼濾波的核心思想。
2.2 理論
下面是卡爾曼濾波器抽象出的7個步驟，
\(x\)表示濾波器的狀態向量，\(F\)是狀態轉移矩陣，\({x}' = Fx+u\)，表示了狀態 \(x\to x^{'}\) 的過程，是前一個狀態對當前狀態的預測。\(P\)是狀態協方差矩陣，\(Q\)是過程噪聲，\(P^{'}=FPF^{T}+Q\) 表示了 \(P\to P^{'}\) 的過程，是前一個不確定程度對當前狀態的不確定程度的預測。
\(z\)表示觀測狀態量，\(H\)是測量矩陣，\(y=z-H{x}'\)表示了觀測值和與預測值的差值。\(R\)是測量噪聲矩陣，\(K\)為卡爾曼增益，是狀態更新中的權重，在\(x={x}'+Ky\)中是y的權值，而在\(P=(I-KH){P}'\)中是\({P}'\)的權值。
卡爾曼濾波器的最后兩個步驟是濾波器的閉環。\(x={x}'+Ky\)表示當前狀態向量\(x\)的更新，\(P=(I-KH){P}’\)表示系統的不確定度\(P\)的更新，這些都被用於下一個周期的運算。
2.3 實例（一維追蹤）
假設在恆定速度\(v\)的前提下，預測物體的位置\(s\)，這是一維卡爾曼濾波器的追蹤問題，其中狀態向量x為 \([s,v]^{T}\)，因為v保持不變，則當\(\Delta t = 1s\)時，\(F\)表示為\([[1., 1.], [0, 1.]]\)，當前預測的狀態為 \({x}'\)，\(P\)的初始值可以設置為\([[1000., 0.], [0., 1000.]]\)，意味着速度和位置是兩個不相關的變量，因為觀測值只有\(s\)，則\(H\)為\([[1., 0.]]\)。因為實例是簡化的模型，所以\(Q\)和\(R\)都設置為0，一般情況下，\(Q\)是那些\(P\)無法表示的噪聲，而\(R\)由傳感器廠家提供。
# 卡爾曼濾波，對當前位置的估計
def kalman_filter(x, P):
for n in range(len(measurements)):
# measurement update
z = matrix([[measurements[n]]])
y = z - H*x
S = H*P*H.transpose() + R
K = P*H.transpose()*S.inverse()
x = x + (K*y)
P = (I - (K*H))*P
# prediction
x = (F*x) + u
P = F*P*F.transpose()
return x,P
measurements = [1, 2, 3]
x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
print(kalman_filter(x, P))
# output should be:
# x: [[3.9996664447958645], [0.9999998335552873]]
# P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]
2.4 實例（二維追蹤）
假設傳感器為激光雷達，可以提供二維測量數據，首先要定義狀態轉移矩陣\(F\)，來估算二維位置\((s_x, s_y)\)和二維速度\((v_x,v_y)\)，則當\(\Delta t = 1s\)時，\(F\)表示為\([[1, 0,1,0], [0, 1,0,0,1],[0,0,1,0],[0,0,0,1]]\)，當前預測的狀態為 \(x^{'}\)，又因為激光雷達只能測量距離，則對應的測量矩陣\(H=[[1,0,0,0],[0,1,0,0]]\)，而只能測量距離不能測量速度的性質，體現在協方差矩陣上就是位置測量的不確定度較低，速度測量的不確定度較高，則\(P = [[1,0,0,0],[0,1,0,0],[0,0,1000,0],[0,0,0,1000]]\)。該處理方式和一維追蹤中的相同。
三、擴展卡爾曼濾波
3.1 介紹
擴展卡爾曼濾波是為了解決不同傳感器融合時采集數據不統一的問題，比如激光雷達的觀測值是在笛卡爾坐標系的，而毫米波雷達的觀測值是在極坐標系的，在進行傳感器融合時，需要統一坐標系，但是當坐標系轉換時會引入非線性變化，於是這種非線性的卡爾曼濾波就稱為擴展卡爾曼濾波。
3.2 毫米波雷達
毫米波雷達的原理是多普勒效應，其測量的數據都是在極坐標系下的，狀態向量x為\([\rho ,\phi, \dot{\rho } ]^T\)。\(\rho\)為目標在極坐標下離雷達的距離，\(\phi\)為方向角，\(\dot{\rho }\)為距離的變化率。
因為預測的狀態向量\({x}'\)為 \([{s_x}', {s_y}', {v_x}',{v_y}']^T\)，觀測值和與預測值的差值的非線性的模型時，習慣於將計算差值\(y=z-H{x}'\)中的\(H{x}'\)用\(h({x}')\)表示。此時\(H{x}'\)不在是常數矩陣，高斯分布在經過非線性函數的變換后，其結果將不再符合高斯分布，這直接導致卡爾曼濾波器失效。因此我們需要將\(h({x}')\)先轉化為近似的線性函數。\(y=h(x)\)可通過泰勒公式在點\((x_0,y_0)\)處展開為泰勒級數，並忽略二次以上的高階項，於是可以如下的式子，
\(h(x)\approx h(x_0)+\dot{h}(x_0)(x-x_0)\)
其中，\(\dot{h}(x_0)\) 表示\(h(x)\)在\(x_0\)處對\(x\)的偏導，即\(\dot{h}(x_0) =\frac{\partial h(x_0)}{\partial x}\)，這一項被稱為雅可比（Jacobian）式。
求得非線性函數\(h(x')\)對\({s_x}'\)，\({s_y}'\)，\({v_x}'\)，\({v_y}'\)的一階偏導數，並排列成的矩陣，最終得到雅克比（Jacobian）矩陣\(H_{3\times 4 }\)，將\(\rho ,\phi, \dot{\rho }\)帶入上述公式可得實際的測量矩陣\(H_{3\times 4 }\)，其余是卡爾曼濾波的標准步驟。
四、總結
擴展卡爾曼濾波，只是增加了對非線性觀察矩陣H的線性化處理，依此來滿足高斯分布的性質，即高斯分布在經過預測和測量值更新后，還是符合高斯分布。
卡爾曼濾波器的這些性質適合融合多個傳感器，以此減少單個傳感器的誤差，降低了單個傳感器的精度成本。
