""" 現(xiàn)在利用卡爾曼濾波對小車的運動狀態(tài)進行預(yù)測。主要流程如下所示：導(dǎo)入相應(yīng)的工具包小車運動數(shù)據(jù)生成參數(shù)初始化利用卡爾曼濾波進行小車狀態(tài)預(yù)測可視化：觀察參數(shù)的變化與結(jié)果 """#導(dǎo)入包 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 #小車運動數(shù)據(jù)生成 #在這里我們假設(shè)小車作加速度為2的勻加速運動 # 生成1000個位置，從1到1000，是小車的實際位置 z = np.array([0.5*u*(i**2) for i in range(1000)]) #位移距離公式：1/2 * 加速度a * 單位時間秒的平方。i相當(dāng)于dt時間。 v = np.array([u*i for i in range(1000)]) #因為加速度a=dv/dt，所以dv=加速度a*dt。i相當(dāng)于dt時間。 # 添加噪聲 mu,sigma = 0,1 noise = np.random.normal(mu,sigma,1000) # 小車位置的觀測值 z_nosie = z+noise#參數(shù)初始化 # dim_x 狀態(tài)向量size,在該例中為[p,v]，即位置和速度,size=2 # dim_z 測量向量size，假設(shè)小車為勻加速，速度為1，測量向量只觀測位置，size=1 my_filter = KalmanFilter(dim_x=2, dim_z=1)# 定義卡爾曼濾波中所需的參數(shù) # x 初始狀態(tài)為[0,0],即初始位置為0，速度為0. # 這個初始值不是非常重要，在利用觀測值進行更新迭代后會接近于真實值 my_filter.x = np.array([[0.], [0.]])""" 1.u 即加速度a，亦即狀態(tài)控制向量。加速度a=dv/dtm/s^2：米每二次方秒，米除以秒的二次方。m*s^-2：米乘以秒的負二次方，負二次方表示二次方的倒數(shù)。 2.B 即狀態(tài)控制矩陣：狀態(tài)控制矩陣的公式為[[1/2 △t^2] [△t]]。那么傳入[0.5] 代表 1/2 △t^2 中的系數(shù)1/2 ，[1]就是△t中的系數(shù)1?！鱰為時間差，此處亦即為遍歷位置P的間隔數(shù)(步數(shù))作為時間差. 3.初始化位移距離、速度#位移距離公式：1/2 * 加速度a * 單位時間秒的平方。下面的i相當(dāng)于dt時間。z = np.array([0.5*u*(i**2) for i in range(1000)])#因為加速度a=dv/dt，所以dv=加速度a*dt。下面的i相當(dāng)于dt時間。v = np.array([u*i for i in range(1000)]) 4.預(yù)測predict(u, B = np.array([[0.5],[1]]))也可以設(shè)置my_filter.B = np.array([[0.5],[1]])predict(u) """ # B 狀態(tài)控制矩陣：[0.5] 代表 1/2 △t^2 中的系數(shù)1/2 ，[1]就是△t中的系數(shù)1?！鱰為時間差，此處亦即為遍歷位置P的間隔數(shù)(步數(shù))作為時間差 # my_filter.B = np.array([[0.5],[1]])# p 協(xié)方差矩陣，表示狀態(tài)向量內(nèi)位置與速度的相關(guān)性 # 假設(shè)速度與位置沒關(guān)系，協(xié)方差矩陣為[[1,0],[0,1]] my_filter.P = np.array([[1., 0.], [0., 1.]])# F 初始的狀態(tài)轉(zhuǎn)移矩陣，假設(shè)為勻加速運動模型，可將其設(shè)為如下所示 my_filter.F = np.array([[1., 1.], [0., 1.]])# Q 狀態(tài)轉(zhuǎn)移協(xié)方差矩陣，也就是外界噪聲， # 在該例中假設(shè)小車勻加速，外界干擾小，所以我們對F非常確定，覺得F一定不會出錯，所以Q設(shè)的很小 my_filter.Q = np.array([[0.0001, 0.], [0., 0.0001]])# 觀測矩陣 Hx = p # 利用觀測數(shù)據(jù)對預(yù)測進行更新，觀測矩陣的左邊一項不能設(shè)置成0 my_filter.H = np.array([[1, 0]]) # R 測量噪聲，方差為1 my_filter.R = 1#卡爾曼濾波進行預(yù)測 # 保存卡爾曼濾波過程中的位置和速度 z_new_list = []#位移的預(yù)測值 v_new_list = []#速度的預(yù)測值 # 對于每一個觀測值，進行一次卡爾曼濾波 for k in range(len(z_nosie)):# 預(yù)測過程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]) #位移的預(yù)測值v_new_list.append(x[1][0]) #速度的預(yù)測值#可視化 #預(yù)測誤差的可視化 # 位移的偏差 dif_list = [] #位移/位置(預(yù)測值，真實值)) dif_list_pair = [] for k in range(len(z)):dif_list.append(z_new_list[k]-z[k]) #位移的預(yù)測值 - 位移的真實值 = 位移的偏差dif_list_pair.append([z_new_list[k], z[k]]) #位移/位置(預(yù)測值，真實值)) # 速度的偏差 v_dif_list = [] #速度(預(yù)測值，真實值) v_dif_list_pair = [] for k in range(len(z)):v_dif_list.append(v_new_list[k]-v[k]) #速度的預(yù)測值 - 速度的真實值 = 速度的偏差v_dif_list_pair.append([v_new_list[k],v[k]]) #速度(預(yù)測值，真實值)print('位移/位置(預(yù)測值，真實值)):',dif_list_pair) print('位移偏差:',dif_list) print('速度(預(yù)測值，真實值):',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="預(yù)測位置") plt.legend() plt.subplot(3, 2, 6) plt.scatter(range(len(z)), v_new_list, color='y', label="預(yù)測速度") plt.legend() plt.show()#2.卡爾曼濾波器參數(shù)的變化 #首先定義方法將卡爾曼濾波器的參數(shù)堆疊成一個矩陣，右下角補0，我們看一下參數(shù)的變化。 # 定義一個方法將卡爾曼濾波器的參數(shù)堆疊成一個矩陣，右下角補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#對參數(shù)變化進行可視化： # 保存卡爾曼濾波過程中的位置和速度 z_new_list = [] v_new_list = [] # 對于每一個觀測值，進行一次卡爾曼濾波 for k in range(1):# 預(yù)測過程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，其他的參數(shù)F，Q，H,R為變換。另外狀態(tài)變量x和卡爾曼系數(shù)K也是變化的。 #3.概率密度函數(shù) #為了驗證卡爾曼濾波的結(jié)果優(yōu)于測量的結(jié)果，繪制預(yù)測結(jié)果誤差和測量誤差的概率密度函數(shù)： # 生成概率密度圖像 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()

輸出：

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。