CN106772351B - 基于有限步记忆的卡尔曼滤波方法 - Google Patents

基于有限步记忆的卡尔曼滤波方法 Download PDF

Info

Publication number
CN106772351B
CN106772351B CN201611030384.7A CN201611030384A CN106772351B CN 106772351 B CN106772351 B CN 106772351B CN 201611030384 A CN201611030384 A CN 201611030384A CN 106772351 B CN106772351 B CN 106772351B
Authority
CN
China
Prior art keywords
state
current
covariance
target
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
Application number
CN201611030384.7A
Other languages
English (en)
Other versions
CN106772351A (zh
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.)
Xidian University
Original Assignee
Xidian University
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 Xidian University filed Critical Xidian University
Priority to CN201611030384.7A priority Critical patent/CN106772351B/zh
Publication of CN106772351A publication Critical patent/CN106772351A/zh
Application granted granted Critical
Publication of CN106772351B publication Critical patent/CN106772351B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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
    • 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/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01S7/415—Identification of targets based on measurements of movement associated with the target

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

本发明公开了一种基于有限步记忆的卡尔曼滤波方法，主要解决现有技术中目标跟踪准确性与稳定性低的问题。其技术方案是：由传统卡尔曼滤波方法得到目标航迹的前N步状态及状态协方差；按当前状态向后回溯N步，得到目标航迹的参考状态；根据参考状态判断目标的机动性，若发生机动，则对上一帧滤波的速度作修正；再根据参考状态判断当前量测的有效性，若无效，则对新息加一个小于1的权值，得到新的新息；根据上一时刻的状态协方差得到一步预测协方差，再计算增益矩阵；根据预测状态和增益矩阵及新的新息更新当前的状态；根据一步预测协方差和增益矩阵更新状态协方差，完成目标跟踪。本发明提高了目标跟踪的准确性与稳定性，可用于雷达数据处理。

Description

