基于平方根卡尔曼滤波的粒子滤波算法
基于迭代平方根容积粒子滤波目标跟踪算法
基于迭代平方根容积粒子滤波目标跟踪算法陈超波;刘叶楠;高嵩【期刊名称】《测控技术》【年(卷),期】2015(034)007【摘要】针对粒子滤波目标跟踪算法粒子退化及跟踪精度问题,提出了一种基于马尔可夫链-蒙特卡罗(MCMC,Markov Chain Monte Carlo)的迭代平方根容积粒子滤波(ISRCPF,iterated square root cubature Kalman particle filter)算法(ISRCPF-MCMC).在该滤波算法中,利用容积数值积分原则计算非线性随机函数的均值和方差,通过正交矩阵分解代替矩阵开方,在生成的粒子滤波建议分布中融入当前量测值,提高对系统后验概率的逼近程度.然后在此基础上融合MCMC抽样算法(MH,Metropolis Hasting)对所选建议分布进行优化,增加粒子多样性,以提高跟踪精度.仿真试验结果表明,ISRCPF-MCMC算法的估计误差与其他算法相比降低至0.403％.【总页数】5页(P120-124)【作者】陈超波;刘叶楠;高嵩【作者单位】西安工业大学电子信息工程学院,陕西西安710021;西安工业大学电子信息工程学院,陕西西安710021;西安工业大学电子信息工程学院,陕西西安710021【正文语种】中文【中图分类】TP391【相关文献】1.基于噪声补偿的迭代平方根CKF的汽车雷达目标跟踪算法 [J], 熊星;王彩玲;荆晓远2.基于迭代积分粒子滤波的目标跟踪算法∗ [J], 毛少锋;冯新喜;鹿传国;危璋3.基于平方根容积信息滤波的弹道目标跟踪算法 [J], 刘俊;刘瑜;熊伟;孙顺4.基于量化新息的容积粒子滤波融合目标跟踪算法 [J], 徐小良;汤显峰;葛泉波;管冰蕾5.基于迭代容积平方根粒子滤波的城市雨水利用控制模型 [J], 罗义;代学民;马立山;龚建英因版权原因，仅展示原文概要，查看原文内容请购买。
平方根扩展卡尔曼滤波
考虑随机非线性离散广义系统(1)((),)((),)()Mx k f x k k g x k k w k +=+ (0-1)(1)((1),1)(1)y k h x k k v k +=++++(0-2)其中：k 为离散时间，状态变量()n x k ∈ℜ，观测变量()m y k ∈ℜ，输入噪声()w k ，观测噪声()v k ，()f ⋅，()g ⋅和()h ⋅对()x k 是可微的，p n M ⨯∈ℜ是奇异常数矩阵，min{,}rank M r p n =<。
假设1 ()w k 和()v k 是零均值的不相关白噪声()()()()()()()()T Tkj T w k Q k S k E w j v j v k S k R k δ⎧⎫⎡⎤⎡⎤⎡⎤=⎨⎬⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎩⎭ 其中E 为均值符号，T 为转置符号，1kk δ=，0()kj k j δ=≠。
假设2 非线性广义系统(0-1)~(0-2)是强可控的。
如果在k 时刻，状态变量()x k 的最小方差线性估值ˆ(|)x k k 已知，则可以将非线性系统(0-1)在ˆ()(|)x k x k k =，(0-2)在ˆ()(1|)x k x k k =+附近进行Taylor 级数展开，只保留一次项，近似可得ˆ()(|)((),)ˆˆ(1)((|),)|(()(|))((),)()()x k x k k Tf x k k Mx k f xk k k x k x k k g x k k w k x k =∂+=+-+∂(0-3) ˆ(1|)(1|)((1|),)ˆˆ(1)((1|),)|(()(|))(1)(1|)x k k x k k T h x k k k y k hxx k k k x k xk k v k x k k +=+∂++=++-++∂+(0-4) 其中：离散状态一步预测为ˆˆ(1|)((|),)x k k f x k k k +=，并认为ˆ((),)((|),)g x k k g x k k k =。
粒子滤波算法在图像处理中的应用
粒子滤波算法在图像处理中的应用一、背景图像处理技术的发展已经广泛应用于人们的日常生活中，如视频监控、医学图像诊断、无人驾驶、虚拟现实等领域。
图像处理技术中的超分辨率恢复和目标跟踪等问题，需要解决信号处理领域经典问题：估计和滤波问题。
二、传统滤波和粒子滤波的比较传统滤波技术方法主要包括线性滤波、非线性滤波和尺度空间方法。
这些方法的局限性在于图像中最普遍的问题是非高斯的噪声以及非线性/非高斯分布的像素值。
相比传统滤波技术，粒子滤波(PF)算法具有较强的适用性。
PF 是基于贝叶斯滤波的非线性卡尔曼滤波(Extended Kalman Filter, EKF)的一个扩展，利用非参数方法实现对状态方程非线性函数的逼近，适用于一般的非线性、非高斯问题。
PF算法的基本思想是通过表示概率分布的一组粒子在已知观测数据的情况下进行递推，得到概率分布随时间的演化过程。
与传统的滤波算法不同，PF算法利用粒子对状态量进行数值估计，而不是利用直接的数值估计，避免了高斯和线性假设的限制。
三、粒子滤波算法在图像处理中的应用1、目标跟踪在图像处理中，通过PF算法进行目标跟踪是一种有效的方法。
对于跟踪过程，PF算法可以用于估计目标区域和跟踪目标的运动状态，比如目标的速度和方向等。
在PF算法中，需要选择重采样方法，可以采用系统自适应粒子滤波算法(SIR-PF)来解决这个问题。
这种方法可以通过自适应的方法估计粒子的权重和重采样的样本数量，从而提高重采样抽样和跟踪的性能。
2、图像匹配图像匹配是估计两幅图像之间空间关系的过程。
在图像处理中，PF算法可以应用于偏移估计、匹配关键点、图像对齐等问题。
PF算法的主要优势在于可以接受非线性的变换模型和非高斯噪声，同时可以在最大后验概率(MAP)框架下进行求解。
在PF算法中，需要利用连续的状态极其之间的相互联系，利用高斯混合模型(GMM)来代替样本的权重分布。
这样，可以更好地利用概率密度函数对隐藏变量进行建模，提高状态的估计精度。
平方根体积积分卡尔曼滤波
平方根体积积分卡尔曼滤波郭文艳;姬春艳;周吉瑞【摘要】Cubature quadrature rule is a kind of higher algebra precision integration method to improve nonlinear state estimate problems. To enhance the stability of nonlinear filter, based on a square root decomposition of covariance matrix, a new Square Root Cubature Quadrature Kalman Filter(SRCQKF)is proposed. The spherical radial cubature rule and Gauss-Laguerre quadrature rule are applied to compute the integral points. The square root of covariance matrix is obtained and propagated by matrix QR decomposition. Two typical nonlinear examples show that the SRCQKF improves the accuracy of nonlinear estimation and has more stability.%体积积分是一种新的具有较高代数精度的积分方法。
为了提高非线性滤波算法的精度和数值稳定性，将体积积分规则和平方根分解引入卡尔曼滤波框架中，提出了平方根体积积分卡尔曼滤波算法（SRCQKF）。
新算法采用球半径体积规则和高斯-拉盖尔积分规则计算积分点，利用矩阵的QR分解得到协方差矩阵的平方根并传播平方根。
两个典型的非线性系统的实验结果表明，与体积卡尔曼滤波相比，新算法提高了非线性状态的估计精度，具有较高的数值稳定性。
平方根容积Rao-Blackwillised粒子滤波SLAM算法
第40卷第2期自动化学报Vol.40,No.2 2014年2月ACTA AUTOMATICA SINICA February,2014平方根容积Rao-Blackwillised粒子滤波SLAM算法宋宇1,2李庆玲3康轶非1闫德立1摘要面向大尺度环境中的移动机器人同时定位与地图构建(Simultaneous localization and mapping,SLAM)问题,提出平方根容积Rao-Blackwillised粒子滤波SLAM算法.算法主要特点在于:1)采用容积律计算SLAM中的非线性函数高斯权重积分,达到减小SLAM非线性模型线性化误差、提高SLAM精度的目的;2)在SLAM中直接传播误差协方差矩阵的平方根因子,避免了耗费时间的协方差矩阵分解与重构过程,提高了SLAM计算效率.通过仿真、实验将提出的SLAM算法与FastSLAM2.0、UFastSLAM两种算法进行对比,结果表明本文算法在SLAM性能上优于另两者.关键词移动机器人,同时定位与地图构建,粒子滤波,容积律,高斯权重积分引用格式宋宇,李庆玲,康轶非,闫德立.平方根容积Rao-Blackwillised粒子滤波SLAM算法.自动化学报,2014,40(2): 357−367DOI10.3724/SP.J.1004.2014.00357SLAM with Square-root Cubature Rao-Blackwillised Particle FilterSONG Yu1,2LI Qing-Ling3KANG Yi-Fei1YAN De-Li1Abstract In this paper,we derive a new large-scale environment simultaneous localization and mapping(SLAM)al-gorithm based on square-root cubature Rao-Blackwillised particlefilter.The main contributions are:1)to enhance the SLAM performance,the effective cubature rule is utilized to calculate the Gaussian weighted integral of the nonlinear function;2)the covariance square-root factors are directly propagated in our SLAM process.Hence,the time-expensive decompositions on covariance matrixes are avoided.The performance of the proposed algorithm is compared with Fast-SLAM2.0and UFastSLAM using a serial simulations and experiments.Results show that the proposed SLAM outperforms FastSLAM2.0and UFastSLAM.Key words Mobile robot,simultaneous localization and mapping(SLAM),particlefilter,cubature rule,Gaussian weighted integral(GWI)Citation Song Yu,Li Qing-Ling,Kang Yi-Fei,Yan De-Li.SLAM with square-root cubature Rao-Blackwillised particle filter.Acta Automatica Sinica,2014,40(2):357−367即时定位与地图构建(Simultaneous localiza-tion and mapping,SLAM)是“鸡生蛋、蛋生鸡”的悖论问题,一方面移动机器人利用自载传感器感知未知环境,形成对局部环境的理解,并增量式创建收稿日期2012-09-05录用日期2013-02-04Manuscript received September5,2012;accepted February4, 2013国家自然科学基金(60905055,61005070),哈尔滨工业大学机器人技术与系统国家重点实验室开放基金(SKLRS-2009-ZD-04),中央高校基本科研业务费(2014JBM014)资助Supported by National Natural Science Foundation of China (60905055,61005070),Open Function of State Key Labora-tory of Robotics and System of Harbin Institute of Technology (SKLRS-2009-ZD-04),and Fundamental Research Funds for the Central Universities of China(2014JBM014)本文责任编委胡小平Recommended by Associate Editor HU Xiao-Ping1.北京交通大学电子信息工程学院北京1000442.哈尔滨工业大学机器人技术与系统国家重点实验室哈尔滨1500803.中国矿业大学(北京)机电与信息工程学院北京1000831.School of Electronic and Information Engineering,Beijing Jiaotong University,Beijing1000442.State Key Laboratory of Robotics and System,Harbin Institute of Technology,Harbin 1500803.School of Mechanical Electronic and Information Engineering,China University of Mining and Technology,Bei-jing100083全局环境地图;另一方面,机器人利用对已建立环境地图的重复观测机制来处理不确定信息(包括运动和观测的不确定性),实现对其自身准确定位.近20年来,SLAM问题已经成为国际机器人与自动化领域的研究热点,并被认为是实现移动机器人真正自主的“圣杯”[1−2].基于Rao-Blackwellized particlefilter的Fast-SLAM算法为学者Montemerlo于2003年提出[3].相比于传统的卡尔曼滤波SLAM方法,FastSLAM 具有计算代价小、适用于大尺度环境、对数据关联误差鲁棒等优点,已经成为SLAM研究中流行的方法框架.FastSLAM算法根据SLAM问题的条件独立特性,将高维的移动机器人轨迹和环境地图联合后验概率密度估计解耦为低维状态估计,从而解决了状态空间SLAM算法的维数灾难问题,提高了SLAM的求解效率.FastSLAM算法有两个版本: FastSLAM1.0和FastSLAM2.0.FastSLAM1.0利用粒子滤波器跟踪移动机器人位姿参数,并且每一粒子单独维护一幅环境特征地图、每一个环境路标358自动化学报40卷利用卡尔曼滤波(Extended Kalmanfilter,EKF)来维护.Montemerlo在其学位论文中指出,当移动机器人的环境观测噪声较小时(即环境观测传感器过于精确,如激光传感器),FastSLAM1.0容易发生粒子集退化,使得SLAM发散[3].为克服FastSLAM1.0存在的上述问题,FastSLAM2.0算法利用EKF将当前机器人的环境观测信息融入粒子滤波器的提议分布设计之中,使得粒子集中地分布于高观测似然区域.结果表明,FastSLAM2.0较好地解决了粒子退化问题,使用较少的粒子就可得到满意的SLAM结果.基于类似的学术思想,学者Grisetti等提出栅格地图模型FastSLAM算法[4],该方法联合激光雷达扫描匹配技术和机器人里程计信息来优化粒子滤波器的提议分布,实现了提高有效粒子数和SLAM精度的目的;Sim等提出了立体视觉FastSLAM算法[5],该算法的主要特点在于利用立体视觉里程计来设计粒子滤波器的重要性函数;Kim等提出了UFastSLAM算法[6],其核心在于利用无迹变换[7]来估计SLAM中的转移概率密度,与其类似还有中心差分粒子滤波Fast-SLAM算法[8];在UFastSLAM基础上,Kim等在文献[9]中还对粒子滤波器提议分布设计问题进行了探讨,更正了原始UFastSLAM中的过信估计问题,给出了改进型的UFastSLAM算法.在我们的前期工作中,提出利用容积卡尔曼滤波计算Fast-SLAM的提议分布并维护环境特征地图,初步探讨了CFastSLAM算法[10].值得说明的是,与传统SLAM算法相似,在CFastSLAM算法中会以协方差形式来传播SLAM的不确定性.因而,为利用容积律计算转移概率密度,需要在SLAM滤波的迭代过程中(包括预测和更新)不断执行数值敏感且繁琐的协方差矩阵平方根分解、协方差矩阵重构操作.为此,在继承CFastSLAM优点的基础上,探讨如何在SLAM中实现直接传播协方差平方根因子,避免协方差矩阵平方根分解、协方差重构过程,进一步提高SLAM性能,是本文工作的基本出发点.面向特征地图SLAM问题,提出平方根容积FastSLAM算法,并通过仿真、实验验证了其性能.算法主要特点在于:1)利用容积律计算非线性转移概率密度,继承了CFastSLAM的高阶矩估计精度、无需计算雅可比矩阵等优点;2)粒子结构由SLAM 状态及其不确定性协方差平方根因子构成,基于容积律的数值特性,深入探讨并实现了在SLAM迭代过程中的直接协方差平方根因子传播,避免了SLAM迭代中数值敏感且繁琐的协方差平方根分解与重构过程,并可严格保证SLAM协方差矩阵的正定性与对称性.1背景知识1.1FastSLAM算法框架FastSLAM算法的核心思想在于利用机器人对环境观测的历史信息z k=z1,···,z k和机器人的控制的历史信息u k−1=u1,···,u k−1来估计环境地图Θ=θ1,···,θM(环境路标的数量用M来表示)与机器人运动轨迹s k=s1,···,s k的联合后验概率分布p(s k,Θ|z k,u k−1).基于SLAM问题的条件独立性结论:若移动机器人轨迹s k已知,环境特征路标Θ=θ1,···,θM相互独立.因而,联合后验分布p(s k,Θ|z k,u k−1)可以表示为p(s k,Θ|z k,u k−1)=p(s k|z k,u k−1)Mm=1p(θm|s k,z k,u k−1)(1)基于上式表达的条件独立特性,FastSLAM算法采用不同的估计器来估计机器人轨迹后验概率与环境路标的后验概率:对于轨迹后验p(s k|z k,u k−1),采用N个粒子的粒子滤波器估计,其中,每一个粒子单独维护一幅环境地图Θ和机器人路径s k的假设;在机器人轨迹s k已知前提下,由于路标之间相互独立,则对每个环境路标后验p(θm|s k,z k,u k−1)利用单独的卡尔曼滤波器估计.这样,FastSLAM共由N×M+1个滤波器组成.FastSLAM算法中每一个粒子可以表述为S[i]k= (s[i]k,P[i]k),···,(µ[i][m]k,Σ[i][m]k), (2)式中,S[i]k表示FastSLAM算法中的索引为i的粒子;s[i]k为该粒子对机器人当前位姿的假设,P[i]k为s[i]k的协方差;(µ[i][m]k,Σ[i][m]k)为该粒子所维护的特征地图中第m个路标的全局坐标及其不确定性协方差.1.2高斯权重积分的容积律方法为实现高斯域中的贝叶斯滤波,最为重要的步骤是计算高斯分布的非线性转移密度,即计算高斯权重积分(Gaussian weighted integral,GWI),表示为I=g(x)N(x,P x)d x(3)其中,x∈R n x,n x代表变量x的维数,g(x)表示非线性函数;N(x,P x)代表n x维高斯分布.在文献[11]中,学者Arasaratnam等面向GWI 的求解问题提出了容积律方法,该方法利用2n x个等权重容积点计算积分I:I≈12n x2n xj=1g(P xζj+x)(4)2期宋宇等:平方根容积Rao-Blackwillised粒子滤波SLAM算法359式中,容积点集为√P xζj+x,点集ζj表示为{ζj }=√n x{[1]j},j=1,2,···,2n x(5)式中,{[1]j}代表n x维笛卡尔坐标系的坐标轴与n x 维单位超球的交点坐标.2提出的SLAM算法为研究SLAM问题,不失一般性地定义移动机器人系统的含噪声运动学方程和环境观测方程为s k=f(s k−1,u k−1+δu k−1)z k=h(s k,µ[m]k )+δz k(6)其中,s k∈R n s为机器人位姿参数;z k∈R n z为环境观测;f和h分别为非线性机器人运动学模型和环境观测模型;u k−1∈R n u为在离散时间段[k,k−1]内对机器人的控制作用,δu k−1∈R n u为控制噪声,其协方差Q∈R n u×n u;δz k∈R n z为观测噪声,其协方差为R∈R n z×n z;µ[m]k∈R nµ代表第m个环境路标的全局坐标系位置参数.不同于由式(2)所定义的传统FastSLAM算法粒子结构,提出算法的粒子结构中利用协方差矩阵平方根因子代替传统算法粒子结构中的协方差矩阵,并且在SLAM迭代过程中利用平方根因子传播代替传统算法中的协方差传播.提出算法中的粒子结构为S[i] k = (s[i]k,v[i]k),···,(µ[i][m]k,l[i][m]k), (7)其中,s[i]k 为该粒子对机器人当前位姿的假设,v[i]k为协方差矩阵P[i]k 的平方根因子,满足v[i]k(v[i]k)T=P[i] k ;µ[i][m]k为该粒子中索引为m的环境路标位置,l[i][m] k 为其协方差矩阵Σ[i][m]k的平方根因子,满足l[i][m] k (l[i][m]k)T=Σ[i][m]k.2.1机器人状态估计2.1.1状态预测为预测k时刻机器人位姿状态,需要联合控制输入u k−1及其不确定性协方差的平方根因子来增广k−1时刻机器人状态及其协方差平方根因子a=s[i]k−1u k−1,V a=v[i]k−10S Q(8)式中,a∈R n a、V a∈R n a×n a(n a=n s+n u)分别为增广机器人状态及增广协方差平方根因子;s[i] k−1、v[i]k−1为k−1时刻的机器人状态及协方差因子;S Q为控制噪声的协方差平方根因子,满足S Q S TQ=Q.增广状态a服从多维高斯分布N(a,V a V Ta ).为计算机器人在k−1的状态s[i]k−1经运动学模型f传播后的转移概率密度,需按下式计算多维高斯分布N(a,V a V Ta)的容积点集{C[i]j}:{C[i]j}={V aζj+a},j=1,2,···,2n a(9)式中,j为容积点索引.这里值得注意的是,由于算法以协方差平方根因子传播代替协方差传播不确定性传播过程,上式计算容积点时,无需执行数值敏感且计算复杂度较高的协方差矩阵的平方根分解操作.将容积点C[i]j记为机器人状态s和控制作用u的组合形式,即C[i]j=[C s[i]j,C u[i]j]T,并通过机器人运动学模型f传递容积点集,得到变换容积点集{C∗[i]j}={f(C s[i]j,C u[i]j)}.则机器人在k时刻的状态预测为s[i]k|k−1=f(a)N(a,V a V Ta)d a≈12n a2n aj=1C∗[i]j(10)为预测机器人状态协方差的平方根因子v[i]k|k−1,定义误差矩阵Err∈R n s×2n aErr=1√2n a[···,C∗[i]j−s[i]k|k−1,···](11)通过对Err T进行QR分解,机器人状态协方差平方根因子v[i]k|k−1为上三角方阵的转置,表示为[q,r]=qr(Err T),v[i]k|k−1=r T(12)式中,qr(·)为QR分解操作符;r为n s×n s上三角阵.2.1.2状态更新当索引为m的环境路标被移动机器人重新观测到后,利用机器人对该路标的重复观测信息来更新预测状态s[i]k|k−1及其协方差平方根因子v[i]k|k−1.首先,联合环境路标状态及其协方差平方根因子对s[i]k|k−1和v[i]k|k−1进行增广.b=s[i]k|k−1µ[i][m]k−1,V b=v[i]k|k−10l[i][m]k−1(13)式中,b∈R n b、V b∈R n b×n b(n b=n s+nµ)为增广机器人状态和增广协方差平方根因子.增广状态b服从多维高斯分布N(b,V b V Tb).与式(9)类似,分布N(b,V b V Tb)的容积点集为{X[i][m]j}={V bζj+b},j=1,2,···,2n b(14)将每一个容积点表述为机器人状态和环境路标状态的组合形式X[i][m]j=[X s[i][m]j,Xµ[i][m]j]T,并利用机器人环境观测模型h传播容积点集,得到变换360自动化学报40卷容积点集{X∗[i][m]j }={h(X s[i][m]j,Xµ[i][m]j)},进而机器人对该路标的观测预测为下式的高斯权重积分解:z[i][m] k|k−1=h(b)N(b,V b V Tb)d b≈12n b2n bj=1X∗[i][m]j(15)为利用卡尔曼滤波技术更新机器人状态,需计算移动机器人状态与环境路标观测间的交互协方差与观测新息协方差.为此,定义误差矩阵如下:Err z=1√2n b[···,X∗[i][m]j−z[i][m]k|k−1,···]Err s=1√2n b[···,X s[i][m]j−s[i]k|k−1,···](16)进而,机器人状态与观测间的交互协方差为P[i][m]sz=Err s Err z T(17)观测新息协方差平方根因子d[i][m]zz由误差矩阵[Err z,S R]T的QR分解计算:[q,r]=qr([Err z,S R]T),d[i][m]zz=r T(18)式中,r为n z×n z上三角方阵;S R为观测噪声协方差R的平方根因子,即S R S TR=R.基于卡尔曼滤波算法,机器人状态更新为K s=P[i][m]sz (d[i][m]zzd[i][m]zzT)−1s[i] k =s[i]k|k−1+K s(z[m]k−z[i][m]k|k−1)(19)式中,z[m]k为机器人在当前时刻k对索引为m的环境路标的真实传感器测量.机器人状态的协方差平方根因子v[i]k由误差矩阵[Err s−K s Err z,K s S R]T的QR分解计算,即[q,r]=qr([Err s−K s Err z,K s S R]T),v[i]k=r T(20)式中,r为n s×n s上三角方阵;S R为观测噪声协方差R的平方根因子;K s为式(19)中计算的滤波增益.2.1.3重要性采样如果当前时刻移动机器人同时观测到多个已存储在地图中的环境路标,则依次根据对每一个路标的观测对机器人状态s[i]k 及其协方差平方根因子v[i]k进行更新(依次执行式(13)∼(20)),每次更新均以前次更新结果作为初始值.更新完毕后,从机器人状态分布中采集新一代粒子,即s[i] k ∼N(s[i]k,v[i]kv[i]kT)(21)进而,根据观测新息及新息协方差来计算新粒子的重要性权重ω[i]k:ω[i]k∝m=1exp−(z[m]k−z[i][m]k|k−1)T(z[m]k−z[i][m]k|k−1)d[i][m]zzd[i][m]zzT(22)2.2环境地图估计对于提出算法中的每一个粒子S[i]k,在更新其中代表移动机器人位姿的部分后,进入更新环境路标地图阶段.在该阶段,需对当前时刻的重复观测路标和首次观测路标采用不同的策略分别处理.2.2.1重复观测环境路标的更新在当前时刻k,对于那些已经存储在地图中且被移动机器人重复观测到的环境路标m,其全局坐标的高斯描述为N(µ[i][m]k−1,l[i][m]k−1l[i][m]k−1T).则基于容积律,路标状态分布的容积点集为{V[i][m]j}={l[i][m]k−1ζj+µ[i][m]k−1},j=1,2,···,2nµ.容积点集{V[i][m]j}经非线性环境观测模型h作用后,得到变换容积点集{Z∗[i][m]j}={h(s[i]k,V[i][m]j)}.因而,在机器人位姿参数s[i]k已知的前提下,机器人对索引为m的路标的观测预测计算为z∗[i][m]k|k−1=h(s[i]k,µ)N(µ,ll T)dµ≈12nµ2nµj=1Z∗[i][m]j(23)式中,s[i]k为由式(21)计算.与机器人状态更新类似,定义误差矩阵:Err∗z=12nµ[···,Z∗[i][m]j−z∗[i][m]k|k−1,···]Err∗µ=12nµ[···,V[i][m]j−µ[i][m]k−1,···](24)则路标状态与环境测量间的交互协方差矩阵为P[i][m]µz=Err∗µErr∗zT(25)观测新息协方差平方根因子d∗[i][m]zz由矩阵[Err∗z,S R]T的QR得到,即[q,r]=qr([Err∗z,S R]T),d∗[i][m]zz=r T(26)基于卡尔曼滤波,重复观测环境路标状态更新为Kµ=P[i][m]µz(d∗[i][m]zzd∗[i][m]zzT)−1µ[i][m]k=µ[i][m]k−1+Kµ(z[m]k−z∗[i][m]k|k−1)(27)式中,z[m]k为机器人在当前时刻k对索引为m的环境路标的真实传感器测量.2期宋宇等:平方根容积Rao-Blackwillised粒子滤波SLAM算法361环境路标的协方差平方根因子l[i][m]k由对误差矩阵[Err∗µ−KµErr∗z,KµS R]T的QR分解得到:[q,r]=qr([Err∗µ−KµErr∗z,KµS R]T),l[i][m]k=r T(28)其中,r为nµ×nµ上三角方阵;S R为观测噪声协方差R的平方根因子;Kµ为式(27)计算的滤波增益.2.2.2首次观测环境路标的初始化对于首次观测到的路标(这里记其索引为n),利用观测z[n]k 计算其全局坐标并加入粒子S[i]k维护的全局路标地图.由于环境测量z[n]k不确定性分布的容积点集为{F[i][n]j }={S Rζj+z[n]k},j=1,2,···,2n z,则经逆观测模型µ=h−1(s,z)传播后的变换容积点集为{F∗[i][n]j }={h−1(s[i]k,F[i][n]j)}.进而,索引为n的环境路标状态µ[i][n]k为µ[i][n] k =h−1(s[i]k,z)N(z,R)d z≈12n z2n zj=1F∗[i][n]j(29)为初始化该路标的协方差平方根因子,定义n z×2n z误差矩阵Errµ:Errµ=1√2n z[···,F∗[i][n]j−µ[i][n]k,···](30)通过对误差矩阵ErrµT执行QR分解,路标状态协方差平方根因子l[i][n]k计算为[q,r]=qr(ErrµT),l[i][n]k=r T(31)式中,r为nµ×nµ上三角方阵.2.3算法流程综上,提出的SLAM算法具体执行过程见算法1.算法1.本文提出的SLAM算法给定粒子数N,控制噪声Q,观测噪声R,初始化粒子集S0=∅;For k=1,2,3,···//对于每一离散时刻For i=1,2,3,···//对于每一粒子1)索引粒子S[i]k−1;2)根据式(8)∼(12)来预测移动机器人状态及其协方差平方根因子;3)对当前机器人观测执行数据关联,区分重复观测路标和首次观测路标;4)根据机器人对已存储在地图中路标的重复观测,执行机器人状态更新(式(13)∼(20));5)采样新粒子(式(21))并计算其重要性权重(式(22));6)对已存储在粒子中的重复观测路标,根据式(23)∼(28)更新其状态及其协方差平方根因子;7)对于首次观测路标,根据式(29)∼(31)初始化路标状态及其协方差平方根因子,加入环境地图;End for基于粒子权重对粒子集执行重采样处理;返回粒子集S[i]k;End for2.4提出算法的优势分析非线性模型线性化引入的截断误差将在SLAM 迭代过程中逐步累积.因而,为提高SLAM性能,有效减小误差累积是值得关注的核心问题.本质上,SLAM中非线性模型线性化的目的在于计算高斯分布经非线性函数作用后的转移密度,如式(10)、(15)、(23)、(29),其被积函数均具有非线性函数×高斯密度的基本形式.为说明该问题,不失一般性地定义SLAM问题中涉及的非线性函数为y=g(x),其中,x∈R n x服从高斯分布N(ˆx,P x),为计算转移密度N(ˆy,P y),需要计算如下两个GWI:ˆy=g(x)N(ˆx,P x)d xP y=[g(x)−ˆy][g(x)−ˆy]T N(ˆx,P x)d x(32)在FastSLAM2.0中,以上两个GWI是通过对非线性函数g(x)在ˆx处进行线性化所得到,即g(x)=g(ˆx)+∇g(ˆx)(x−ˆx).其中,∇g(ˆx)为雅可比矩阵.因而,根据高斯分布的性质有ˆy=g(ˆx), P y=∇g(ˆx)P x∇g(ˆx)T.显而易见,当g的非线性程度较高时,转移密度N(ˆy,P y)的计算将不准确.对于UFastSLAM算法,上述积分由Sigma支撑点集的线性回归计算,由于Sigma点方法在理论上可以达到3阶非线性,UFastSLAM的精度要高于FastSLAM2.0.然而,Sigma点方法存在于数值稳定性差、难以保证协方差矩阵的正定性、需设定的自由参数多等问题,限制了其在SLAM中的应用.2.4.1提出算法中利用容积律的优势在矩特性方面,容积律(式(4)和(5))是从数值积分观点提出的一种GWI求解方法[11].已经证明了多变量矩积分x i11x i22···x i nnN(ˆx,P x)d x(其中x j指明向量x的j-th分量)在阶数满足条件i nj=1i j≤3时可由容积律来精确计算.对于非线性函数y=g(x),通过对g(x)在x=ˆx处进行泰勒展开,则积分g(x)N(ˆx,P x)d x可以表示为多个矩积分的线性组合.由于利用容积律可以精确计算展开式中所有前三阶矩积分,故而从泰勒展开的意义上,容积律具有3阶非线性估计精度.从估计的矩特性角度,提出SLAM算法中采用的容积律与UFastSLAM和FastSLAM2.0中分别采用的无迹变换、线性化方法的对比见表1.对于SLAM问题,均为多维高斯情况,因而在SLAM算362自动化学报40卷法中采用容积律可以达到与无迹变换相同的矩估计特性.表1不同高斯权重积分求解方法的矩特性对比Table 1Moment characteristic for GWI solutionsGWI 解1维高斯(n x =1)高维高斯(n x >1)线性化方法1阶精度1阶精度无迹变换5阶精度3阶精度容积律3阶精度3阶精度从数值积分的角度,GWI 数值ωi g (x i )解应满足的基本条件是:所有权重ωi 非负.如式(4),显而易见,容积律方法中的所有容积点权重均为1/(2n x ),满足该条件.进而,基于容积律计算的协方差矩阵P y 可保证其非负定性.与之相比较,UFastSLAM 中采用的无迹变换并不能满足该条件.当高斯维数大于3时(对于SLAM,增广状态一般高于3维,即n x >3),其中心Sigma 点的权重ω0=1−n x /3<0,这将可能导致求解的协方差矩阵P y 负定,进而无法求解其平方根,导致SLAM 无法执行.另外,如文献[11]所述,对于数值积分解,当稳定性因子 ωi /|ωi |大于1时,在有限字长的计算机中实施数值计算会引入圆整误差.对于容积律,由于全部容积点的权重均为1/(2n x ),因而其稳定性因子将始终为1.与之相比较,无迹变换的稳定性因子为n x /3+|1−n x /3|,即当维数n x ≤3时数值稳定性为1,而当n x >3时,其稳定性因子将伴随着维数的增长而线性增长,数值计算稳定性变差.提出SLAM 中采用的容积律和UFastSLAM 中采用的无迹变换的数值特性对比见表2.对于SLAM 实际应用,一般增广状态向量的维数高于3(在式(8)、式(13)中,机器人状态包括其坐标和航向角,则增广后状态维数大于3),因而在SLAM 中应用容积律较无迹变换具有数值计算上的优势:数值精度高、可严格保证协方差的正定性.表2容积律与无迹变换的数值特性对比Table 2Numerical characteristic for GWI solutions数值特性无迹变换容积律数值稳定性因子(n x ≤3)11数值稳定性因子(n x >3)2n x3−1>11协方差P y 正定性(n x ≤3)可保证可保证协方差P y 正定性(n x >3)不确定可保证计算代价方面,提出SLAM 算法较基于无迹变换的UFastSLAM 在计算代价方面也具有优势,其原因在于:1)提出算法采用容积律计算GWI,对于相同的高斯维数n x ,容积点数为2n x ,数量低于UFastSLAM 中无迹变换需要的2n x +1个Sigma 点;2)提出算法中的容积点为等权重点,因而利用容积点计算GWI 仅执行简单的平均操作(1次乘法操作),而无迹变换中的Sigma 点为非等权重点,需要使用线性权重回归方式来计算GWI.2.4.2提出算法中采用平方根因子传播的优势利用容积律来计算高斯权重积分,需要计算“容积点集”,而计算“容积点集”需要利用到协方差矩阵的平方根因子,如式(4)所说明.因而,为计算式(32)的非线性转移概率密度N (ˆy ,P y ),需要首先计算协方差矩阵P x 的平方根√P x ,并根据√P x 计算容积点集.容积点集经非线性函数g 传播后,最终用来重构均值ˆy及协方差P y ,完成转移概率密度的估计.由文献[12],利用Cholesky 分解来计算协方差矩阵P x 平方根的复杂度为O(n 3x 6),且由于容积点的数量为2n x ,则重构协方差P y 的复杂度为O(2n 3x ),即传播协方差矩阵的复杂度为O(2n 3x +n 3x /6).与其相比较,本文算法在SLAM 中直接传播协方差平方根因子√P x ,并利用QR 分解实现协方差平方根因子的更新,其计算复杂度为O(2n 3x ).另外,计算协方差矩阵的平方根√P x 是一种应尽力避免的数值敏感操作.为利用Cholesky 分解计算√P x ,其基本前提是要严格保证协方差矩阵P x 的正定性.如果在SLAM 中传播协方差矩阵P x ,由于在计算过程难以避免引入圆整误差,在卡尔曼滤波的协方差更新方程中并不能保证更新后协方差的正定性(卡尔曼滤波算法的协方差更新方程具有矩阵差的形式,形如P +=P −KQK T ),这将可能导致滤波过程的数值不稳定[11−12],甚至滤波器无法执行.因而,在卡尔曼滤波器协方差更新的具体实施中还需要协方差矩阵正定性的检验过程[13].而提出SLAM 算法采用直接传播协方差平方根因子√P x ,由其重构的协方差矩阵为√P x √P x T,可以始终严格保证正定性与对称性.因而,在SLAM 中直接传播协方差平方根因子比传播协方差矩阵具有数值计算稳定性上的明显优势.再者,本文算法中协方差平方根因子的预测和更新均由QR 分解实现,得到的协方差平方根因子为三角方阵,这不仅严格保证了重构的协方差的对称性与正定性,还可以利用高效的回代算法(Back substitution)求解SLAM 算法中涉及矩阵逆,如求解式(19)、(22)和式(27)中的矩阵逆,实现进一步提高SLAM 的求解效率.3仿真研究在Matlab2010平台下(计算机主频2.6GHz),基于悉尼大学野外机器人中心发布的SLAM 算法仿真器开展仿真研究[13].并通过将提出的算法与2期宋宇等:平方根容积Rao-Blackwillised 粒子滤波SLAM 算法363FastSLAM2.0、UFastSLAM 两种方法的综合对比来验证算法性能.在该部分研究中,机器人运动学模型为前轮驱动并转向的Car-like 运动学模型,环境观测模型为Bearing-Rang 激光雷达扫描模型.相关仿真参数设定为:移动机器人航向运动速度v =4m/s;机器人车体前后轮间距WB =4m;环境观测传感器最大观测距离范围40m,最大前向视角180◦;控制周期25ms,环境路标观测周期200ms.仿真中设计的地图环境为图1所示的大尺度、密集路标环境.其地图尺度为230×190,包含路标302个(图中“*”表示实际路标位置,“.”表示估计路标位置).仿真中设定移动机器人从初始状态s 0=[0,0,0]T开始运动,并按照全局规划的一系列目标点(图中“o”表示)行走,直至运动一周后返回起始点(环形闭合).图1大尺度、密集路标下仿真环境Fig.1Large map with dense landmarks3.1增长测量噪声条件下的SLAM 算法性能由于在粒子滤波器的重要性函数设计中缺少了当前的环境观测信息,经典的FastSLAM 算法的主要缺点在于粒子集退化问题(如FastSLAM1.0).特别当环境观测传感器精确时,观测似然函数的外形尖锐,容易造成绝大多数粒子权重很小甚至为零,导致粒子集退化、SLAM 滤波器失效.为验证提出的SLAM 算法有效地避免了粒子退化问题,在图1所示的特征地图下开展一系列的仿真实验:仿真中设定粒子数为10个,车辆速度控制噪声设定为0.3m/s;驾驶角控制噪声设定为3◦;环境特征测量的角度噪声为0.5◦;环境特征测量的距离噪声为11组增长的值,分别为0.01m,0.03m,0.05m,0.07m,0.09m,0.1m,0.15m,0.2m,0.5m,0.7m, 1.0m.算法性能评估标准为均方根误差(Root mean square error,RMSE)和有效粒子百分比(Number of effective particles,NEFF),定义为RMSE =N sk =0|s k −ˆsk |2N s −112NEFF =100%N N i =1(ω[i ]k )2(33)式中,s k 为机器人在k 时刻的真实状态;ˆsk 为SLAM 算法的估计值;N s 为一次SLAM 仿真中的离散采样点数量;ω[i ]k 为第i 个粒子的重要性权重;N 为粒子数.对于每组选定的距离测量噪声水平,均对Fast-SLAM2.0、UFastSLAM 和提出SLAM 算法执行50次仿真实验.并以50次仿真的机器人路径RMSE 平均值(评估SLAM 估计精度)及标准差(评估SLAM 执行稳定性)和NEFF 平均值(及标准差)作为评价标准,实验结果如图2和图3所示.图2为三种算法的RMSE 均值及标准差对比.可以看出,随着测量噪声增长,三种SLAM 算法对车辆路径的估计误差及标准差都逐步增加.然而,提出SLAM 算法的RMSE 均值要低于另外两种算法,意味着提出方法的估计精度要优于另两者,同时提出SLAM 算法在不同观测噪声下的RSME 标准差增长速度也低于另外两种算法,说明其具有较好的运行稳定性.进一步地,这三种算法都避免了粒子集退化问题,当测量噪声较低时(如0.01m,0.03m,0.05m,0.07m,0.09m),SLAM 算法并没有出现较高的RMSE,其原因在于在这三种算法的重要性函数设计过程中均融入了当前环境观测信息.即便观测噪声较低、观测似然p (z k |x k )外形尖锐,经运动图2增长测量噪声条件下的3种算法RMSE Fig.2Performance of robot path estimation withincreasing measurement noise levels in range。
卡尔曼和粒子滤波
( k )} R
1
( k ) ( k )
F ( n 1, n ) x ( n )......... .......... .......... .......... ....( 23 )
五，卡尔曼滤波
若定义
G ( n ) E { x ( n 1) ( k )} R ( k )
x ( n 1) F ( n 1, n ) x ( n ) v1 ( n ).......( 1)
式中，向量x(n)表示系统在离散时间n的状态向量， 矩阵F（n+1,n）成为状态转移矩阵， 向量 v1 (n ) 为过程噪声向量，
五，卡尔曼滤波 考虑一离散时间的动态系统，它由描述状态向量的 过程方程和描述观测向量的观测方程共同表示。 2，观测方程
X ( Z )- - - - X ( t )
^
一，系统估计问题
一般的，估计问题可以分为两类：
a,状态估计（动态估计） b,参数估计（静态估计）
下面我们只讨论状态估计问题。
二，贝叶斯状态估计
1，系统定义 X为被估计量; p(X)为先验分布; Z1:k为X的k个观测值; p(Z1:k|X)为条件概率函数; 则根据贝叶斯公式有
（1）、新息过程的性质 y(n)的新息过程定义为：
( n ) y ( n ) y 1( n )......... .( 6 ) ˆ
式中，N 1向量( n )表示观测数据y(n)的新的信息，简称新息。
五，卡尔曼滤波 新息 (n ) 具有以下性质： 性质1 n时刻的新息 (n ) 与所有过去的观测数据y(1), ...， y(n-1)正交，即：
x 1 ( n 1)
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理所有滤波问题其实都是求感兴趣的状态的后验概率分布，只是由于针对特定条件的不同，可通过求解递推贝叶斯公式获得后验概率的解析解（KF、EKF、UKF），也可通过大数统计平均求期望的方法来获得后验概率（PF）。
1 KF、EKF、UKF1.1 定义KF、EKF、UKF 都是一个隐马尔科夫模型与贝叶斯定理的联合实现。
是通过观测信息及状态转移及观测模型对状态进行光滑、滤波及预测的方法。
而KF、EKF及UKF的滤波问题都可以通过贝叶斯估计状态信息的后验概率分布来求解。
Kalman在线性高斯的假设下，可以直接获得后验概率的解析解；EKF是非线性高斯模型，通过泰勒分解将非线性问题转化为线性问题，然后套用KF的方法求解，缺陷是线性化引入了线性误差且雅克比、海塞矩阵计算量大；而UKF也是非线性高斯模型，通过用有限的参数来近似随机量的统计特性，用统计的方法计算递推贝叶斯中各个积分项，从而获得了后验概率的均值和方差。
1.2 原理KF、EKF、UKF滤波问题是一个隐马尔科夫模型与贝叶斯定理的联合实现。
一般的状态模型可分为状态转移方程和观测方程，而状态一般都是无法直接观测到的，所以时隐马尔科夫模型。
然后，它将上一时刻获得的状态信息的后验分布作为新的先验分布，利用贝叶斯定理，建立一个贝叶斯递推过程，从而得到了贝叶斯递推公式，像常用的卡尔曼滤波、扩展卡尔曼滤波、不敏卡尔曼滤波以及粒子滤波都是通过不同模型假设来近似最优贝叶斯滤波得到的。
这也是滤波问题的基本思路。
所有贝叶斯估计问题的目的都是求解感兴趣参数的后验概率密度。
并且后验概率的求解是通过递推计算目标状态后验概率密度的方法获得的。
在贝叶斯框架下，通过状态参数的先验概率密度和观测似然函数来求解估计问题；在目标跟踪背景下（隐马尔科夫模型），目标动态方差决定状态转移概率，观测方程决定释然函数。
一般化的整个计算过程可以分为3步：01. 一步状态预测：通过状态转移概率及上一时刻的后验概率算出一步预测概率分布。
基于辅助模型的改进粒子滤波算法
ｍａ ｅ ｕ ｌ ｎ ｆｅ ｔ ｅｕ ｅｏ ｂ ｅｖ ｄ ｉｆｒ ｔｏ ｆｔ ｅｐ ｒｉｌｓ Ｓ ｈ ｔｔｅｐ ｏ ｌｍ ｆｐ ｒｉｌ ｅ ｒｄ ｔ ｎ ｋ ｓｆ ｌ ａ ｄｅｆｃｉ ｓ ｆｏ ｓ ｒｅ ｎｏ ｍａｉｎ ｏ ｈ ａｔｃｅ ， Ｏｔ ａ ｈ ｒ ｂｅ ｏ ａｔｃｅｄ ｇ ａ ａｉ ｖ ｏ
Ｋ ｅ ｒ ｓ： ｏ ｌ ｅ ｒｅ ｔ ｔ ｎ Ｐ Ｓ ｙ ｗｏ ｄ ｎ ｎｉ ａ ｓｉ ｉ ； Ｆ； ＲＵＫＦ； ＰＰ ｎ ｍａ ｏ Ｓ Ｆ
０ 引 言
非线性 系统 的估 计 问 题 广 泛存 在 于 许 多 领 域 ，
Ａ． ｎ在 ＵＫＦ的 基 础 上 给 出 了 Ｓ Ｗａ ＲＵＫＦ（ ｑ ａｅ Ｓ ｕ ｒ
（． １ 西北 工业大 学航 海学 院 ， 陕西 西安 ７０７ ；． １ ０ ２ ２ 西安 邮 电学院 电信 系 ， 陕西 西 安 ７０ ６ ） １ ０ １
摘 要 ： 针对非线性、 非高斯系统的目 标跟踪精度不太高这一问题， 提出一种改进 Ｓ ｍ 粒子滤波算法（ Ｐ ｉ ａ ｇ ＭＳ —
Ｐ） Ｆ 。该算法是 由主模 型产生第一个粒子 ， 剩余 的粒子则 由辅助模 型和平方根无迹 卡尔曼滤 波（ ＲＵ ） 递 Ｓ ＫＦ 来 归生成 ， 辅助模型使粒子的观测信息得到充 分有效 地利用 ， 解决 了粒子 滤波算 法所 面临的粒 子退化 和匮乏 问 题 。仿真表 明， 提出的改进 Ｓｇ 粒子滤波算法（ Ｐ Ｆ 的估计性能要 明显 优于粒子 滤波（ Ｆ 、 迹粒子 滤 ｉ ｍａ ＭＳ Ｐ ） Ｐ ）无
平方根容积卡尔曼滤波
平方根容积卡尔曼滤波平方根容积卡尔曼滤波（Square Root Cubature Kalman Filter，SRCKF）是一种经典的非线性滤波算法。
本文将以生动有趣的方式，全面介绍SRCKF的概念、原理、应用以及其在实际工程中的指导意义。
首先，让我们来了解一下什么是卡尔曼滤波（Kalman Filter）。
卡尔曼滤波是一种常用的线性时变系统状态估计算法，其基本思想是通过最小化估计值与真实值之间的误差来获得对系统状态的最优估计。
然而，在很多实际应用中，系统常常呈现非线性特征。
为了处理这些非线性问题，研究者们提出了各种改进的非线性滤波算法，而SRCKF便是其中一种。
SRCKF是基于卡尔曼滤波的算法改进，它主要是针对非线性系统进行状态估计。
与传统的扩展卡尔曼滤波（Extended Kalman Filter，EKF）相比，SRCKF具有较好的收敛性和鲁棒性。
SRCKF的核心思想是将系统状态用均值和协方差矩阵表示，通过采用均方根信息滤波的方法来等效地更新状态。
与EKF不同的是，SRCKF采用了容积取样技术，通过选取合适的样本点，可以更好地描述系统的非线性特征。
SRCKF的工作原理可以简要地概括为以下几个步骤：首先，通过容积取样方法构造一组样本点，这些样本点将充分覆盖整个状态空间。
然后，在每个样本点上，利用非线性系统模型进行状态预测，并计算相应的权重。
接下来，通过对各个样本点进行权重加权平均，估计出系统的状态均值和协方差矩阵。
最后，根据测量值与估计值之间的差异，更新状态的估计。
SRCKF在实际工程中有广泛的应用。
例如，在无人驾驶汽车的导航系统中，SRCKF可以用于车辆位置和姿态的估计，提高导航的精度和稳定性。
另外，SRCKF还可以应用于航天器的姿态控制、机器人定位与导航、生物医学工程中的生理信号分析等领域。
总结一下，平方根容积卡尔曼滤波是一种非线性滤波算法，通过容积取样和均方根信息滤波等技术，能够较好地处理非线性系统状态估计问题。
自适应平方根球型无迹卡尔曼滤波算法
自适应平方根球型无迹卡尔曼滤波算法
自适应平方根球型无迹卡尔曼滤波算法（Adaptive Square-Root Cubature Kalman Filtering Algorithm）在近年来受到了互联网行业的广泛关注。
这种算法归结为：一种利用滤波理论从一系列混合系统动态中估计和提取信息的空间序列分析技术。
它有效解决了复杂系统动态状态参数估计问题，可以有效应用在大数据环境中，进而提升了信息提取和处理的准确性以及效率。
算法的优势体现在数据处理的准确性、可靠性与时效性的发挥上。
由于算法的
自适应特性，估计目标状态参数的准确性得到了很大提升，可以更有效的发挥数据的价值，从而更好的满足用户的需求。
此外，无迹的特性也使得整体数据处理过程具备更强的鲁棒性，对于复杂系统可以获得更为稳定的参数估计，能够很好地抵抗外部噪声干扰。
此外，算法的弹性也得到了极大加强，它可以很好地应对复杂系统动态的变化，而且具有更强的自适应性，能够有效的去除虚假的数据，并根据新的条件快速调节状态参数，从而大幅提高数据处理的速度与准确度。
通过上述分析，自适应平方根球型无迹卡尔曼滤波算法在互联网行业中有非常
重要的应用价值。
在大数据环境下，借助该算法，能够更加快速且准确地处理复杂系统动态状态参数变化，从而有效地促进了信息提取与处理的发展，有助于提高用户体验，实现互联网行业的健康发展。
粒子滤波和卡尔曼滤波 java
粒子滤波和卡尔曼滤波 java粒子滤波(Particle Filter)和卡尔曼滤波(Kalman Filter)是两种常用的滤波算法，用于处理动态系统中的状态估计问题。
本文将介绍这两种滤波算法的原理和应用，并探讨它们在Java中的实现方式。
一、粒子滤波粒子滤波是一种基于蒙特卡洛方法的非线性滤波算法，用于在非线性系统中进行状态估计。
它通过一系列随机抽样的粒子来对系统状态进行表示和估计。
每个粒子都有一个权重，用于反映其对状态估计的贡献程度。
粒子滤波的算法步骤如下：1. 初始化粒子集合，可以根据先验知识或观测数据进行初始化。
2. 预测阶段：根据系统的状态转移模型，对每个粒子进行状态预测，并更新粒子的权重。
3. 更新阶段：根据观测数据，对每个粒子进行状态更新，并更新粒子的权重。
4. 重采样阶段：根据粒子的权重，进行重采样操作，得到下一时刻的粒子集合。
粒子滤波的优点是能够处理非线性系统和非高斯噪声，但需要大量的粒子才能获得准确的估计结果。
在Java中，可以使用随机数生成器来生成粒子，并使用权重数组来表示粒子的权重。
通过遍历粒子集合，可以进行状态预测、更新和重采样操作。
二、卡尔曼滤波卡尔曼滤波是一种基于贝叶斯概率理论的线性滤波算法，用于系统状态的估计和预测。
它假设系统的状态和观测数据都服从高斯分布，并通过最小均方误差准则来进行估计。
卡尔曼滤波的算法步骤如下：1. 初始化状态向量和协方差矩阵，可以使用先验知识或观测数据进行初始化。
2. 预测阶段：根据系统的状态转移模型，通过状态向量和协方差矩阵进行状态预测。
3. 更新阶段：根据观测数据，通过状态向量和协方差矩阵进行状态更新。
4. 修正阶段：根据观测数据的精度和先验知识的可靠性，进行状态修正。
卡尔曼滤波的优点是能够高效地处理线性系统和高斯噪声，但对非线性系统和非高斯噪声的处理效果较差。
在Java中，可以使用矩阵库来进行矩阵运算，并通过矩阵乘法、矩阵加法和矩阵逆运算等操作，实现卡尔曼滤波算法。
新型粒子滤波算法及其在纯方位目标跟踪中的应用
新型粒子滤波算法及其在纯方位目标跟踪中的应用作者：王法胜张应博来源：《计算机应用》2010年第01期摘要:针对基本粒子滤波算法没有融合当前时刻观测值的缺点,提出了一种卡尔曼粒子滤波算法。
该算法针对每一个粒子使用卡尔曼滤波器进行更新,在更新过程中融合最新的观测信息,提高粒子滤波器的估计精度。
针对纯方位目标跟踪问题进行实验,与基本粒子滤波算法及卡尔曼滤波进行了对比。
实验结果表明,卡尔曼粒子滤波算法的跟踪性能明显优于其他两种算法。
关键词:卡尔曼滤波器;粒子滤波;目标运动分析;线性跟踪系统中图分类号: TP39文献标志码:ANovel particle filtering algorithm with application to bearingonly trackingWANG Fasheng1, ZHANG Yingbo21. Department of Computer Science and Technology, Dalian Neusoft Institute of Information,2. City Institute, Dalian University of Technology, Dalian Liaoning 116600, China)Abstract: The conventional bootstrap filter suffers a main drawback of not incorporating the latest observations. Therefore, this paper proposed a Kalman Particle Filter (KPF) algorithm, and applied this new algorithm to bearingonly target tracking. An improved scheme was presented to handle this problem and yield a Kalman particle filter. The underlying idea of the new algorithm is that each particle is updated using Kalman filter incorporating the coming observations. A bearingonly tracking model was experimented and compared with bootstrap filter and KPF. The experimental results verify its superiority.Key words: Kalman filter; particle filtering;target motion analysis; linear tracking system0 引言纯方位目标跟踪(BearingOnly Tracking, BOT)在许多领域尤其是军事领域中(航空、航海、水下)具有非常广泛的应用[1]。
基于平方根卡尔曼滤波的粒子滤波算法
基于平方根卡尔曼滤波的粒子滤波算法余家祥;李华;梁德清【期刊名称】《探测与控制学报》【年(卷),期】2012(034)001【摘要】在普通粒子滤波器中,基于先验概率的重要性密度不能容纳最新测量信息,导致跟踪精度难以提高.针对该问题,给出一种基于平方根卡尔曼滤波(SRUKF)的新型粒子滤波算法(SRUPF).该算法以普通粒子滤波器(PF)为基础,运用SRUKF生成重要性密度.与运用先验知识生成重要性密度的普通粒子滤波器不同,SRUPF的重要性密度中包含了最新的观测信息,从而能够更好地逼迫状态变量的分布规律.此外,由于SRUPF在计算重要性密度时不需要在每一个迭代步骤都对状态协方差阵进行分解,因而SRUPF比PF具有更好的数值稳定性.在非线性测角跟踪问题中的应用表明:SRUPF滤波器的跟踪精度优于PF和SRUKF.【总页数】5页(P19-23)【作者】余家祥;李华;梁德清【作者单位】海军大连舰艇学院训练部,辽宁大连 116018;海军大连舰艇学院训练舰支队,辽宁大连116018;海军大连舰艇学院训练部,辽宁大连 116018【正文语种】中文【中图分类】TP391.12【相关文献】1.基于Sage-Husa的线性自适应平方根卡尔曼滤波算法 [J], 周勇;张玉峰;张超;张举中2.基于无迹卡尔曼滤波和权值优化的改进粒子滤波算法 [J], 冉星浩;陶建锋;杨春晓3.基于平方根序贯处理的两阶段卡尔曼滤波算法 [J], 全振中;石志勇;王怀光;王毅4.基于修正的自适应平方根容积卡尔曼滤波算法 [J], 李春辉;马健;杨永建;肖冰松;邓有为;盛涛5.基于粒子群算法和平方根平淡卡尔曼滤波的北斗导航系统定位估计算法 [J], 陈小玲;茅旭初因版权原因，仅展示原文概要，查看原文内容请购买。
基于平方根卡尔曼滤波的车辆融合跟踪
第!"卷!第#期$%%&年#月!西!安!交!通!大!学!学!报’()*+,-(./01,+’0,(2(+3)+045*6027489:!"!;#’<=:$%%&基于平方根!"#$%"&%’卡尔曼滤波的车辆融合跟踪陈!莹!韩崇昭!西安交通大学电子与信息工程学院">?%%@""西安#摘要"针对车辆运动的机动性和跟踪系统的非线性!提出了一种基于平方根)=A B C=D C E卡尔曼滤波#6*F)G.$的多传感器融合跟踪方法:该方法采用动力学模型建立系统的状态方程和量测方程!充分利用了多传感器的量测信息!更好地满足了目标的机动特性:采用基于)G.的数据融合方法处理系统的非线性问题!避免了扩展卡尔曼滤波#5G.$产生的线性化误差:同时!在滤波过程中!以协方差平方根阵代替协方差阵参加迭代运算!有效地避免了滤波器的发散!提高了滤波算法的收敛速度和稳定性:实验证明!与基于5G.的融合算法相比!基于6*F)G.的融合算法使系统的位置和方向角的跟踪精度分别提高了?H I$$J和!@I H?J:关键词$车辆跟踪%非线性滤波%数据融合中图分类号$2K$>@!文献标识码$,!文章编号$%$&!!"H>/!$%%&#%#!%&"@!%@(%)*$+%,-#*."/01$2*"341#%’."56-10%7..&!"#$%"&%’81+91",*+&%0!"#$%&$’"()$!"*$’+")*!6B L8898M59C B D N8=O B A P=E0=M8N Q P D O8=5=R O=C C N O=R"/O1P=’O P8D8=R)=O S C N A O D T"/O1P=>?%%@""U L O=P#:;#&01$&$,O Q O=R P D D L C=8=F9O=C P N O D T8M D N P B V O=R A T A D C Q AP=ED L C Q P=C<S C N P W O9O D T8M D L CN8P ES C L O B9C Q8S C Q C=D"P=C XM<A O8=P9R8N O D L QW P A C E8=A Y<P N C N88D F<=A B C=D C EG P Q P=M O9D C N!6*F)G.#X P A Z N C A C=F D C E:,E T=P Q O BQ8E C9"X L O B L W C D D C N A P D O A M O C E D L CQ8W O9C Z C N M8N Q P=B C8M D L C D P N R C D P=E M<99T<D O9O[C EQ<9D O F A C=A8NQ C P A<N C Q C=D A"X P A P E8Z D C E D8C A D P W9O A L D L C A D P D C P=EQ C P A<N C Q C=D C Y<P D O8=A:)G.F W P A C E E P D P M<FA O8=P9R8N O D L Q X P A Z N8Z8A C E D8L P=E9C=8=9O=C P N Z N8W9C Q8M D L C D N PB V O=R A T A DC Q"P=E C9O Q O=PD C D L C C N N8N AB P<AC E W T9O=C P N O[PD O8=8M C\D C=E C EG P9Q P=M O9D C N!5G.#:.<N D L C N Q8N C"B8S P N O P=B C A Y<P N C N88DQ P D N O\"O=F A D C P E8M B8S P N O P=B C8=C"X P A D P V C=O=M O9D C N O D C N P D O8="X L O B LC M M C B D O S C9T P S8O E C E M O9D C N O=R E O S C N R C=B C"P=E Q C P=X L O9C O Q Z N8S C E D L C B8=S C N R C=B C S C98B O D T P=EA D P W O9O D T8M D L C P9R8N O D L Q:2L C C\Z C N O Q C=D A A L8XD L P D D L C Z8A O D O8=P=E8N O C=D P D O8=D N P B V O=R P B B<N P B T W T6*F)G.F W P A C E M<A O8=P9R8N O D L QP N C O Q Z N8S C E?H I$$J P=E!@I H?JN C A Z C B D O S C9T B8Q Z P N C EX O D L5G.F W P A C E8=C:8%<=.0’#$,#"&-.#/0)-1&$’%$*$.&$#)02&./#0%3)/)245&*$!!利用多个传感器的量测信息实现对路面车辆运动状态的融合跟踪是目前智能车研究的一个热点:在统一的直角坐标系下所建立的描述此问题的数学模型通常是非线性的:针对扩展卡尔曼滤波!5G.#处理非线性滤波问题的不足&?’"’<9O C N等人提出了采用基于)=A B C=D C E变换的卡尔曼滤波!)G.#方法&$’"在处理状态方程时先进行)=A B C=D C E变换!)变换#"再用)变换后的状态变量进行滤波估计"以减小估计误差:本文针对机动车辆融合跟踪系统的非线性问题"在)G.中用协方差平方根阵代替协方差阵参加迭代运算"并结合分布式数据融合技术"提出了基于平方根)G.!6Y<P N C*88D F)=A B C=D C E G P9Q P=.O9D C N"6*F)G.#的车辆多传感器融合跟踪方法:实验证明"该方法能充分利用多传感器数据之间的冗余和互补特性"提高跟踪精度:收稿日期$$%%@!%"!%#:!作者简介$陈!莹!?">#"#"女"博士生%韩崇昭!联系人#"男"教授"博士生导师:!基金项目$国家重点基础研究发展规划资助项目!$%%?U]!%"@%!#:?!系统描述毫米波雷达和图像传感器由于具有优势互补的特点!从而成为车辆融合跟踪系统的两种主要传感器"!#:本系统采用著名的]O B TB 9C 动力学模型"@#来建立目标的状态方程:该模型可反映机体运动与车轮转速$转向角之间的关系!比通常的线性模型能更好地满足目标的机动特性:前方目标的纵向和侧向信息分别来自毫米波雷达和图像传感器!系统的融合结构将充分利用二者的量测信息!实现对前方机动目标的精确跟踪:>?>!状态方程的建立为了更好地满足目标的机动特性并充分利用多传感器的量测信息!系统采用]O B T B 9C 模型描述车辆的运动状态:此时!前方目标车辆的运动方程为6%7,B 8A !!897,A O =!!8,7)!%7,D P =":!"%7"#$;&?’式中(:为车辆的轴距))$;为控制参数)6$9分别为车辆在坐标轴上的位移),为车辆的速度)!是车辆的方向角)"为车辆的前轮转向角<将式&?’离散化!假定采样间隔为=!且控制信号&)!;’在采样间隔内大致不变<设在时刻1!状态变量定义为!&1’7"6&1’!9&1’!,&1’!!&1’!"&1’#2!控制向量定义为"&1’7")&1’!;&1’#2!且满足)&1’7)&1’&?>#Z ’>#);&1’7;&1’&?>#A ’>#"#$W &$’式中(#)$#;为干扰噪声)#Z $#A 为相乘噪声!表示当车辆的速度和转向角发生变化时!车辆行为不确定性的增加)#)$#;$#Z $#A 都是均值为%$协方差已知的白噪声序列<状态方程记为!&1>?’7#&1!!&1’!"&1’’>$&1’&!’式中(过程噪声$&1’是具有%均值和已知协方差阵%&1’的高斯噪声向量<>?@!量测方程的建立毫米波雷达能在任何天气条件下实时监测车辆前方!可测得目标的距离0$接近速度,0和视线角$<在时刻1!量测方程记为&?&1’7’?&1!!&1’’>(?&1’&@’式中’?&1!!&1’’70&1’7*"6&1’?6L &1’#$>"9&1’?9L &1’#$+?$,0&1’7,&1’?,L &1’$&1’7D P =??9&1’?9L &1’6&1’?6L &1"#’%&’(!(?&1’7&@0!@,0!@$’&&’其中&6L !9L ’为自身车辆位移!,L 为自身车辆速度!由速度传感器测得!量测噪声@0$@,0和@$分别是方差为%$0$%$,0和%$$的高斯白噪声<相对于毫米波雷达!图像传感器能获得广视角信息以及精确的角测量值<在系统中!图像的量测为&6!9!!’!其中位移&6!9’是在雷达的位移估计&6A !A 9’的基础上!通过融合图像的灰度信息自适应地调整目标中心位置而获得的"&#!方向角!采用三维模型技术"##获取<系统采用改进的^P <A E 8N M M 距离作为目标图像与模型投影的匹配量测!用带有权值的轮廓点代替二值图像对模型进行匹配!可避免建立点&点对应!降低计算损耗!还可避免远离边界的噪声点对模型匹配的影响<通过求解匹配函数的最优解获得!<在时刻1!图像的量测方程记为&$&1’7’$&1’!&1’>($&1’&#’式中’$&1’7?%%%%%?%%%%&’(%%%?%($&1’7&@6!@9!@!’&>’其中量测噪声@6$@9$@!分别是方差为%$6$%$9和%$!的高斯白噪声<$!基于6*F )G .的状态数据融合式&?’’式&>’的多传感车辆跟踪系统满足了车辆的机动特性!且充分利用了雷达和图像的量测信息!但其状态方程和量测方程均具有较强的非线性<处理非线性滤波的经典方法为5G .!在5G .中!为了求取估计误差协方差的传播!将动力学模型与量测模型2P T 98N 展开线性化:由于2P T 98N 展开方法存在着函数的整体特性&平均值’被局部特性&导数’所代替的缺点!因此在状态的估值过程中会产生较大的偏差!使滤波结果无法满足精度要求!还可能导致滤波发散<而且!系统的雅可比矩阵)&1’7)#)!!7*!&1’和+?&1’7)’?)!!7*!&1>?*1’计算复杂!在实际应用中容易产生编码错误<’<9O C N 等人">#指出!)G .与二阶5G .精度相同!且)G .不用计算雅&"&!第#期!!!!!!!!!!!!!陈!莹!等(基于平方根)=A B C =D C E 卡尔曼滤波的车辆融合跟踪可比矩阵和海赛矩阵!算法易于实现:此外!由于)G .比5G .对过程噪声的变化更加不敏感!对初始条件的不确定性更具鲁棒性"H #!因此更适用于机动目标的在线跟踪:本文在)变换的基础上建立了融合跟踪算法!通过一系列带权值的样本点来捕获状态分布的均值和协方差!以改善系统的滤波性能<@?>!57A !"#%$"&%’变换设$维随机向量,’($,!-6%与B 维随机向量.成非线性函数关系.72$,%$H %)变换就是根据$,!-6%!设计一系列的点!&$&7%!?!&!C %!称其为%点<对设定的%点计算其经过传播2$’%所得的结果"&$&7%!?!&!C %!然后基于"&计算.的均值9和协方差阵-9<通常%点的数量取为$$>?!即C 7$$<-6通常为非负定矩阵<然而!在滤波的迭代过程中!由于计算误差等因素的影响!致使滤波协方差阵不对称或负定!从而导致滤波器发散!影响滤波算法的收敛速度和稳定性!因此将-6分解为-67/26/6$"%式中(/6称为-6的平方根矩阵!可通过U L 89C A V T 分解获得!它在滤波更新算法中的导出依赖于矩阵正交三角化过程!可通过^8<A C L 89E C N 变换或改进的3N P Q F 6B L Q O E D 来实现""#<在滤波过程中!用/6代替-6参加迭代运算!从而保证了协方差阵的正定性<@?@!基于57A !8,的融合算法针对式$?%’式$>%的非线性多传感器车辆跟踪系统!给定1??时刻的状态!基于全局信息的融合估计值!A $1??%及估计误差方差平方根阵/$1??%!利用6*F )变换设计%点!$&%$1??%$&7%!?!&!$$%!$%%$1??%7!A $1??%!$&%$1??%7!A $1??%>$$>)%?$/$1??"#%&!!!&7?!$!&!$!$&%$1??%7!A $1??%?$$>)%?$/$1??"#%&?$!!&7$>?!$>$!&!$"#$$$?%%式中("/#&表示矩阵/的第&列<传感器D $D 7?!$%的一步预测$!A D $1*1??%!/D$1*1??%%和量测估计$&A D$1*1??%!/&!D$1*1??%%分别利用$$>?个%点通过状态方程和量测方程的传播得到!从而获取1时刻基于各传感器的状态估计和协方差平方根估计"?%#*!D $1%)/D $1%<最后!采用基于最小方差的数据融合算法"??#!得到1时刻基于全局信息的融合估计值!A $1%及估计误差方差平方根阵/$1%为!A $1%7!A $1E 1??%>/$1%/F $1%+$D7?*"/D $1%’!!!/F D $1%#??"!A D $1%?!A $1E 1??%#+/$1%7B L 89*"+$D7?"/D $1%/F D $1%#???!!!"/$1E 1??%/F $1E 1??%#?#??+"#$?$??%式中(!A $1*1??%)/$1*1??%分别为运用6*F )G .所求的状态一步预测和预报误差协方差平方根阵,B L 89*’+为U L 89C A V T 分解算子<!!仿真结果及讨论利用三维视景仿真软件4C R P 及其U 语言应用程序接口,K 0!以4U #I %为开发环境!创建了三维仿真环境!如图?所示<图中前方车辆初期作近乎匀速的直线行驶!>%帧之后车辆开始转弯!此时车体的转角)速度都在时刻发生着变化!?%%帧后车辆重新进入直道!其运动轨迹如图$P 中实线所示<$P %第?$帧!!!!!$W %第H %帧图?!仿真场景在仿真实验中!假定采样间隔=7?A !初始状态均值!A $%%7"$#&?Q !$#"HQ !!%Q -A !%_!%_#!初始方差平方根矩阵/$%%7$%I %?%0&!系统过程噪声%7$%I %?%$0&!雷达量测噪声1?7E O P R $?$!%I $$!%I %!$%!图像量测噪声1$7E O P R$%I %?$!%I %?$!%I %?$%<应用基于6*F )G .的数据融合方法!按上述过程进行滤波!并与基于5G .方法的结果进行比较!得到二者在位置和姿态角的估计对比!以及?%%次‘8=D C F U P N 98仿真得出的二者均方根$*‘6%误差统计结果!如图$所示<从图$可以看出!当前方车辆发生急剧转弯时!滤波器的跟踪误差有所上升!但由于融合算法充分#"&西!安!交!通!大!学!学!报!!!!!!!!!!!!!!!!!!第!"卷!!P"位置估计对比!W"方向角估计对比!B"6方向位置误差!!!!E"9方向位置误差!C"方向角误差图$!仿真结果对比利用了雷达和图像的量测信息#因此跟踪器仍然能准确地对目标进行跟踪#且相对于基于5G.的融合算法#本文所提出的基于6*F)G.的融合算法在目标位置和方向角的跟踪精度上分别提高了?H I$$J 和!@I H?J#且在一定程度上提高了收敛速度:@!结!论本文针对车辆的状态跟踪问题#提出了一种基于6*F)G.的多传感融合跟踪算法:采用]O B T B9C 动力学模型描述车辆运动#利用雷达和图像传感器的量测信息建立量测方程#并针对融合跟踪系统的非线性问题#将6*F)G.与分布式数据融合技术结合#避免了5G.的线性化误差#很好地改善了滤波效果#提高了跟踪精度:参考文献$%?&!a P=5,#S P=E C N‘C N X C*:2L C<=A B C=D C EG P9Q P= M O9D C N M8N=8=9O=C P N C A D O Q P D O8=%,&:2L C0555,E P Z FD O S C6T A D C Q A M8N6O R=P9K N8B C A A O=R#U8Q Q<=O B P D O8=A#P=EU8=D N896T Q Z8A O<Q#-P V C-8<O A C#U P=P E P#$%%%: %$&!’<9O C N6’#)L9Q P==’G:,=C XC\D C=A O8=8M D L CG P9F Q P=M O9D C N D8=8=9O=C P N A T A D C Q A%,&:2L C??D L0=D C N F =P D O8=P96T Q Z8A O<Q8=,C N8A Z P B C’b C M C=A C6C=A O=R# 6O Q<9P D O8=P=EU8=D N89A#(N9P=E8#)6,#?"">I%!&!U P S C=C T b#.C9E Q P=]#^C E N O B V’G:U8Q Z N C L C=A O S C M N P Q C X8N V M8NQ<9D O A C=A8NQ<9D O D P N R C D D N P B V O=R O=D L C P E P Z D O S C B N<O A C B8=D N89C=S O N8=Q C=D%,&:0=D C N=P D O8=P9 6T Q Z8A O<Q8=,E S P=B C E4C L O B9CU8=D N89#^O N8A L O Q P#’P Z P=#$%%$:%@&!a L T D C^b#]C995#,S C N T K:2L C E C A O R=8M P N P E P N F W P A C E=P S O R P D O8=A T A D C Q M8N9P N R C8<D E88N S C L O B9C A%,&:05550=D C N=P D O8=P9U8=M C N C=B C8=*8W8D O B AP=E,<D8Q P D O8=#+P R8T P#’P Z P=#?""&:%&&!陈!莹#韩崇昭:运动车辆的多传感融合跟踪%’&:西安交通大学学报#$%%@#!H!?%"$?%!&!?%!":%#&!2P=2+#6<99O S P=3b#]P V C NG b:5M M O B O C=D O Q P R C R N P E O C=D F W P A C E S C L O B9C98B P9O[P D O8=%’&:05552N P=A P B FD O8=A8=0Q P R C K N8B C A A O=R#$%%%#"!H"$?!@!!?!&#: %>&!’<9O C N6#)L9Q P==’#b<N N P=D F a L T D C^.:,=C X Q C D L8E M8N D L C=8=9O=C P N D N P=A M8N Q P D O8=8MQ C P=A P=E B8S P N O P=B C A O=M O9D C N A P=E C A D O Q P D8N A%’&:05552N P=AF P B D O8=A8=,<D8Q P D O BU8=D N89#$%%%#@&!!"$@>>!@H$: %H&!]N<=V C6#U P Q Z W C99‘:5A D O Q P D O8=P N B L O D C B D<N CM8N M<D<N C P<D8=8Q8<A S C L O B9C A%,&:2L C,Q C N O B P=U8=FD N89U8=M C N C=B C#,=B L8N P R C#)6,#$%%$:%"&!^P T V O=6:,E P Z D O S C M O9D C N D L C8N T%‘&:@D LC E O D O8=: ]C O c O=R$K<W9O A L O=R^8<A C8M59C B D N8=O B A0=E<A D N T# $%%$:@%!!@$&:%?%&a P=5,#S P=E C N‘C N X C*:5M M O B O C=D E C N O S P D O S C F M N C CG P9Q P=M O9D C N A M8N8=9O=C9C P N=O=R%,&:5<N8Z C P=6T Q FZ8A O<Q8=,N D O M O B O P9+C<N P9+C D X8N V A#]N<R C A#]C9F R O<Q#$%%?:%??&何!友#王国宏#陆大金#等:多传感器信息融合及应用%‘&:北京$电子工业出版社#$%%%:!编辑!刘!杨!>"&!第#期!!!!!!!!!!!!!陈!莹#等$基于平方根)=A B C=D C E卡尔曼滤波的车辆融合跟踪。
卡尔曼滤波与粒子滤波课件
• 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度 值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k 时刻的温度预测值是跟k-1时刻一样的，假设是23度(公式1*)，同时 该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出 的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方 相加再开方，就是5 (公式2)）。
7
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔 曼滤波器是最优的信息处理器。 1.首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状 态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) … (1)(预测结果) 式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，
• 可以看出，因为温度计的covariance比较小（比较相信温度计），所 以估算出的最优温度值偏向温度计的值。
11
• 现在我们已经得到k时刻的最优温度值，下一步就是要进入k+1时 刻，进行新的最优估算。
• 在进入k+1时刻之前，还要算出k时刻那个最优值（24.56度）的偏 差。算法如下：((1-Kg)*5^2)^0.5=2.35(公式5)。这里的5就是上 面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入 k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最 优的温度值。
16
基于信息的AKF
• 基于信息的AKF主要是通过调整噪声统计特性达到自适应的目的， 解决了因为噪声统计特性不明确或噪声发生变化的情况。但是对 于系统其它模型发生变化不能达到自适应的目的。
平方根嵌入式容积卡尔曼粒子滤波算法
平方根嵌入式容积卡尔曼粒子滤波算法刘华;缪晨;吴文【摘要】In order to improve the accuracy of existing particle filters for general nonlinear systems with non-Gaussian noises,a new square-root imbedded cubature particle filter( SICPF) algorithm is proposed here. Unlike the conventional particle filter( PF) ,the new algorithm employs a square-root imbedded cubature Kalman filter( SICKF) to generate the importance density function. By integrating the latest observation information, the generated importance density function can match the real posterior density much more closely. Finally, a simulation based on a classical nonlinear and non-Gaussian system model is carried out. The simulation results show that the estimation error of the SICPF algorithm is about one-fourth of that of the EPF, two-thirds of that of the UPF and three fourths of that of CPF. The SICPF is an effective filter algorithm.%为了提高现有粒子滤波算法对非线性、非高斯系统的状态估计精度,该文提出了一种平方根嵌入式容积粒子滤波( Square-root imbedded cubature particlefilter,SICPF)算法. 该算法采用平方根嵌入式容积卡尔曼滤波( Square-root imbedded cubature Kalman filter,SICKF)产生重要性密度函数. 该算法融合了最新的观测信息,由其产生的重要性密度函数更接近系统状态的真实后验概率分布,最后采用经典非线性、非高斯状态模型对该文算法的性能进行仿真测试. 仿真结果表明:SICPF算法的估计误差约为扩展粒子滤波( Extended particle filter,EPF)算法1/4、无迹粒子滤波(Unscented particle filter,UPF)算法误差的2/3、容积粒子滤波(Cubature particle filter,CPF)算法的估计误差的3/4,SICPF算法是一种有效的滤波算法.【期刊名称】《南京理工大学学报（自然科学版）》【年(卷),期】2015(034)004【总页数】6页(P471-476)【关键词】非线性非高斯;粒子滤波;重要性密度函数;平方根嵌入式容积卡尔曼滤波;扩展粒子滤波;无迹粒子滤波;容积粒子滤波【作者】刘华;缪晨;吴文【作者单位】南京理工大学近程高速目标探测技术国防重点学科实验室,江苏南京210094;南京理工大学近程高速目标探测技术国防重点学科实验室,江苏南京210094;南京理工大学近程高速目标探测技术国防重点学科实验室,江苏南京210094【正文语种】中文【中图分类】TP391在信号处理、人工智能、卫星导航、目标跟踪等领域存在着大量非线性滤波问题。
平方根中心差分卡尔曼滤波
平方根中心差分卡尔曼滤波
平方根中心差分卡尔曼滤波（Square Root Central Difference Kalman Filter）是一种可用于估计系统状态的滤波算法。
它是卡尔曼滤波的一种变种，通过采用平方根形式的滤波方程和中心差分法来处理滤波过程中的数值计算，提高了算法的数值稳定性和计算效率。
该滤波算法的主要步骤如下：
1. 初始化滤波器的初始状态和协方差矩阵。
2. 预测阶段：根据系统的动力学模型，通过状态转移方程和控制输入，预测系统的状态和协方差矩阵。
3. 更新阶段：利用观测值和观测模型，计算状态的增益矩阵，根据观测值对预测值进行修正，同时更新状态和协方差矩阵。
4. 重复执行预测和更新步骤，以实现连续的状态估计。
平方根中心差分卡尔曼滤波的主要优点是在滤波计算过程中，通过使用平方根形式的滤波方程和中心差分法，避免了滤波过程中状态协方差矩阵的奇异性，并减少了舍入误差的累积。
此外，该算法还能够提供对状态估计的不确定性的更准确估计。
因此，它在实际应用中具有较好的性能和稳定性。
总结起来，平方根中心差分卡尔曼滤波是一种改进的卡尔曼滤波算法，通过平方根形式和中心差分法来处理滤波计算，提高了滤波过程中的数值稳定性和计算效率。
改进的平方根容积卡尔曼滤波及其在POS中的应用
第44卷第9期红外与激光工程2015年9月Vol.44No.9Infrared and Laser Engineering Sep.2015改进的平方根容积卡尔曼滤波及其在POS中的应用赵兵1,2，曹剑中1，杨洪涛1,2，周祚峰1，史魁1，徐伟高1,2(1.中国科学院西安光学精密机械研究所，陕西西安710119；2.中国科学院大学，北京100049)摘要：为解决扩展卡尔曼滤波在处理复杂非线性状态估计时，存在收敛速度慢、估计精度低及数值稳定性差等问题，引入一种改进的平方根容积卡尔曼滤波算法(A-SRCKF)。
该算法在容积卡尔曼滤波基础上引入矩阵QR分解、Cholesky分解因数更新等技术，避免了矩阵分解、求逆及求导等复杂运算，极大降低了计算复杂度；并针对系统时变及统计特性未知情况下量测噪声协方差阵难以获取问题，通过引入自适应噪声估计器并结合小波卡尔曼滤波思想，构造出加权量测噪声协方差阵，提高了数值精度及稳定性。
将A-SRCKF应用于机载定姿定位系统中，仿真结果表明：该算法有效地提升了估计精度，并且运行速度较快。
关键词：平方根容积卡尔曼滤波；非线性滤波；容积规则；数据融合；POS中图分类号：V249.3文献标志码：A文章编号：1007-2276(2015)09-2819-06Approved square root Cubature Kalman Filtering andits application to POSZhao Bing1,2,Cao Jianzhong1,Yang Hongtao1,2,Zhou Zuofeng1,Shi Kui1,Xu Weigao1,2(1.Xi′an Institute of Optics and Precision Mechanics,Chinese Academy of Sciences,Xi′an710119,China;2.University of Chinese Academy of Sciences,Beijing100049,China)Abstract:To solve the problems that extended Kalman filter is difficult to obtain the optimal state estimation of complex nonlinear system with fast convergence speed and high estimate accuracy,an improved square root Cubature Kalman Filtering algorithm was proposed by introducing the matrix QR decomposition and Cholesky factorization updating technology to traditional Cubature Kalman Filter,via it can validly avoid the complicated calculating of matrix decomposition and inverse.Moreover,aiming at the uncertainty of system′s variable and statistical properties,a weighted adaptive noise covariance matrix estimator was constructed,through integrating the adaptive noise estimator under wavelet Kalman Filtering ideology.A-SRCKF was applied to airborne positioning and orientation system,the simulation results demonstrate that the proposed method can effectively improve the accuracy of POS outputs as well as enhance the efficiency.Key words:square root CKF;nonlinear filter;cubature rule;data fusion;POS收稿日期：2015-01-10；修订日期：2015-02-20基金项目：国家自然科学基金(61201376)；陕西省自然科学基金(2012JQ8003)；中国科学院“西部之光”作者简介：赵兵(1989-)，男，博士生，主要从事航空遥感与目标定位等方面的研究。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
（ ． ｐ ｒｍ ｅ ｔｏ ａ ｎ ｎ Ｄａｉｎ Ｎａ ａ ａ ｅ ｙ Ｄａｉｎ １ ６ １ Ｃｈ ｎ ；２ Ｔｒ ｉ ｉ ｇ Ｄｉ ｉｉｎ ， １ Ｄｅ ａ ｔ ｎ ｆＴｒ ｉ ｉｇ， ｌ ｖ ｌＡｃ ｄ ｍ ， ｌ １ ０ ８， ｉ ａ ． ａｎ ｎ ｖ ｓ ｍ ａ ａ ｏ
Ｓ ＵＰ Ｒ Ｆ在计算 重要性 密度时不需要在每一个迭代 步骤都对 状态协 方差 阵进行分 解 ， 因而 Ｓ ＲＵＰ Ｆ比 Ｐ Ｆ具有
更好 的数值 稳定性 。在非线性测角跟踪问题中的应用表明 ：ＲｕＰ Ｓ Ｆ滤波器的跟踪精度优于 Ｐ Ｆ和 Ｓ ＵＫ 。 Ｒ Ｆ
关 键词 ： 非线性滤波； 粒子滤波； 测角跟踪
第３ ４卷 第 １期
２１ ０ ２年 ２月
探 测 与 控 制 学 报
●
Ｖｏ ．３ ． １ ４ ＮＯ １ Ｆｅ ． Ｏ ２ ｂ ２ １
Ｊ ｕ ｎ ｌｏ ｔ ｃ ｉ ｎ ＆ Ｃｏ ｔｏ ｏ ｒ ａ ｆＤｅ ｅ ｔｏ ｎｒｌ
基 于 平 方 根 卡 尔 曼滤 波 的粒 子 滤 波 算 法
ｔ ｒａｇ ｒ ｈ ｗａ ｅ ｅｏ ｅ ． ｅｎ ｗ ｉ ｅｉｇａｇ ｒ ｈ ｗａ ｌｍｅ ｔｄａ ｃ ｒ ｉｇｔ ｈ ｒ ｃ ｄ ｒ ｄ ｐｅ ｎ ｅ ｌｏ ｉ ｍ ｓｄ ｖ ｌｐ ｄ Ｔｈ ｅ ｆ ｔｒ ｌｏ ｉ ｍ ｓｉ ｅ ｎ ｅ ｃｏ ｄｎ ｏｔ ｅｐ ｏ ｅ ｕ ｅａ ｏ ｔｄｉ ｔ ｌ ｎ ｔ ｍｐ
Ｄ ｌ ｎ Ｎａ ａ Ａ ａ ｅ ， ｅ ｉ １ ０ ８ Ｃ ｉ ） ａｉ ｖ ｌ ｃ ｄ ｍｙ Ｂ ｉｎ １ ６ １ ， ｈｎ ａ ｊｇ ａ ￣
Ａ ｂ ｔａ ｔ Ｉ ｏ ｓｒ ｃ ：ｎｃ ｍｍｏ ａ ｔ ｌ ｉ ｅ ｉｇ，ｈ ｍｐ ｒａ ｃ ｅ ｓｔ ａ ｅ ｎ ｐ ｉｒｐ ｏ ａ ｉｔ ａ ｏ ｏ ｔｉ ｅ ｎ ｐ ｒｉ ｅｆｌ ｒｎ ｔ ｅ ｉ ｏ ｔｎ ｅｄ ｎ ｉ ｂ ｓ ｄ ｏ ｒｏ ｒｂ ｂｌ ｙ ｃ ｎ ｎ ｔｃ ｎ ａｎ ｎ ｗ ｃ ｔ ｙ ｉ
余 家 祥 李 华 梁 德 清 ， ，
（． １ 海军 大连舰 艇 学院训 练 部 ， 宁 大连 １ ６ １ ；．海军 大连舰 艇 学院训 练舰 支 队 ， 辽 １０ ８ ２
辽宁 大连 １６ １ ） １ ０ ８
摘 要 ： 在普通粒子滤波器 中， 于先验概率的重要性密度不 能容纳最新 测量信息 ， 基 导致 跟踪精 度难 以提 高 。
ｃ ｍｍｏ ａ ｔ ｌ ｉ ｅ ＰＦ ． ｔｍａ ｅ ｕ ｅ ｏ ｅ ｅ ｔｙ ｄ ｖ ｌ ｐ ｄ ｓ ｕ ｒ ｏ ｔｕ ｓ ｅ ｔ ｄ Ｋａｍａ ｉ ｅ Ｓ ｏ ｎ ｐ ｒ ｉ ｅ ｆｌ ｒ（ ） Ｉ ｄ ｓ ｆａ ｒ ｃ ｎ ｌ ｅ ｅｏ ｅ ｑ ａ ｅ ｒ ｏ ｎ ｃ ｎ ｅ ｌ ｎ ｆ ｔ ｒ（ ＲＵＫＦ ｃ ｔ ｌ ） ｔ ｅ ｅ ａ ｅ ｔ ｅ ｉ ｐ ｒａ ｃ ｅ ｓｔ ． ｏ ｇ ｎ ｒ ｔ ｈ ｍ ｏ ｔ ｎ ｅ ｄ ｎ ｉｙ Ｕｎ ｉ ｅ ｔ ｅｆ ｒ ｒ Ｐ ｉｈ ｓｍｐ ｙ ａ ｐ ｉｄ ｔ ｅ ｐ ｉ ｒｉ ｆ ｒ ｔ ｎ ａ ｈ ｍ— ｌ ｈ ｏ ｍｅ Ｆ ｗｈ ｃ ｉ ｌ ｐ ｌ ｈ ｒｏ ｎ ｏ ｍａ ｉ ｓ ｔ ｅ ｉ ｋ ｅ ｏ ｐ ｒａ ｃ ｅ ｓｔ ， ｈ ｒ ｓ ｎ ｅ ｌｏ ｉｈ ｅ ｂ ｄ ｅ ｈ ａ ｅ ｔ ｏ ｓ ｒ ａ ｉ ｎ ｉ ｆ ｒ ａ ｉ ｎ Ｓ ｔ ｃ ｕ ｄ ｗｅｌａ ｐ ｏ ｉ ｏ ｔ ｎ ｅ ｄ ｎ ｉｙ ｔ ｅ ｐ ｅ ｅ ｔ ｄ ａ ｇ ｒｔ ｍ ｍ ｏ ｉｄ ｔ ｅ ｌ ｔ ｓ ｂ ｅ ｖ ｔｏ ｎ ｏ ｍ ｔ ， Ｏ ｉ ｏ ｌ ｌ ｐ ｒ ｘ— ｏ ｍａ ｅ ｔ ｅｄ ｓ ｒｂ ｔｏ ｆ ｈ ｔ ｔ ａ ｉｂ ｅ Ｍｏ ｅ ｖ ｒ Ｓ ｔ ｈ ｉｔ ｉ ｕ ｉｎ ｏ ｅ ｓａ ｅ ｖ ｒａ ｌ． ｔ ｒ ｏ ｅ ， ＲＵＰ ｄ ｄ ｎ ｔ ｅ ｄ ｔ ｃｏ ｉｅ ｔ ｅ ｓ ａ ｅｃ ｖ ｒ ｎ ｅａ ａ ｈ Ｆ ｉ ｏ ｅ ｏ ｆ ｔ ｒｚ ｈ ｔ ｔ ｏ ａ ｉ ｃ ｔｅ ｃ ｎ ａ ａ ｓ ｅ ｅ ｔｗａ ａ ｃ ｌ ｔ ｇ ｔ ｅ ｉ ｐ ｒ ａ ｃ ｅ ｓ ｔ ， ｈ ｓ ｉ ｈ ｄ ａ ｇ ｏ ｕ ． ｅ ｌ ｔ ｂ ｌ Ｙ Ｔｈ ｓ ｆ ｔ ｅ ａ ｇ — ｔ ｐ ｗｈ ｎ ｉ ｓｃ ｌ ｕａ ｉ ｈ ｎ ｍ ｏ ｔ ｎ ｅ ｄ ｎ ｉｙ ｔ ｕ ，ｔ ａ ｏ ｄ ｎ ｍｅ ｉａ ａ ｉ ｌ ． ｅ ｕ ｅ ｏ ｈ ｌｏ ｒ ｓ ｉ
中图分 类 号 ： Ｐ９．２ 文献标 志 码 ： 文 章编 号 ：０８１９ ｛０２０—０９ ５ Ｔ ３ １１ Ａ １０—１４２ １）１ １－ ０ ０
Ｐａ ｔｃ ｅ Ｆｉｔ ｒ ｎ ｇ ｒ ｔ ｍ ｓ ｄ ｏ ｑ ａ ｅ ｒ ｉ ｌ ｌｅ ｉ ｇ Ａｌ ｏ ｉｈ Ｂａ ｅ ｎ Ｓ ｕ ｒ Ｒｏ ｔＵｎ ｃ ｎｔ ｄ Ｋ ａ ｍ ａ ｌｅ ｉ ｏ ｓ ｅ ｅ ｌ ｎ Ｆｉｔ ｒ ｎｇ ＹＵ ］ａ ｉｎ ＬＩＨｕ 。 ＬＩ ｉｘ ａ ｇ ， ａ ， ＡＮＧ ｑｎ Ｄｅ ｉｇ
ｍ ｅ ｓ ｒ ｍｅ ｔｉ ｆ ｒ ａ ｉ ｎ Ｓ ｈ ｒ ｃ ｉ ｇ ｐ ｅ ｉｉ ｎ ｉ ｈ ｒ ｏ ｉ ｒ ｖ ． ｏ ｖ ｈ ｒ ｔ ｅｔ ａ ｋ ｎ ｒ ｃｓｏ ｓ ａ ｄ ｔ ｍｐ ｏ ｅ Ｔｏ ｓ ｌｅ ｔ ｅｐ ｏ ｌｍ ｏ ｏ ｅ ｒｉ ｅｆｌ ｐ ｃ —