""" 現在利用卡爾曼濾波對小車的運動狀態進行預測。主要流程如下所示：導入相應的工具包小車運動數據生成參數初始化利用卡爾曼濾波進行小車狀態預測可視化：觀察參數的變化與結果 """#導入包 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#小車運動數據生成 #在這里我們假設小車作速度為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#參數初始化 # 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.]])# 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()# 利用觀測值進行更新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 = [] 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.卡爾曼濾波器參數的變化 #首先定義方法將卡爾曼濾波器的參數堆疊成一個矩陣，右下角補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：小车匀速案例的全部內容，希望文章能夠幫你解決所遇到的問題。

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