基于有限步记忆的卡尔曼滤波方法
技术领域
本发明属于雷达数据处理技术领域，主要涉及慢速运动目标的跟踪滤波技术，可用于对地面与海面目标的稳定跟踪。
背景技术
雷达对运动目标的跟踪即是跟踪滤波的过程，这是雷达跟踪的核心内容，它的作用是对目标运动状态作出估计和预测。跟踪的任务是通过相关和滤波处理建立目标的运动轨迹。雷达***根据在建立目标轨迹过程中对目标运动状态作出的估计和预测来评估目标运动的安全态势和机动性的安全效果。因此，雷达跟踪环节工作性能的优劣直接影响到雷达***的安全效能。
鉴于目标跟踪在增进雷达效能中的重要作用，各国在军用和民用等领域中一直非常重视发展这一雷达技术。机动目标跟踪理论有了很大的发展，尤其是在跟踪算法的研究上，理论更是日趋成熟。在跟踪算法中，主要有线性自回归滤波，两点外推滤波，维纳滤波，加权最小二乘滤波，α-β滤波和卡尔曼滤波，其中卡尔曼滤波算法在目标跟踪理论中占据了主导地位。
传统的标准卡尔曼滤波算法是一种叠代最小均方RLS滤波技术，其是把目标的位置，速度和加速度作为目标状态矢量，通过目标的动力学方程来描述目标状态的变化，利用递推的计算方法，估计目标的状态，以建立目标的航迹。卡尔曼滤波算法虽具有收敛速度快、运算量小的特点。但该方法普遍存在的问题就是跟踪误差大，跟踪结果可能不收敛甚至有丢失目标的状况，即它的稳定性不高。
发明内容
本发明的目的在于针对传统的标准卡尔曼滤波算法的不足，提出一种基于有限步记忆的卡尔曼滤波方法，以减小跟踪误差，提高目标跟踪的准确性与稳定性。
实现本发明目的的技术方案，包括如下：
(1)根据已知目标航迹的前N步滤波状态和预测状态以及状态协方差P(k-1|k-1)，按照目标航迹的当前状态向后回溯N步，得到目标航迹的参考状态其中，k＝1,2,…,N表示时刻；
(2)根据目标航迹的参考状态判断目标是否发生机动：
2a)根据参考状态到当前时刻预测状态的位移a和当前时刻预测状态到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：
其中，<a,c>表示a与c之间的内积。
2b)根据角度θ判断目标是否发生机动：
若则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤(4)；
若则表示目标发生了机动，此时对当前量测的速度加一个权值wX和对上一时刻滤波的速度加一个权值wZ：
2c)根据当前量测的速度的权值wX和上一时刻滤波的速度的权值wZ修正上一时刻的滤波，得到更新后的滤波从而得到更新后的预测再根据当前时刻的预测计算当前时刻预测的量测
(3)根据目标航迹的参考状态判断当前量测Z(k)的可靠性：
3a)根据参考状态到当前时刻预测状态的位移a和参考状态到量测状态Z(k)的位移b，计算这两个位移之间的夹角α：
其中，<a,b>表示a与b之间的内积；
3b)根据当前量测Z(k)和当前预测的量测得到当前新息v(k)：
3c)根据角度α判断当前量测Z(k)的可靠性：
若cosα≥0，则表示当前量测可靠；
若cosα<0，则表示当前量测不可靠，对当前新息v(k)加一个权值w，得到新的新息v'(k)，其中，w<1；
(4)根据上一时刻的状态协方差P(k-1|k-1)得到一步预测协方差P(k|k-1)，进而计算出当前增益矩阵G(k)；
(5)根据当前预测状态和增益矩阵G(k)及新的新息v'(k)，更新当前时刻的滤波状态
(6)根据一步预测协方差P(k|k-1)和增益矩阵G(k)，更新当前状态协方差矩阵P(k|k)。
本发明与现有的传统卡尔曼滤波方法相比具有以下优点：
1.本发明在目标发生机动时，对上一帧滤波状态的速度进行加权修正，避免了跟踪结果不收敛甚至目标丢失的情况；
2.本发明通过将目标航迹向后回溯N步来判断当前量测的可靠性，对于不可靠的量测再通过对当前新息加一个小于1的权值使其满足可靠性的要求，从而大大减小了跟踪误差；
3.本发明使用基于有限步记忆的卡尔曼滤波方法，使目标跟踪的准确性与稳定性得以大幅提高。
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将结合实施例和附图对本发明作进一步描述。
附图说明
图1为本发明的实现流程图；
图2为本发明中量测可靠性判断示意图；
图3为用传统卡尔曼滤波方法处理间断航迹的仿真结果图；
图4为用本发明中基于有限步记忆的卡尔曼滤波方法处理间断航迹的仿真结果图；
图5为用传统卡尔曼滤波方法处理连续航迹的仿真结果图；
图6为用本发明中基于有限步记忆的卡尔曼滤波方法处理连续航迹的仿真结果图；
具体实施方式
参照图1，本发明的实现步骤包括如下：
步骤一、获取目标航迹的参考状态。
由传统卡尔曼滤波方法得到目标航迹的前N步滤波状态和预测状态以及状态协方差P(k-1|k-1)，其中，k＝1,2,…,N表示时刻；
按照航迹的当前状态向后回溯N步，得到的滤波状态称为目标航迹的参考状态
步骤二、根据目标航迹的参考状态判断目标是否发生机动。
参照图2，本步骤的具体实现如下：
2a)根据参考状态到当前时刻预测状态的位移a和当前时刻预测状态到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：
其中，<a,c>表示a与c之间的内积；
2b)根据角度θ判断目标是否发生机动：
若则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤三；
若则表示目标发生了机动，此时分别给定当前量测的速度的权值wZ和上一时刻滤波的速度的权值wX：
2c)根据当前量测的速度vZ的权值wZ和上一时刻滤波的速度vX的权值wX修正上一时刻滤波的速度vX，得到修正后的速度v'X：
v′X＝wZ·vZ+wX·vX，
用修正后的速度v'X代替修正前的速度vX，得到修正后的滤波进而更新当前时刻的预测为：
其中，F(k-1)为状态转移矩阵；
再根据当前时刻的预测得到当前时刻预测的量测
其中，H(k)为量测矩阵。
步骤三、根据目标航迹的参考状态判断当前量测Z(k)的可靠性。
3a)根据参考状态到当前时刻预测状态的位移a和参考状态到量测状态Z(k)的位移b，计算这两个位移之间的夹角α：
其中，<a,b>表示a与b之间的内积；
3b)根据当前量测Z(k)和当前时刻预测的量测得到当前新息v(k)：
3c)根据角度α判断当前量测Z(k)的可靠性：
若cosα≥0，则表示当前量测可靠；
若cosα<0，则表示当前量测不可靠，则要对当前新息v(k)加一个权值w，得到新的新息v'(k)，其中，w<1。
步骤四、计算一步预测协方差矩阵和当前增益矩阵。
4a)根据上一时刻的状态协方差矩阵P(k-1|k-1)得到一步预测协方差矩阵P(k|k-1)：
P(k|k-1)＝F(k-1)P(k-1|k-1)FT(k-1)，
其中，FT(k-1)为状态转移矩阵F(k-1)的转置。
4b)根据一步预测协方差矩阵P(k|k-1)计算出当前增益矩阵G(k)：
G(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k))-1，
其中，HT(k)为量测矩阵H(k)的转置，(H(k)P(k|k-1)HT(k))-1为H(k)P(k|k-1)HT(k)的逆。
步骤五、更新当前时刻的滤波状态。
根据当前预测状态和增益矩阵G(k)及新的新息v'(k)来更新当前时刻的滤波状态
步骤六、更新当前时刻的状态协方差矩阵。
根据一步预测协方差矩阵P(k|k-1)和增益矩阵G(k)来更新当前时刻状态协方差矩阵P(k|k)：
P(k|k)＝P(k|k-1)-G(k)H(k)P(k|k-1)。
本发明的效果可以通过以下对实测数据的处理进一步说明：
1.实验环境与条件:
数据率：40s/帧
目标数：1个
测距误差：10000m
测速误差：5m/s
在仿真中，两组数据的帧数分别为122帧和347帧。
2.实验内容与结果：
实验1，用传统卡尔曼滤波方法对间断的目标航迹进行滤波处理，结果如图3。
实验2，用本发明方法对间断的目标航迹进行滤波处理，结果如图4。
比较图3和图4可见，传统卡尔曼滤波方法滤波值随量测值的波动较大，滤波平滑效果较差，尤其在航迹间断处的滤波误差更大。而本发明滤波值随量测值的波动很小，滤波效果得到明显改善，航迹间断处的滤波误差也大大降低。
实验3，用传统卡尔曼滤波方法对连续的目标航迹进行滤波处理，结果如图5。
实验4，用本发明方法对连续的目标航迹进行滤波处理，结果如图6。
比较由图5和图6可见，传统卡尔曼滤波方法滤波值随量测值的波动较大，滤波平滑效果较差。而本发明滤波值随量测值的波动很小，滤波平滑效果得到明显改善。
综上，本发明与传统卡尔曼滤波方法相比，大大减小了跟踪误差，提高了跟踪的稳定性。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (7)

