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

权利要求书

1.一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于：

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

2.如权利要求1所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于，具体包括如下步骤：

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

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

建立量测方程：

其中，为k时刻的系统状态，Z_k为k时刻的系统状态的量测值；H为量测矩阵；

S2，给定卡尔曼滤波的初值b，k，其中，为第一次滤波时的状态，设为协方差矩阵，为量测噪声，为量测噪声协方差矩阵，为系统随机噪声，为系统随机噪声协方差矩阵；b为比例因子；k表示第k个滤波周期，k的初值为1；

进入迭代周期：

S3，计算k时刻的新息向量ε_k：

S4，计算k时刻的协方差矩阵P_k/k-1

其中，P_k-1为k-1时刻的协方差矩阵，P_k/k-1表示k时刻由k-1时刻的协方差矩阵估计的协方差矩阵；为k-1时刻的系统随机噪声协方差矩阵；G为噪声驱动转移矩阵；为噪声二阶矩阵；

S5，进行收敛判断，计算加权值S_k：

判断不等式

当不等式成立，则S_k＝1；

当不等式不成立，则

其中，为k-1时刻的系统随机噪声协方差矩阵，G为噪声驱动矩阵；为噪声二阶矩阵；R为量测噪声，为滤波的已知条件；P_k-1为k-1时刻的协方差矩阵；ξ略大于1，符号Tr[]表示矩阵的迹；

S6，代入加权值S_k，更新协方差矩阵P_k/k-1：

S7，根据P_k/k-1，计算卡尔曼增益K_k：

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R)^-1

S8，更新k时刻状态的最优估计值

S9，更新k时刻的协方差矩阵P_k：

P_k＝(I-K_kH)P_k/k-1

S10，计算遗忘因子d_k-1：

其中，k≥2；

S11，噪声二阶矩阵半正定判断：

当噪声二阶矩阵半正定时，其更新为：

否则，其更新为：

S12：k＝k+1，重复S2-S11。

3.如权利要求2所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于，所述状态的状态向量Z_k为：

其中，d_k-1为k-1时刻目标点与本车的纵向距离值，v_k-1为k-1时刻目标点与本车的纵向相对速度，a_k-1为k-1时刻目标点与本车的纵向相对加速度，aa_k-1为k-1时刻目标点与本车的纵向相对加速度的变化率；

所述状态转移矩阵A为：

所述量测矩阵H为：

所述噪声驱动矩阵G为：

其中，T为k-1时刻到k时刻的时间间隔。