CN102788976B - 高量级扩展卡尔曼滤波方法 - Google Patents
高量级扩展卡尔曼滤波方法 Download PDFInfo
- Publication number
- CN102788976B CN102788976B CN201210218531.9A CN201210218531A CN102788976B CN 102788976 B CN102788976 B CN 102788976B CN 201210218531 A CN201210218531 A CN 201210218531A CN 102788976 B CN102788976 B CN 102788976B
- Authority
- CN
- China
- Prior art keywords
- target
- state
- filtering
- partiald
- moment
- 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.)
- Expired - Fee Related
Links
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明提供一种高量级扩展卡尔曼滤波方法,解决由远距离带来的系统量测方程非线性大,从而造成跟踪精度下降的问题。第一步:假设目标在二维平面内运动,分别建立目标的状态方程和量测方程;第二步:由k时刻的滤波结果得到目标的k+1时刻目标状态预测方程;第三步:计算预测协方差矩阵;第四步:计算扩展卡尔曼滤波器的增益矩阵;第五步:计算目标k+1的状态更新方程;第六步:计算目标k+1时刻的协方差更新误差;第七步:根据目标在k+1时刻的状态滤波向量和滤波误差的协方差矩阵,按第三步至第六步计算目标在k+2时刻的状态滤波向量和滤波误差的协方差矩阵,以此循环往复,最终的输出滤波结果再换回原来的量级。
Description
技术领域
本发明涉及一种高量级扩展卡尔曼滤波方法,属于目标运动状态估计领域。
背景技术
对目标运动状态进行估计时,学者Kalman将状态变量法引入到滤波理论中来,使状态空间描述与离散时间更新联系起来,对状态进行线性最小均方根误差估计,应用最为广泛,并称之为卡尔曼滤波方法。在雷达进行目标跟踪的过程中,由于在直角坐标系中易于对目标的运动状态进行描述,所以,目标状态方程通常是在直角坐标系中建立的。然而,对目标位置的量测通常是在极/球坐标系中得到的,即在极/球坐标系中,进行目标位置相对于雷达的距离、方位角或俯仰角(包括3D雷达的俯仰角)的量测。这就使得目标的运动状态与雷达量测值之间的关系是非线性的,雷达滤波系统必然是非线性系统,从而导致经典的Kalman滤波算法不能对目标进行跟踪。
卡尔曼滤波器是在线性高斯情况下利用最小均方误差准则(MMSE)获得目标状态估计的方法,但在实际应用中,许多情况下观测数据与目标状态参数间的关系是非线性的,即球/极坐标系和直角坐标系间的转换是非线性的,随着目标距离越远,坐标转换的非线性程度越大。
对于非线性滤波问题,至今尚未得到完善的解法。通常的处理方法是利用线性化技巧将非线性问题转化为近似的线性滤波问题,然后套用线性滤波理论得到求解原非线性滤波问题的次优滤波算法,其最常用的线性化方法是一阶泰勒级数展开,所得到的滤波方法是扩展卡尔曼滤波器(EKF)。然而,当目标相对于雷达的距离很远时,雷达极坐标的量测与目标在直角坐标系下的状态间的非线性会增大,导致传统的基于一阶泰勒展开的EKF发散,而SimonJulier提出的UKF[i]也会在非高斯噪声强非线性系统下无法很好的跟踪目标。
发明目的
本发明的目的在于解决由远距离带来的系统量测方程非线性大,从而造成跟踪精度下降的问题。
本发明的高量级扩展卡尔曼滤波方法,包括以下步骤:
第一步:假设目标在二维平面内运动,分别建立目标的状态方程和量测方程,如式(1)和式(2)所示;
X(k+1)=F(k)X(k)+V(k) (1)
其中X(k)是目标k时刻的状态向量,F(k)是状态转移矩阵,V(k)是目标运动的过程噪声,其协方差矩阵用Q(k)表示;Z(k+1)表示目标在k+1时刻的量测值,二维平面内包含两种量测量,即目标相对雷达的距离和方位角,分别用和arctan[y(k+1)/x(k+1)]表示,x(k+1)和y(k+1)是k+1目标在直角坐标系下x轴和y轴上的位置分量,W(k+1)是k+1时刻雷达的量测误差;
第二步:根据经典一阶扩展卡尔曼滤波算法,由k时刻的滤波结果得到目标的k+1时刻目标状态预测方程
第三步:计算预测协方差矩阵
P(k+1|k)=F(k)P(k|k)FT(k)+Q(k) (4)
其中P(k|k)是扩展卡尔曼滤波器k时刻滤波误差的协方差矩阵;
第四步:计算扩展卡尔曼滤波器的增益矩阵
其中,hX(k+1)是雅克比矩阵,表示为
第五步:计算目标k+1的状态更新方程
量测值Z(k+1)使用高量级单位,数值上变小;
第六步:计算目标k+1时刻的协方差更新误差
P(k+1|k+1)=P(k+1|k)-K(k+1)hX(k+1)P(k+1|k) (8)
第七步:根据目标在k+1时刻的状态滤波向量和滤波误差的协方差矩阵P(k+1|k+1),按第三步至第六步计算目标在k+2时刻的状态滤波向量和滤波误差的协方差矩阵P(k+2|k+2),以此循环往复,最终的输出滤波结果再换回原来的量级。
具体实施方式
本发明首先阐述坐标转换非线性度量问题,假设目标是在二维平面内运动,目标的观测值是在雷达极坐标下得到的,假设雷达极坐标的原点与直角坐标系的原点重合,且目标相对于雷达的真实距离为r0,真实方位角为a0,实际中,由于雷达的测角精度与测角精度的限制,雷达并不能获得目标真实的距离和方位角,而是包含量测误差的目标观测值,那么,目标在雷达极坐标下的量测值为,
其中,距离维量测噪声nr和方位维量测噪声na的量测噪声通常是统计独立的零均值的高斯白噪声。
通常目标状态是在直角坐标系下描述的,从而导致目标状态与雷达量测间的对应关系是非线性的。将极坐标系下的量测值转换到直角坐标系,得直角坐标系下的观测为,
而且坐标转换是有偏的,其偏差为,
本发明经过以上分析,可知由于坐标转换的非线性,使得坐标转换是有偏的。
本发明对极-直坐标转换曲率进行分析,若目标运动方程为y=f(x),且f(x)具有二阶导数,则表征目标运动曲线的曲率公式如下,
其中,曲率κ满足0≤κ<+∞,根据其含义可知,当κ越大时则目标运动的非线性程度越大,而当κ=0时,目标做直线运动,非线性程度最小。
将式(2)关于目标方位角a进行求曲率,得
将式(6)取平方并对目标距离r求偏导,可知,当0<r≤r0x时,κx随着r的增加而增加;当r≥r0x时,κx随着r的增加而减小。同理,当0<r≤r0y时,κy随着r的增加而增加;当r≥r0y时,κy随着r的增加而减小。其中,
定义雷达量测二维极-直坐标转换函数的曲率为
κ=max(κx,κy) (10)
当0<r≤r0κ时,κ随着r的增加而增加,当r≥r0x时,κ随着r的增加而减小。其中,
r0κ=max(r0x,r0y) (11)
当目标方位角位于坐标轴附近时,r0κ→∞,即随着目标距离的增加,曲率增大,进而非线性也会变大,可以采用高量级的单位表示目标距离。
通过对二维坐标转换非线性成因的分析可知,随着目标距离的增加,雷达极坐标系下的量测与对应的直角坐标系下的目标状态间的非线性关系增大。本发明针对远距离目标,提出使用高量级扩展卡尔曼滤波(HEKF)的方法对远距离目标进行跟踪,从而降低远距离的影响,提高远距离目标跟踪精度。
本发明描述的状态空间模型包括状态方程和量测方程,状态方程用来描述状态随时间演变的过程,量测方程表示雷达量测与目标状态间的关系。
状态方程设为
X(k+1)=f(X(k))+V(k) (12)
其中,X(k)为k时刻目标运动的状态矢量。f(·)为目标运动状态转移函数,当目标运动是非线性的,此函数呈现非线性。并假定过程噪声V(k)是加性零均值白噪声,其方差为,
E[V(k)VT(j)]=Q(k)δkj (13)
量测方程为,
Z k+1=h X k+1+W k+1 (14)
其中,h(·)为量测函数。一般情况下,由于雷达极坐标量测与直角坐标状态间的非线性关系,h(·)是非线性的。并假定量测噪声W(k+1)为加性零均值的高斯白噪声,其方差为,
E(W(k+1)WT(j))=R(k+1)δkj (15)
贝叶斯滤波理论是基于上述状态空间模型,利用所有已知信息来构造目标状态的后验概率密度,进而估计目标状态的最优方法。当状态空间模型呈现非线性时,EKF是贝叶斯估计的一种次优实现方法。它使用泰勒级数展开的一次项对非线性函数进行线性近似,然后在Kalman滤波框架下进行目标状态估计。
本发明考虑二维平面极坐标量测方程的情况,此时k+1时刻的雷达量测向量Z(k+1)=[r(k+1) a(k+1)]T,其中r(k+1)和a(k+1)分别为目标径向距离和方位角。相应的量测函数为,
显然式(16)是式(2)的逆变换,因而式(16)坐标转换的非线性是与式(2)式相对应的。若极坐标对直角坐标的坐标转换的非线性程度不高,则相应的直角坐标对极坐标的坐标转换的非线性程度亦不高,反之亦然。
本发明对目标运动状态进行估计时,卡尔曼滤波算法将状态变量法引入到滤波理论中来,将状态空间描述与离散时间更新联系起来,对状态进行线性最小均方根误差估计,应用最为广泛。在非线性滤波系统中,一阶扩展卡尔曼滤波(EKF)应用较多,EKF使用非线性方程的一阶泰勒展开来近似非线性方程,将非线性滤波系统进行线性化处理,从而可以将传统的卡尔曼滤波应用到非线性系统,得到目标状态的估计。
当目标相对雷达的距离较远时,传统的一阶扩展卡尔曼滤波(EKF)算法随着线性化误差的积累而发散,因此针对远距离目标,对一阶扩展卡尔曼滤波算法进行改进,得到高量级扩展卡尔曼滤波(HEKF)算法,其具体步骤如下,
目标状态的一步预测,
目标状态一步预测对应的协方差矩阵为,
fX(k)为函数f X k的Jacobian阵,
通过对式(22)取数学期望,并略去二阶及以上的高阶项,得到从k到k+1时刻的量测预测值
相应的量测的预测协方差(或新息协方差)为,
S(k+1)=hX(X(k+1))P(k+1|k)h′X(X(k+1))+R(k+1) (23)
其中,hX(X(k+1))为量测方程中的非线性函数在状态预测处进行一阶泰勒展开对应的雅可比矩阵,
式中
HEKF的增益为,
从而,可求得k+1时刻的状态更新方程为,
协方差更新方程
P(k+1|k+1)=[I-K(k+1)hX(k+1)]P(k+1|k)[I+K(k+1)hX(k+1)]T
(28)
-K(k+1)R(k+1)KT(k+1)
其中,I是与状态协方差矩阵同维的单位矩阵。
当目标相对雷达距离很远时,坐标转换非线性程度高,使用一阶泰勒近似得到的关于量测的预测和新息矩阵的式(22)和式(23)会很不准确,因此可以使用更高量级的单位来表示目标相对雷达的距离,将实际中的“远距离”目标转换成数据处理中的“近距离”目标,降低了非线性函数的线性化误差,使得量测的预测和协方差估计的更为准确,从而产生了高量级扩展卡尔曼滤波算法(HEKF)。
本发明针对目标在二维平面内均匀直线运动且雷达观测在极坐标获得的场景,对远距离和近距离目标分别进行跟踪,将高量级扩展卡尔曼滤波(HEKF)与EKF和UKF进行对比,验证了HEKF的有效性。
Claims (1)
1.高量级扩展卡尔曼滤波方法,其特征在于,包括以下步骤:
第一步:假设目标在二维平面内运动,分别建立目标的状态方程和量测方程,如式(1)和式(2)所示;
X(k+1)=F(k)X(k)+V(k) (1)
其中X(k)是目标k时刻的状态向量,F(k)是状态转移矩阵,V(k)是目标运动的过程噪声,其协方差矩阵用Q(k)表示;Z(k+1)表示目标在k+1时刻的量测值,二维平面内包含两种量测量,即目标相对雷达的距离和方位角,分别用和arctan[y(k+1)/x(k+1)]表示,x(k+1)和y(k+1)是k+1目标在直角坐标系下x轴和y轴上的位置分量,W(k+1)是k+1时刻雷达的量测误差;
第二步:根据经典一阶扩展卡尔曼滤波算法,由k时刻的滤波结果得到目标的k+1时刻目标状态预测方程
第三步:计算预测协方差矩阵
P(k+1|k)=F(k)P(k|k)FT(k)+Q(k) (4)
其中P(k|k)是扩展卡尔曼滤波器k时刻滤波误差的协方差矩阵;
第四步:计算扩展卡尔曼滤波器的增益矩阵
其中,hX(k+1)是雅克比矩阵,表示为
第五步:计算目标k+1的状态更新方程
量测值Z(k+1)使用高量级单位,数值上变小;
第六步:计算目标k+1时刻的协方差更新误差
P(k+1|k+1)=P(k+1|k)-K(k+1)hX(k+1)P(k+1|k) (8)
第七步:根据目标在k+1时刻的状态滤波向量和滤波误差的协方差矩阵P(k+1|k+1),按第三步至第六步计算目标在k+2时刻的状态滤波向量和滤波误差的协方差矩阵P(k+2|k+2),以此循环往复,最终的输出滤波结果再换回原来的量级;
算子R(k)的含义为量测噪声的协方差矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210218531.9A CN102788976B (zh) | 2012-06-27 | 2012-06-27 | 高量级扩展卡尔曼滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201210218531.9A CN102788976B (zh) | 2012-06-27 | 2012-06-27 | 高量级扩展卡尔曼滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102788976A CN102788976A (zh) | 2012-11-21 |
CN102788976B true CN102788976B (zh) | 2014-07-02 |
Family
ID=47154428
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201210218531.9A Expired - Fee Related CN102788976B (zh) | 2012-06-27 | 2012-06-27 | 高量级扩展卡尔曼滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102788976B (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103684349B (zh) * | 2013-10-28 | 2016-09-21 | 北京理工大学 | 一种基于递推协方差阵估计的卡尔曼滤波方法 |
CN104202019B (zh) * | 2014-08-25 | 2017-05-10 | 北京理工大学 | 带有未知过程噪声协方差阵递推估计的卡尔曼滤波方法 |
CN105631063B (zh) * | 2014-10-30 | 2019-01-08 | 北京航天长征飞行器研究所 | 一种基于目标特征的交汇时刻预报方法 |
CN106054171B (zh) * | 2016-05-27 | 2021-07-13 | 中国电子科技集团公司信息科学研究院 | 一种基于信息熵的多雷达节点自适应选择跟踪方法 |
CN106786561B (zh) * | 2017-02-20 | 2019-06-18 | 河海大学 | 一种基于自适应卡尔曼滤波的低频振荡模态参数辨识方法 |
US10564276B2 (en) * | 2017-03-02 | 2020-02-18 | GM Global Technology Operations LLC | Adaptive process noise description for improved kalman filter target tracking |
CN108872975B (zh) * | 2017-05-15 | 2022-08-16 | 蔚来(安徽)控股有限公司 | 用于目标跟踪的车载毫米波雷达滤波估计方法、装置及存储介质 |
CN109117965B (zh) * | 2017-06-22 | 2022-03-01 | 毫末智行科技有限公司 | 基于卡尔曼滤波器的系统状态预测装置和方法 |
CN107728124B (zh) * | 2017-09-08 | 2021-07-13 | 中国电子科技集团公司信息科学研究院 | 一种基于信息熵的多雷达动态调节方法及装置 |
CN107478990B (zh) * | 2017-09-11 | 2019-11-12 | 河海大学 | 一种发电机机电暂态过程动态估计方法 |
CN117195946B (zh) * | 2023-09-08 | 2024-06-18 | 兰州理工大学 | 一种基于扩展卡尔曼滤波的wsn机动目标跟踪方法 |
CN117219284B (zh) * | 2023-09-11 | 2024-05-07 | 湖北中医药大学 | 一种具有时序性智慧医疗大数据管理的系统 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7275008B2 (en) * | 2005-09-02 | 2007-09-25 | Nokia Corporation | Calibration of 3D field sensors |
CN102171583B (zh) * | 2008-10-06 | 2015-02-18 | 天宝导航有限公司 | 位置估计方法和设备 |
-
2012
- 2012-06-27 CN CN201210218531.9A patent/CN102788976B/zh not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN102788976A (zh) | 2012-11-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102788976B (zh) | 高量级扩展卡尔曼滤波方法 | |
CN106950562B (zh) | 一种基于预测值量测转换的状态融合目标跟踪方法 | |
CN107045125B (zh) | 一种基于预测值量测转换的交互多模型雷达目标跟踪方法 | |
CN110514225B (zh) | 一种矿井下多传感器融合的外部参数标定及精准定位方法 | |
CN104035083B (zh) | 一种基于量测转换的雷达目标跟踪方法 | |
CN108226920B (zh) | 一种基于预测值处理多普勒量测的机动目标跟踪系统及方法 | |
CN108896986B (zh) | 一种基于预测值的量测转换序贯滤波机动目标跟踪方法 | |
CN101661104B (zh) | 基于雷达/红外量测数据坐标转换的目标跟踪方法 | |
CN110501696B (zh) | 一种基于多普勒量测自适应处理的雷达目标跟踪方法 | |
CN105549049A (zh) | 一种应用于gps导航的自适应卡尔曼滤波算法 | |
CN101221238B (zh) | 基于高斯均值移动配准的动态偏差估计方法 | |
CN104182609B (zh) | 基于去相关的无偏转换量测的三维目标跟踪方法 | |
CN106443661A (zh) | 基于无迹卡尔曼滤波的机动扩展目标跟踪方法 | |
CN110209180B (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
CN105785358B (zh) | 一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法 | |
CN105954742B (zh) | 一种球坐标系下带多普勒观测的雷达目标跟踪方法 | |
CN112986906B (zh) | 一种半正定规划的rss-toa联合定位方法 | |
CN110231620B (zh) | 一种噪声相关系统跟踪滤波方法 | |
CN108134640A (zh) | 一种基于节点运动状态约束的协作定位系统和方法 | |
CN108303095B (zh) | 适用于非高斯系统的鲁棒容积目标协同定位方法 | |
CN101127121A (zh) | 一种基于自适应初始搜索点预测的目标跟踪算法 | |
CN103914628B (zh) | 一种空间遥操作系统输出状态预测方法 | |
Fan et al. | 3d mapping of multi-floor buildings based on sensor fusion | |
CN111190173B (zh) | 一种基于预测值量测转换的相控阵雷达目标跟踪方法 | |
CN104268597A (zh) | 基于ahcif的集中式测量值扩维融合方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20140702 Termination date: 20160627 |
|
CF01 | Termination of patent right due to non-payment of annual fee |