1.基于有限步记忆的卡尔曼滤波方法，包括：
(1)根据传统卡尔曼滤波方法得到目标航迹的前N步滤波状态和预测状态以及状态协方差P(k-1|k-1)，按照目标航迹的当前状态向后回溯N步，得到目标航迹的参考状态其中，k＝1,2,…,N表示时刻；
(2)根据目标航迹的参考状态判断目标是否发生机动：
2a)根据参考状态到当前时刻预测状态的位移a和当前时刻预测状态到量测状态Z(k)的位移c，计算这两个位移之间的夹角θ：
其中，＜a,c＞表示a与c之间的内积；
2b)根据角度θ判断目标是否发生机动：
若则表示量测在当前目标航迹方向的±45°范围内，目标没有发生机动，直接执行步骤(4)；
若则表示目标发生了机动，此时对当前量测的速度加一个权值wX和对上一时刻滤波的速度加一个权值wZ：
2c)根据当前量测的速度的权值wX和上一时刻滤波的速度的权值wZ修正上一时刻的滤波速度，得到更新后的滤波从而得到更新后的预测再根据当前时刻的预测计算当前时刻预测的量测
v’X＝wZ·vZ+wX·vX，
(3)根据目标航迹的参考状态判断当前量测Z(k)的可靠性：
3a)根据参考状态到更新后预测状态的位移a和参考状态到量测状态Z(k)的位移b，计算这两个位移之间的夹角α：
其中，＜a,b＞表示a与b之间的内积；
3b)根据当前量测Z(k)和当前预测的量测得到当前新息v(k)：
3c)根据角度α判断当前量测Z(k)的可靠性：
若cosα≥0，则表示当前量测可靠；
若cosα＜0，则表示当前量测不可靠，对当前新息v(k)加一个权值w，得到新的新息v'(k)，其中，w＜1；
(4)根据上一时刻的状态协方差P(k-1|k-1)得到一步预测协方差P(k|k-1)，进而计算出当前增益矩阵G(k)；
(5)根据当前预测状态和增益矩阵G(k)及新的新息v'(k)，更新当前时刻的滤波状态
(6)根据一步预测协方差P(k|k-1)和增益矩阵G(k)，更新当前状态协方差矩阵P(k|k)。
2.根据权利要求1所述方法，其中，步骤(2)中得到更新后的预测预测通过下式进行：
其中，F(k-1)为状态转移矩阵，为更新后的滤波。
3.根据权利要求1所述方法，其中，步骤(2)中根据当前时刻的预测计算当前时刻预测的量测通过下式进行：
其中，H(k)为量测矩阵。
4.根据权利要求1所述方法，其中，步骤(4)中根据上一时刻的状态协方差P(k-1|k-1)得到一步预测协方差P(k|k-1)，通过下式进行：
P(k|k-1)＝F(k-1)P(k-1|k-1)FT(k-1)
其中，F(k-1)为状态转移矩阵，FT(k-1)为F(k-1)的转置。
5.根据权利要求1所述方法，其中步骤(4)中根据一步预测协方差P(k|k-1)计算当前增益矩阵G(k)，通过下式进行：
G(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k))-1
其中，H(k)为量测矩阵，HT(k)为H(k)的转置,(H(k)P(k|k-1)HT(k))-1为H(k)P(k|k-1)HT(k)的逆。
6.根据权利要求1所述方法，其中，步骤(5)中根据当前预测状态和增益矩阵G(k)及新的新息v'(k)，更新当前时刻的滤波状态通过下式进行：
7.根据权利要求1所述方法，其中，步骤(6)中根据一步预测协方差P(k|k-1)和增益矩阵G(k)，更新当前状态协方差矩阵P(k|k)，通过下式进行：
P(k|k)＝P(k|k-1)-G(k)H(k)P(k|k-1)
其中，H(k)为量测矩阵。
CN201611030384.7A 2016-11-16 2016-11-16 基于有限步记忆的卡尔曼滤波方法 Active CN106772351B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611030384.7A CN106772351B (zh) 2016-11-16 2016-11-16 基于有限步记忆的卡尔曼滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611030384.7A CN106772351B (zh) 2016-11-16 2016-11-16 基于有限步记忆的卡尔曼滤波方法

