""" 現在利用卡爾曼濾波對小車的運動狀態進行預測。主要流程如下所示：導入相應的工具包小車運動數據生成參數初始化利用卡爾曼濾波進行小車狀態預測可視化：觀察參數的變化與結果 """#導入包 from matplotlib import pyplot as plt import seaborn as sns import numpy as np from filterpy.kalman import KalmanFilter from pylab import mpl mpl.rcParams["font.sans-serif"] = ["SimHei"] #支持中文顯示 mpl.rcParams["axes.unicode_minus"] = False# u 即 加速度a u = 2 #小車運動數據生成 #在這里我們假設小車作加速度為2的勻加速運動 # 生成1000個位置，從1到1000，是小車的實際位置 z = np.array([0.5*u*(i**2) for i in range(1000)]) #位移距離公式：1/2 * 加速度a * 單位時間秒的平方。i相當于dt時間。 v = np.array([u*i for i in range(1000)]) #因為加速度a=dv/dt，所以dv=加速度a*dt。i相當于dt時間。 # 添加噪聲 mu,sigma = 0,1 noise = np.random.normal(mu,sigma,1000) # 小車位置的觀測值 z_nosie = z+noise#參數初始化 # dim_x 狀態向量size,在該例中為[p,v]，即位置和速度,size=2 # dim_z 測量向量size，假設小車為勻加速，速度為1，測量向量只觀測位置，size=1 my_filter = KalmanFilter(dim_x=2, dim_z=1)# 定義卡爾曼濾波中所需的參數 # x 初始狀態為[0,0],即初始位置為0，速度為0. # 這個初始值不是非常重要，在利用觀測值進行更新迭代后會接近于真實值 my_filter.x = np.array([[0.], [0.]])""" 1.u 即加速度a，亦即狀態控制向量。加速度a=dv/dtm/s^2：米每二次方秒，米除以秒的二次方。m*s^-2：米乘以秒的負二次方，負二次方表示二次方的倒數。 2.B 即狀態控制矩陣：狀態控制矩陣的公式為[[1/2 △t^2] [△t]]。那么傳入[0.5] 代表 1/2 △t^2 中的系數1/2 ，[1]就是△t中的系數1。△t為時間差，此處亦即為遍歷位置P的間隔數(步數)作為時間差. 3.初始化位移距離、速度#位移距離公式：1/2 * 加速度a * 單位時間秒的平方。下面的i相當于dt時間。z = np.array([0.5*u*(i**2) for i in range(1000)])#因為加速度a=dv/dt，所以dv=加速度a*dt。下面的i相當于dt時間。v = np.array([u*i for i in range(1000)]) 4.預測predict(u, B = np.array([[0.5],[1]]))也可以設置my_filter.B = np.array([[0.5],[1]])predict(u) """ # B 狀態控制矩陣：[0.5] 代表 1/2 △t^2 中的系數1/2 ，[1]就是△t中的系數1。△t為時間差，此處亦即為遍歷位置P的間隔數(步數)作為時間差 # my_filter.B = np.array([[0.5],[1]])# p 協方差矩陣，表示狀態向量內位置與速度的相關性 # 假設速度與位置沒關系，協方差矩陣為[[1,0],[0,1]] my_filter.P = np.array([[1., 0.], [0., 1.]])# F 初始的狀態轉移矩陣，假設為勻加速運動模型，可將其設為如下所示 my_filter.F = np.array([[1., 1.], [0., 1.]])# Q 狀態轉移協方差矩陣，也就是外界噪聲， # 在該例中假設小車勻加速，外界干擾小，所以我們對F非常確定，覺得F一定不會出錯，所以Q設的很小 my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])# 觀測矩陣 Hx = p # 利用觀測數據對預測進行更新，觀測矩陣的左邊一項不能設置成0 my_filter.H = np.array([[1, 0]]) # R 測量噪聲，方差為1 my_filter.R = 1#卡爾曼濾波進行預測 # 保存卡爾曼濾波過程中的位置和速度 z_new_list = []#位移的預測值 v_new_list = []#速度的預測值 # 對于每一個觀測值，進行一次卡爾曼濾波 for k in range(len(z_nosie)):# 預測過程my_filter.predict(u, B = np.array([[0.5],[1]]))# 利用觀測值進行更新my_filter.update(z_nosie[k])# do something with the outputx = my_filter.x# 收集卡爾曼濾波后的速度和位置信息z_new_list.append(x[0][0]) #位移的預測值v_new_list.append(x[1][0]) #速度的預測值#可視化 #預測誤差的可視化 # 位移的偏差 dif_list = [] #位移/位置(預測值，真實值)) dif_list_pair = [] for k in range(len(z)):dif_list.append(z_new_list[k]-z[k]) #位移的預測值 - 位移的真實值 = 位移的偏差dif_list_pair.append([z_new_list[k], z[k]]) #位移/位置(預測值，真實值)) # 速度的偏差 v_dif_list = [] #速度(預測值，真實值) v_dif_list_pair = [] for k in range(len(z)):v_dif_list.append(v_new_list[k]-v[k]) #速度的預測值 - 速度的真實值 = 速度的偏差v_dif_list_pair.append([v_new_list[k],v[k]]) #速度(預測值，真實值)print('位移/位置(預測值，真實值)):',dif_list_pair) print('位移偏差:',dif_list) print('速度(預測值，真實值):',v_dif_list_pair) print('速度偏差:',v_dif_list)plt.figure(figsize=(20,9)) plt.subplot(1,1,1) plt.xlim(-50,1050) plt.ylim(-3.0,3.0) plt.scatter(range(len(z)),dif_list,color ='b',label = "位置偏差") plt.scatter(range(len(z)),v_dif_list,color ='y',label = "速度偏差") plt.legend() plt.show()plt.figure(figsize=(20, 8)) plt.xlim(-5, 100+5) plt.ylim(-3, +5) plt.subplot(3, 2, 1) plt.scatter(range(len(z)), dif_list, color='b', label="位置偏差") plt.plot(range(len(z)),[0]*len(z),color='r') plt.legend() plt.subplot(3, 2, 2) plt.scatter(range(len(z)), v_dif_list, color='y', label="速度偏差") plt.plot(range(len(z)),[0]*len(z),color='r') plt.legend() plt.subplot(3, 2, 3) plt.scatter(range(len(z)), z_nosie, color='b', label="真實位置") plt.legend() plt.subplot(3, 2, 4) plt.scatter(range(len(z)), v, color='y', label="真實速度") plt.legend() plt.subplot(3, 2, 5) plt.scatter(range(len(z)), z_new_list, color='b', label="預測位置") plt.legend() plt.subplot(3, 2, 6) plt.scatter(range(len(z)), v_new_list, color='y', label="預測速度") plt.legend() plt.show()#2.卡爾曼濾波器參數的變化 #首先定義方法將卡爾曼濾波器的參數堆疊成一個矩陣，右下角補0，我們看一下參數的變化。 # 定義一個方法將卡爾曼濾波器的參數堆疊成一個矩陣，右下角補0 def filter_comb(p, f, q, h, r):a = np.hstack((p, f))b = np.array([r, 0])b = np.vstack([h, b])b = np.hstack((q, b))a = np.vstack((a, b))return a#對參數變化進行可視化： # 保存卡爾曼濾波過程中的位置和速度 z_new_list = [] v_new_list = [] # 對于每一個觀測值，進行一次卡爾曼濾波 for k in range(1):# 預測過程my_filter.predict()# 利用觀測值進行更新my_filter.update(z_nosie[k])# do something with the outputx = my_filter.xc = filter_comb(my_filter.P,my_filter.F,my_filter.Q,my_filter.H,my_filter.R)plt.figure(figsize=(32,18))sns.set(font_scale=4)#sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels==False,cbar=False)sns.heatmap(c,square=True,annot=True,xticklabels=False,yticklabels=False,cbar=False)#從圖中可以看出變化的P，其他的參數F，Q，H,R為變換。另外狀態變量x和卡爾曼系數K也是變化的。 #3.概率密度函數 #為了驗證卡爾曼濾波的結果優于測量的結果，繪制預測結果誤差和測量誤差的概率密度函數： # 生成概率密度圖像 z_noise_list_std = np.std(noise) z_noise_list_avg = np.mean(noise) z_filterd_list_std = np.std(dif_list)import seaborn as sns plt.figure(figsize=(16,9)) ax = sns.kdeplot(noise,shade=True,color="r",label="std=%.3f"%z_noise_list_std) ax = sns.kdeplot(dif_list,shade=True,color="g",label="std=%.3f"%z_filterd_list_std) plt.show()

輸出：

總結

以上是生活随笔為你收集整理的智慧交通day02-车流量检测实现05：小车匀加速案例的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。