CN110907911A - 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法 - Google Patents

基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法 Download PDF

Info

Publication number
CN110907911A
CN110907911A CN201911196442.7A CN201911196442A CN110907911A CN 110907911 A CN110907911 A CN 110907911A CN 201911196442 A CN201911196442 A CN 201911196442A CN 110907911 A CN110907911 A CN 110907911A
Authority
CN
China
Prior art keywords
matrix
noise
moment
filtering
time
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911196442.7A
Other languages
English (en)
Inventor
欧阳祥文
冯作居
苏亮
陈卫强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xiamen King Long United Automotive Industry Co Ltd
Original Assignee
Xiamen King Long United Automotive Industry Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xiamen King Long United Automotive Industry Co Ltd filed Critical Xiamen King Long United Automotive Industry Co Ltd
Priority to CN201911196442.7A priority Critical patent/CN110907911A/zh
Publication of CN110907911A publication Critical patent/CN110907911A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/41—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

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

Description

基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法
技术领域
本发明涉及车辆目标跟踪技术领域，尤其涉及一种基于Sage-Husa自适应卡尔曼滤波的车辆目标跟踪算法。
背景技术
自动紧急制动系统(AEB)需要毫米波雷达采集前方车辆的位置、速度和加速度等参数，但是实际情况下，因为环境、雷达自身的测量精度、车辆行驶环境等因素的影响下，毫米波雷达采集到的数据需要滤波后才能对目标准确估计。
经典卡尔曼滤波要求有精确已知系统的数学模型及其参数和噪声统计特性两者的先验信息，然而由于系统建模时的状态转移方程具有一定的误差，该误差包括非线性系统线性化时的误差、模型的简化误差及建模方法的误差等等，因此在实际情况下，系统噪声协方差矩阵Qk和量测噪声协方差矩阵Rk难以事先确定。这些问题将导致经典卡尔曼滤波的精度下降甚至是发散，因而有必要对此情况作改进。
Sage-Husa自适应卡尔曼滤波在进行滤波计算的递推过程中，利用实际测量数据提供的信息实时估计系统状态向量，同时也对位置的或不确切知道的系统模型参数和噪声统计参数进行估计和修正。这一位置，自适应卡尔曼滤波利用前K个数据对系统状态Xk进行估计得到
Figure BDA0002294766800000011
同时利用这些测量数据及噪声统计公式对系统未知的、时变的系统噪声统计qk、Qk和未知的、时变的量测噪声统计rk、Rk进行估计，并获得噪声统计的估计值
Figure BDA0002294766800000012
代替初值。在虚拟噪声补偿技术的思想下，一般的自适应卡尔曼滤波主要应用在系统模型参数已知(即系统模型准确)，而噪声统计参数
Figure BDA0002294766800000013
未知的或不确切、时变或时不变的场合，包括贝叶斯法、极大似然法、相关法、协方差法和Sage-Husa法等，其中Sage和Husa提出的次优无偏极大厚颜(MAP)噪声统计估值器计算简单，可同时估计出系统噪声和联测噪声的一阶矩(均值)和二阶矩(协方差)，应用较为广泛。
发明内容
有鉴于现有技术的上述缺陷，本发明的目的是提供一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，以解决经典卡尔曼滤波下，没有对滤波是否发散进行监控，发生滤波发散，算法失效的问题，本专利对滤波是否发散进行了监控，在出现隐患时，调整部分参数使滤波不易发散。
为实现上述目的，本发明提供了一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，采用Sage-Husa自适应卡尔曼滤波，并在每个迭代周期，计算新息向量εk，进行收敛判断，更新加权值Sk和协方差矩阵Pk/k-1：(1)通过发散判据-不等式
Figure BDA0002294766800000021
是否成立判断滤波是否发散，在滤波发散时，修正加权值Sk，间接调整协方差矩阵Pk/k-1，可有效防止滤波发散；(2)对噪声二阶矩阵
Figure BDA0002294766800000022
进行正定性判断，当噪声二阶矩阵
Figure BDA0002294766800000023
失去正定性时，采用噪声二阶矩阵
Figure BDA0002294766800000024
的有偏估值器代替原无偏估值器，阻止滤波发散。
进一步的，所述基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，具体包括如下步骤：
S1，根据车辆目标跟踪的状态参数，建立卡尔曼滤波状态转移方程：
Figure BDA0002294766800000025
其中，
Figure BDA0002294766800000026
为k-1时刻的系统状态的最优估计值
Figure BDA0002294766800000027
A为状态转移矩阵；
Figure BDA0002294766800000028
表示由k-1时刻的系统状态的最优估计值
Figure BDA0002294766800000029
预测的k时刻的系统状态；
建立量测方程：
Figure BDA0002294766800000031
其中，
Figure BDA0002294766800000032
为k时刻的系统状态，Zk为k时刻的系统状态的量测值；H为量测矩阵；
S2，给定卡尔曼滤波的初值
Figure BDA0002294766800000033
b，k，其中，
Figure BDA0002294766800000034
为第一次滤波时的状态，
Figure BDA0002294766800000035
为协方差矩阵，
Figure BDA0002294766800000036
为量测噪声，
Figure BDA0002294766800000037
为量测噪声协方差矩阵，
Figure BDA0002294766800000038
为系统随机噪声，
Figure BDA0002294766800000039
为系统随机噪声协方差矩阵；b为比例因子；k表示第k个滤波周期，k的初值为1；
进入迭代周期：
S3，读取k时刻的系统状态的量测值Zk；
S4，计算k时刻的新息向量εk：
Figure BDA00022947668000000310
S5，计算k时刻的协方差矩阵Pk/k-1：
Figure BDA00022947668000000311
其中，Pk/k-1为k时刻由k-1时刻的协方差矩阵预测的协方差矩阵；
Figure BDA00022947668000000312
表示第k-1周期的系统随机噪声协方差矩阵；G为噪声驱动矩阵,；
Figure BDA00022947668000000313
为噪声二阶矩阵；
S6，进行收敛判断，计算加权值Sk：
判断不等式
Figure BDA00022947668000000314
当不等式成立，则Sk＝1；
当不等式不成立，则
Figure BDA00022947668000000315
其中，
Figure BDA00022947668000000316
为k-1时刻的系统随机噪声协方差矩阵，G为噪声驱动矩阵；
Figure BDA00022947668000000317
为噪声二阶矩阵；R为量测噪声，为滤波的已知条件；Pk-1为k-1时刻的协方差矩阵；ξ略大于1，符号Tr[]表示矩阵的迹；
S7，代入加权值Sk，更新协方差矩阵一步预测估计值Pk/k-1：
Figure BDA0002294766800000041
S8，根据Pk/k-1计算卡尔曼增益Kk：
Kk＝Pk/k-1HT(HPk/k-1HT+R)-1
S9，更新k时刻状态的最优估计值
Figure BDA0002294766800000042
Figure BDA0002294766800000043
S10，更新k时刻的协方差矩阵Pk：
Pk＝(I-KkH)Pk/k-1
S11，计算遗忘因子dk-1：
Figure BDA0002294766800000044
其中，k≥2；
S12，噪声二阶矩阵
Figure BDA0002294766800000045
半正定判断：
当噪声二阶矩阵
Figure BDA0002294766800000046
半正定时，其更新为：
Figure BDA0002294766800000047
否则，其更新为：
Figure BDA0002294766800000048
S13：k＝k+1，重复S2-S12。
进一步的，所述状态的状态向量Zk为：
Figure BDA0002294766800000049
其中，dk为k时刻目标点与本车的纵向距离值，vk为k时刻目标点与本车的纵向相对速度，ak为k时刻目标点与本车的纵向相对加速度，aak为k时刻点与本车的纵向相对加速度的变化率；
所述状态转移矩阵A为：
Figure BDA0002294766800000051
所述量测矩阵H为：
Figure BDA0002294766800000052
所述噪声驱动矩阵G为：
Figure BDA0002294766800000053
其中，T为k-1时刻到k时刻的时间间隔。
进一步的，所述S2中的
Figure BDA0002294766800000054
为：
Figure BDA0002294766800000055
进一步的，所述S11中的遗忘因子b的取值范围为0.9～0.95。
进一步的，所述ξ的取值范围为：1～1.2。
进一步的，本发明还提供了一种车辆目标跟踪系统，包括毫米波雷达和电子控制单元；所述毫米波雷达用于实时采集k时刻的状态向量Zk的相关数据；所述电子控制单元接收该相关数据，并运行如上所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法进行迭代，获取k时刻状态的最优估计值
Figure BDA0002294766800000056
进一步的，本发明还提供了一种存储介质，存储有一段计算机程序，所述计算机程序可被调入电子控制单元运行如上所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，获取k时刻状态的最优估计值
Figure BDA0002294766800000057
本发明实现了如下有益效果：
本发明的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，在迭代周期中，通过在卡尔曼滤波的预测阶段和更新阶段对滤波是否发散进行监控，并及时调整参数，以解决滤波发散，算法失效的问题。
附图说明
图1是本发明的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法；
图2是采用Sage-Husa自适应卡尔曼滤波、经典卡尔曼滤波和真实目标数据的对比结果；
图3是图2的局部放大图。
具体实施方式
为进一步说明各实施例，本发明提供有附图。这些附图为本发明揭露内容的一部分，其主要用以说明实施例，并可配合说明书的相关描述来解释实施例的运作原理。配合参考这些内容，本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点。图中的组件并未按比例绘制，而类似的组件符号通常用来表示类似的组件。
现结合附图和具体实施方式对本发明进一步说明。
如图1所示的是本发明的一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法流程图，包括以下步骤：
S1，根据车辆目标跟踪的状态参数，建立卡尔曼滤波转移方程：
Figure BDA0002294766800000061
其中，
Figure BDA0002294766800000062
为k-1时刻的系统状态的最优估计值
Figure BDA0002294766800000063
A为状态转移矩阵；
Figure BDA0002294766800000064
表示由k-1时刻的系统状态的最优估计值
Figure BDA0002294766800000065
预测的k时刻的系统状态；
建立量测方程：
Figure BDA0002294766800000066
其中，
Figure BDA0002294766800000067
为k时刻的系统状态，Zk为k时刻的系统状态的量测值；H为量测矩阵；
在本发明中，车辆的目标跟踪采用毫米波雷达读取车辆和前方或四周目标的相对距离、相对速度和相对加速度等信息，因此，系统状态的量测值Zk为：
Figure BDA0002294766800000071
状态转移矩阵A为：
Figure BDA0002294766800000072
量测矩阵H为：
Figure BDA0002294766800000073
其中，T表示k时刻和k-1时刻的时间间隔；dk为k时刻目标点与本车的纵向距离值，vk为k时刻目标点与本车的纵向相对速度，ak为k时刻目标点与本车的纵向相对加速度，aak为k时刻点与本车的纵向相对加速度的变化率。
S2，给出滤波所需初值
Figure BDA0002294766800000074
P0，
Figure BDA0002294766800000075
b，k，其中，
Figure BDA0002294766800000076
设为第一次上电时刻(k＝1)的状态，
Figure BDA0002294766800000077
为协方差矩阵Pk的初值，设置为单位矩阵，
Figure BDA0002294766800000078
为噪声协方差矩阵R的初值；
Figure BDA0002294766800000079
为系统随机噪声协方差矩阵
Figure BDA00022947668000000710
的初值；其它噪声初值可设为0；
Figure BDA00022947668000000711
为与噪声有关的矩阵或向量，分别为k＝1时刻的量测噪声，量测噪声协方差矩阵，系统随机噪声，系统随机噪声协方差矩阵的初值，b为比例因子，用以计算权值，k用以表示第k个滤波周期。
进入迭代周期：
S3，读取k时刻的系统状态的量测值Zk；
S4，计算k时刻的新息：
Figure BDA00022947668000000712
其中，εk为新息值，表示k时刻的状态的预测值和量测值的残差，表示两者误差的大小，Zk为k时刻的状态的量测值。
S5，预测k时刻的协方差矩阵Pk/k-1：
Figure BDA0002294766800000081
其中，Pk-1为k-1时刻的协方差矩阵，Pk/k-1为k时刻由k-1时刻的协方差矩阵预测的协方差矩阵，
Figure BDA0002294766800000082
为k-1时刻的系统随机噪声协方差矩阵；
Figure BDA0002294766800000083
Figure BDA0002294766800000084
为噪声驱动转移矩阵。
S6，进行收敛判断，计算加权值Sk：
判断不等式
Figure BDA0002294766800000085
当不等式成立，则滤波不发散，Sk＝1；
反之，当不等式不成立，表示滤波发散，更新Sk：
Figure BDA0002294766800000086
其中，ξ＝1时，为最严格的发散判据形式，在实际应用中，ξ略大于1，防止误判，优选的ξ的取值范围为：1～1.2；R为量测噪声协方差矩阵；符号Tr[]表示矩阵的迹。
S7：代入加权值Sk，更新协方差矩阵Pk/k-1：
Figure BDA0002294766800000087
S8：计算卡尔曼增益Kk：
Kk＝Pk/k-1HT(HPk/k-1HT+R)-1
其中，Kk为卡尔曼增益，表征一步预测值与两侧之两者在滤波输出值中的比例关系。
S9，更新k时刻系统状态的最优估计值
Figure BDA0002294766800000088
Figure BDA0002294766800000089
S10，更新k时刻的协方差矩阵Pk：
Pk＝(I-KkH)Pk/k-1
S11，计算遗忘因子dk-1：
Figure BDA0002294766800000091
其中，参数b为比例因子，取值范围为0.9～0.95，计算遗忘因子dk-1用来表示计算k时刻噪声时k-1时刻噪声值所占的比例。
S12，噪声二阶矩阵
Figure BDA0002294766800000092
半正定判断：
当
Figure BDA0002294766800000093
半正定时，
Figure BDA0002294766800000094
采用无偏估值器进行估算，其更新为：
Figure BDA0002294766800000095
否则，
Figure BDA0002294766800000096
采用有偏估值器进行估算，其更新为：
Figure BDA0002294766800000097
【半正定矩阵定义】
设A为实对称矩阵，若对于每个非零实向量X，都有X'AX≥0，则称A为半正定矩阵，称X'AX为半正定二次型。(其中，X'表示X的转置。)
一个n阶的实对称矩阵M是正定的的条件是当且仅当对于所有的非零实系数向量z，都有zTMz>0。其中zT表示z的转置。
S13：重复步骤3-步骤12。
本发明的实施例采用Sage-Husa自适应卡尔曼滤波，在每个迭代周期，进行收敛判断，更新加权值Sk和系统随机噪声协方差矩阵
Figure BDA0002294766800000098
(1)通过发散判据-不等式
Figure BDA0002294766800000099
是否成立判断滤波是否发散，在滤波发散时，修正Sk，间接调整Pk/k-1，可有效防止滤波发散；(2)对
Figure BDA00022947668000000910
进行正定性判断，当
Figure BDA00022947668000000911
失去正定性时，采用
Figure BDA00022947668000000912
的有偏估值器代替原无偏估值器，阻止滤波发散。从而解决卡尔曼滤波的滤波发散、算法失效的问题。
仿真及结果：
如图2和图3所示，采用Sage-Husa自适应卡尔曼滤波获得的预期值，较经典卡尔曼滤波明显更接近目标的真实距离，采用Sage-Husa自适应卡尔曼滤波获得的预期值的曲线围绕真实距离的曲线波动，可见收敛有效。
本发明还公开了一种车辆目标跟踪系统，包括毫米波雷达和电子控制单元；毫米波雷达实时采集车辆前方的目标数据，所述目标数据为当前时刻(k时刻)的状态向量Zk的相关数据，并将目标数据输入电子控制单元；所述电子控制单元运行如上所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，进行迭代，获取k时刻状态的最优估计值
Figure BDA0002294766800000101
本发明还公开了一种存储介质，该存储介质可以是电子控制单元的内部存储器，也可以是电子控制单元外部的存储器，所述的存储器为Flash存储器。该存储器存储有一段计算机程序，所述计算机程序可被调入电子控制单元运行如上所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，进行迭代，获取k时刻状态的最优估计值
Figure BDA0002294766800000102
尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式上和细节上可以对本发明做出各种变化，均为本发明的保护范围。

Claims (8)

1.一种基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于：
采用Sage-Husa自适应卡尔曼滤波，并在每个迭代周期，计算新息向量εk，进行收敛判断，更新加权值Sk和协方差矩阵Pk/k-1：(1)通过发散判据-不等式
Figure FDA0002294766790000011
是否成立判断滤波是否发散，在滤波发散时，修正加权值Sk，间接调整协方差矩阵Pk/k-1，可有效防止滤波发散；(2)对噪声二阶矩阵
Figure FDA0002294766790000012
进行正定性判断，当噪声二阶矩阵
Figure FDA0002294766790000013
失去正定性时，采用噪声二阶矩阵
Figure FDA0002294766790000014
的有偏估值器代替原无偏估值器，阻止滤波发散。
2.如权利要求1所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于，具体包括如下步骤：
S1，根据车辆目标跟踪的状态参数，建立卡尔曼滤波状态转移方程：
Figure FDA0002294766790000015
其中，
Figure FDA0002294766790000016
为k-1时刻的系统状态的最优估计值
Figure FDA0002294766790000017
A为状态转移矩阵；
Figure FDA0002294766790000018
表示由k-1时刻的系统状态的最优估计值
Figure FDA0002294766790000019
预测的k时刻的系统状态；
建立量测方程：
Figure FDA00022947667900000110
其中，
Figure FDA00022947667900000111
为k时刻的系统状态，Zk为k时刻的系统状态的量测值；H为量测矩阵；
S2，给定卡尔曼滤波的初值
Figure FDA00022947667900000112
b，k，其中，
Figure FDA00022947667900000113
为第一次滤波时的状态，
Figure FDA00022947667900000114
设为协方差矩阵，
Figure FDA00022947667900000115
为量测噪声，
Figure FDA00022947667900000116
为量测噪声协方差矩阵，
Figure FDA00022947667900000117
为系统随机噪声，
Figure FDA00022947667900000118
为系统随机噪声协方差矩阵；b为比例因子；k表示第k个滤波周期，k的初值为1；
进入迭代周期：
S3，计算k时刻的新息向量εk：
Figure FDA00022947667900000119
S4，计算k时刻的协方差矩阵Pk/k-1
Figure FDA0002294766790000021
其中，Pk-1为k-1时刻的协方差矩阵，Pk/k-1表示k时刻由k-1时刻的协方差矩阵估计的协方差矩阵；
Figure FDA0002294766790000022
为k-1时刻的系统随机噪声协方差矩阵；G为噪声驱动转移矩阵；
Figure FDA0002294766790000023
为噪声二阶矩阵；
S5，进行收敛判断，计算加权值Sk：
判断不等式
Figure FDA0002294766790000024
当不等式成立，则Sk＝1；
当不等式不成立，则
Figure FDA0002294766790000025
其中，
Figure FDA0002294766790000026
为k-1时刻的系统随机噪声协方差矩阵，G为噪声驱动矩阵；
Figure FDA0002294766790000027
为噪声二阶矩阵；R为量测噪声，为滤波的已知条件；Pk-1为k-1时刻的协方差矩阵；ξ略大于1，符号Tr[]表示矩阵的迹；
S6，代入加权值Sk，更新协方差矩阵Pk/k-1：
Figure FDA0002294766790000028
S7，根据Pk/k-1，计算卡尔曼增益Kk：
Kk＝Pk/k-1HT(HPk/k-1HT+R)-1
S8，更新k时刻状态的最优估计值
Figure FDA0002294766790000029
Figure FDA00022947667900000210
S9，更新k时刻的协方差矩阵Pk：
Pk＝(I-KkH)Pk/k-1
S10，计算遗忘因子dk-1：
Figure FDA00022947667900000211
其中，k≥2；
S11，噪声二阶矩阵
Figure FDA00022947667900000212
半正定判断：
当噪声二阶矩阵
Figure FDA0002294766790000031
半正定时，其更新为：
Figure FDA0002294766790000032
否则，其更新为：
Figure FDA0002294766790000033
S12：k＝k+1，重复S2-S11。
3.如权利要求2所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于，所述状态的状态向量Zk为：
Figure FDA0002294766790000034
其中，dk-1为k-1时刻目标点与本车的纵向距离值，vk-1为k-1时刻目标点与本车的纵向相对速度，ak-1为k-1时刻目标点与本车的纵向相对加速度，aak-1为k-1时刻目标点与本车的纵向相对加速度的变化率；
所述状态转移矩阵A为：
Figure FDA0002294766790000035
所述量测矩阵H为：
Figure FDA0002294766790000036
所述噪声驱动矩阵G为：
Figure FDA0002294766790000037
其中，T为k-1时刻到k时刻的时间间隔。
4.如权利要求2所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于：所述S2中的
Figure FDA0002294766790000041
为：
Figure FDA0002294766790000042
5.如权利要求2所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于：所述S11中的遗忘因子b的取值范围为0.9～0.95。
6.如权利要求2所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，其特征在于：所述ξ的取值范围为：1～1.2。
7.一种车辆目标跟踪系统，其特征在于：包括毫米波雷达和电子控制单元；
所述毫米波雷达用于实时采集k时刻的状态向量Zk的相关数据；
所述电子控制单元接收该相关数据，并运行如权利要求1-6任一项所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法进行迭代，获取k时刻状态的最优估计值
Figure FDA0002294766790000043
8.一种存储介质，其特征在于：存储有一段计算机程序，所述计算机程序可被调入电子控制单元运行如权利要求1-6任一项所述的基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法，获取k时刻状态的最优估计值
Figure FDA0002294766790000044
CN201911196442.7A 2019-11-29 2019-11-29 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法 Pending CN110907911A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911196442.7A CN110907911A (zh) 2019-11-29 2019-11-29 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911196442.7A CN110907911A (zh) 2019-11-29 2019-11-29 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法

Publications (1)

Publication Number Publication Date
CN110907911A true CN110907911A (zh) 2020-03-24

Family

ID=69820403

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911196442.7A Pending CN110907911A (zh) 2019-11-29 2019-11-29 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法

Country Status (1)

Country Link
CN (1) CN110907911A (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111595592A (zh) * 2020-05-21 2020-08-28 东南大学 一种自适应巡航控制系统性能测评方法
CN112260590A (zh) * 2020-09-30 2021-01-22 西安理工大学 一种基于iaukf在永磁直线同步电机中的控制方法
CN112364292A (zh) * 2020-09-24 2021-02-12 北京电子工程总体研究所 一种基于Ransac的密集目标跟踪方法、装置、设备和介质
CN112859004A (zh) * 2021-01-04 2021-05-28 南京理工大学 基于改进卡尔曼滤波的野值剔除方法
CN113504012A (zh) * 2021-07-14 2021-10-15 国网福建省电力有限公司检修分公司 基于卡尔曼滤波算法的sf6压力状态判别方法及装置
CN113534997A (zh) * 2021-07-09 2021-10-22 深圳市康冠商用科技有限公司 基于残差的卡尔曼滤波模型的参数调节方法、系统及设备
CN113810026A (zh) * 2021-09-14 2021-12-17 西安艾科特声学科技有限公司 一种自适应滤波算法迭代状态的判断方法
CN114252797A (zh) * 2021-12-17 2022-03-29 华中科技大学 一种基于不确定性估计的锂电池剩余使用寿命预测方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1105747A1 (en) * 1998-08-11 2001-06-13 Northrop Grumman Corporation Method for tracking a target having substantially constrained movement
US7898466B1 (en) * 2008-12-08 2011-03-01 Lockheed Martin Corporation Optimization of radar resources
CN108509379A (zh) * 2018-03-08 2018-09-07 衢州学院 全局估计自适应两阶段平方根容积滤波的方法
CN109001699A (zh) * 2018-01-30 2018-12-14 哈尔滨工业大学 基于带噪声目的地信息约束的跟踪方法
CN110031798A (zh) * 2019-04-15 2019-07-19 哈尔滨工程大学 一种基于简化Sage-Husa自适应滤波的室内目标跟踪方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1105747A1 (en) * 1998-08-11 2001-06-13 Northrop Grumman Corporation Method for tracking a target having substantially constrained movement
US7898466B1 (en) * 2008-12-08 2011-03-01 Lockheed Martin Corporation Optimization of radar resources
CN109001699A (zh) * 2018-01-30 2018-12-14 哈尔滨工业大学 基于带噪声目的地信息约束的跟踪方法
CN108509379A (zh) * 2018-03-08 2018-09-07 衢州学院 全局估计自适应两阶段平方根容积滤波的方法
CN110031798A (zh) * 2019-04-15 2019-07-19 哈尔滨工程大学 一种基于简化Sage-Husa自适应滤波的室内目标跟踪方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
庞成: "基于测距雷达和机器视觉数据融合的前方车辆检测系统", 《中国优秀硕士学位论文全文数据库-信息科技辑》 *
路小燕 等: "改进的Sage_Husa自适应滤波算法在MEMS航姿参考系统中的应用", 《导航与控制》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111595592A (zh) * 2020-05-21 2020-08-28 东南大学 一种自适应巡航控制系统性能测评方法
CN111595592B (zh) * 2020-05-21 2021-10-12 东南大学 一种自适应巡航控制系统性能测评方法
CN112364292A (zh) * 2020-09-24 2021-02-12 北京电子工程总体研究所 一种基于Ransac的密集目标跟踪方法、装置、设备和介质
CN112364292B (zh) * 2020-09-24 2024-05-03 北京电子工程总体研究所 一种基于Ransac的密集目标跟踪方法、装置、设备和介质
CN112260590A (zh) * 2020-09-30 2021-01-22 西安理工大学 一种基于iaukf在永磁直线同步电机中的控制方法
CN112859004A (zh) * 2021-01-04 2021-05-28 南京理工大学 基于改进卡尔曼滤波的野值剔除方法
CN113534997A (zh) * 2021-07-09 2021-10-22 深圳市康冠商用科技有限公司 基于残差的卡尔曼滤波模型的参数调节方法、系统及设备
CN113534997B (zh) * 2021-07-09 2024-07-09 深圳市康冠商用科技有限公司 基于残差的卡尔曼滤波模型的参数调节方法、系统及设备
CN113504012B (zh) * 2021-07-14 2023-12-22 国网福建省电力有限公司检修分公司 基于卡尔曼滤波算法的sf6压力状态判别方法及装置
CN113504012A (zh) * 2021-07-14 2021-10-15 国网福建省电力有限公司检修分公司 基于卡尔曼滤波算法的sf6压力状态判别方法及装置
CN113810026A (zh) * 2021-09-14 2021-12-17 西安艾科特声学科技有限公司 一种自适应滤波算法迭代状态的判断方法
CN114252797A (zh) * 2021-12-17 2022-03-29 华中科技大学 一种基于不确定性估计的锂电池剩余使用寿命预测方法
CN114252797B (zh) * 2021-12-17 2023-03-10 华中科技大学 一种基于不确定性估计的锂电池剩余使用寿命预测方法

Similar Documents

Publication Publication Date Title
CN110907911A (zh) 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法
CN109829938B (zh) 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法
CN111178385B (zh) 一种鲁棒在线多传感器融合的目标跟踪方法
CN111985093A (zh) 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN109813342B (zh) 一种惯导-卫星组合导航系统的故障检测方法和系统
CN109163720A (zh) 基于渐消记忆指数加权的卡尔曼滤波跟踪方法
CN112949026B (zh) 一种考虑年龄和状态依赖的退化设备剩余寿命预测方法
CN109239604A (zh) 一种基于状态检测机制的无迹卡尔曼滤波车载电池soc估计方法
CN105929340B (zh) 一种基于arima估算电池soc的方法
CN109827579B (zh) 一种组合定位中滤波模型实时校正的方法和系统
CN110287537B (zh) 用于频标输出跳变检测的抗野值自适应卡尔曼滤波方法
CN103776449B (zh) 一种提高鲁棒性的动基座初始对准方法
CN114877926B (zh) 传感器故障检测与诊断方法、介质、电子设备及系统
CN110375772A (zh) 自适应卡尔曼滤波的环形激光器随机误差建模与补偿方法
CN112487694A (zh) 一种基于多退化指标的复杂设备剩余寿命预测方法
CN111444474B (zh) 一种基于乘性噪声相关自适应ckf的目标跟踪方法
US20200025828A1 (en) Method for determining a state noise covariance matrix for adjusting an observer of the state of charge of a battery and corresponding device
CN110514209B (zh) 一种交互式多模型组合导航方法
CN116893658A (zh) 控制机器人设备的方法、控制装置、计算机程序和介质
CN115438728A (zh) 一种基于鲁棒滤波的目标融合方法及系统
CN110716219A (zh) 一种提高定位解算精度的方法
CN114611068A (zh) 一种高机动目标跟踪方法
Dingler State estimation with the Interacting Multiple Model (IMM) method
CN109684771B (zh) 基于交互式多模型的机动目标状态预测优化方法
CN109581284B (zh) 基于交互多模型的非视距误差消除方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20200324

RJ01 Rejection of invention patent application after publication