Publications (2)

Publication Number Publication Date
CN106772351A CN106772351A (zh) 2017-05-31
CN106772351B true CN106772351B (zh) 2019-04-23

Family

ID=58970822

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611030384.7A Active CN106772351B (zh) 2016-11-16 2016-11-16 基于有限步记忆的卡尔曼滤波方法

Country Status (1)

Country Link
CN (1) CN106772351B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108469609B (zh) * 2018-06-11 2022-02-18 成都纳雷科技有限公司 一种用于雷达目标跟踪的检测信息滤波方法
CN109581353B (zh) * 2018-11-27 2022-11-25 北京信息科技大学 一种基于汽车雷达的多目标跟踪方法及***
CN109990789A (zh) * 2019-03-27 2019-07-09 广东工业大学 一种飞行导航方法、装置及相关设备
CN111289965B (zh) * 2019-12-04 2023-06-16 南京长峰航天电子科技有限公司 一种多目标雷达快速跟踪方法及***
CN111398948B (zh) * 2020-04-08 2021-12-10 成都汇蓉国科微***技术有限公司 一种强杂波背景下的机动小目标航迹关联方法
CN113628274A (zh) * 2021-08-16 2021-11-09 南京理工大学工程技术研究院有限公司 一种基于交互式多模型无色滤波的机动目标状态估计方法
CN114740448B (zh) * 2022-06-10 2022-09-02 南京隼眼电子科技有限公司 用于车载雷达的目标状态估计方法、装置及存储介质

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2008051204A2 (en) * 2005-10-13 2008-05-02 Bae Systems Information And Electronic Systems Integration Inc. Surface rf emitter passive ranging accuracy confirmation algorithm
WO2008103682A1 (en) * 2007-02-19 2008-08-28 Viewzi Inc. Multi-view internet search mashup
CN102157790A (zh) * 2010-02-12 2011-08-17 宗鹏 一种用于动中通的天线跟踪***
CN102721951B (zh) * 2012-05-04 2013-12-25 西安电子科技大学 一种高机动目标跟踪方法
CN102706345B (zh) * 2012-06-11 2015-01-28 杭州电子科技大学 一种基于衰减记忆序贯检测器的机动目标跟踪方法
CN104076342B (zh) * 2014-06-25 2016-05-18 西安电子科技大学 一种雷达跟踪状态下预测目标rcs的方法
CN104297748B (zh) * 2014-10-20 2017-03-08 西安电子科技大学 一种基于轨迹增强的雷达目标检测前跟踪方法
CN104793201B (zh) * 2015-05-04 2018-04-24 哈尔滨工业大学 一种跟踪临近空间高超声速目标的修正变结构网格交互多模型滤波方法

