IMM-EKF结合应用对车辆轨迹数据进行航迹分类与跟踪，需要车辆XY坐标与速度、时间信息

一、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