智慧交通day02-车流量检测实现05：小车匀加速案例

"""
现在利用卡尔曼滤波对小车的运动状态进行预测。主要流程如下所示：导入相应的工具包小车运动数据生成参数初始化利用卡尔曼滤波进行小车状态预测可视化：观察参数的变化与结果
"""#导入包
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：小车匀加速案例相关推荐

智慧交通day02-车流量检测实现05：小车匀速案例
""" 现在利用卡尔曼滤波对小车的运动状态进行预测.主要流程如下所示:导入相应的工具包小车运动数据生成参数初始化利用卡尔曼滤波进行小车状态预测可视化:观察参数的变化与结果 ...
智慧交通day02-车流量检测实现05：卡尔曼滤波器实践(小车模型)
1.filterpy FilterPy是一个实现了各种滤波器的Python模块,它实现著名的卡尔曼滤波和粒子滤波器.我们可以直接调用该库完成卡尔曼滤波器实现.其中的主要模块包括: filterpy.k ...
智慧交通day02-车流量检测实现01：总览
随着城市交通量的迅猛增加,车流量统计已成为智能交通系统中一项关键技术和热门研究方向.高效而精确的车流量检测可以交通管理者和决策者,以及驾驶员提供数据支撑,从而为交通调度,降低拥堵情况的发生,提高道路利 ...
智慧交通day02-车流量检测实现04：卡尔曼滤波器
1.背景介绍卡尔曼滤波(Kalman)无论是在单目标还是多目标领域都是很常用的一种算法,我们将卡尔曼滤波看做一种运动模型,用来对目标的位置进行预测,并且利用预测结果对跟踪的目标进行修正,属于自动控制 ...
智慧交通day02-车流量检测实现14：代码汇总+问题修正
代码+权重文件+资源https://download.csdn.net/download/qq_39237205/43072746https://download.csdn.net/download/ ...
智慧交通day02-车流量检测实现12：基于yoloV3的目标检测
在本章节代码编写中,发现之前的代码所处的环境是python3,因此导致了cv2.dnn.readNetFromDarknet()在代码运行中导致了i[0]的获值失败,故总结如下: cv2.dnn.re ...
智慧交通day02-车流量检测实现11：yoloV3模型
yoloV3以V1,V2为基础进行的改进,主要有:利用多尺度特征进行目标检测:先验框更丰富:调整了网络结构:对象分类使用logistic代替了softmax,更适用于多标签分类任务. 1.算法简介 Y ...
智慧交通day02-车流量检测实现10：多目标追踪实现
在这里我们主要实现了一个多目标跟踪器,管理多个卡尔曼滤波器对象,主要包括以下内容: 初始化:最大检测数,目标未被检测的最大帧数目标跟踪结果的更新,即跟踪成功和失败的目标的更新初始化 def __i ...
智慧交通day02-车流量检测实现09：SORT/deepSORT
SORT和DeepSORT是多目标跟踪中两个知名度比较高的算法.DeepSORT是原团队对SORT的改进版本.现在来解析一下SORT和DeepSORT的基本思路. 1.SORT SORT核心是卡尔曼滤 ...

智慧交通day02-车流量检测实现05：小车匀加速案例

智慧交通day02-车流量检测实现05：小车匀加速案例相关推荐

最新文章

热门文章