Also Published As

Publication number Publication date
CN106772351A (zh) 2017-05-31

Similar Documents

Publication Publication Date Title
CN106772351B (zh) 基于有限步记忆的卡尔曼滤波方法
CN108802707B (zh) 改进的用于目标跟踪的卡尔曼滤波方法
CN107688179B (zh) 基于多普勒信息辅助的综合概率数据互联方法
CN109472418B (zh) 基于卡尔曼滤波的机动目标状态预测优化方法
JP5784526B2 (ja) 目標追跡装置
CN106647257B (zh) 一种基于正交最小二乘的前馈控制方法
CN104793201B (zh) 一种跟踪临近空间高超声速目标的修正变结构网格交互多模型滤波方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导***行进间初始对准方法
CN104964683B (zh) 一种室内环境地图创建的闭环校正方法
CN112432644B (zh) 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法
Lammas et al. A 6 DoF navigation algorithm for autonomous underwater vehicles
CN104166989B (zh) 一种用于二维激光雷达点云匹配的快速icp方法
CN109765530B (zh) 一种运动平台雷达波束解耦方法
CN115143954B (zh) 一种基于多源信息融合的无人车导航方法
CN108710125A (zh) 用于目标跟踪的距离方位滤波方法
JP2011002266A (ja) 目標追尾処理器及びそれに用いる誤差共分散行列の補正方法
JP2014157113A (ja) 車両方位検出方法および車両方位検出装置
CN111291471A (zh) 一种基于l1正则无迹变换的约束多模型滤波方法
CN105372653B (zh) 面向岸基空管雷达***中一种高效转弯机动目标跟踪方法
CN110702093B (zh) 基于粒子滤波的定位方法、装置、存储介质及机器人
CN106291530B (zh) 一种基于最近邻法的概率数据关联优化方法
CN109582026B (zh) 基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法
Fan et al. A modified adaptive Kalman filtering method for maneuvering target tracking of unmanned surface vehicles
CN112986977A (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN108680162A (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
GR01 Patent grant
GR01 Patent grant