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
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。