簡易介紹卡爾曼濾波 (Kalman Filter)
在卡爾曼濾波中，一般看到的最佳估計值的公式為
由上式子可以很清楚知道，卡爾曼濾波只是靠卡爾曼增益當做一個合成的權重，用來評估在 時間點的實際量測值($z_t$)重要還是模型預測的估計值($x’_t $)
用範例來解釋
假設有一場景，無人車感測到一個物件，我們就需要針對此物件進行預測和判斷，所以建立此物件的狀態(state)，假設狀態為𝐱=[𝐩,𝐯]T
預測: 1. 狀態預測
簡易版本
假設這個物體速度是均速的 (𝐯: 物件的速度不變)，但我們可以預測這個物體的在t+1時間的位置是會移動的，位置(𝐩)的變化為
變化時間(Δ𝑡): 就是觀察時間的間隔，比如說感測器為10Hz輸出，也就是每100ms更新一次，所以Δ𝑡=100𝑚𝑠=0.1𝑠 。
所以上式子也就是由 1. (位移距離 = t時間點的速度(𝑣𝑡𝑥和𝑣𝑡𝑦)*變化時間(Δ𝑡) ) + 2. 時間t的位置(𝑝𝑡𝑥和𝑝𝑡𝑦) = 得到t+1時間點新的位置(𝑝𝑡+1𝑥和𝑝𝑡+1y)
假設
然後我們要預測200ms後的結果，直接針對𝐱0的結果來推論𝐱2，所以要預測𝐱1然後再預測𝐱2
我們可以把狀態(State)的預測方程式寫成矩陣
𝐀𝑡是用來狀態進行轉換的動作，因此𝐀稱為狀態轉移矩陣(state-transition matrix)。
Example:用矩陣來寫
然後我們要預測200ms後的結果(直接針對𝐱0的結果來推論𝐱2)，所以要預測𝐱1然後再預測𝐱2
所以當我們觀察到𝑡=0的狀態後，我們可以預估200ms後的結果(也就是預測的軌跡𝐱1和𝐱2)。
正常版本
但實際上一般文章看到還有一個控制輸入(𝐁 𝑢𝑡)和過程噪音(process noise 𝜔𝑡)，我們可以想像是追蹤物體時候把它的內部控制考慮進去，所以實際上State預測的寫法為
我們簡化將𝐁 𝑢𝑡設定為0，一般文章在介紹都有寫𝐁 𝑢𝑡這項，但程式操作也是設定為0矩陣，因為這項也稱為控制訊號(Control)或是輸入訊號，你可以想像這項是我們追蹤目標內部實際的控制狀態，也就是這輛車(或是人)的實際操作狀態(油門踩多少導致速度增加之類的)，但實際上我們只能利用感知器得到觀察狀態(State)，不可能知道目標的內部運作狀態，因此這項可以先簡化為0。
我們簡化將𝐁 𝑢𝑡設定為0，我們只考慮過程噪音( 𝜔𝑡)，也就是我們的預測State給一個隨機的noise讓他不會這麼穩定，因為在實際的應用上沒有任何一個感知器對於物體的偵測永遠這麼穩定
𝜔𝑡為過程噪音，我們假設此noise是車輛或是行人忽然加減速，所以將設定為
因為𝜔𝑡是noise本質上服從常態分佈(𝑁(0,𝐐))
Example:
然後我們要預測200ms後的結果(直接針對𝐱0的結果來推論𝐱2)，所以要預測𝐱1然後再預測𝐱2
預測: 2. 預測的不確定性(觀察噪音)
每次預測多少都帶來了多少不確定性，我們前面有提到卡爾曼濾波是要加權合成預測值和觀察值，所以我們要怎麼來進行加權，答案是利用不確定性，所謂不確定性就是我們也不知道預測的結果到底正不正確。
我們這邊用𝐏𝑡來表示t時間點的不確定性，用來度量估計值的精確程度，此項目為預測估計的共變異數矩陣(covariance matrix，大部分的文章都是寫協方差矩陣，這是中國的用詞，台灣應該都是用共變異數矩陣)。
𝐏𝑡的共變異數為何?因為通常第一次觀察到物件(t=0)的情況都是未知，所以只能先經由假設一個先驗機率方式，先估計物件可能的不確定性，因為element之間的covariance未知，所以通常先設定為0，但element自己的variance是可以先設定的。
例如:
如何讓不確定性在每個時刻傳遞呢，答案是乘上狀態轉換矩陣(𝐀)
t+1時間點不確定性為
同時我們也要考慮我們的預測模型並不是一定準確的，它本身也是包含雜訊的。前面的觀測噪聲，目前需要加入的預測模型本身的噪聲。所以我們還要加入模型本身的過程噪音(𝐐為過程噪音𝜔𝑡的共變異數矩陣，因為𝜔𝑡是noise本質上服從常態分佈(𝑁(0,𝐐)))。
修正
我們先看一下最後量測更新的公式:
修正: 1. 預測值和量測值的誤差
預測值出來的𝐱𝑡+1要跟值計量測值(𝐳𝑡+1)計算誤差 → (𝐳𝑡+1−𝐱𝑡+1) 存在嗎?
預測值和量測值的誤差 (𝐯𝑡+1)
所以其實量測值(𝐳𝑡+1)是預測值(𝐇𝐱𝑡+1)加上觀測noise(也就是誤差，𝐯𝑡+1)
觀測noise本身我們也是假設服從常態分佈
量測noise的共變異數矩陣為𝐑:
𝜎2𝑣𝑥和 𝜎2𝑣𝑦用來表示感測器的量測誤差能有多大。
修正: 2. 卡爾曼增益
我們用單維度的Kalman gain公式來說會比較清楚，Kalman Gain為(最後面最佳解的推導)
𝑝′𝑡+1在前面寫到是用來表示t時間點的不確定性，但因為前面用到的是共變異數矩陣，這邊直接表示一個變異數非矩陣，所以可以直接用除法。 𝑟為量測噪音的變異數。
由公式可以得到𝐾𝑡∈[0,1]，所以可以拿來做為量測更新的權重和。
但在多維度沒有除法，因此要用inverse來處理，外加𝐏′𝑡+1是基於狀態維度(𝑅4×1)算估計的共變異數矩陣(𝑅4×4) 我們也是要將此狀態維度(𝑅4×1)轉換成量測維度(𝑅2×1)空間，因此𝐏′𝑡+1矩陣也需要乘上觀察矩陣𝐇。
我們先看一下最後量測更新的公式:
多維度的卡爾曼增益，看起來很複雜:
有了卡爾曼增益後，進行最後求得更新預測不確定性的𝐏𝑡:
Kalman Filter公式總整理:
初始化:
初始化 𝐑,𝐏,𝐐
狀態的預測:
預測規劃狀態
預測規劃誤差共變異數矩陣
量測更新(修正):
計算卡爾曼增益
通過量測更新估計
更新誤差共變異數矩陣
卡爾曼增益推導
卡爾曼增益為什麼長這麼畸形? 我們這邊來看一下卡爾曼增益是怎麼推出來的。
一切的源頭我們要從誤差共變異數矩陣出發，因為卡爾曼增益是用來評估量測值和估計值的權重，所以用估計誤差來評判估計值的權重。
如果要得到最佳的卡爾曼增益，我們希望誤差期望值越小越好，也就是利用最小平方法讓 𝐱𝑡−𝐱̂ 𝑡最小化
也就是希望
所以就是上面那一大段很複雜的公式找最小解，一樣利用偏微分的概念找最佳解，所以針對𝐏𝑡對𝐊𝑡做偏微分:
這麼畸形的卡爾曼增益就是這麼來的。
範例程式
題目: 我們要先製造一些假數據，我們的目標是假設有一個物件(行人)移動速度X軸是20 𝑚/𝑠2，Y軸是10 𝑚/𝑠2，我們進行20秒資料的觀察和做Kalman Filter。
隨機產生一些量測數據 (200筆，假設0.1秒一筆，所以為20秒的資料)
假設真實速度
- X速度是20 𝑚/𝑠2
- Y速度是10 𝑚/𝑠2
然後我們程式生成的觀察資料，就是真實速度加上一些random noise (高斯分布)，所以下一段是我們產生出來的vx和vy速度。
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
m = 200 # Measurements
vx = 20 # in X
vy = 10 # in Y
mx = np.array(vx + np.random.randn(m)*1)
my = np.array(vy + np.random.randn(m)*1)
measurements = np.vstack((mx,my))
print('Standard Deviation of Acceleration Measurements = {:.2f}'.format(np.std(measurements[0,:])))
fig = plt.figure(figsize = (16,5))
plt.step(range(m), measurements[0,:], label = '$v_x$')
plt.step(range(m), measurements[1,:], label = '$v_y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Mesaurements')
plt.legend(loc = 'best', prop = {'size':18})Result:
依照前面的卡爾曼濾波流程: 1.初始化 2.狀態的預測 3. 量測更新(修正)
1. 初始化: 初始化 𝐑,𝐏,𝐐
還要初始化觀察植我們假設是物件State(x)的座標(px,py)和橫向縱向速度(vx, vy)，然後間格時間(dt)為100ms，也就是0.1秒。A和H矩陣也依造前面文章介紹的寫法。然後我額外寫了一個物件States_Kalman用來儲存每次的執行出來的所有產出。
import numpy as np
# initial object state (px,py, vx, vy)
x = np.matrix([[0.0,0.0,0.0,0.0]]).T
# 物件的不確定因素: 先驗估計的共變異數矩陣
P = np.diag([1000.0,
1000.0,
1000.0,
1000.0])
# 量測的時間間隔 𝑑𝑡 假設100ms
dt = 0.1 # time step between filter step
# 狀態轉移矩陣 𝐅
A = np.matrix([
[1.0 , 0.0, dt, 0.0],
[0.0, 1.0, 0.0, dt],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0],
])
# 觀測矩陣 𝐇
H = np.matrix([
[0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,1.0]
])
# 量測noise的共變異數矩陣 𝐑
ra = 0.9
R = np.matrix([[ra, 0.0],
[0.0, ra]])
# 過程Noise的共變異數矩陣 𝐐
sv = 0.5 # m/s^2
G = np.matrix([[0.5 * dt **2],
[0.5 * dt **2 ],
[dt],
[dt]])
Q = G * G.T * sv **2
I = np.eye(4)
class States_Kalman():
def __init__(self):
self.initial()
def savestates(self, x, Z, P, R, K):
self.xt.append(float(x[0]))
self.yt.append(float(x[1]))
self.dxt.append(float(x[2]))
self.dyt.append(float(x[3]))
self.Zx.append(float(Z[0]))
self.Zy.append(float(Z[1]))
self.Px.append(float(P[0,0]))
self.Py.append(float(P[1,1]))
self.Pdx.append(float(P[2,2]))
self.Rdy.append(float(P[3,3]))
self.Rdx.append(float(R[0,0]))
self.Rdy.append(float(R[1,1]))
self.Kx.append(float(K[0,0]))
self.Ky.append(float(K[1,0]))
self.Kdx.append(float(K[2,0]))
self.Kdy.append(float(K[3,0]))
def initial(self):
self.xt, self.yt = [], []
self.dxt, self.dyt = [], []
self.Zx, self.Zy = [], []
self.Px, self.Py = [], []
self.Pdx, self.Pdy = [], []
self.Rdx, self.Rdy = [], []
self.Kx, self.Ky = [], []
self.Kdx, self.Kdy = [], []
states_kalman = States_Kalman()我們開始執行Kalman Filter 步驟:
2.狀態的預測 3. 量測更新(修正)
Note: 在我們的範例我們預測state的時候控制輸入(𝐁 𝑢𝑡)和過程noise(𝜔𝑡)我們直接跳過不納入運算，如果目標的控制輸入和noise你們能控制有興趣也可以自己加。
for n in range(len(measurements[0])):
# Time update (Prediction)
# 1. project the state ahead
x = A * x
# 2. Project the error covariance ahead
P = A * P * A.T + Q
# Measurement update (Correction)
# 1. Compute the Kalman Gain
S = H * P * H.T + R
K = (P * H.T) * np.linalg.pinv(S)
# 2. update the estimate by z
Z = measurements[:,n].reshape(2,1)
y = Z - H*x
x = x + K*y
# 3. update the error covariance
P = (I - K*H) * P
## Save states (for plot results)
states_kalman.savestates(x,Z,P,R,K)所有的結果都被儲存在states_kalman，我們在寫一隻程式碼把結果畫出來。
第一個呈現結果是量測的速度經由Kalman Filter處理過的結果。
def plot_x(states_kalman):
fig = plt.figure(figsize = (16,9))
plt.step(range(len(measurements[0])), measurements[0], label = '$measured v_x$')
plt.step(range(len(measurements[0])), measurements[1], label = '$measured v_y$')
plt.axhline(vx, color = 'k', label = '$ture v_x$')
plt.axhline(vy, color = 'k', label = '$ture v_y$')
plt.step(range(len(measurements[0])), states_kalman.dxt, label = '$estimate v_x$', linewidth=2)
plt.step(range(len(measurements[0])), states_kalman.dyt, label = '$estimate v_y$', linewidth=2)
plt.xlabel('Filter Step')
plt.ylabel('Velocity')
plt.title('Estimate (Elements from State Vectir $x$)')
plt.legend(loc = 'best', prop = {'size': 11})
plot_x(states_kalman)
黑線是真實我們設定的速度，藍線和橘線是我們產生出來有noise的觀察數據(也就是實際量測值)，然後經由Kalman Filter結果是綠色和紅色線，所以結果就比較smooth了。
第二個呈現結果是量測的速度經由Kalman Filter處理過的結果，物件移動的變化(x, y)。
def plot_xy(states_kalman):
fig = plt.figure(figsize = (5,5))
plt.scatter(states_kalman.xt,
states_kalman.yt,
s=1, label='State', c='k')
plt.scatter(states_kalman.xt[0], states_kalman.yt[0], s=10, label = 'Start', c='g')
plt.scatter(states_kalman.xt[-1], states_kalman.yt[-1], s=10, label = 'Goal', c='b')
plt.scatter(states_kalman.xt[0]+dt*vx*m, states_kalman.yt[0]+dt*vy*m, s=10, label = 'Real Goal', c='r')
plt.xlabel('x')
plt.ylabel('y')
plt.title('Position \n pred Goal(x,y): ({:.2f},{:.2f})\n Real Goal(x,y): ({:.2f},{:.2f})'.format(states_kalman.xt[-1],
states_kalman.yt[-1],
states_kalman.xt[0]+dt*vx*m,
states_kalman.yt[0]+dt*vy*m)
)
plt.legend(loc = 'best')
plt.axis('equal')
plot_xy(states_kalman)
這個物件出發點是(0,0)(Start)的位置
經由物件速度經由Kalman Filter修正物件會移動變化是黑色的點，最後的終點是藍色的點(Goal)，也就次圖片title上寫的pred Goal，物件20秒後走到X是400.58公尺，Y是199.11公尺的位置。
但如果假設速度是恆定都沒有noise，真實的目標點(Real Goal)是，物件20秒後走到X是401.92公尺，Y是200.92公尺的位置。
經由Kalman Filter後結果滿接近真實狀況。
讀者可以自己試看看如果不做Kalman Filter看看移動的狀況，但我先給答案，結果會差不多，因為noise本身是高斯分布(平均數是0)，經由200次的移動，這noise已經被修正回來，所以會走到跟真實目標點差不多的位置。
有些人會問那幹嘛要做Kalman Filter，最大的差異是你在看vx和vy如果沒有filter變異太大(原本的觀察值)，但做了filter之後變得比較smooth，好處是假設你是做車輛控制，如果你在控制車子之間的距離的時候你不會希望你在預測前面那台車子，前面那台車子一下20米一下25米一下又15米，你會希望他穩定都是20米 20 米 20米。
後續有空再來寫一篇非線性的Kalman Filter。
