""" 現(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#小車運動數(shù)據(jù)生成 #在這里我們假設(shè)小車作速度為1的勻速運動 # 生成1000個位置，從1到1000，是小車的實際位置 z = np.linspace(1,1000,1000) # 添加噪聲 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.]])# 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 = [] v_new_list = [] # 對于每一個觀測值，進行一次卡爾曼濾波 for k in range(len(z_nosie)):# 預(yù)測過程my_filter.predict()# 利用觀測值進行更新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])#可視化 #預(yù)測誤差的可視化 # 位移的偏差 dif_list = [] for k in range(len(z)):dif_list.append(z_new_list[k]-z[k]) # 速度的偏差 v_dif_list = [] for k in range(len(z)):v_dif_list.append(v_new_list[k]-1)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()#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()

輸出：

創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎勵來咯，堅持創(chuàng)作打卡瓜分現(xiàn)金大獎

總結(jié)

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

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