IMM-EKF结合应用对车辆轨迹数据进行航迹分类与跟踪,需要车辆XY坐标与速度、时间信息
2023-12-29 14:21:06
一、IMM-EKF原理
1.1IMM
IMM核心思想是用多个不同的运动模型来匹配机动目标的不同运动模型,说简单点便是匹配运动状态,知道你在干嘛。不同模型之间的转移概率是一个马尔可夫矩阵,目标的状态和模型概率的更新采用卡尔曼滤波或其它滤波。
简单介绍一下IMM算法流程:
第一步:输入交互
第二步:滤波模型
第三步:模型概率更新
第四步:输出交互
想看每一步具体的步骤与具体公式可以私聊我。
1.2EKF
EKF-拓展卡尔曼滤波,由于车辆轨迹数据是非线性的,采用卡尔曼滤波拟合效果不好,因此需要采用EKF或者UKF-无迹卡尔曼滤波。EKF核心思想便是将非线性函数以一阶泰勒公式展开进行线性化,关键在于构造雅可比矩阵,其余跟卡尔曼滤波步骤几乎一模一样。
二、车辆数据特点
车辆数据采集来自于法国街道,分两个时间段采集,早晨7-9点采集一次,晚上17点到19点采集一次,数据量大,达百万级以上,通过观察数据特点,适合用于分析数据使用。以下是提取某个车辆所有数据部分截图。
三、采用模型
1.CV模型
匀速直线模型,最经典的模型之一,适合于车辆平稳行驶
2.CA模型
匀变速直线模型,适合于车辆加速与减速
3.CT模型
转弯模型,适用于车辆转弯运动
四、实验结果
直接上图:三张图合为一
五、部分代码
以下是IMM-EKF部分代码,有需要完整代码的可以私聊我。
def ekf_predict(state, state_covariance, transition_matrix, process_noise_cov):
predicted_state = np.dot(transition_matrix, state)
predicted_covariance = np.dot(np.dot(transition_matrix, state_covariance), transition_matrix.T) + process_noise_cov
return predicted_state, predicted_covariance
def ekf_update(predicted_state, predicted_covariance, measurement, measurement_matrix, measurement_noise_cov):
kalman_gain = np.dot(np.dot(predicted_covariance, measurement_matrix.T),
np.linalg.inv(np.dot(np.dot(measurement_matrix, predicted_covariance),
measurement_matrix.T) + measurement_noise_cov))
residual = measurement - np.dot(measurement_matrix, predicted_state)
updated_state = predicted_state + np.dot(kalman_gain, residual)
updated_covariance = predicted_covariance - np.dot(np.dot(kalman_gain, measurement_matrix), predicted_covariance)
return updated_state, updated_covariance
文章来源:https://blog.csdn.net/beiweixiazairuan/article/details/135165144
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符,请联系我的编程经验分享网邮箱:veading@qq.com进行投诉反馈,一经查实,立即删除!
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符,请联系我的编程经验分享网邮箱:veading@qq.com进行投诉反馈,一经查实,立即删除!