Unscented卡尔曼滤波卡尔曼滤波算法
第30卷第4期
・
系统工程与电子技术
V01.30No.4
2008年4月SystemsEngineeringandElectronicsApr.2008
■_●—●-●__■_●__●■-_●__—_●—-●_●____■__●●_●●■●_●_■_-●-I●___●●●____■_●__●■--●_●●_—●__■●●■__●●●___—-●■__●■■___■_●●●_-●___-●___--●■●■__I
文章编号:1001—506X(2008)04—0617—04
Unscented卡尔曼滤波・卡尔曼滤波算法
尹建君,张建秋,林
摘
青
(复旦大学电子工程系,上海200433)
要:针对条件线性高斯状态空间模型,提出unscented卡尔曼滤波一卡尔曼滤波unscentedKalmanfihe—
ring-Kalmanfiltering,UKF-KF算法,该方法用UKF估计条件线性高斯状态空间模型中的非线性状态。用KF估
计线性状态.为了有效地融合UKF和KF估计的后验状态分布,将蒙特卡罗方法应用于KF估计的线性状态均值和方差,获得了与UKFsigma点相同数量的后验线性状态估计分布的样本,然后将这些样本与UKF中sigma点进行合成去获得系统中非线性状态的估计。该算法应用于机动目标艰踪的仿真结果表明:与Rao-Blackwellized粒子滤波器(Rao-Blackwellized降低,有效提高了实时性。
particle
filter,RBPF)相比,该算法虽在估计精度上略有下降,然而计算时间明显
。
关键词:信息处理;Unscented卡尔曼滤波一卡尔曼滤波,仿真;条件线性高斯;Rao-Blackwellized粒子滤波(RBPF);目标跟踪
中图分类号:TP
391
文献标志码:A
UnscentedKalmanfilter-Kalmanfilteralgorithm
Y1N
Jian-jun,ZHANGJian-qiu,LINQing
(Dept.ofElectronicEngineering,FudanUniv.,Shanghai200433,CIllina)
Abstract:Anewfilteringmethod,called
theunscentedKalmanfihering-Kalmanfiltering(UKF—KF)al—
state
as
gorithmispresentedforconditionallylinearGaussiannonlinear
tO
states
space
models.Themethod
states
are
uses
UKF
tO
estimatethe
of
a
conditionallylinearGaussianmodelwhileitslinear
states
estimatedbytheKF.Inorder
fusethetwoposteriordistributionsoftheestimated
tO
obtainedbytheUKFandKFrespectively,Monte
CO—
Carlomethodisapplied
generating
samplesfromGaussiandistributionswiththeKFestimatedmeansand
as
variance.Thegeneratedsampleswiththesamenumberthesigmapoints
tO
sigmapointshave
are
thencombinedtogetherwith
estimatethenonlinear
tO
statesin
themodelusingtheUKF.Thesimulationresultsofthepro—
posedmethodapplyingtrackingthemaneuvertarget,comparedwiththeRBPF(Rao-Blackwellizedparticle
a
filter),shoWthattheproposedUKF—KFonlyconsumes8%thecomputingtimerequiredbytheRBPFwithlittlebitfilteringperformancedecline.
Keywords:signalprocessing;unscentedKalmanfihering-Kalmanfilt(UKF—KF);simulation)coditionallylinearGaussian;Rao-Blackwellizedparticlefiltering(RBPF)ltargettracking
0引言
算量大,一般很难满足实时性的要求.由于RBPF的计算量主要来自于非线性状态估计的PF,因此如何降低非线性状态估计的计算成本,是提高其实时性的关键。当非线性状态的噪声为高斯的情况下,本文提出一种条件线性高斯状态空间模型的新滤波方法,即unscented卡尔曼滤波一卡尔曼滤波(unscentedKalmanfihering-Kalmanfiltering。UKF-KF)算法。对模型的非线性部分采用计算量小于PF的UKF(”1进行处理.对于线性部分仍然采用KF进行处
对于既有线性状态。又有非线性状态的条件线性高斯模型,状态估计的常用方法是采用Rao-Blackwellized粒子滤波器(Rao-Blackwellized
particle
filter,RBPF)【I-S],RBPF
的基本思想是对模型中的非线性状态采用粒子滤波器
(particlefilter,PF)进行估计。对线性状态采用卡尔曼滤波
器(Kalmanfilter。KF)进行估计。RBPF的最大问题就是计
收稿日期:2007一04—29f修回日期:2007—08一02.
基金项目:国家自然科学基金(60572023),上海市科委政关项目(055115021)资助课题
作者简介:尹建君;(1980一)。男,博士研究生,主要研究方向为信息融合,序列蒙特卡罗(粒子滤波)方法,导航与组合导航.E-mail:yinjianjun@fudan.edu.cn
・
618
・
系统工程与电子技术
第30卷
理。文献E83也讨论了类似的想法,但是没有给出UKF和KF的结合方式,而这正是UKF-KF的关键所在。考虑到UKF估计的状态后验概率分布是用sigma点描述的,而KF估计的状态后验高斯分布则是用估计状态的均值和方差描述的。为此为了有效地融合UKF和KF状态估计的后验分布,先将蒙特卡罗方法应用于KF线性状态估计的估计均值和估计方差,获得与UKF中sigma点相同数量的线性状态估计的后验分布的样本。然后将这些样本与UKF中的sigma点合成进行非线性状态的估计。UKF-KF方法应用于机动目标跟踪的仿真结果表明,UKF.KF虽然在估计精度上比RBPF稍有下降,却明显提高了计算速度,状态估计的时间约为RBPF的8%,有效提高了机动目标跟踪的实时性。1
UKF-KF滤波算法
1.1模型描述
考虑如下动态状态空间模型。
p(xl‘lzl,I)
(1)工2l=A,(zlf)zzPl+B,(zlI)UI+Gf(zl。)m
(2)≈一G(z1。)z“+Df(zl,)%+H,(工l。)q.
(3)
式(1)一(3)所示模型称为条件线性高斯状态空间模型‘”].其中,z.,为一个马尔可夫过程,叫、口为零均值的高斯噪声,协方差阵分别为Q和R,U,为控制信号。我们感兴趣的是
在观测轧,(钆。={动'.-・,≈})条件下,估计状态轧和状态z:,的后验概率分布。由于当给定乩后.z:,满足一个线性
状态空间模型,式(2)、(3)也可表示为如下形式。
p(xz,J工1,,z2PI)=
N(A,(z“)z2卜I+B,(zl。)“。,G,(zI,)Q(G,(zI。))T)
p(≈fzlI,工2I)=
N(G(工I,)z2r+D(zl,)Ut",H,(zl。)R(H,(zl。))‘。)
(4)
式中:N(卢,,)表示均值为_fl,方差为,的高斯分布。
1.2
UKF-KF滤波算法
对式(1)一(3)的条件线性高斯状态空间模型,本文用
UKFc”1进行非线性状态z。,的估计,用KF进行条件线性
状态如的估计。
(1)UKF估计非线性状态蜀。
假设已知上一时刻的状态估计值量。。和叠,。以及状态估计方差P。和K。。对仝¨_。进行状态扩展,得到
.钟,l=[量0I
行.Tl矶T—I]7
(5)
式中:r/t一.为状态.r¨,的过程噪声。研一。为观测噪声.记扩展状态量:H的维数为行。,于是f一1时刻UKF的2n。+1个sigma点可表示如下。
z钆l=L2kI
卫ol士 ̄/(行。+A)P:_-J
(6)
式中:A为合成比例参数,且A一口2(理,+弗)一以,,参数Ⅳ≥o,
o≤a≤1,村,为状态zn的维数;雌。为扩展的状态估计方
差,表达式如下。
厂P。0o]
昨l
2
l
0S0
(7)
【o
o
Rj
式中:S表示z。,过程噪声方差,进一步将式(6)作如下分解。
艏,-=[(麻-)7(】fk。)7(zk。)’]7
(8)
即表示成状态z卜。、过程噪声竹。、观测噪声q一。的sigma
点的形式。
根据式(8)所示的sigma点并由式(1),得到状态而,的预测值2。小一。和预测方差只卜。,如式(9)所示。
{Xi川rI~p(砖I疗・一-))銎o
2~
2川,。=∑wl…z‰,一
i=0
2~
Af,t=乏:w:。[ziIIf,t一量川,t][ziIIf,t一2t。f,・]7
i=0
(9)
式中:w;为权重,定义如下.
W:”’==^/(行,+A)
呱“=A/(竹,+A)+(1一口。+伊)
w:”=w∥一1/[2(n.+A)],i=1’..-,2nx(10)
式中:参数廖o。
下面求取观测预测值Z卜.。由式(3)可知,t时刻观测值z,是t时刻线性状态z:。的函数,而由式(2)可知如是
测t时刻观测值。问题是,在求取t时刻观测值的同时,还要利用zn的2n。+1个预测sigma点{疗lfJ一}‰m,而由KF得到的只是z。。估计的均值和方差,如何进行匹配?考虑到工妒。的状态估计均值2:。和方差K。已知,且
如一。满足高斯分布,那么据下式可由蒙特卡罗方法获得
2n。+1个样本,并将之与{珏川一}‰进行匹配。
zf.2Pl~N(仝2PI,KPl),i=0,…,2n。
(11)
又根据式(2),{乩一-}氇的预测值可表示如下。
Z1.2tle-I
2
At(珏I—Irl)蜀.2卜I+Bt(疗川一)嘶+G(妨¨Irl)m.t
(12)
式中
毗.。~N(O,Q),i=0,…,2n。
(13)
于是,调整后的观测预测值三f卜。的求取可表示如下.
zf.2卜l~N(量2卜l,K卜1),i==0,…。2超。
五驯一I2
A(殖1tIrl)五.2一I+Bf(珏l・I—I)%+G(菇¨I—1)撕.t
螽.“一l
2
G(矮圳rI)墨.2ffH+口(西nf,1)%+H(贬川rI)让.t
2~
^f㈣=∑州1’‰,t
(14)
式中
{z,m}‰=zkl
(15)
最后求取状态z。,的估计值£。和估计方差P。。可以表
示为
2~
尸_扫,=乏:w;‘’[研.fl,。一龟f,。][≈川,。一毛l,。]7
zz川的函数,又z:H的估计值叠:。已知,于是可用仝:。预
第4期
2~
尹建君等:Unscented卡尔曼滤波一卡尔曼滤波算法
・619・
掌2I=Fx2Pl+m¨+啊
z。=Ck2,+坼
(18)
P‘y,=乏:’y;订[静.¨I卜t一仝川卜I][:“}卜-一^fI卜-]T
Ji|=P¨jP茹?
2l,==量l,I卜l+.,It(≈一毛l卜I)
式中:打和口为零均值的高斯噪声,方差分别为Q和R。施表示四维状态,分别对应目标的z方向的位置、速度和y方向的
(16)
P。=P,I,I一几P珐.,T1。
式中:.,。表示UKF卡尔曼增益。
位置、速度,即勘,=b,M,%,吼]7lz。,表示目标的机动值,为
随机变量,而,∈{--1,O,1},且P(xl,I五。)给定如下。
.
(2)KF估计条件线性状态如
下面讨论状态如的估计,在获得了非线性状态钆的
估计.17。。后。联立式(2)和式(3)进行卡尔曼滤波[3朋,可得
z圳Pl=A,(2“)22,l+Bf(jI,)Uf
f0.8,z1I=2
zlPl
p(xu
h”1)2{0.1,其他
仿真中采用的数据如下:F=[1,0,0.3,0;0。1,0,0.3,0,0,1,0;o,0,0,1]’,B一[1.25,一1.25;o.25;--0.25]7,c=[1,o,0,0;o,1,0,o],g=o.32×I(4),Jt=32×J(2),其qp,I(竹)表示行维单位阵,位置、速度的初始真实值为娩。=[zo,30,5,5]7,RBPF仿真中的粒子数为300,UKF中口=1,口=
2,Ⅳ=0。
Z,le-l=G(量l。)z纠卜l+D,(矗。)地
K,I,I=G,(2l。)Q(G(2I,))7+A,(量l,)K,I(A,(A,))7Z=H,(毛,)R(H,(量“))7+G(2l,)K,I,I(G(2¨))7
^=K?I,I(cf(量I,))177,1
2n=z2,It-I+.,l(蜀一蜀lPl)K,=(f一.,。C。(21。))K,I,l
(17)
图1和图2分别给出了采用UKF-KF和RBPF两种方法的仿真结果,具体仿真数据如表1所示。其中图1两幅子图分
式中:_,,为KF卡尔曼增益。
(3)UKF-KF滤波算法小结波算法如算法1所示.
算法1
’’
。
别为真实轨迹(直线表示)、估计轨迹(UKF-KF用虚线表示,RBPF用点表示)以及真实机动值(直线表示),估计机动值
根据上述两节关于UK卜KF的推导,总结UKF-KF滤
UKF-KF滤波算法
毛o,220,Po,Ko,‘
’’
(噼KF用点表示,RBPF用星号表示)。图2四幅子图分别
及表1可知,UKF-KF的估计误差均方根(rootme目ffl
square
表示z、啦置和速度的估计值和真实值,其中直线表示真实值,
虚线表示UKF-KF估计值,点表示RBPF估计值。由图1和2
er.
步骤1初始化
其中P、K分别表示状态z。和z。的估计方差}1
步骤2对每一个时间步数f(f≥2)
(1)从z:。估计值中采样,获得与z¨_。的sigma点数相同的样本
zf.2Pl~N(22PI,Krl),i=0,…,2n.I
for,RMSE)虽比RBPF稍大一些,然而计算时间却明显减少,约为RBPF的8%,有效提高了实时性。(其中RMSE的计算
厂1——。—————一
规则如下√∑G;一五)2/t.其中五表示估计值,置表示真实
Y
j—I
(2)通过UKF,利用五.2,一。和2l,-l计算得到il,’i=0。
…,2n.
值。t表示时间步数。)
①获得sigma点
硝,-=L甜,-硝,t士 ̄/(%+A)P=_,j②时间更新(预测)
由式(9)获得状态而,的预测值z川H,预测方差只卜l,由式(14)获得观测预测值毛卜¨③测量更新(估计)
由式(16)获得状态z。,的估计值叠。估计方差只,(3)通过KF,利用仝lf’仝"I计算2。
根据式(17)获得z。的状态估计j:,和估计方差K,。算法中值得注意的是从z。,的估计值2:。中采样(式(11))以及UKF中观测预测值的计算(式(14)).该两步骤是UKF和KF有效融合的关键。
j位置
一:真实值;…:UKF.KF真实值;一….1.RBPF真实值;‘a)真实和估计轨迹
2仿真分析
考虑如下机动跟踪目标模型m3。
一:真实值;
图1
表I两种方法仿真数据比较
・:uKF—KF真实值;(b)机动估计
毫:RBPF真实值;
目标轨迹及机动值的真实值,估计值比较
・620・
系统工程与电子技术第30卷
时间步数(b)J速度时间步数(c)y位置
一:真实值;…:UKF-KF估计值;……:RBPF估计值
图2目标位置.速度的真实值,估计值比较
3
结束语
本文提出了适合条件线性高斯模型(非线性状态噪声
Ottawa’Canada’2006.
[4]DoucetA,FreitasN.GordonNJ.SequentialMonteCarloin
practice[M].Newy0心ISpringer。2001.
[5]FreitasN.Rao-blackwellisedparticlefilteringforfaultdiagnosis
[c]//IEEEAerospaceConferenceProceedings.2002。4:1767—
1772.
为高斯的情况)的UK卜KF滤波算法.算法对模型中的非线性状态用UKF进行估计,线性状态用KF进行估计。为了有效融合UKF和KF。新算法根据KF估计的线性状态均值和方差运用蒙特卡罗方法进行采样,进而与UKF中的sigma点进行匹配。仿真的结果表明,UKF-KF滤波算法估计的RMSE虽稍大于RBPF方法,而运算时间远小于RBPF,约为后者的8%。有效提高了实时性.
[63JulietSJ,UhlmannJI(.A
nonlinear
newextensionoftheKalmanfilterto
systems[C]//Proc.ofAero&nse{m
oll
llthInternational
and
SymposiumAerospace/DefenceSensing.Simulation
Con-
trols,SPIE。Orlando。Florida,UsA,19971182~193.
[7]Merwe
R.DoucetA,Freitas
N.WanE.Theunscentedparticle
参考文献:
[1]DoucetA.GodsillS,Andrieuc.OnsequentialMonteCarlo
samplingmethodsfor
Bayesian
filter[R].TechnicalreportcUEDfF.1NFENG7TR380.Cam—
bridge
UniversityEngineeringDepartment。2000.
point
filtering[J].StatisticsandCom-
[8]MorelandeMR,Ristic&Reducedsigma
tiallylinear
filteringforpar-
puting。2000(10)1197—208.
models[C]//ICASSP.2006:37—40.
[2]DoucetA.Onsequentialsimulation-basedmethodsforBayesian
filtering[R].TechnicalreportCUED/F-INFENG/TR310.
CambridgeUniversityEngineeringDepartment,1998.
[93StevenMK.统计信号处理基础——估计与检测理论[M].北
京z电子工业出版社。2003.
[103MustiereF,BolicM。BouchardM.AmodifiedRao-black—
wellisedparticle
[3]MustiereF,BolicM,BouehardM.Rao-Blackwellisedparticle
filtersl
examplesof
on
filterEC]|{IEEEInternationalCanyefence
off
applications[c]//IEEECanadianCan-
and
Acoustics,SpeechandSignalProcessing,Toulouse。France。2006:2l一24.
ference
Electrical
ComputerEngineering(CCECE).
Unscented卡尔曼滤波-卡尔曼滤波算法
作者:作者单位:刊名:英文刊名:年,卷(期):引用次数:
尹建君, 张建秋, 林青, YIN Jian-jun, ZHANG Jian-qiu, LIN Qing复旦大学电子工程系,上海,200433系统工程与电子技术
SYSTEMS ENGINEERING AND ELECTRONICS2008,30(4)1次
参考文献(10条)
1.Doucet A.Godsill S.Andrieu C On sequential Monte Carlo sampling methods for Bayesian filtering2000(10)
2.Doucet A ()n sequential simulation-based methods for Bayesian filtering[Technical report CUED/F-INFENG/TR 310] 1998
3.Mustiere F.Bolic M.Bouchard M Rao-Blackwellised particle filters:examples of applications 20064.Doucet A.Freitas N.Gordon N J Sequential Monte Carlo in practice 20015.Freitas N Rao-blackwellised particle filtering for fault diagnosis 2002
6.Julier S J.Uhlmann J K A new extension of the Kalman filter to nonlinear systems 1997
7.Merwe R.Doueet A.Freitas N.Wan E The unscented particle filter[Technical report CUED/F-INFENG/TR380] 2000
8.Morelande M R.Ristic B Reduced sigma point filtering for partially linear models 20069.Steven M K 统计信号处理基础--估计与检测理论 2003
10.Mustiere F.Bolie M.Bouchard M A modified Rao-blaekwellised particle filter 2006
相似文献(0条)
引证文献(1条)
1.刘元魁.樊养余.赵炯.黄爱萍 一种快速估计线性-非线性状态的粒子滤波器[期刊论文]-测控技术 2009(5)
本文链接:http://d.g.wanfangdata.com.cn/Periodical_xtgcydzjs200804008.aspx
下载时间:2010年5月19日