平方根容积卡尔曼滤波算法及其应用

2011-06兵工自动化30(6)OrdnanceIndustryAutomation·11·doi:10.3969/j.issn.1006-1576.2011.06.005平方根容积卡尔曼滤波算法及其应用穆静1,2，蔡远利2（1.西安工业大学计算机科学与工程学院，西安710032；2.西安交通大学电子与信息工程学院，西安710049）摘要：针对使用扩展卡尔曼算法（extendedKalmanfilter，EKF）对复杂非线性状态估计时收敛速度慢、估计精度低的问题，提出一种平方根容积滤波算法（squarerootcubatureKalmanfilter，SRCKF）。SRCKF使用基于容积原则的数值积分方法直接计算非线性随机函数的均值和方差。该算法实现时只需计算函数值，避免了求导运算，降低了计算复杂度。且该算法传播了状态协方差的平方根，确保了协方差矩阵的对称性和半正定性，改进了数值精度和稳定性。把平方根容积卡尔曼滤波算法（SRCKF）应用到未知弹道系数的再入弹道目标的状态进行估计中。MonteCarlo数值仿真表明，平方根容积滤波算法大大降低了未知弹道系数的再入弹道目标的状态估计误差，提高估计精度，且运行速度较快。关键词：非线性状态估计；容积原则；容积卡尔曼滤波；再入弹道目标中图分类号：TP311.6文献标志码：ASquareRootCubatureKalmanFilterAlgorithmandApplicationMuJing1,2,CaiYuanli21.SchoolofComputerScience&Engineering,Xi’anTechnologicalUniversity,Xi’an710032,China;2.SchoolofElectronic&InformationEngineering,Xi’anJiaotongUniversity,Xi’an710049,China)Abstract:TosolvetheslowconvergencespeedandlowestimationaccuracyoftheextendedKalmanfilter(EKF)forthecomplexnonlinearstateestimation,thesquarerootcubatureKalmanfilter(SRCKF)isintroducedinthestudy.IntheSRCKFalgorithm,thecubaturerulebasednumericalintegrationmethodisdirectlyusedtocalculatethemeanandcovarianceofthenonlinearrandomfunction.Thealgorithmisimplementedonlyusingthefunctionalevaluationandisderivative-freesothecomputationalcomplexisdecreased.AndtheSRCKFpropagatesthesquarerootofthecovariancesothatitguaranteesthesymmetryandpositivesemi-definitenessofthecovariancematrixandimprovesnumericalstabilityandnumericalaccuracy.Thealgorithmisappliedtostateestimationforreentryballistictargetwithunknownballisticcoefficient.ThesimulationresultsindicatethatthestateerrorintheSRCKFislargelydecreasedanditsestimationaccuracyisimproved.Moreover,therunspeedofSRCKFisfaster.Keywords:nonlinearstateestimation;cubaturerule;cubatureKalmanfilter;reentryballistictarget0引言实际应用中存在着许多非线性滤波问题。解决非线性滤波问题的最优方案需要计算状态的条件后验概率分布，而精确地描述状态的条件后验概率分布则需求解状态的多维积分。因只有在极少的情况下才能精确描述状态的多维积分，故人们提出了大量次优的滤波方法。目前，广泛使用的次优非线性滤波器是扩展卡尔曼滤波器（extendedKalmanfilter，EKF）[1]，其基本思想是使用非线性函数的泰勒级数一阶项，将非线性问题转化为线性问题。对于高度非线性滤波问题，使用EKF算法会引入较大的误差，在有些情况下，可能获得发散的状态估计，且实现EKF需要计算状态和测量方程的Jacobian矩阵，但实际上很难求得系统的Jacobian矩阵，甚至有些系统不存在Jacobian矩阵。为了提高滤波精度，提出了一些EKF的改进方法，如二阶EKF、迭代EKF等，但是以增加计算量为代价的[2]。近来，提出了多个免微分的状态估计方法。其中，无迹卡尔曼滤波方法（unscentedKalmanfilter，UKF）通过一组确定的加权采样点来逼近随机变量的分布函数，通过这组采样点的非线性变换，捕获随机变量经非线性转换后的统计特性[3]。使用UKF需要选择合适的参数，才能达到较好的滤波效果。粒子滤波方法（particlefilter，PF）依据蒙特卡洛仿真的思想[4-5]，随机产生大量粒子近似计算后验概率密度。PF实现时需要产生大量的粒子，计算量非常大，很难满足实时性的需求，且有粒子退化和贫化等问题。求积滤波器（quadratureKalmanfilter，QKF），使用高斯-厄尔米特数值积分获得随机变量非线性变换后的统计特性，QKF方法使用的积分点数按维数的指数增长，会引起维数灾难[6-7]。最近提出的容积卡尔曼滤波（cubatureKalman收稿日期：2011-02-22；修回日期：2011-03-14基金项目：国家自然科学基金（60972146，60602025）作者简介：穆静（1979—），女，安徽人，博士，讲师，从事状态估计、非线性滤波方法研究。兵工自动化·12·第30卷filter，CKF）[8]使用基于容积原则的数值积分方法计算非线性变换的随机变量后的均值和协方差，CKF实现简单，且滤波精度较高。再入弹道目标的状态估计是目标跟踪中一个的重要方面。由于再入弹道目标受到气动阻力的作用，使状态和测量方程是高度非线性的，所以未知弹道系数的再入弹道目标的状态估计问题是个复杂的非线性滤波问题。因此，笔者运用平方根滤波的数值鲁棒性，使用基于平方根滤波的平方根容积卡尔曼滤波器（squarerootcubatureKalmanfilter，SRCKF）对未知弹道系数的再入弹道目标的状态进行估计。1平方根容积卡尔曼滤波算法考虑具有加性噪声的离散非线性动态系统11()()kkkkkkxfxwzhxv−−=+=+(1)xnkxR∈为系统状态向量；kz为量测值。过程噪声1kw−和量测噪声kv相互独立，且1kw−～N1(0,)k−Q,kv～N(0,)kR。实现平方根容积卡尔曼滤波算法，首先要计算基本的容积点和对应的权值。使用三阶容积原则获得的基本容积点和对应权值为[4]：1[1],1,2jjjmjmm===ω，ξ(2)m表示容积点总数。使用三阶容积原则，容积点总数是状态维数的2倍，即：2xmn=。基本容积点按照下列方式产生的，记xn维单位向量为T[1,0,0]=e，使用符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，称为完整全对称点集，[1]j表示点集中[1]的第j个点。若1k−时刻的后验概率为11:1()kkpxz−−～N111ˆ(;,)kkkxxP−−−，获得方差1kP−的乔列斯基分解因子1kS−，即：11{}kkSCholP−−=。平方根容积卡尔曼滤波算法步骤如下：1)时间更新①计算容积点,111ˆjkkjkXSxξ−−−=+(3)②计算通过状态方程的传播的容积点*,,1()jkjkXfX−=(4)③计算状态预测和方差预测的乔列斯基分解因子*,1*,1()mkiikikkQkxXTriaS=−=⎡⎤=⎣⎦∑ωSχ(5)式中Tria()表示对矩阵进行三角化，获得矩阵的方阵。矩阵*kχ定义为：****1,2,,1,,kkkkkmkkXxXxXxm⎡⎤=−−−⎣⎦χ(6)2)测量更新①计算容积点,jkkjkXxξ=+S(7)②计算通过量测方程的传播的容积点,,()jkjkZhX=(8)③计算测量预测、新息方差的乔列斯基分解因子和协方差,1mkiikizZω==∑(9),,T,|1()zzkkRkxzkkkkTriaSP−⎡⎤=⎣⎦=Sχϒϒ(10)其中，矩阵kϒ,kχ定义为：1,2,,1,2,,1,,1,,kkkkkmkkkkkkkmkkZzZzZzmXxXxXxm⎡⎤=−−−⎣⎦⎡⎤=−−−⎣⎦ϒχ(11)④计算增益，k时刻的状态和方差的乔列斯基分解因子估计为：T,,,(/)/kxzkzzkzzkWPSS=(12),ˆ()()kkkkkkkkkkRkxxWzzTriaWW=+−⎡⎤=−⎣⎦χϒSS(13)式中，符号/为矩阵的右除运算符。2再入弹道目标状态估计中的应用使用SRCKF算法对未知弹道系数的再入状态进行估计，并把SRCKF算法与EKF和UKF算法进行性能比较。2.1状态方程和测量方程假设地球为平面，且假定目标在再入段时只受到地球引力和空气阻力[9]，可建立如下的再入弹道目标的离散非线性状态方程。11()kkkfxw−−=+x(14)穆静，等：平方根容积卡尔曼滤波算法及其应用·13·第6期式中，状态向量kx定义为[]Tkkkkkkxxyyβ=x。kw为过程噪声，假定其服从均值为零的高斯白噪声，即：kw～N(0,)kQ，其协方差矩阵建模为[1]：112000000kqqqTθθ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦Q322322TTTT⎡⎤=⎢⎥⎣⎦θ(15)T为连续2个雷达测量间隔时间，1q，2q是过程噪声强度参数。式(14)中表达式1()kfx−为：111()()kkdkg−−−=++ΦfxxGfxGf(16)00100,01001T⎡⎤⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦⎢⎥⎣⎦φΦφφ，1211020,00TT⎡⎤⎡⎤⎢⎥==⎢⎥⎢⎥⎣⎦⎢⎥⎣⎦gGgg(17)式(16)中表达式[]T221111111()0.5()dkkkkkkkgyxyxyρβ−−−−−−−=−+fx为气动阻力对再入目标所产生的阻力加速度，[]T0gg=−f为地球引力产生的加速度。ρ为空气密度，当高度小于90km，空气密度可建模为随目标高度下降按指数衰减的函数[10]，即21()cyyceρ−=，其中12,cc为常数。假定雷达位于考虑的笛卡尔坐标原点，其测量弹道目标的距离和俯仰角。根据几何关系，可建立量测方程为：()+kkkhxv=z(18)式中[]Tkkkrε=z是测量值，距离kr和俯仰角kε与再入目标状态的关系为：tankkkrkkrxyvayxvεε=++=+(19)测量噪声[]Trvvε=kv为均值为零的高斯白噪声，即kv～N(0,)kR，其协方差矩阵：22[]krdiagεσσ=R(20)式中rσ，εσ为距离和俯仰角的量测误差标准差。状态噪声、测量噪声和初始状态相互对立。2.2仿真和分析仿真中使用的参数为：0.1sT=，雷达测量的距离和俯仰角的标准偏方差选取为100mrσ=和εσ=0.017。目标弹道系数选取为240000kg(ms)=⋅β。目标的初始位置和速度选取为0232kmx=，060kmy=和03000m/sv=，目标的再入角0210γ=°，初始状态的方差选取为222220([500,50,500,50,10000])diag=P，仿真中跟踪目标60s，蒙特卡罗仿真次数为100次。将平方根容积卡尔曼滤波算法（SR