CN110311652A - 一种欠观测条件下的增量求积分卡尔曼滤波方法 - Google Patents
一种欠观测条件下的增量求积分卡尔曼滤波方法 Download PDFInfo
- Publication number
- CN110311652A CN110311652A CN201910511809.3A CN201910511809A CN110311652A CN 110311652 A CN110311652 A CN 110311652A CN 201910511809 A CN201910511809 A CN 201910511809A CN 110311652 A CN110311652 A CN 110311652A
- Authority
- CN
- China
- Prior art keywords
- equation
- integral
- measurement
- gaussian
- integration
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 230000002950 deficient Effects 0.000 title abstract 3
- 238000001914 filtration Methods 0.000 claims abstract description 80
- 238000005259 measurement Methods 0.000 claims abstract description 67
- 230000010354 integration Effects 0.000 claims description 58
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000009826 distribution Methods 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 38
- 239000002245 particle Substances 0.000 description 5
- 238000009499 grossing Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- JJWKPURADFRFRB-UHFFFAOYSA-N carbonyl sulfide Chemical compound O=C=S JJWKPURADFRFRB-UHFFFAOYSA-N 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
- H03H17/0248—Filters characterised by a particular frequency response or filtering method
- H03H17/0255—Filters based on statistics
- H03H17/0257—KALMAN filters
Landscapes
- Physics & Mathematics (AREA)
- Probability & Statistics with Applications (AREA)
- Engineering & Computer Science (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Filters That Use Time-Delay Elements (AREA)
Abstract
本发明公开的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,包括以下步骤:步骤1:通过相邻时刻的量测值之差建立非线性系统的量测增量方程;步骤2:根据非线性系统状态方程通过贝叶斯滤波理论计算得到系统的预测状态值;步骤3:通过步骤1中的量测增量方程对步骤2中得到的系统的预测状态值进行修正,得到系统的更新状态值。本发明一种欠观测条件下的增量求积分卡尔曼滤波方法,针对非线性滤波问题,当系统中存在未知的量测系统误差时,采用本发明增量求积分卡尔曼滤波方法,与标准的求积分卡尔曼滤波算法相比较,能够消除这种未知的量测系统误差,并且获得更高的滤波精度。
Description
技术领域
本发明属于目标跟踪技术领域,具体涉及一种欠观测条件下的增量求积分卡尔曼滤波方法。
背景技术
目标跟踪技术的主要思想是采用各种量测和计算方法,实现对被量测目标的系统建模、状态估计以及连续跟踪的过程。从上世纪70年代末开始该技术公开出版在各种技术文献中,国内外许多研究人员经过几十年持续性研究使得该技术得到了飞速的发展。随着航空、航天、航海事业的不断发展以及现代战争信息化特征的日益凸显,对海底、海面、陆面、空中以及太空中目标跟踪技术的精确性要求不断提高。该技术在军事领域发挥着至关重要的作用,例如:军事指挥自动化系统、战略预警与防御以及精确制导武器等。此外,该技术也广泛应用于民用领域,例如:导航系统、医学诊断、故障诊断等。
目标跟踪技术通常包含三个部分:数据关联、状态估计以及融合、航迹管理,其中状态估计以及融合是在目标运动模型的基础上进行估计、预测以及平滑的过程,状态估计问题分为预测、滤波以及平滑,预测是滤波的基础,滤波是平滑的基础,因此我们讨论的重点是目标跟踪技术中的滤波以及融合方法,滤波算法作为目标跟踪系统中的重要组成部分,直接决定着目标跟踪的精确程度以及整个系统的时间复杂度。
在状态估计问题中,如果系统为线性且噪声为高斯分布,则使用卡尔曼滤波算法(Kalman Filter,KF)对目标状态进行最优估计,可以获得最小方差。而在实际应用过程中,目标跟踪问题多数为非线性系统,针对非线性高斯系统,通常使用扩展卡尔曼滤波(ExtendedKalman Filter,EKF),该算法在滤波值附近采用泰勒级数展开法将系统的非线性方程展开,并截取前一阶项或二阶项,从而将原非线性系统近似化为线性系统,然后采用标准卡尔曼滤波算法进行滤波。虽然扩展卡尔曼滤波算法计算量小,但该算法忽略了非线性方程泰勒级数展开的高阶项,仅采用其一阶或二阶项,使得非线性方程只在局部线性化,对于强非线性系统容易造成滤波过程发生扩散。为了进一步提高非线性系统的滤波精度,出现了粒子滤波算法(Particle Filter,PF)、不敏卡尔曼滤波(Unscented KalmanFilter,UKF)、求积分卡尔曼滤波(Quadrature Kalman Filtering,QKF),这些方法均使用采样方法近似化非线性函数的概率密度,能够取得更高的滤波精度。PF算法的滤波精度可以逼近最优估计,是一种很有效的非线性滤波技术,但是该算法需要随机产生大量粒子才能很好地近似系统的后验概率密度,观测环境越复杂,尤其是当状态向量的维数较高时,需要采样的粒子数就更多,算法的时间复杂度就越会高。UKF算法的核心思想通过非线性变换—U变换,该变换利用西格玛点集近似获取非线性系统真实的均值和方差,其精度可以达到二阶(泰勒展开)。该算法不需要对非线性系统进行线性化近似,即使系统模型复杂,也不会增加算法实现的难度,从而也得到了广泛的应用。然而,当系统维数达到三阶及以上时,其滤波精度会有所降低。随后,Arasaratnam和Haykin等人提出了一种新的非线性滤波算法QKF,与UKF滤波算法相似,同样采用确定点采样方法,是一种基于高斯-厄米特积分准则的递归贝叶斯滤波方法,采用加权积分点近似获取非线性函数的概率密度,选取的积分点根据求积分点个数的不同而不同,因此能够取得比UKF稍高的滤波精度。学者们为了进一步提高QKF的滤波精度,在该算法的基础上提出了一系列改进算法,例如平方根求积分卡尔曼滤波算法(Square-Root Quadrature Kalman Filtering,SQKF)、求积分卡尔曼粒子滤波算法(Quadrature Kalman Particle Filter,QKRF)等。
然而,在许多实际应用中,特别是在航天领域深空探测等工程实践中,由于太空环境因素的影响(包括太阳风、磁场、宇宙射线等),量测设备自身不稳定而产生的误差,系统模型和参数选取不当等因素,往往会造成非线性滤波系统中的量测系统误差随时间变化而漂移。此时,采用标准的非线性滤波算法很难消除这种未知的量测系统误差。为了解决该问题,前人提出了增量扩展卡尔曼滤波算法(Incremental Extended Kalman Filtering,IEKF)、增量容积卡尔曼滤波算法(Incremental Cubature Kalman Filtering,ICKF),增量粒子滤波算法(Incremental Particle Filtering,IPKF)等。目前增量滤波算法比较少,提出一种新的增量滤波算法具有重要意义。
发明内容
本发明的目的在于提供一种欠观测条件下的增量求积分卡尔曼滤波方法,解决了非线性状态估计问题中,由于周围环境因素的影响、量测设备不稳定性、模型和参数选取不当的因素,造成量测误差随时间变化而漂移的问题。
本发明所采用的技术方案是:一种欠观测条件下的增量求积分卡尔曼滤波方法,包括以下步骤:
步骤1:通过相邻时刻的量测值之差建立非线性系统的量测增量方程;
步骤2:根据非线性系统状态方程通过贝叶斯滤波理论计算得到系统的预测状态值;
步骤3:根据步骤1中的量测增量方程对步骤2中得到的系统的预测状态值进行修正,得到系统的更新状态值。
本发明的特点还在于,
步骤1具体包括:
假定非线性系统如下:
xk=f(xk-1)+wk-1 (1)
zk=h(xk)+ak+vk (2)
其中,表示状态变量;f(·)表示系统非线性状态函数;表示量测向量;h(·)表示系统非线性量测函数;wk-1和vk依次为互不相关的零均值过程噪声和量测噪声,且均服从高斯分布,其方差分别为Qk和Rk;ak表示未知的量测系统误差;
令:
Δzk=zk-zk-1=h(xk)-h(xk-1)+ak-ak-1+vk-vk-1 (3)
在实际工程应用中,当对系统状态值进行采样密度足够密集时,式(3)中ak-ak-1的取值趋近于零,由此得:
Δzk=zk-zk-1=h(xk)-h(xk-1)+vk-vk-1 (4)
其中,将Vk=vk-vk-1定义为增量量测系统随机噪声分量,式(4)转换为:
Δzk=zk-zk-1=h(xk)-h(xk-1)+Vk (5)
由于vk满足高斯分布,而两个高斯分布的随机变量之和或者之差仍然服从高斯分布,同时它们的均值为两个高斯分布随机变量之和或者之差,那么,随机噪声分量的协方差即可定义为:
cov(Vk)=cov(vk)+cov(vk-1)±2cov(vk,vk-1) (6)
而对于相邻时刻的量测噪声分量而言它们是互不相关的,可得:
cov(Vk)=cov(vk)+cov(vk-1)=Rk+Rk-1 (7)
式(5)即为量测增量方程。
步骤2具体包括:
针对式(1)非线性系统的状态方程,已知k-1时刻的先验概率密度过程噪声p(wk-1)~N(0;Qk-1),根据贝叶斯理论,得k时刻非线性系统的预测状态值及其协方差Pk|k-1如下:
步骤3具体包括:
针对式(5)非线性系统量测增量方程,假定k时刻的预测概率密度过程噪声p(Vk)~N(0;Rk),根据贝叶斯理论,得k时刻状态的估计值及其协方差Pk|k如下:
其中,为预测增量量测值:
PΔzΔz,k|k-1为协方差矩阵:
PxΔz,k|k-1为状态向量和量测向量之间的协方差矩阵:
Wk为增益矩阵:
根据高斯-厄米特积分准则的积分点积分方法,通过积分点加权求和的方式对步骤2和步骤3中的积分近似求解,具体为:
假定可积非线性函数f(x),其积分表示为:
其中,ω(x)是一个高斯密度函数,如果该积分不存在解析解时,则通过数值积分的方法对其进行近似求解,即找到一组具有权值的点集,对这些点集通过加权求和的方式对积分I(f)进行近似得:
其中,ξl是积分点,ωl是相应的权值,m表示积分点的个数;
根据高斯-厄米特积分准则,计算一个标准高斯加权积分,其数值化近似为:
其中积分点及其权值的计算方法为:假定J是一个具有0对角元素的对称三对角矩阵,且则积分点的计算表示为其中εl是矩阵J第l个特征值,其对应权值计算为其中(vl)1是矩阵J第l个归一化特征向量的第一个元素;
对于高斯密度函数其中为其均值,P为相应的方差,根据高斯-厄米特积分准则,得其高斯加权积分的数值化近似为:
上式中Xl计算如下:
步骤2中式(12)和式(13)的求解步骤为:
首先,在状态空间中找到m个积分点及其权值这些积分点的均值为xk-1|k-1,协方差为Pk-1|k-1,根据式(24)得:
其中,Pk-1|k-1满足
其次,将积分点通过非线性函数(1)传播后,得一步预测积分点
最后,根据式(23)高斯-厄米特积分准则,将式(12)做如下变换,
式(13)协方差,通过积分点的加权和求得,
步骤3中式(16)、式(17)和式(18)的求解步骤为:
首先,在状态空间中找到m个回归点及其权值这些回归点的均值为协方差为Pk|k-1,根据式(24)得:
其中,预测协方差Pk|k-1应满足
其次,根据式(5)计算增量量测积分点
ΔZl,k|k-1=h(Xl,k|k-1)-h(Xl,k-1|k-1) (30)
最后,根据式(23)高斯-厄米特积分准则,将式(16)做如下转换
式(17)增量量测的新息协方差矩阵,通过积分点的加权和求得,
式(18)互协方差矩阵,通过积分点的加权和求得,
利用式(19)计算增益矩阵,利用式(14)、式(15)分别计算估计值及其协方差Pk|k。
本发明的有益效果是:本发明一种欠观测条件下的增量求积分卡尔曼滤波方法,针对非线性滤波问题,当系统中存在未知的量测系统误差时,采用本发明增量求积分卡尔曼滤波方法,与标准的求积分卡尔曼滤波算法相比较,能够消除这种未知的量测系统误差,并且获得更高的滤波精度。
附图说明
图1是采用本发明欠观测条件下的增量求积分卡尔曼滤波方法与标准的求积分卡尔曼滤波方法得到的系统真实状态值、求积分卡尔曼滤波更新状态值以及增量求积分卡尔曼滤波更新状态值的曲线对比图;
图2是非线性系统的真实状态值和求积分卡尔曼滤波更新状态值之间的均方根误差、非线性系统的真实状态值和增量求积分卡尔曼滤波更新状态值之间的均方根误差对比图。
具体实施方式
下面结合附图以及具体实施方式对本发明进行详细说明。
步骤1:通过相邻时刻的量测值之差建立量测增量方程,从而消除这种未知的量测系统误差。
现假定增量非线性状态估计系统如下:
xk=f(xk-1)+wk-1 (1)
zk=h(xk)+ak+vk (2)
其中,表示状态变量;f(·)系统非线性状态函数;表示量测向量;h(·)系统非线性量测函数;wk-1和vk分别为互不相关的零均值过程噪声和量测噪声,且均服从高斯分布,其方差分别为Qk和Rk;ak表示未知的量测系统误差。此时,若直接采用标准求积分卡尔曼滤波算法,由于未知量测系统误差的存在,将会导致状态估计值出现较大偏差,滤波精度降低。为了解决该问题,提高滤波精度,本发明中引入量测增量方程,令:
Δzk=zk-zk-1=h(xk)-h(xk-1)+ak-ak-1+vk-vk-1 (3)
在实际工程应用中,当对系统状态值进行采样密度足够密集时,上式中ak-ak-1的取值是趋近于零的,可得:
Δzk=zk-zk-1=h(xk)-h(xk-1)+vk-vk-1 (4)
其中,将Vk=vk-vk-1定义为增量量测系统随机噪声分量,上式可转换为:
Δzk=zk-zk-1=h(xk)-h(xk-1)+Vk (5)
由于vk满足高斯分布,而两个高斯分布的随机变量之和或者之差仍然服从高斯分布,同时它们的均值为两个高斯分布随机变量之和或者之差。那么,随机噪声分量的协方差即可定义为:
cov(Vk)=cov(vk)+cov(vk-1)±2cov(vk,vk-1) (6)
而对于相邻时刻的量测噪声分量而言它们是互不相关的,可得:
cov(Vk)=cov(vk)+cov(vk-1)=Rk+Rk-1 (7)
那么,式(5)即为量测增量方程,通过引入量测增量方程即可消除量测系统中的未知误差。
步骤2:假定过程噪声wk-1服从高斯分布,根据贝叶斯滤波理论,通过增量求积分卡尔曼滤波算法的预测更新步骤,计算得到系统的预测状态值。下面,首先给出贝叶斯滤波的框架,然后给出系统预测状态值的计算方法。
现考虑如下的非线性离散系统:
xk=f(xk-1)+wk-1 (8)
ck=h(xk)+vk (9)
式中,分别表示状态变量和量测向量;f(·),h(·)分别为系统非线性状态函数和非线性量测函数;wk-1和vk分别为过程噪声和量测噪声,且都是零均值高斯的,其方差分别为Qk和Rk。
在贝叶斯框架下,非线性滤波算法的实现是通过k时刻之前的所有的量测值集合Ck={z1,z2,…,zk}递归地取得状态的后验概率密度p(xk|Ck),从而得到状态向量的均值和方差。根据贝叶斯理论,非线性滤波过程可分为预测和更新两个步骤。
预测:
假定k-1时刻的先验概率密度p(xk-1|Ck-1)是已知的,那么,通过系统状态方程式(8)与过程噪声分布p(wk-1),可计算得到k时刻状态的预测概率密度p(xk|Ck-1),即:
其中,p(xk|xk-1)是状态转移概率密度,可由系统状态方程确定。
更新:
假定k时刻的量测值ck是已知的,那么,通过系统量测方程式(9)与量测噪声分布p(vk),可计算得到k时刻状态的后验概率密度p(xk|Ck),即:
其中,p(ck|xk)是似然概率密度,可由量测方程确定。p(ck|Ck-1)是量测的预测概率密度,计算公式为
式(10)和式(11)构成了贝叶斯滤波算法,在得到系统状态的后验概率密度p(xk|Ck)后,即可通过最小方差、极大似然等估计准则计算得到状态的均值及方差。
现根据上述贝叶斯滤波理论给出系统预测状态值的计算方法。
针对式(1)系统状态方程,现假定非线性系统的过程噪声服从高斯分布,并且已知k-1时刻的先验概率密度过程噪声p(wk-1)~N(0;Qk-1),根据贝叶斯理论,可得k时刻系统的预测状态值及其协方差Pk|k-1。
步骤3:通过增量求积分卡尔曼滤波算法的量测更新步骤对系统的预测状态值进行修正,从而得到系统的更新状态值
针对式(5)系统量测增量方程,现假定k时刻的预测概率密度过程噪声p(Vk)~N(0;Rk),根据贝叶斯理论,可得k时刻状态的估计值及其协方差Pk|k。
其中,为预测增量量测值:
PΔzΔz,k|k-1为协方差矩阵:
PxΔz,k|k-1为状态向量和量测向量之间的协方差矩阵:
Wk为增益矩阵:
由于上述步骤2、步骤3贝叶斯滤波过程中存在着多处的非线性函数和高斯概率密度函数乘积的积分形式,如式(12)、式(13)、式(16)、式(17)及式(18)所示,而这种积分通常不解析。因此,后续的关键问题就在于如何找到该积分的数值解法,下面给出基于高斯-厄米特积分准则的积分点积分方法。现考虑一个可积非线性函数f(x),其积分表示为:
其中,ω(x)是一个高斯密度函数。如果该积分不存在解析解时,则可通过数值积分的方法对其进行近似求解,即找到一组具有权值的点集,对这些点集通过加权求和的方式对积分I(f)进行近似可得:
其中,ξl是积分点,ωl是相应的权值,m表示积分点的个数,在实际应用过程中,m通常取值为3。
根据上述高斯-厄米特积分准则,计算一个标准高斯加权积分,其数值化近似为:
其中积分点及其权值的计算方法为:假定J是一个具有0对角元素的对称三对角矩阵,且则积分点的计算可以表示为其中εl是矩阵J第l个特征值,其对应权值计算为其中(vl)1是矩阵J第l个归一化特征向量的第一个元素。
对于高斯密度函数其中为其均值,P为相应的方差,根据上述高斯-厄米特积分准则,可得其高斯加权积分的数值化近似为:
上式中Xl计算如下:
那么,对于带有高斯加权函数的非线性函数的积分计算,即可通过积分点及其权值的加权求和近似获得。
针对步骤2中出现的积分,即式(12)、式(13),根据上述高斯-厄米特积分准则的积分点积分方法,可通过积分点加权求和的方式对积分近似求解:
首先,在状态空间中可找到m个积分点及其权值这些积分点的均值为xk-1|k-1,协方差为Pk-1|k-1,根据式(24)可得:
其中,Pk-1|k-1满足
其次,将积分点通过非线性函数(1)传播后,可得一步预测积分点
最后,根据式(23)高斯-厄米特积分准则,可将式(12)做如下变换,
式(13)协方差,也可通过积分点的加权和求得,
针对步骤3中出现的积分,即式(16)、式(17)及式(18),同样可通过积分点加权求和的方式对积分近似求解:
首先,在状态空间中可找到m个回归点及其权值这些回归点的均值为协方差为Pk|k-1,根据式(24)可得:
其中,预测协方差Pk|k-1应满足
其次,根据式(5)计算增量量测积分点
ΔZl,k|k-1=h(Xl,k|k-1)-h(Xl,k-1|k-1) (30)
最后,根据式(23)高斯-厄米特积分准则,可将式(16)做如下转换
式(17)增量量测的新息协方差矩阵,可通过积分点的加权和求得,
式(18)互协方差矩阵,同样可通过积分点的加权和求得,
利用式(19)计算增益矩阵,利用式(14)、式(15)分别计算估计值及其协方差Pk|k。
结果分析:
下面以一具体实例说明本发明方法的有效性,现考虑如下的一维非线性系统:
xk=0.9xk-1+wk-1 (34)
zk=xk+0.001ln(xk+1)+ak+vk (35)
其中,wk和vk是相互独立的过程噪声与量测噪声,且服从均值为0方差为Q和R的高斯分布。实验仿真中设Q=0.1,R=1;量测噪声ak=3;初始状态值x0=10,相应的初始协方差P0=0.1。
为验证本发明的有效性,对由式(34)和式(35)所构成的具有量测误差的非线性系统通过MTALAB进行仿真,仿真步数为100,滤波过程分别采用标准的求积分卡尔曼滤波算法以及本发明增量求积分卡尔曼滤波算法进行滤波处理,滤波结果如图1和图2所示。图1是中显示的是系统的真实状态值、求积分卡尔曼滤波更新状态值以及增量求积分卡尔曼滤波更新状态值。由图1可知,对含有未知量测噪声的非线性系统进行滤波,本发明增量求积分卡尔曼滤波算法与标准的求积分卡尔曼滤波算法相比较,采用本发明所获得的状态更新值更加趋近于真实状态值,能够获得更好的滤波效果。
图2分别计算了系统的真实状态值和求积分卡尔曼滤波更新状态值之间的均方根误差、系统的真实状态值和增量求积分卡尔曼滤波更新状态值之间的均方根误差,从而构成了图2所示的两组均方根误差的对比图。由图2可知,增量求积分卡尔曼滤波算法的误差在0值附近,而标准求积分卡尔曼滤波算法的误差在-2.5值附近。然而,并不是任意时刻中增量求积分卡尔曼滤波算法的误差均比标准求积分卡尔曼滤波算法的误差低,其中第1步和第2步,本发明的误差较大。但从整体滤波效果来看,增量求积分卡尔曼滤波算法取得的滤波精度是高于标准求积分卡尔曼滤波算法的。
Claims (7)
1.一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,包括以下步骤:
步骤1:通过相邻时刻的量测值之差建立非线性系统的量测增量方程;
步骤2:根据非线性系统状态方程通过贝叶斯滤波理论计算得到系统的预测状态值;
步骤3:根据步骤1中的量测增量方程对步骤2中得到的系统的预测状态值进行修正,得到系统的更新状态值。
2.如权利要求1所述的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,所述步骤1具体包括:
假定非线性系统如下:
xk=f(xk-1)+wk-1 (1)
zk=h(xk)+ak+vk (2)
其中,表示状态变量;f(·)表示系统非线性状态函数;表示量测向量;h(·)表示系统非线性量测函数;wk-1和vk依次为互不相关的零均值过程噪声和量测噪声,且均服从高斯分布,其方差分别为Qk和Rk;ak表示未知的量测系统误差;
令:
Δzk=zk-zk-1=h(xk)-h(xk-1)+ak-ak-1+vk-vk-1 (3)
在实际工程应用中,当对系统状态值进行采样密度足够密集时,式(3)中ak-ak-1的取值趋近于零,由此得:
Δzk=zk-zk-1=h(xk)-h(xk-1)+vk-vk-1 (4)
其中,将Vk=vk-vk-1定义为增量量测系统随机噪声分量,式(4)转换为:
Δzk=zk-zk-1=h(xk)-h(xk-1)+Vk (5)
由于vk满足高斯分布,而两个高斯分布的随机变量之和或者之差仍然服从高斯分布,同时它们的均值为两个高斯分布随机变量之和或者之差,那么,随机噪声分量的协方差即可定义为:
cov(Vk)=cov(vk)+cov(vk-1)±2cov(vk,vk-1) (6)
而对于相邻时刻的量测噪声分量而言它们是互不相关的,可得:
cov(Vk)=cov(vk)+cov(vk-1)=Rk+Rk-1 (7)
式(5)即为量测增量方程。
3.如权利要求2所述的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,所述步骤2具体包括:
针对式(1)非线性系统的状态方程,已知k-1时刻的先验概率密度过程噪声p(wk-1)~N(0;Qk-1),根据贝叶斯理论,得k时刻非线性系统的预测状态值及其协方差Pk|k-1如下:
4.如权利要求3所述的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,所述步骤3具体包括:
针对式(5)非线性系统量测增量方程,假定k时刻的预测概率密度过程噪声p(Vk)~N(0;Rk),根据贝叶斯理论,得k时刻状态的估计值及其协方差Pk|k如下:
其中,为预测增量量测值:
PΔzΔz,k|k-1为协方差矩阵:
PxΔz,k|k-1为状态向量和量测向量之间的协方差矩阵:
Wk为增益矩阵:
5.如权利要求4所述的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,根据高斯-厄米特积分准则的积分点积分方法,通过积分点加权求和的方式对步骤2和步骤3中的积分近似求解,具体为:
假定可积非线性函数f(x),其积分表示为:
其中,ω(x)是一个高斯密度函数,如果该积分不存在解析解时,则通过数值积分的方法对其进行近似求解,即找到一组具有权值的点集,对这些点集通过加权求和的方式对积分I(f)进行近似得:
其中,ξl是积分点,ωl是相应的权值,m表示积分点的个数;
根据高斯-厄米特积分准则,计算一个标准高斯加权积分,其数值化近似为:
其中积分点及其权值的计算方法为:假定J是一个具有0对角元素的对称三对角矩阵,且则积分点的计算表示为其中εl是矩阵J第l个特征值,其对应权值计算为其中(vl)1是矩阵J第l个归一化特征向量的第一个元素;
对于高斯密度函数其中为其均值,P为相应的方差,根据高斯-厄米特积分准则,得其高斯加权积分的数值化近似为:
上式中Xl计算如下:
6.如权利要求5所述的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,所述步骤2中式(12)和式(13)的求解步骤为:
首先,在状态空间中找到m个积分点及其权值这些积分点的均值为xk-1|k-1,协方差为Pk-1|k-1,根据式(24)得:
其中,Pk-1|k-1满足
其次,将积分点通过非线性函数(1)传播后,得一步预测积分点
最后,根据式(23)高斯-厄米特积分准则,将式(12)做如下变换,
式(13)协方差,通过积分点的加权和求得,
7.如权利要求5所述的一种欠观测条件下的增量求积分卡尔曼滤波方法,其特征在于,所述步骤3中式(16)、式(17)和式(18)的求解步骤为:
首先,在状态空间中找到m个回归点及其权值这些回归点的均值为协方差为Pk|k-1,根据式(24)得:
其中,预测协方差Pk|k-1应满足
其次,根据式(5)计算增量量测积分点
ΔZl,k|k-1=h(Xl,k|k-1)-h(Xl,k-1|k-1) (30)
最后,根据式(23)高斯-厄米特积分准则,将式(16)做如下转换
式(17)增量量测的新息协方差矩阵,通过积分点的加权和求得,
式(18)互协方差矩阵,通过积分点的加权和求得,
利用式(19)计算增益矩阵,利用式(14)、式(15)分别计算估计值及其协方差Pk|k。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910511809.3A CN110311652A (zh) | 2019-06-13 | 2019-06-13 | 一种欠观测条件下的增量求积分卡尔曼滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910511809.3A CN110311652A (zh) | 2019-06-13 | 2019-06-13 | 一种欠观测条件下的增量求积分卡尔曼滤波方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110311652A true CN110311652A (zh) | 2019-10-08 |
Family
ID=68077129
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910511809.3A Pending CN110311652A (zh) | 2019-06-13 | 2019-06-13 | 一种欠观测条件下的增量求积分卡尔曼滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110311652A (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110687793A (zh) * | 2019-11-04 | 2020-01-14 | 青岛科技大学 | 一种基于输入增量的船舶动力定位系统非线性无偏预测控制方法 |
CN110995203A (zh) * | 2019-12-27 | 2020-04-10 | 广东海洋大学深圳研究院 | 一种基于条件数融合的非线性可观测度分析方法 |
CN112098849A (zh) * | 2020-09-19 | 2020-12-18 | 西安科技大学 | 基于求积分卡尔曼滤波的锂电池剩余电量估计方法 |
CN117353705A (zh) * | 2023-10-08 | 2024-01-05 | 哈尔滨工业大学 | 一种带有未知延迟概率的一步时延跟踪滤波方法及系统 |
-
2019
- 2019-06-13 CN CN201910511809.3A patent/CN110311652A/zh active Pending
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110687793A (zh) * | 2019-11-04 | 2020-01-14 | 青岛科技大学 | 一种基于输入增量的船舶动力定位系统非线性无偏预测控制方法 |
CN110687793B (zh) * | 2019-11-04 | 2022-03-22 | 青岛科技大学 | 一种基于输入增量的船舶动力定位系统非线性无偏预测控制方法 |
CN110995203A (zh) * | 2019-12-27 | 2020-04-10 | 广东海洋大学深圳研究院 | 一种基于条件数融合的非线性可观测度分析方法 |
CN110995203B (zh) * | 2019-12-27 | 2023-03-21 | 广东海洋大学深圳研究院 | 一种基于条件数融合的非线性可观测度分析方法 |
CN112098849A (zh) * | 2020-09-19 | 2020-12-18 | 西安科技大学 | 基于求积分卡尔曼滤波的锂电池剩余电量估计方法 |
CN112098849B (zh) * | 2020-09-19 | 2023-10-13 | 西安科技大学 | 基于求积分卡尔曼滤波的锂电池剩余电量估计方法 |
CN117353705A (zh) * | 2023-10-08 | 2024-01-05 | 哈尔滨工业大学 | 一种带有未知延迟概率的一步时延跟踪滤波方法及系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110311652A (zh) | 一种欠观测条件下的增量求积分卡尔曼滤波方法 | |
CN111798491B (zh) | 一种基于Elman神经网络的机动目标跟踪方法 | |
CN105205313A (zh) | 模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置 | |
CN112884237A (zh) | 配电网预测辅助状态估计方法及系统 | |
CN111181529B (zh) | 一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法 | |
UmaMageswari et al. | A comparitive study of Kalman filter, extended kalman filter and unscented Kalman filter for harmonic analysis of the non-stationary signals | |
CN109115228B (zh) | 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法 | |
Zhang et al. | Low-cost adaptive square-root cubature Kalman filter for systems with process model uncertainty | |
Chang et al. | A new fuzzy strong tracking cubature Kalman filter for INS/GNSS | |
CN111798494B (zh) | 广义相关熵准则下的机动目标鲁棒跟踪方法 | |
CN112713587A (zh) | 一种基于平方根容积卡尔曼滤波器的配电网动态状态估计方法及系统 | |
CN110532517A (zh) | 基于改进的arukf的燃气管道参数估计方法 | |
CN117890901A (zh) | 一种广义未知扰动下带有色量测噪声的目标状态估计方法 | |
CN109521488A (zh) | 用于卫星重力梯度数据的arma最优滤波模型构建方法 | |
CN107421543B (zh) | 一种基于状态扩维的隐函数量测模型滤波方法 | |
CN115859626A (zh) | 针对周期运动目标的自适应无迹卡尔曼滤波器设计方法 | |
CN106931966B (zh) | 一种基于泰勒高阶余项拟合的组合导航方法 | |
CN110595470A (zh) | 一种基于外定界椭球集员估计的纯方位目标跟踪方法 | |
Yan et al. | An adaptive algorithm based on levenberg-marquardt method and two-factor for iterative extended Kalman filter | |
CN110649911B (zh) | 一种基于α散度的分布式非线性卡尔曼滤波方法 | |
Ma et al. | Variational Bayesian cubature Kalman filter for bearing-only tracking in glint noise environment | |
Ou et al. | An improved node localization based on adaptive iterated unscented Kalman filter for WSN | |
Sun et al. | A modified marginalized Kalman filter for maneuvering target tracking | |
CN114430525B (zh) | 一种封闭空间基于传感器网络的分布式定位方法 | |
Wang et al. | Target tracking algorithm of automotive radar based on iterated square-root CKF |
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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20191008 |