[发明专利]基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法

说明书

本发明公开了一种基于Sage‑Husa卡尔曼滤波的车辆目标跟踪算法，采用Sage‑Husa自适应卡尔曼滤波，在每个迭代周期，进行收敛判断，更新加权值S_k和协方差矩阵P_k/k‑1：(1)通过发散判据‑不等式是否成立判断滤波是否发散，在滤波发散时，修正加权值S_k，间接调整协方差矩阵P_k/k‑1，可有效防止滤波发散；(2)对噪声二阶矩阵进行正定性判断，当噪声二阶矩阵失去正定性时，采用噪声二阶矩阵的有偏估值器代替原无偏估值器，阻止滤波发散。从而解决卡尔曼滤波的滤波发散、算法失效的问题。

技术领域

本发明涉及车辆目标跟踪技术领域，尤其涉及一种基于Sage-Husa自适应卡尔曼滤波的车辆目标跟踪算法。

背景技术

自动紧急制动系统(AEB)需要毫米波雷达采集前方车辆的位置、速度和加速度等参数，但是实际情况下，因为环境、雷达自身的测量精度、车辆行驶环境等因素的影响下，毫米波雷达采集到的数据需要滤波后才能对目标准确估计。

经典卡尔曼滤波要求有精确已知系统的数学模型及其参数和噪声统计特性两者的先验信息，然而由于系统建模时的状态转移方程具有一定的误差，该误差包括非线性系统线性化时的误差、模型的简化误差及建模方法的误差等等，因此在实际情况下，系统噪声协方差矩阵Q_k和量测噪声协方差矩阵R_k难以事先确定。这些问题将导致经典卡尔曼滤波的精度下降甚至是发散，因而有必要对此情况作改进。

Sage-Husa自适应卡尔曼滤波在进行滤波计算的递推过程中，利用实际测量数据提供的信息实时估计系统状态向量，同时也对位置的或不确切知道的系统模型参数和噪声统计参数进行估计和修正。这一位置，自适应卡尔曼滤波利用前K个数据对系统状态X_k进行估计得到同时利用这些测量数据及噪声统计公式对系统未知的、时变的系统噪声统计q_k、Q_k和未知的、时变的量测噪声统计r_k、R_k进行估计，并获得噪声统计的估计值代替初值。在虚拟噪声补偿技术的思想下，一般的自适应卡尔曼滤波主要应用在系统模型参数已知(即系统模型准确)，而噪声统计参数未知的或不确切、时变或时不变的场合，包括贝叶斯法、极大似然法、相关法、协方差法和Sage-Husa法等，其中Sage和Husa提出的次优无偏极大厚颜(MAP)噪声统计估值器计算简单，可同时估计出系统噪声和联测噪声的一阶矩(均值)和二阶矩(协方差)，应用较为广泛。

发明内容

有鉴于现有技术的上述缺陷，本发明的目的是提供一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，以解决经典卡尔曼滤波下，没有对滤波是否发散进行监控，发生滤波发散，算法失效的问题，本专利对滤波是否发散进行了监控，在出现隐患时，调整部分参数使滤波不易发散。

为实现上述目的，本发明提供了一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，采用Sage-Husa自适应卡尔曼滤波，并在每个迭代周期，计算新息向量ε_k，进行收敛判断，更新加权值S_k和协方差矩阵P_k/k-1：(1)通过发散判据-不等式是否成立判断滤波是否发散，在滤波发散时，修正加权值S_k，间接调整协方差矩阵P_k/k-1，可有效防止滤波发散；(2)对噪声二阶矩阵进行正定性判断，当噪声二阶矩阵失去正定性时，采用噪声二阶矩阵的有偏估值器代替原无偏估值器，阻止滤波发散。

进一步的，所述基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，具体包括如下步骤：

S1，根据车辆目标跟踪的状态参数，建立卡尔曼滤波状态转移方程：

其中，为k-1时刻的系统状态的最优估计值A为状态转移矩阵；表示由k-1时刻的系统状态的最优估计值预测的k时刻的系统状态；