CN103312297B - 一种迭代扩展增量卡尔曼滤波方法 - Google Patents

一种迭代扩展增量卡尔曼滤波方法 Download PDF

Info

Publication number
CN103312297B
CN103312297B CN201310233364.XA CN201310233364A CN103312297B CN 103312297 B CN103312297 B CN 103312297B CN 201310233364 A CN201310233364 A CN 201310233364A CN 103312297 B CN103312297 B CN 103312297B
Authority
CN
China
Prior art keywords
measurement
formula
error
state
increment
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
Application number
CN201310233364.XA
Other languages
English (en)
Other versions
CN103312297A (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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN201310233364.XA priority Critical patent/CN103312297B/zh
Publication of CN103312297A publication Critical patent/CN103312297A/zh
Application granted granted Critical
Publication of CN103312297B publication Critical patent/CN103312297B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Complex Calculations (AREA)
  • Navigation (AREA)
  • Feedback Control In General (AREA)

Abstract

一种迭代扩展增量卡尔曼滤波方法,它包括以下步骤:(1)针对工程问题建立非线性离散增量系统;(2)对非线性离散增量系统进行线性化,得到线性化后的状态方程和增量量测方程;(3)对非线性离散增量系统的状态和误差方差阵进行初始化和时间更新;(4)进行量测更新时使用迭代方法;(5)得到相应的状态估计值和估计的误差方差阵。通过上述五个步骤,达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的,而且利用迭代方法优化对非线性量测方程进行线性化产生的误差,从而减少了发散现象,提高了滤波精度,增强了滤波稳定性。

Description

一种迭代扩展增量卡尔曼滤波方法
技术领域
本发明属于非线性滤波技术领域,具体涉及一种迭代扩展增量卡尔曼滤波方法。
背景技术
扩展卡尔曼滤波(EKF)目前已广泛应用于飞机和舰船的惯性导航系统、组合导航系统、卫星轨道/姿态的估计系统和探测器的导航系统等非线性动态系统中,对这类系统进行有效的状态估计。
扩展卡尔曼滤波方法的算法结构与线性卡尔曼滤波方法基本相同,都是通过对状态估计值和协方差进行时间更新和量测更新得到滤波结果。但是,扩展卡尔曼滤波方法对量测方程的精度要求较高,否则在滤波过程中会产生较大递推误差,甚至导致发散。在工程实际中,由于环境因素的影响、测量设备的不稳定性、模型和参数的选取不当等原因往往带有未知的系统误差,从而影响量测数据的精准性。但是,传统的扩展卡尔曼滤波方法对这种未知的系统误差无法进行补偿和校正,造成滤波精度低,甚至导致发散。文献“欠观测条件下的扩展增量卡尔曼滤波方法[J].航空动力学报,2012,27(4):777-781”针对非线性系统提出了一种扩展增量卡尔曼滤波方法,成功地消除了这种未知的系统误差,有效地提高了滤波精度。但是由于量测方程的非线性,使用泰勒级数展开进行线性化时不可避免地存在省略二阶以上项的误差,这些误差也会影响滤波的精度,造成滤波的不稳定性等问题。
发明内容一种迭代扩展增量卡尔曼滤波方法
本发明的目的是提供一种迭代扩展增量卡尔曼滤波方法(IteratedEIKF).为了改进现有扩展卡尔曼滤波方法不能消除未知的系统误差,滤波精度低甚至发散的现象.该方法利用量测方程的增量形式来消除量测方程中未知的系统误差,并利用迭代方法来优化对非线性量测方程进行线性化产生的误差,从而达到减少发散现象,提高滤波精度,增强滤波稳定性的效果。
本发明一种迭代扩展增量卡尔曼滤波方法,它包括以下五个步骤:
(a)建立非线性离散增量系统
针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程,特别是具有加性噪声的系统噪声和量测噪声的非线性系统,将初始量测数据做差处理,得到相应的非线性离散增量系统,该系统为:
Xk=fk-1(Xk-1)+Wk-1(1)
ΔZk=hk(Xk)-hk-1(Xk-1)+Vk(2)
式中,Xk为状态向量;fk(·)为非线性离散函数;Wk为系统噪声向量;ΔZk=Zk-Zk-1为量测向量增量;Zk为量测向量;hk(·)为非线性离散函数;Vk为量测噪声向量;k是指第k步,代表tk时刻。系统噪声向量Wk和量测噪声向量Vk的方差阵分别为Qk和Rk,并且Wk和Vk满足
E [ W k ] = 0 Cov [ W k , W j ] = E [ W k W j T ] = Q k δ kj E [ V k ] = 0 Cov [ V k , V j ] = E [ V k V j T ] = R k δ kj Cov [ W k , V j ] = E [ W k V j T ] = 0 - - - ( 3 )
在工程实际中,通常量测向量Zk存在系统误差,而量测向量增量ΔZk=Zk-Zk-1的系统误差为相对小量,可以忽略不计;
(b)对非线性离散增量系统进行线性化:首先,将式(1)中的非线性离散函数fk-1(Xk-1)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的状态方程
Xkk/k-1Xk-1+Uk-1+Wk-1(4)
式中,
Φ k / k - 1 = ∂ f k - 1 ( X k - 1 ) ∂ X k - 1 | X k - 1 = X ^ k - 1 - - - ( 5 )
U k - 1 = f k - 1 ( X ^ k - 1 ) - ∂ f k - 1 ( X k - 1 ) ∂ X k - 1 | X k - 1 = X ^ k - 1 · X ^ k - 1 - - - ( 6 )
然后,再将式(2)中的非线性离散函数hk(Xk)-hk-1(Xk-1)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的增量量测方程
ΔZk=HkXk-Hk-1Xk-1+Yk+Vk(7)
式中,
H k = ∂ h k ( X k ) ∂ X k | X k = X ^ k / k - 1 - - - ( 8 )
H k - 1 = ∂ h k - 1 ( X k - 1 ) ∂ X k - 1 | X k - 1 = X ^ k / k - 1 - - - ( 9 )
Y k = h k ( X ^ k / k - 1 ) - h k - 1 ( X ^ k - 1 ) - ( H k · X ^ k / k - 1 - H k - 1 · X ^ k - 1 ) - - - ( 10 )
通过上述过程,就得到了线性化后的状态方程和增量量测方程,式中Uk和Yk可看作确定性输入矩阵。
(c)对由式(4)和式(7)组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新:首先,对状态和误差方差阵分别赋予初值
X ^ 0 = E [ X 0 ] - - - ( 11 )
P 0 = E [ ( X 0 - X ^ 0 ) ( X 0 - X ^ 0 ) T ] - - - ( 12 )
其次,设第k-1步即tk-1时刻的状态估计值和误差方差阵分别为在此基础之上对第k步即tk时刻的状态向量Xk和误差方差阵Pk进行时间更新分别得到状态的一步预测
X ^ k / k - 1 = f k - 1 ( X ^ k - 1 ) - - - ( 13 )
和相应的一步预测的误差方差阵
P k / k - 1 = Φ k / k - 1 P k - 1 Φ k / k - 1 T + Q k - 1 - - - ( 14 )
(d)使用迭代方法进行量测更新:当i=1,2,3,…时,循环进行如下步骤:
(1)计算滤波的第i步增益矩阵
K k i = [ P k / k - 1 ( H k i ) T - Φ k / k - 1 P k - 1 ( H k - 1 i ) T ] ( Ω k i ) - 1 - - - ( 15 )
(2)计算第i步量测信息残差
ΔZ ~ k i = ΔZ - [ h k ( X ^ k i - 1 ) - h k - 1 ( X ^ k - 1 ) ] - - - ( 16 )
(3)得到第i步的状态估计
X ^ k i = X ^ k / k - 1 + K k i { ΔZ ~ k i - H K i [ X ^ k / k - 1 - X ^ k i - 1 ] } - - - ( 17 )
式中,
H k i = ∂ h k ( X k ) ∂ X k | X k = X ^ k i - 1 - - - ( 18 )
H k - 1 i = ∂ h k - 1 ( X k - 1 ) ∂ X k - 1 | X k - 1 = X ^ k - 1 - - - ( 19 )
Ω h i = H k i P k / k - 1 ( H k i ) T + R k - H k - 1 i P k - 1 Φ k / k - 1 T ( H k i ) T -
H k i Φ k / k - 1 P k - 1 ( H k - 1 i ) T + H k - 1 i P k - 1 ( H k - 1 i ) T ( 20 )
在上述使用迭代方法进行量测更新过程中,当所得状态向量的估计值满足向量的2范数满足的条件(其中阈值设为εlimit):
| | X ^ k i - X ^ k i - 1 | | 2 < &epsiv; limit - - - ( 21 )
时即停止计算。
(e)利用步骤(d)中的状态估计值及相应的参数,给出估计的误差方差阵
P k = P k / k - 1 - K k i &Omega; k i ( K k i ) T - - - ( 22 )
通过上述(a)、(b)、(c)、(d)、(e)五个步骤,达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的,而且利用迭代方法优化对非线性量测方程进行线性化产生的误差,从而减少了发散现象,提高了滤波精度,增强了滤波稳定性。
其中,步骤(a)中的初始量测数据做差处理是指将当前的测量值减去上一步的测量值,也即量测向量增量是通过公式ΔZk=Zk-Zk-1得到。(a)中的k取值为k=1,2,3,…,N,其中N由滤波时间和采样周期决定。比如当滤波时间为50秒,采样周期为1赫兹时,N=50/1=50。
其中,步骤(b)中的泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式:
式中,n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数。实际应用中,泰勒级数需要截断,只取有限项,因此会产生相应的截断误差。
其中,步骤(d)中的向量x=(x1,x2,…,xn)的2范数是x中各个元素平方之和再开根号,即
| | x | | 2 = x 1 2 + x 2 2 &CenterDot; &CenterDot; &CenterDot; + x n 2 - - - ( 24 )
本发明的有益效果是:本发明利用量测方程的增量形式消除了量测方程中未知的系统误差,从而建立了精确的增量量测方程。在此基础之上,利用迭代方法来优化对非线性量测方程进行线性化产生的误差,从而达到减少发散现象,提高滤波精度,增强滤波稳定性的效果。本发明提出的方法计算简单,便于工程应用。经仿真验证,滤波精度提高了54%,误差均值为0.0029,误差方差为0.0012,具有良好的稳定性。
附图说明
图1是本发明的迭代扩展增量卡尔曼滤波方法的流程图;
图2是本发明的迭代扩展增量卡尔曼滤波方法中步骤三的迭代量测更新流程图;
图3是利用本发明方法与现有滤波方法的一次独立试验的状态估计结果对比图;
图4是利用本发明方法与现有滤波方法的状态误差对比图。
具体实施方式
下面结合附图和实施例对本发明作详细说明。
本发明提出的一种迭代扩展增量卡尔曼滤波方法(IteratedEIKF),其流程图图1和迭代量测更新流程图图2所示,它包括以下五个步骤:
步骤一:针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程,特别是具有加性噪声的系统噪声和量测噪声的非线性系统,将初始量测数据做差处理,建立相应的非线性离散增量系统,该系统为:
Xk=fk-1(Xk-1)+Wk-1(1)
ΔZk=hk(Xk)-hk-1(Xk-1)+Vk(2)
式中,Xk为状态向量;fk(·)为非线性离散函数;Wk为系统噪声向量;ΔZk=Zk-Zk-1为量测向量增量;Zk为量测向量;hk(·)为非线性离散函数;Vk为量测噪声向量;k是指第k步,代表tk时刻。系统噪声向量Wk和量测噪声向量Vk的方差阵分别为Qk和Rk,并且Wk和Vk满足
E [ W k ] = 0 Cov [ W k , W j ] = E [ W k W j T ] = Q k &delta; kj E [ V k ] = 0 Cov [ V k , V j ] = E [ V k V j T ] = R k &delta; kj Cov [ W k , V j ] = E [ W k V j T ] = 0 - - - ( 3 )
步骤二:此步骤是在步骤一的基础之上对非线性离散增量系统进行线性化。首先,将式(1)中的非线性离散函数fk-1(Xk-1)围绕估计值展开成泰勒级数,仅保留一阶项,将二阶及以上项略去,便得线性化之后的状态方程
Xkk/k-1Xk-1+Uk-1+Wk-1(4)
式中,
&Phi; k / k - 1 = &PartialD; f k - 1 ( X k - 1 ) &PartialD; X k - 1 | X k - 1 = X ^ k - 1 - - - ( 5 )
U k - 1 = f k - 1 ( X ^ k - 1 ) - &PartialD; f k - 1 ( X k - 1 ) &PartialD; X k - 1 | X k - 1 = X ^ k - 1 &CenterDot; X ^ k - 1 - - - ( 6 )
然后,再将式(2)中的非线性离散函数hk(Xk)-hk-1(Xk-1)围绕估计值展开成泰勒级数,仅保留一阶项,将二阶及以上项略去,便得线性化之后的增量量测方程
ΔZk=HkXk-Hk-1Xk-1+Yk+Vk(7)
式中,
H k = &PartialD; h k ( X k ) &PartialD; X k | X k = X ^ k / k - 1 - - - ( 8 )
H k - 1 = &PartialD; h k - 1 ( X k - 1 ) &PartialD; X k - 1 | X k - 1 = X ^ k / k - 1 - - - ( 9 )
Y k = h k ( X ^ k / k - 1 ) - h k - 1 ( X ^ k - 1 ) - ( H k &CenterDot; X ^ k / k - 1 - H k - 1 &CenterDot; X ^ k - 1 ) - - - ( 10 )
通过上述过程,就得到了线性化之后的状态方程和增量量测方程,式中Uk和Yk可看作确定性输入矩阵。
其中,利用泰勒级数对非线性离散函数进行泰勒级数展开时,只取前两项。比如,对一个无穷可微的函数f(x)进行泰勒级数展开,只取f(x)=f(a)+f′(a)(x-a)。
以上两个步骤为前期准备过程,下面三个步骤是迭代扩展增量卡尔曼滤波方法的滤波过程。
步骤三:此步骤中主要对由式(4)和式(7)组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新:首先,对状态和误差方差阵分别赋予初值
X ^ 0 = E [ X 0 ] - - - ( 11 )
P 0 = E [ ( X 0 - X ^ 0 ) ( X 0 - X ^ 0 ) T ] - - - ( 12 )
其次,设第k-1步即tk-1时刻的状态估计值和误差方差阵分别为在此基础之上对第k步即tk时刻的状态向量Xk和误差方差阵Pk进行时间更新分别得到状态的一步预测
X ^ k / k - 1 = f k - 1 ( X ^ k - 1 ) - - - ( 13 )
和相应的一步预测的误差方差阵
P k / k - 1 = &Phi; k / k - 1 P k - 1 &Phi; k / k - 1 T + Q k - 1 - - - ( 14 )
其中,k取值为k=1,2,3,…,N,其中N由滤波时间和采样周期决定。比如当滤波时间为50秒,采样周期为1秒时,N=50/1=50。
步骤四:此步骤中主要是使用迭代方法进行量测更新,增强滤波的稳定性。当i=1,2,3,…时,循环进行如下步骤(参见迭代量测更新流程图图2):
(1)计算滤波的第i步增益矩阵
K k i = [ P k / k - 1 ( H k i ) T - &Phi; k / k - 1 P k - 1 ( H k - 1 i ) T ] ( &Omega; k i ) - 1 - - - ( 15 )
(2)计算第i步量测信息残差
&Delta;Z ~ k i = &Delta;Z k - [ h k ( X ^ k i - 1 ) - h k - 1 ( X ^ k - 1 ) - - - ( 16 )
(3)得到第i步的状态估计
X ^ k i = X ^ k / k - 1 + K k i { &Delta;Z ~ k i - H K i [ X ^ k / k - 1 - X ^ k i - 1 ] } - - - ( 17 )
式中,
H k i = &PartialD; h k ( X k ) &PartialD; X k | X k = X ^ k i - 1 - - - ( 18 )
H k - 1 i = &PartialD; h k - 1 ( X k - 1 ) &PartialD; X k - 1 | X k - 1 = X ^ k - 1 - - - ( 19 )
&Omega; h i = H k i P k / k - 1 ( H k i ) T + R k - H k - 1 i P k - 1 &Phi; k / k - 1 T ( H k i ) T -
H k i &Phi; k / k - 1 P k - 1 ( H k - 1 i ) T + H k - 1 i P k - 1 ( H k - 1 i ) T ( 20 )
在上述使用迭代方法进行量测更新过程中,当所得状态向量的估计值满足向量的2范数满足的条件(其中阈值设为εlimit):
| | X ^ k i - X ^ k i - 1 | | 2 < &epsiv; limit - - - ( 21 )
时即停止计算。
其中,阈值εlimit一般取一个较小的正数,它决定了对滤波的精度要求。比如取εlimit=0.1或者εlimit=0.001。
其中,向量x=(x1,x2,…,xn)的2范数是x中各个元素平方之和再开根号,通过公式 | | x | | 2 = x 1 2 + x 2 2 + &CenterDot; &CenterDot; &CenterDot; + x n 2 进行计算求得。
此上述迭代过程中,只取i=1时,即为扩展增量卡尔曼滤波方法(EKF)。
步骤五:利用步骤四中的状态估计值及相应的参数,给出估计的误差方差阵
P k = P k / k - 1 - K k i &Omega; k i ( K k i ) T - - - ( 22 )
按照步骤三、步骤四和步骤五中的公式(13)至(22)进行循环计算,即为全部的迭代扩展增量卡尔曼滤波过程。
针对本发明的方法,进行仿真验证,考虑下面的一维非线性离散系统:
xk=0.9xk-1+wk-1(23)
zk=xk+0.001ln(xk+1)+a+vk(24)
式中,wk和vk是独立的高斯白噪声序列,且系统噪声wk的均值q=0和方差Q=0.1,量测噪声vk的均值r=0和方差R=1。状态初始值为x0=10,误差阵初值维P0=0.1,量测系统误差a=3为未知量。采样周期为1s。
由于状态方程为线性方程,故只需要线性化增量量测方程,可得线性增量量测方程式(7),其中Hk和Hk-1分别为
H k = 1 + 0.001 X ^ k / k - 1 + 1 - - - ( 25 )
H k - 1 = 1 + 0.001 X ^ k - 1 + 1 - - - ( 26 )
从图3中可以看到,迭代扩展增量卡尔曼滤波得到的状态估计比扩展增量卡尔曼滤波的精度高,能够消除系统误差对滤波的影响,提高滤波的稳定性。图4给出两种滤波结果与真值的误差比较,滤波误差均值分别为0.0063和0.0029,误差方差分别为0.0106和0.0012,可以进一步看到,迭代扩展增量卡尔曼滤波的误差比扩展增量卡尔曼滤波的误差要小得多,能很好跟踪状态xk的变化。

Claims (1)

1.一种迭代扩展增量卡尔曼滤波方法,其特征在于:它包括以下步骤:
步骤一:建立非线性离散增量系统
针对工程实践中所遇到非线性离散增量系统中的状态方程和量测方程,特别是具有加性噪声的系统噪声和量测噪声的非线性系统,将初始量测数据做差处理,得到相应的非线性离散增量系统,该系统为:
Xk=fk-1(Xk-1)+Wk-1(1)
ΔZk=hk(Xk)-hk-1(Xk-1)+Vk(2)
式中,Xk为状态向量;fk(·)为非线性离散函数;Wk为系统噪声向量;ΔZk=Zk-Zk-1为量测向量增量;Zk为量测向量;hk(·)为非线性离散函数;Vk为量测噪声向量;k是指第k步,代表tk时刻;系统噪声向量Wk和量测噪声向量Vk的方差阵分别为Qk和Rk,并且Wk和Vk满足
E &lsqb; W k &rsqb; = 0 C o v &lsqb; W k , W j &rsqb; = E &lsqb; W k W j T &rsqb; = Q k &delta; k j E &lsqb; V k &rsqb; = 0 C o v &lsqb; V k , V j &rsqb; = E &lsqb; V k V j T &rsqb; = R k &delta; k j C o v &lsqb; W k , V j &rsqb; = E &lsqb; W k V j T &rsqb; = 0 - - - ( 3 )
在工程实际中,通常量测向量Zk存在系统误差,而量测向量增量ΔZk=Zk-Zk-1的系统误差为相对小量,忽略不计;
步骤二:对非线性离散增量系统进行线性化:首先,将式(1)中的非线性离散函数fk-1(Xk-1)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的状态方程
Xk=Φk/k-1Xk-1+Uk-1+Wk-1(4)
式中,
&Phi; k / k - 1 = &part; f k - 1 ( X k - 1 ) &part; X k - 1 | X k - 1 = X ^ k - 1 - - - ( 5 )
U k - 1 = f k - 1 ( X ^ k - 1 ) - &part; f k - 1 ( X k - 1 ) &part; X k - 1 | X k - 1 = X ^ k - 1 &CenterDot; X ^ k - 1 - - - ( 6 )
然后,再将式(2)中的非线性离散函数hk(Xk)-hk-1(Xk-1)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的增量量测方程:
ΔZk=HkXk-Hk-1Xk-1+Yk+Vk(7)
式中,
H k = &part; h k ( X k ) &part; X k | X k = X ^ k / k - 1 - - - ( 8 )
H k - 1 = &part; h k - 1 ( X k - 1 ) &part; X k - 1 | X k - 1 = X ^ k - 1 - - - ( 9 )
Y k = h k ( X ^ k / k - 1 ) - h k - 1 ( X ^ k - 1 ) - ( H k &CenterDot; X ^ k / k - 1 - H k - 1 &CenterDot; X ^ k - 1 ) - - - ( 10 )
通过上述过程,就得到了线性化后的状态方程和增量量测方程,式中Uk和Yk看作确定性输入矩阵;
步骤三;对由式(4)和式(7)组成的线性化之后的非线性离散增量系统的状态和误差方差阵进行初始化和时间更新:首先,对状态和误差方差阵分别赋予初值
X ^ 0 = E &lsqb; X 0 &rsqb; - - - ( 11 )
P 0 = E &lsqb; ( X 0 - X ^ 0 ) ( X 0 - X ^ 0 ) T &rsqb; - - - ( 12 )
其次,设第k-1步即tk-1时刻的状态估计值和误差方差阵分别为和Pk-1,在此基础之上对第k步即tk时刻的状态向量Xk和误差方差阵Pk进行时间更新分别得到状态的一步预测
X ^ k / k - 1 = f k - 1 ( X ^ k - 1 ) - - - ( 13 )
和相应的一步预测的误差方差阵
P k / k - 1 = &Phi; k / k - 1 P k - 1 &Phi; k / k - 1 T + Q k - 1 - - - ( 14 )
步骤四:使用迭代方法进行量测更新:当i=1,2,3,…时,循环进行如下步骤:
(1)计算滤波的第i步增益矩阵
K k i = &lsqb; P k / k - 1 ( H k i ) T - &Phi; k / k - 1 P k - 1 ( H k - 1 i ) T &rsqb; ( &Omega; k i ) - 1 - - - ( 15 )
(2)计算第i步量测信息残差
(3)得到第i步的状态估计
式中,
H k i = &part; h k ( X k ) &part; X k | X k = X ^ k i - 1 - - - ( 18 )
H k - 1 i = &part; h k - 1 ( X k - 1 ) &part; X k - 1 | X k - 1 = X ^ k - 1 - - - ( 19 )
&Omega; k i = H k i P k / k - 1 ( H k i ) T + R k - H k - 1 i P k - 1 &Phi; k / k - 1 T ( H k i ) T - H k i &Phi; k / k - 1 P k - 1 ( H k - 1 i ) T + H k - 1 i P k - 1 ( H k - 1 i ) T - - - ( 20 )
在上述使用迭代方法进行量测更新过程中,当所得状态向量的估计值满足向量的2范数满足的条件,其中阈值设为εlimit
| | X ^ k i - X ^ k i - 1 | | 2 < &epsiv; lim i t - - - ( 21 )
时即停止计算;
步骤五:利用步骤四中的状态估计值及相应的参数,给出估计的误差方差阵
P k = P k / k - 1 - K k i &Omega; k i ( K k i ) T - - - ( 22 )
通过上述步骤一、二、三、四、五,达到消除量测方程中未知的系统误差和建立精确的增量量测方程的目的,而且利用迭代方法优化对非线性量测方程进行线性化产生的误差,从而减少了发散现象,提高了滤波精度,增强了滤波稳定性;
其中,在步骤一中的初始量测数据做差处理是指将当前的测量值减去上一步的测量值,也即量测向量增量是通过公式ΔZk=Zk-Zk-1得到;步骤一中的k取值为k=1,2,3,…,N,其中N由滤波时间和采样周期决定;比如当滤波时间为50秒,采样周期为1赫兹时,N=50/1=50;
其中,在步骤二中的泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式:
f ( x ) = &Sigma; n = 0 &infin; f ( n ) ( a ) n ! ( x - a ) n - - - ( 23 )
式中,n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数;实际应用中,泰勒级数需要截断,只取有限项,因此会产生相应的截断误差;
其中,在步骤四中的向量x=(x1,x2,…,xn)的2范数是x中各个元素平方之和再开根号,即
| | x | | 2 = x 1 2 + x 2 2 + ... + x n 2 . - - - ( 24 )
CN201310233364.XA 2013-06-13 2013-06-13 一种迭代扩展增量卡尔曼滤波方法 Expired - Fee Related CN103312297B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310233364.XA CN103312297B (zh) 2013-06-13 2013-06-13 一种迭代扩展增量卡尔曼滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310233364.XA CN103312297B (zh) 2013-06-13 2013-06-13 一种迭代扩展增量卡尔曼滤波方法

Publications (2)

Publication Number Publication Date
CN103312297A CN103312297A (zh) 2013-09-18
CN103312297B true CN103312297B (zh) 2015-12-09

Family

ID=49137153

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310233364.XA Expired - Fee Related CN103312297B (zh) 2013-06-13 2013-06-13 一种迭代扩展增量卡尔曼滤波方法

Country Status (1)

Country Link
CN (1) CN103312297B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106646356B (zh) * 2016-11-23 2019-07-26 西安电子科技大学 一种基于卡尔曼滤波定位的非线性系统状态估计方法
CN107256537B (zh) * 2017-06-06 2020-06-19 桂林电子科技大学 一种设计两通道正交图滤波器组的设计方法
CN107765242A (zh) * 2017-09-16 2018-03-06 太原理工大学 基于状态增广迭代扩展卡尔曼滤波的系统状态估计方法
CN108665400A (zh) * 2018-04-28 2018-10-16 湖南城市学院 一种城乡规划管理案例系统
CN114003045B (zh) * 2021-12-30 2022-04-15 成都星宇融科电力电子股份有限公司 一种光电跟踪仪的目标跟踪方法、终端、可读存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101619985A (zh) * 2009-08-06 2010-01-06 上海交通大学 基于可变形拓扑地图的服务机器人自主导航方法
CN101820269A (zh) * 2010-04-08 2010-09-01 西北工业大学 迭代平方根中心差分卡尔曼粒子滤波方法
JP5089281B2 (ja) * 2007-07-26 2012-12-05 三菱電機株式会社 状態推定装置及び状態推定方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0589281A (ja) * 1991-09-26 1993-04-09 Fuji Facom Corp 誤読修正・検出方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5089281B2 (ja) * 2007-07-26 2012-12-05 三菱電機株式会社 状態推定装置及び状態推定方法
CN101619985A (zh) * 2009-08-06 2010-01-06 上海交通大学 基于可变形拓扑地图的服务机器人自主导航方法
CN101820269A (zh) * 2010-04-08 2010-09-01 西北工业大学 迭代平方根中心差分卡尔曼粒子滤波方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
一种改进扩展卡尔曼滤波新方法;杨宏等;《计算机工程与应用》;20101231;第46卷(第19期);第18-20页 *
欠观测条件下的扩展增量kalman滤波方法;傅惠民等;《航空动力学报》;20120430;第27卷(第4期);第777 -781页 *
自适应扩展增量kalman滤波方法;傅惠民等;《航空动力学报》;20100831;第27卷(第8期);第1734-1737页 *

Also Published As

Publication number Publication date
CN103312297A (zh) 2013-09-18

Similar Documents

Publication Publication Date Title
CN103312297B (zh) 一种迭代扩展增量卡尔曼滤波方法
CN103278813B (zh) 一种基于高阶无迹卡尔曼滤波的状态估计方法
CN104020480B (zh) 一种带自适应因子的交互式多模型ukf的卫星导航方法
CN109000642A (zh) 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN104112079A (zh) 一种模糊自适应变分贝叶斯无迹卡尔曼滤波方法
CN104121907B (zh) 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法
CN103344260B (zh) 基于rbckf的捷联惯导系统大方位失准角初始对准方法
CN108592945A (zh) 一种惯性/天文组合系统误差的在线标定方法
CN104408744A (zh) 一种用于目标跟踪的强跟踪容积卡尔曼滤波方法
CN108319570B (zh) 一种异步多传感器空时偏差联合估计与补偿方法及装置
CN105954742B (zh) 一种球坐标系下带多普勒观测的雷达目标跟踪方法
CN105224737A (zh) 一种空间目标轨道改进初值修正方法
CN105043388A (zh) 基于惯性/重力匹配组合导航的向量搜索迭代匹配方法
CN107290742B (zh) 一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法
Yu et al. A SLAM algorithm based on adaptive cubature kalman filter
CN104374405A (zh) 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法
CN103973263A (zh) 一种新的逼近滤波方法
CN104048676A (zh) 基于改进粒子滤波的mems陀螺随机误差补偿方法
CN103776449A (zh) 一种提高鲁棒性的动基座初始对准方法
CN101299271A (zh) 一种机动目标状态方程的多项式预测模型及跟踪方法
CN104199054A (zh) 一种用于北斗卫星导航系统共视数据的预处理方法
CN103335653B (zh) 火星大气进入段的自适应增量粒子滤波方法
Garcia et al. Localization using a particle filter and magnetic induction transmissions: Theory and experiments in air
CN105549003A (zh) 一种汽车雷达目标跟踪方法
Bezručka The use of a Kalman filter in geodesy and navigation

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: 20151209

Termination date: 20170613

CF01 Termination of patent right due to non-payment of annual fee