CN109829938B - 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 - Google Patents
一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 Download PDFInfo
- Publication number
- CN109829938B CN109829938B CN201910079584.9A CN201910079584A CN109829938B CN 109829938 B CN109829938 B CN 109829938B CN 201910079584 A CN201910079584 A CN 201910079584A CN 109829938 B CN109829938 B CN 109829938B
- Authority
- CN
- China
- Prior art keywords
- state
- noise
- value
- formula
- measurement
- 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.)
- Active
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明涉及一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法。本发明大体包括三部分内容。第一部分根据实际运动目标进行***建模;第二部分,根据改进自适应滤波方法对噪声统计特性进行估计;第三部分,根据卡方检验对故障进行检测,根据检测结果在状态估计时对新息部分作加权处理。本发明既能同时对过程噪声协方差和量测噪声协方差动态估计,又能应对雷达所测数据发生故障时的情况,实现了对目标的有效跟踪。
Description
技术领域
本发明涉及一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法,属于目标跟踪领域。
背景技术
目标跟踪中常使用卡尔曼滤波对目标状态进行估计,通过卡尔曼滤波可以将传感器数据中的量测噪声消除,以达到对目标精准跟踪的效果。然而传统的卡尔曼滤波是建立在***模型准确并且是线性的、过程噪声和量测噪声服从均值为零、方差为固定值的高斯分布且其统计特性已知的情况下。然而,在实际目标跟踪中,大多数***模型是非线性的,***噪声的统计特性也是不完全已知甚至是未知的,这时再用卡尔曼滤波对其进行估计将造成估计结果不准确甚至发散。
为了解决噪声统计特性不准确的问题,提出了Sage-Husa自适应噪声估值器对噪声进行实时估计。然而Sage-Husa自适应滤波只能在对单一过程噪声不准确或者量测噪声不准确时有较好的估计效果,当同时存在过程噪声与量测噪声统计特性不准确时,Sage-Husa自适应滤波算法将不再适用。因此,对于过程噪声与量测噪声统计特性同时不准确的情况,本发明将采用改进的自适应滤波算法,用Sage-Husa自适应噪声估值器对过程噪声统计特性估计,同时采用变分贝叶斯对量测噪声进行迭代估计,此方法可同时估计出过程噪声和量测噪声统计特性。
由于实际工程环境日益复杂,传感器工作时常会受到扰动的影响,导致测量数据常出现一些明显偏离实际值的异常数据,也称野值,在本发明中将这种情况称作故障。故障的出现将会影响自适应滤波对量测噪声统计特性的估计,进而造成滤波精度降低。因此,本发明采用卡方检验的方法对故障进行检测,如若发生故障则对其进行处理,降低故障对滤波的影响,以此来提高***的容错性。然而常规新息卡方检验法对硬故障检测效果好,当传感器出现缓变故障时,由于缓变故障开始很小,不易检测出来,有故障的传感器的输出将污染滤波器输出,使其跟踪故障输出,所以本发明采用一种能检测缓变故障的故障检测方法,提高了故障检测的灵敏度,大大提高了滤波精度。
发明内容
为了应对上面提到的过程噪声和量测噪声统计特性不准确以及量测存在故障等情况,本发明基于Sage-Husa自适应估计和变分贝叶斯估计提出一种新的自适应滤波方法,通过卡方检验对故障进行检测,并对其进行处理,得到自适应容错容积卡尔曼滤波方法。
本发明大体包括三部分内容。第一部分根据实际运动目标进行***建模;第二部分,根据改进自适应滤波方法对噪声统计特性进行估计;第三部分,根据卡方检验对故障进行检测,并对其处理。
利用本发明不仅可以应对噪声统计特性不准确的情况,而且能够处理故障,提高滤波精度,包括以下步骤:
步骤1.***建模,考虑离散时间非线性***模型,其跟踪目标的状态方程和量测方程如下:
式中:xk+1∈Rn是k+1时刻的***状态向量,其是由x方向位移和速度以及y方向位移和速度构成,f和h为已知函数,zk+1∈Rm是k+1时刻***的量测向量,分别由k+1时刻传感器测得目标运动距离和角度构成,其中σ为随机向量,用来描述异常值大小,当ρk+1=0时表示没有出现异常量测,当ρk+1=1时,表示出现异常量测,wk是均值为0方差为Qk的过程噪声向量,vk+1是均值为0方差为Rk+1的量测噪声向量,且wk和vk+1互不相关,即:
式中,δkj为Kronecher-δ函数;
步骤2给出自适应容错容积卡尔曼滤波方法,本算法使用Sage-Husa滤波方法对过程噪声估计,变分贝叶斯方法对未知量测噪声估计,容积卡尔曼滤波处理非线性问题,并采用卡方检验法对异常量测进行检测,以下简称故障诊断,并进行容错处理,具体如下:
步骤2.1给出初始化状态量和估计误差方差矩阵:
步骤2.2给出由k时刻的状态对k+1时刻的状态进行预测:
式中Pk为估计误差方差矩阵,Sk为cholesky分解值,χi,k为第i个容积点,为经过状态方程传播的第i个容积点,为状态预测均值,为***过程噪声均值,Pk+1|k为预测误差协方差矩阵,0<ρ<1为衰减系数,用来描述当前时刻量测噪声因子与前一刻噪声因子的关系,ξi是第i个容积点,L=2n,n是状态向量的维数,而[1]∈Rn,[1]i表示第i个列向量,且
步骤2.3给出上步中过程噪声均值和方差的计算,根据Sage-Husa自适应估计对其进行计算,其公式如下:
步骤2.4给出量测预测:
步骤2.5给出故障诊断步骤为:
式中:Rk+1|k为预测量测噪声方差矩阵,Pzz,k+1|k为预测新息协方差矩阵,λk+1为故障检测函数,TD为预先设定的门限值,可由预先设定的误警率确定,f(k+1)为加权因子。
若没有发生故障,则过程噪声均值和方差由Saga-Husa估计得出,若发生故障,则将公式(10)的过程噪声均值和公式(11)的过程噪声方差矩阵分别替换为qk+1=qkQk+1=Qk。
步骤2.6给出量测更新,对以下部分进行变分贝叶斯迭代:
设置迭代初始值:
步骤2.7迭代结束后对量测噪声逆伽马分布参数、量测噪声、状态估计值以及状态估计误差方差矩阵进行更新。
步骤3给出缓变故障检测方法,具体算法如下:
步骤3.3将和作为k-m+1时刻的伪状态估计值和伪估计误差协方差阵,继续进行时间更新,以此类推得到k时刻的状态预测值和预测误差协方差阵再由公式(12)-(15)和(18)得到量测预测值和新息协方差阵Pzz,k|k-m。得到伪新息向量为:并构造出故障检测函数用于步骤2.5的故障检测。
本发明的有益效果:本发明既能同时对过程噪声协方差和量测噪声协方差动态估计,又能应对雷达所测数据发生故障时的情况,实现了对目标的有效跟踪。
附图说明:
图1:本发明方法的流程框图。
具体实施方法
本发明提出的一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法,首先根据实际目标跟踪的运动状态建立***模型,其次给出自适应容错容积卡尔曼滤波方法的步骤,最后介绍缓变故障的检测算法,其流程框图如图1所示,包括以下几个步骤:
步骤1:***建模,考虑离散时间非线性***模型,其跟踪目标的状态方程和量测方程如下:
式中:xk+1∈Rn是k+1时刻的***状态向量,其是由x方向位移和速度以及y方向位移和速度构成,f和h为已知函数,zk+1∈Rm是k+1时刻***的量测向量,分别由k+1时刻传感器测得目标运动距离和角度构成,其中σ为随机向量,用来描述异常值大小,当ρk+1=0时表示没有出现异常量测,当ρk+1=1时,表示出现异常量测,wk是均值为0方差为Qk的过程噪声向量,vk+1是均值为0方差为Rk+1的量测噪声向量,且wk和vk+1互不相关,即:
式中,δkj为Kronecher-δ函数.
步骤2:设置状态向量估计初始值和估计误差方差矩阵初始值分别为P0,过程噪声协方差矩阵和量测噪声协方差矩阵分别设置为Q0,R0。且αi,0,βi,0为逆伽马分布参数,i=1,2,…,d,d是量测向量的维数,设置遗忘因子b,故障检测阈值TD。
步骤6:本发明对故障检测方法采用缓变故障卡方检测方法,将在步骤8中介绍。根据公式(17)计算得到预测量测噪声协方差矩阵Rk+1|k,并将其用于计算新息协方差矩阵Pzz,k+1|k。根据缓变故障检测方法,检测当前时刻新息与理论新息协方差的不一致程度,判断是否存在发生故障;若没有发生故障,则k+1时刻的过程噪声均值和方差按公式(10)和公式(11)由Saga-Husa估计得出;若发生故障,则将过程噪声均值和过程噪声方差矩阵分别替换为qk+1=qk,Qk+1=Qk;
步骤7:在步骤6中,若没有检测出故障,根据公式(21)~(34)进行量测更新,此时的加权因子f(k+1)=1,采用变分贝叶斯迭代估计得出状态估计值和状态估计误差方差矩阵Pk+1。若检测出故障,则进行第一次变分贝叶斯迭代估计时采用公式(26),剩下次数的迭代将公式(26)替换为
本发明所述自适应容错容积卡尔曼滤波方法能够实现同时估计过程噪声协方差和量测噪声协方差,并提出容错的思想,使得通过雷达所测得的数据出现异常时,降低异常数据对滤波的估计精度的影响。通过缓变故障卡方检验方法检测故障,可以有效检测出缓变故障,有效地实现了目标跟踪功能。
Claims (1)
1.一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法,其特征在于该方法包括以下步骤:
步骤1.***建模,考虑离散时间非线性***模型,其跟踪目标的状态方程和量测方程如下:
式中:xk+1∈Rn是k+1时刻的***状态向量,其是由x方向位移和速度以及y方向位移和速度构成,f和h为已知函数,zk+1∈Rm是k+1时刻***的量测向量,分别由k+1时刻传感器测得目标运动距离和角度构成,其中σ为随机向量,用来描述异常值大小,当ρk+1=0时表示没有出现异常量测,当ρk+1=1时,表示出现异常量测,wk是均值为0方差为Qk的过程噪声向量,vk+1是均值为0方差为Rk+1的量测噪声向量,且wk和vk+1互不相关,即:
式中,δkj为Kronecher-δ函数;
步骤2给出自适应容错容积卡尔曼滤波方法,使用Sage-Husa滤波方法对过程噪声估计,变分贝叶斯方法对未知量测噪声估计,容积卡尔曼滤波处理非线性问题,并采用卡方检验法对异常量测进行检测,并进行容错处理,具体如下:
步骤2.1给出初始化状态量和估计误差方差矩阵:
步骤2.2给出由k时刻的状态对k+1时刻的状态进行预测:
式中Pk为估计误差方差矩阵,Sk为cholesky分解值,χi,k为第i个容积点,为经过状态方程传播的第i个容积点,为状态预测均值,为***过程噪声均值,Pk+1|k为预测误差协方差矩阵,0<ρ<1为衰减系数,用来描述当前时刻量测噪声因子与前一刻噪声因子的关系,ξi是第i个容积点,n是状态向量的维数,而[1]∈Rn,[1]i表示第i个列向量,且
步骤2.3给出上步中过程噪声均值和方差的计算方法,根据Sage-Husa自适应估计对其进行计算,其公式如下:
步骤2.4给出量测预测:
步骤2.5根据k+1时刻的新息残差向量与理论新息协方差矩阵的不一致程度判断传感器采样数据是否出现异常,给出故障诊断步骤为:
式中:Rk+1|k为预测量测噪声方差矩阵,Pzz,k+1|k为预测新息协方差矩阵,λk+1为故障检测函数,TD为预先设定的门限值,可由预先设定的误警率确定,f(k+1)为加权因子;
若没有发生故障,则过程噪声均值和方差由Saga-Husa估计得出;若发生故障,则将公式(10)的过程噪声均值和公式(11)的过程噪声方差矩阵分别替换为qk+1=qk,Qk+1=Qk;
步骤2.6给出量测更新,根据变分贝叶斯估计对以下部分进行迭代,具体步骤如下:
设置迭代初始值:
步骤2.7给出对量测噪声逆伽马分布参数、量测噪声、状态估计值以及状态估计误差方差矩阵进行更新:
步骤3给出缓变故障检测:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910079584.9A CN109829938B (zh) | 2019-01-28 | 2019-01-28 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910079584.9A CN109829938B (zh) | 2019-01-28 | 2019-01-28 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109829938A CN109829938A (zh) | 2019-05-31 |
CN109829938B true CN109829938B (zh) | 2020-12-08 |
Family
ID=66862665
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910079584.9A Active CN109829938B (zh) | 2019-01-28 | 2019-01-28 | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109829938B (zh) |
Families Citing this family (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110196443B (zh) * | 2019-06-06 | 2021-11-09 | 中国人民解放军战略支援部队信息工程大学 | 一种飞行器的容错组合导航方法及*** |
CN110443832B (zh) * | 2019-06-21 | 2022-03-01 | 西北工业大学 | 一种基于观测区间值的证据滤波目标跟踪方法 |
CN110501696B (zh) * | 2019-06-28 | 2022-05-31 | 电子科技大学 | 一种基于多普勒量测自适应处理的雷达目标跟踪方法 |
CN110231620B (zh) * | 2019-07-08 | 2020-02-14 | 哈尔滨工业大学 | 一种噪声相关***跟踪滤波方法 |
CN110503071B (zh) * | 2019-08-29 | 2021-11-23 | 电子科技大学 | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 |
CN111062359B (zh) * | 2019-12-27 | 2023-03-31 | 广东海洋大学深圳研究院 | 基于噪声顺序解相关的两阶段Kalman滤波融合方法 |
CN111444475B (zh) * | 2020-03-24 | 2023-07-14 | 广东海洋大学深圳研究院 | 一种应用在飞行测试数据分析的容错ckf滤波融合方法 |
CN111444474B (zh) * | 2020-03-24 | 2024-02-27 | 宁波飞拓电器有限公司 | 一种基于乘性噪声相关自适应ckf的目标跟踪方法 |
CN111563960B (zh) * | 2020-05-08 | 2023-05-16 | 北京理工大学重庆创新中心 | 基于贝叶斯递推的空间群目标检测方法、装置及存储介质 |
CN111612729B (zh) * | 2020-05-26 | 2023-06-23 | 杭州电子科技大学 | 基于卡尔曼滤波的目标序列跟踪图像恢复方法 |
CN111611718B (zh) * | 2020-05-27 | 2023-04-07 | 江南大学 | 基于贝叶斯学习的执行器故障估计方法 |
US11294365B2 (en) | 2020-05-27 | 2022-04-05 | Jiangnan University | Estimation methods of actuator faults based on bayesian learning |
CN112003527B (zh) * | 2020-07-22 | 2022-08-16 | 西安理工大学 | 用于异步电机转速辨识的迭代扩展卡尔曼滤波的改进方法 |
CN112212860B (zh) * | 2020-08-28 | 2023-03-03 | 山东航天电子技术研究所 | 具有故障容错的分布式滤波微纳卫星姿态确定方法 |
CN112347415B (zh) * | 2020-10-02 | 2022-04-01 | 广东电网有限责任公司广州供电局 | 一种基于短路电流过零点预测***的预测方法 |
CN112284384B (zh) * | 2020-10-26 | 2023-11-17 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
CN112550294B (zh) * | 2020-11-16 | 2021-12-24 | 东南大学 | 基于车辆故障信号隔离的路径跟踪控制方法 |
CN112713877A (zh) * | 2020-12-17 | 2021-04-27 | 中国科学院光电技术研究所 | 一种基于卡方自适应因子的鲁棒滤波方法 |
CN112764345B (zh) * | 2020-12-21 | 2022-08-02 | 杭州电子科技大学 | 基于目标状态跟踪的强非线性***卡尔曼滤波器设计方法 |
CN112788049B (zh) * | 2021-01-24 | 2022-03-22 | 国网山东省电力公司 | 基于自适应卡尔曼滤波器的电力信息物理***攻击检测方法 |
EP4060614A1 (en) * | 2021-03-19 | 2022-09-21 | Aptiv Technologies Limited | Method for determining noise statistics of object sensors |
CN113326618B (zh) * | 2021-06-02 | 2022-07-15 | 江南大学 | 连续发酵过程中估计培养基初始条件的方法 |
CN113670337B (zh) * | 2021-09-03 | 2023-05-26 | 东南大学 | 一种用于gnss/ins组合导航卫星缓变故障检测方法 |
CN114445459B (zh) * | 2022-01-16 | 2024-04-09 | 中国人民解放军空军工程大学 | 基于变分贝叶斯理论的连续-离散最大相关熵目标跟踪方法 |
CN115379560B (zh) * | 2022-08-22 | 2024-03-08 | 昆明理工大学 | 一种无线传感网络中仅有距离量测信息下的目标定位与跟踪方法 |
CN115964603B (zh) * | 2023-02-10 | 2023-05-30 | 成都理工大学 | 一种基于改进Kalman滤波的机动目标跟踪方法 |
CN116067370B (zh) * | 2023-04-03 | 2023-06-27 | 广东智能无人***研究院(南沙) | 一种imu姿态解算方法及设备、存储介质 |
CN117455960B (zh) * | 2023-12-22 | 2024-03-08 | 中国人民解放军海军工程大学 | 时变观测噪声条件下机载光电***对地无源定位滤波方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103591965A (zh) * | 2013-09-12 | 2014-02-19 | 哈尔滨工程大学 | 一种舰载旋转式捷联惯导***在线标定的方法 |
CN108981733A (zh) * | 2018-04-26 | 2018-12-11 | 杭州中恒云能源互联网技术有限公司 | 一种电动汽车充电导航***的速度预测方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110084871A1 (en) * | 2009-10-13 | 2011-04-14 | Mcmaster University | Cognitive tracking radar |
CN103389094B (zh) * | 2013-07-15 | 2017-03-01 | 哈尔滨工程大学 | 一种改进的粒子滤波方法 |
CN104655131B (zh) * | 2015-02-06 | 2017-07-18 | 东南大学 | 基于istssrckf的惯性导航初始对准方法 |
CN107621632A (zh) * | 2016-12-26 | 2018-01-23 | 中国人民解放军63921部队 | 用于nshv跟踪滤波的自适应滤波方法及*** |
-
2019
- 2019-01-28 CN CN201910079584.9A patent/CN109829938B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103591965A (zh) * | 2013-09-12 | 2014-02-19 | 哈尔滨工程大学 | 一种舰载旋转式捷联惯导***在线标定的方法 |
CN108981733A (zh) * | 2018-04-26 | 2018-12-11 | 杭州中恒云能源互联网技术有限公司 | 一种电动汽车充电导航***的速度预测方法 |
Non-Patent Citations (4)
Title |
---|
A new maneuvering target tracking method using adaptive cubature Kalman filter;Yiou Sun et al;《 2014 IEEE International Conference on Control Science and Systems Engineering》;20150827;全文 * |
A Variational Bayesian Based Strong Tracking Interpolatory Cubature Kalman Filter for Maneuvering Target Tracking;Jian Wang;《IEEE Access》;20180906;全文 * |
基于改进容积卡尔曼滤波的认知雷达跟踪算法;张东泽 等;《计算机仿真》;20141231;第31卷(第12期);全文 * |
强跟踪CKF算法及其在非线性***故障诊断中的应用;董鑫 等;《信息与控制》;20141231;第43卷(第4期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN109829938A (zh) | 2019-05-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109829938B (zh) | 一种应用在目标跟踪的自适应容错容积卡尔曼滤波方法 | |
CN111178385B (zh) | 一种鲁棒在线多传感器融合的目标跟踪方法 | |
CN107765347B (zh) | 一种高斯过程回归和粒子滤波的短期风速预测方法 | |
CN109813342B (zh) | 一种惯导-卫星组合导航***的故障检测方法和*** | |
Rigatos | Extended Kalman and particle filtering for sensor fusion in motion control of mobile robots | |
Zhao et al. | Tuning-free Bayesian estimation algorithms for faulty sensor signals in state-space | |
CN103217175B (zh) | 一种自适应容积卡尔曼滤波方法 | |
Durrant-Whyte | Introduction to estimation and the Kalman filter | |
CN112180899B (zh) | 一种间歇异常测量检测下***的状态估计方法 | |
CN110907911A (zh) | 基于Sage-Husa卡尔曼滤波的车辆目标跟踪算法 | |
CN114966762B (zh) | 一种gnss/ins紧组合导航***的故障检测方法 | |
Zhao et al. | Probabilistic monitoring of sensors in state-space with variational Bayesian inference | |
CN115964603B (zh) | 一种基于改进Kalman滤波的机动目标跟踪方法 | |
Li et al. | A wiener-based remaining useful life prediction method with multiple degradation patterns | |
Zhang et al. | Low-cost adaptive square-root cubature Kalman filter for systems with process model uncertainty | |
Hajiyev | Testing the covariance matrix of the innovation sequence with sensor/actuator fault detection applications | |
CN116558406A (zh) | 基于状态域的gnss-加速度计集成桥梁变形监测突变故障探测方法 | |
CN114877926B (zh) | 传感器故障检测与诊断方法、介质、电子设备及*** | |
CN115438728A (zh) | 一种基于鲁棒滤波的目标融合方法及*** | |
CN115905986A (zh) | 一种基于联合策略的稳健卡尔曼滤波方法 | |
Hajiyev | Innovation approach based measurement error self-correction in dynamic systems | |
CN111444475B (zh) | 一种应用在飞行测试数据分析的容错ckf滤波融合方法 | |
Rangegowda et al. | Simultaneous state and parameter estimation using robust receding-horizon nonlinear Kalman filter | |
CN111090281B (zh) | 基于改进粒子滤波算法估算机器人方位的方法和装置 | |
Zhao et al. | A unified framework for fault detection and diagnosis using particle filter |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |