非线性滤波概念和原理介绍(legend08fda整理)

非线性滤波概念和原理介绍

一、背景介绍[1]

“估计”就是从带有随机误差的观测数据中估计出某些参数或某些状态变量。估计问题一般分为三类：从当前和过去的观测值来估计信号的当前值，称为滤波；从过去的观测值来估计信号的将来值，称为预测或外推；从过去的观测值来估计过去的信号值，称为平滑或内插。滤波理论就是在对系统可观测信号进行测量的基础上，根据一定的滤波准则，对系统的状态或参数进行估计的理论和方法。

1795年，高斯（K.Gauss ）提出了最小二乘估计法。该方法不考虑观测信号的统计特性，仅仅保证测量误差的方差最小，一般情况下这种滤波方法的性能较差。但该方法只需要建立测量模型（测量方程) ，因此目前在很多领域仍有应用。

二十世纪40年代，Weiner 和Kolmogorov 提出了维纳滤波理论。维纳滤波充分利用输入信号和量测信号的统计特性推的，不便于实时应用。V.Kucera 于1979年提出了现代维纳滤波方法。该方法可以直接得到可实现的和显式的维纳滤波器，可处理多维信号和非平稳随机信号。卡尔曼（R.E.Kalman ）于1960年提出了卡尔曼滤波（Kalman Filtering）理论。该方法是一种时域方法，对于具有高斯分布噪声的线性系统可以得到系统状态的递推最小均方差估计（Recursive Minimum Mean-Square Estimation，RMMSE ）；将状态空间模型引入最优滤波理论，用状态方程描述系统动态模型（状态转移模型），用观测方程描述系统观测模型，可处理时变系统、非平稳信号和多维信号；采用递推计算，适宜于用计算机来实现。该方法的缺点是要求知道系统的精确数学模型，并假设系统为线性、噪声信号为噪声统计特性已知的高斯噪声，计算量以被估计向量维数的三次方剧增。为了将卡尔曼滤波器应用于非线性系统，Bucy 和Sunahara 等人提出了扩展卡尔曼滤波（Extended Kalman Filtering,EKF），其基本思想是将非线性系统进行线性化，再进行卡尔曼滤波，它是一种次优滤波。

为了解决卡尔曼滤波对高维系统滤波计算量较大问题，1979到1985年间，Speyer ，Bierman ，Kerr 等人先后提出了分散滤波的思想，Carlson 也于1987年提出了联邦滤波理论（Federated Filtering）。

经典卡尔曼滤波应用的一个先决条件是己知噪声的统计特性，但由于实际系统噪声统计特性往往具有不确定性，这就会导致卡尔曼滤波性能下降。为了克服这个缺点，发展起来了一些自适应滤波方法，如极大后验(MAP)估计、虚拟噪声补偿、动态偏差去耦估计，这些方法在一定程度上提高了卡尔曼滤波对噪声的鲁棒性。另外，为了抑止由于模型不准确导致的滤波性能下降，有限记忆滤波方法、衰减记忆滤波方法等被相继提出。人工神经网络技术与扩展卡尔曼滤波相结合，

产生了一种新的自适应扩展卡尔曼滤波方法。该方法通过人工神经网络的在线训练，抑止系统未建模动态特性的影响，使得滤波器对模型不准确具有一定的鲁棒性。同时，利用滤波过程中新息序列的统计特性的自适应滤波方法也发展起来，该方法利用新息对滤波器进行在线的评估、自适应修正和改进，使滤波器具有一定的鲁棒性。

针对卡尔曼滤波要求模型及信号统计特性必须准确这一问题，鲁棒滤波方法提供了另一种新的思路。滤波方法是鲁棒滤波方法中发展较快的一种，该方法以牺牲滤波器的平均估计精度为代价，来保证滤波器对系统模型不准确和噪声统计特性不确定的滤波鲁棒性能。滤波理论则研究在保证滤波鲁棒性的同时，如何进一步提高滤波器的其他性能，特别是平均估计精度。与对非线性函数的近似相比，对噪声高斯分布的近似要简单的多。基于这种思想，Julier 和Uhlmann 发展了UKF(UnscentedKalman Filter,UKF)方法。UKF 方法直接使用系统的非线性模型，假设系统噪声高斯分布具有和EKF 方法相同的算法结构。对于线性系统, UKF 和EKF 具有同样的估计性能，但对于非线性系统,UKF 方法则可以得到更好的估计。Wan 和Merwe 将UKF 方法引入到非线性模型的参数估计和双估计中，提出了UKF 方法的方根滤波算法，该算法在保证滤波鲁棒性的同时大大减少了计算量；Julier 和Simon 将UKF 方法应用于车辆导航定位，得到了一个较EKF 方法更好的结果；Merwe 和Wan 将其应用于神经网络的训练也取得了良好的效果；Peihua ，Li 和Tianwen Zhang 将其应用于视觉跟踪；Stenger 和Mendon 将UKF 方法应用于基于模型的手跟踪。以上研究表明，当系统具有非线性特性时，UKF 方法与传统的EKF 方法相比，对系统状态的估计精度均有不同程度的提高。EKF 和UKF 都是递推滤波算法，它们或采用参数化的解析形式对系统的非线性进行近似，或对系统采用高斯假设，而在实际情况中非线性、非高斯随机系统估计问题更具普遍意义。粒子滤波（Particle Filtering）是英国学者Gordon ，Salmond 等于1993年提出的基于Bayesian 原理的非参数化序贯Monte-Carlo 模拟递推滤波算法，其核心是利用一些随机样本（粒子）来表示系统随机变量的后验概率分布，适合于强非线性、非高斯噪声系统模型的滤波。Kalman 滤波是Bayesian 估计在线性条件下的实现形式，而粒子滤波是Bayesian 估计在非线性条件下的实现形式。Bayesian 估计的主要问题是先验和后验概率密度不易获取，而粒子滤波采用样本形式而不是函数形式对先验信息和后验信息进行描述。该算法可以解决传统EKF 的非线性误差积累问题，精度逼近最优，数值稳健性也很好，只是计算量较大（约为EKF 的两万倍）。目前，在实际应用中非线性滤波算法的选取应根据具体应用场合和条件，在估计精度、实现难易程度、数值稳健性及计算量等各种指标之间综合权衡。例如，在雷达对再入飞行目标进行跟踪问题中，由于目标再入速度极快，受到复杂的空气动力影响而呈现出很强的非线性，通常用UKF

方法更适合；但是如果再入飞行器的空气动力特性已知(即弹道系数已知) ，则系统模型呈弱非线性，此时EKF 效果优于UKF ；基于插值展开近似方法计算简单、精度高、适应面广、数值稳健性好，是一种很有发展前途的非线性估计方法；Un-scented 滤波（无迹滤波，也称为无气味滤波）和粒子滤波用非线性变换代替传统的线性变换，体现了非线性滤波算法应更接近系统的非线性本质的思想，代表了非线性滤波的发展方向。

二、非线性估计理论的发展[2]

自九十年代中期以来，非线性估计理论出现了重大突破，人们抛弃了传统的非线性模型Taylor 展开近似方法，采用计算简便、精度更高的插值多项式近似方法，甚至抛弃了对非线性模型进行线性变换的基本思想，而采用非线性变换思想。

下面从非线性估计革新的两条发展思路出发，分别对插值滤波、Unscented 滤波、粒子滤波和神经网络滤波这四种最具代表性的新方法进行介绍和评述, 将非线性估计最新进展的思想传承、本质特点、地位与作用予以展现，最后指出非线性估计领域的现存问题和发展前景。

1 非线性估计的基本问题

一般的非线性系统其状态方程和观测方程可表示为

x k +1=f (x k , u k , v k ) y k =g (x k , w k ) (2.1) (2.2)

式中，x k 为状态量，y k 为观测量，u k 为系统输入，v k 为系统噪声，且

v k ⎡v k , Q k ⎤，w k 为观测噪声，w , R ⎤且w k ⎡⎣⎦⎣k k ⎦，v k 和w k 相互独立且系统状态x 无

关。根据Bayesian 估计原理同，可按条件期望表示出系统的状态估值。

但在非线性条件下，条件期望是难以处理的，非线性模型(2.1)、(2.2)只能采用各种近似方法。因此，可以说所有的非线性估计都是近似的，都只能得到次优估计。非线性估计的核心就在于近似，各种非线性估计方法的不同就在于其近似处理的思想和实现手段不同。近似的本质就是对难以计算的非线性模型施加某种数学变换，变换成线性模型，然后用Bayesian 估计原理进行估计。进一步说，非线性变换、线性变换主要有两种实现手段：一种是Taylor 式展开；一种是插值多项式展开，非线性变换有U 变换、序贯Monte-Carlo 法和神经网络等。传统非线性估计仅采用了线性变换中的Taylor 展开法，而基于插值展开及非线性变换的方法属于非线性估计发展的最新方向。

k 对非线性模EKF 是传统非线性估计的代表，其基本思想是围绕状态估值x

型进行一阶Taylor 展开，然后应用线性系统Kalman 滤波公式。它的主要缺陷有两点：（1）必须满足小扰动假设，即假设非线性方程的理论解与实际解之差为小

量。也就是说EKF 只适合弱非线性系统，对于强非线性系统，该假设不成立，此时EKF 滤波性能极不稳定，甚至发散；（2）必须计算Jacobian 矩阵及其幂，这是一件计算复杂、极易出错的工作。针对EKF 的缺陷，众多学者提出了各种分解及补偿算法，如U-D 分解、奇异值分解、L-D 分解、平方根滤波等。这些努力在一定程度上解决了EKF 数值稳健性问题，相应地提高了计算效率，但仍无法避免上述EKF 的两个缺陷。另外，标准EKF 是取Taylor 展开式的一阶近似，为提高估计精度也可取二阶近似，构成SONF （Second Order NonlinearFilter）滤波，但其实现复杂性和计算量大大增加。级数展开法中还有一种称为统计线性化方法，将非线性模型按某种不带导数的级数（例如幂级数) 展开，避免了求导计算，不要求f ，g 必须可导。从统计的观点来看，所得的表达式比Taylor 级数更为精确。但该方法需要计算多重无穷积分，因计算量太大而妨碍了其推广应用。

2 插值滤波

EKF 的一个特点是要计算Taylor 展开式中的导数，这样不仅要求函数各点都是可微的，而且计算量较大。EKF 问题的根源是采用了Taylor 展开近似，为摆脱EKF 的种种困扰，方法之一就是采用插值多项式展开代替Taylor 展开。挪威学者Schei 于 1995年提出用中心差分改善EKF ，成为插值法的起源。丹麦学者Norgaard 等于2000 年系统阐述和创立了基于插值多项式的非线性估计理论。插值滤波利用了Stirling 内插公式将非线性模型按多项式展开，不需计算函数的偏导数，可应用于任意函数，甚至非线性函数不连续且存在奇异点(无导数存在) 时也能进行状态估计，其估计精度高于EKF ，达到或超越Unscented 滤波。具体原理见附录。

3 Unscented 滤波（UF ）

Unscented 滤波译为无气味滤波，它是一种典型的非线性变换估计方法。在施加非线性变换之后，仍采用标准Kalman 滤波，所以也称为UKF(Unscented Kalman Filtering)。Unscented 滤波由牛津大学Julier ，Uhlmann 等于1995年首次提出，其后又得到美国学者Wan ，Vander Merwe 的进一步发展。其核心是通过一种非线性变换——U 变换(Unscented变换) 来进行非线性模型的状态与误差协方差的递推和更新，所以UF 的关键在于U 变换。与EKF 不同，UKF 不是对非线性模型做近似，而是对状态的概率密度函数（Probability Density Function，PDF ）做近似。

图 1 U变换原理

U 变换的原理如图 1所示，首先选择有限个近似高斯分布离散点（称为σ点），它们的均值为x ，方差为P x 。对每个σ点施以非线性变换（经过非线性系

统的状态方程和量测方程传播后），得到一簇变换后的点，将它们的均值和方差经过加权处理，可求出非线性系统状态估值的均值和协方差。UF 算法有以下特点：

（1）UF 可以准确估计均值和协方差达到Taylor 级数的四阶精度，而EKF 估计均值达到二阶精度，协方差达到四阶精度。

（2）σ点俘获到的均值和协方差不会因采取不同的平方根分解方法而改变，因此可以采用效率高、鲁棒性强的Cholesky 分解方法，在实时应用场合这将显得尤为重要。

（3）经过U 变换后就不需计算状态方程与测量方程的Jacobian 矩阵，实现相对简单。UF 表面上看似乎与粒子滤波一样是一种Monte-Carlo 方法，其实不然。首先，σ点不是随机抽取的，它有确定的含义(有给定的均值和方差) ，因此状态变量的一、二阶矩才能被这些数量有限的σ点俘获；再者，σ点的加权方式与粒子滤波中样本点的分配方式不一样，它不是通常意义上的加权，而是一种“广义”加权，其权系数不一定都为正，不一定分布在［0，1］区间。所以虽然U 变换也需要采样，但不能将其理解为通常的抽样统计。而粒子滤波中的粒子退化现象是比较难以解决的问题，其加权系数由退化现象影响，逐渐失去粒子的多样性。

由上面的三条性质可以得知，Unscented 滤波中的加权系数具有广义加权性，且粒子分布多样性好，没有退化现象的发生，因此将其它滤波方式与粒子滤波结合，也是当前粒子滤波的发展方向之一。

4 粒子滤波

EKF 和UKF 都是递推滤波算法，其基本思想是通过采用参数化的解析形式对系统的非线性进行近似，而且都是基于高斯假设。在实际情况中非线性、非高斯随机系统估计问题更具普遍意义，解决这一问题的一种有效方法是以非参数化的Monte Carlo模拟为特色的粒子滤波（Particle filtering）法。粒子滤波是英国学者Gordon ，Salmond 等于1993年提出的基于Bayesian 原理的序贯Monte Carlo

模拟方法。该方法的核心是利用一些随机样本（粒子）来表示系统随机变量的后验概率密度，能得到基于物理模型的近似最优数值解，而不是对近似模型进行最优滤波，最适合于强非线性、非高斯噪声系统模型的滤波。

Kalman 滤波是Bayes 估计在线性条件下的实现形式，而粒子滤波是Bayes 估计在非线性条件下的实现形式。Bayes 估计的主要问题是先验和后验概率密度不易获取，而粒子滤波采用样本形式而不是以函数形式对先验信息和后验信息进行描述。如何得到后验概率分布的样本是粒子滤波的关键，其基本思路是选取一个重要性概率密度来得到后验概率分布的带有相关权值的随机样本（粒子），然后在测量的基础上，调节权值的大小和粒子的位置，当粒子数非常大时，此时的概率估算将等同于后验概率密度，从而得到状态的估值。该算法可以解决传统EKF 的非线性误差积累问题，因而精度逼近最优，数值稳健性也很好，只是计算量较大。

粒子滤波器最常见的问题就是退化（degeneracy ）现象，即经过几次迭代，除一个粒子外，所有粒子都只具有微小的权值。退化程度可以用有效样本个数进行度量，退化现象意味着大量的计算工作都被用来更新那些对后验概率密度的估计几乎没有影响的粒子上。减小这一不利影响的一个方法是增加粒子数，但这通常是有限度的。所以主要还是依靠选取适当的重要性概率密度和再采样两种方法解决：

（1）重要性概率密度的选择：通常选取系统状态的转移概率作为重要性概率密度，但用转移概率分布来产生预测样本没有考虑系统状态的最新观测，由此产生的样本同真实的后验概率产生的样本偏差较大。文献[3]提出一种确定的次优化方法——高斯-厄米特粒子滤波器(GHPF)，高斯-厄米特滤波(GHF)是一种基于高斯-厄米特数值积分的Bayes 滤波方法，它不用计算Jacobian 矩阵，没有非线性映射必须为可微映射的限制。V anderMerwe [4]等提出将Unscented 滤波与粒子滤波相结合，用U 变换来获取重要性概率密度，从而构成一种新的粒子滤波器UPF （Unscented Particle Filter ）。该方法有两个特点，一是将最新观测信息加入到先验信息更新循环中，二是通过U 变换产生的重要性概率密度更接近真实后验概率密度。Higuchi 采用遗传算法产生重要性概率密度函数，仿真结果表现出良好的估计性能。

（2）再采样：其基本思想是消除小权值粒子而集中大权值粒子，方法是对后验概率密度再次采样，生成新的粒子集。再采样又会带来采样枯竭问题，即权值较大的粒子被多次选取，采样结果中包含了许多重复点，从而失去粒子的多样性。已经提出一些方法来解决该问题，如增加马尔可夫链Monte-Carlo 移动步骤、粒子正则再采样、非等权值粒子确定性算法等。

5 神经网络滤波

自20世纪90年代以来，以神经网络为代表的人工智能技术得到快速发展，促进了控制理论向智能化、鲁棒、自适应方向发展。神经网络方法具有并行计算、自学习的特点，智能性、自适应性强，很适合解决非线性问题，近十余年来在非线性估计领域应用神经网络的研究一直未曾中断，也取得了相应的成果。但这类方法的问题是经验性太强、普适性差、工程实现性不好且估计精度不甚理想，所以单纯使用神经网络进行非线性估计的理论探讨虽然很热烈，但真正大量解决工程问题还尚需时日。随着研究的深入，神经网络等智能化方法在非线性估计领域中的地位将日渐重要。除了进一步探索神经网络的应用模式之外，一个比较现实的发展思路是将神经网络与其他估计方法相结合，充分利用其他估计方法的估计能力和神经网络的学习能力，组成自适应性强、智能的、高精度的组合非线性估计器。

以上对五种主要的非线性估计方法进行了简要的介绍，迄今为止，在非线性估计领域尚没有一种最好的算法，必须根据具体应用场合和条件，在估计精度、实现难易程度、数值稳健性及计算量等各种指标之间综合权衡。例如，在雷达对再入飞行目标进行跟踪问题中，由于目标再入速度极快，受到复杂的空气动力影响而呈现出很强的非线性，所以通常UKF 方法更适合该问题。但是如果再入飞行器的气动力特性已知，即弹道系数已知，则系统模型呈弱非线性，此时EKF 效果优于UKF 。传统的Taylor 展开近似方法应用最为广泛，但工作机理存在固有缺陷，估计性能不尽如人意。基于插值展开近似方法计算简单、精度高、适应面广、数值稳健性好，是一种很有发展前途的非线性估计方法。Unscented 滤波和粒子滤波用非线性变换代替传统的线性变换，体现了一种先进的思想，这种先进思想就是“非线性估计算法应更接近系统的非线性本质”。神经网络等人工智能技术与其他估计方法的组合将更具工程可实现性。

三、粒子滤波综述[5]

粒子粒波是一种基于蒙特卡罗和递推贝叶斯估计的滤波方法。其基本思想是：首先依据系统状态向量的经验分布，在状态空间产生一组随机样本集合（粒子集）；然后根据观测量不断地调整粒子的权重和位置，通过调整后的粒子的信息，修正最初的经验条件分布。它可以应用于任何动态状态空间模型，在传统维纳滤波、卡尔曼滤波及扩展的卡尔曼滤波不能处理的非线性非高斯问题上，粒子滤波显得尤为重要。这种方法的核心思想是：用由粒子及其权重组成的离散随机测度近似相关的概率分布，并且根据算法递推更新离散随机测度。这种滤波算法采用递推公式，易于在计算机上实现，并且该算法能较好地适应观测信息出现异常突变时的情况。

20世纪50年代，Hammeraley 等人提出一种被称为SIS （Sequential Important Samplong ，序贯重要性采样）的方法。七十年代一系列改进的SIS 算法相继出现。

但是粒子退化现象影响了它在应用中的推广。1993年Gordan 等提出了采样重要性重采样算法（SIR ）才基本解决了粒子退化问题。尽管重采样在一定程度上解决了退化问题，但是导致采样后的粒子由大量重复的点组成。如果系统噪声较小，那么重采样后的粒子会集中在少数几个点上，即再采样解决退化问题的同时又带来粒子耗尽问题，这些问题如果处理得不好，直接影响滤波的性能，所以很多研究者对基本粒子滤波进行改进。Pitt 和Shephard 在文献[6]中提出的ASIR 粒子滤波就是在选取重要性密度函数上作了变化；而Musso 和Oudjance 在文献[7]中介绍的正则粒子滤波（RPF ）则对再采样过程作了优化。

粒子滤波首先在核物理领域核武器试验仿真方向获得成功应用。近年来，随着计算机能力的快速增长和计算成本的不断降低，粒子滤波已经成为研究非线性非高斯动态系统最优估计问题的一个热点和有效方法，它已经广泛应用到目标跟踪、信号处理和数字通信等多个领域。

四、粒子滤波原理及算法描述[8]

在介绍算法之前先介绍下贝叶斯估计的思想，它是非线性滤波算法的理论基础，它也是PF 算法的其中一个假设。

4.1 贝叶斯方法

英国学者T. 贝叶斯1763年在《论有关机遇问题的求解》中提出一种归纳推理的理论，后被一些统计学者发展为一种系统的统计推断方法，称为贝叶斯方法。采用这种方法作统计推断所得的全部结果，构成贝叶斯统计的内容。认为贝叶斯方法是唯一合理的统计推断方法的统计学者，组成数理统计学中的贝叶斯学派，其形成可追溯到20世纪30 年代。到50～60年代，已发展为一个有影响的学派。时至今日，其影响日益扩大。

贝叶斯统计中的两个基本概念是先验分布和后验分布。①先验分布。总体分布参数θ的一个概率分布。贝叶斯学派的根本观点是认为在关于总体分布参数的任何统计推断问题中，除了使用样本所提供的信息外，还必须规定一个先验分布，它是在进行统计推断时不可缺少的一个要素。他们认为先验分布不必有客观的依据，可以部分地或完全地基于主观信念。②后验分布。根据样本分布和未知参数的先验分布，用概率论中求条件概率分布的方法，求出在样本已知下未知参数的条件分布。因为这个分布是在抽样以后才得到的，故称为后验分布。贝叶斯推断方法的关键是任何推断都必须且只须根据后验分布，而不能再涉及样本分布。贝叶斯学派的观点认为θ为随机变量且具有先验分布，而经典学派认为θ为未知常数，显然前者的观点更具有实际意义。后验概率由贝叶斯公式描述为 p (A i B )=p (B A i )p (A i )p (B )=p (B A i )p (A i )

n (4.1)

∑p (B

i =1A i )p (A i )

贝叶斯公式反映了先验分布向后验分布的转化。在(4.1)中先验概率分布的选取是一个重大问题，Bayes 认为θ在其取值范围内为均匀分布。

4.2 贝叶斯估计

贝叶斯估计有三种形式：

估计θ 1）用后验期望（后验均值）θ

设θ的后验密度为h (θX )，θ的后验期望为：

=E θX =θ⋅h θX d θ θ()⎰()(4.2) =E θX 去估计θ是一个很自然的想法。用θ()

2）最小均方误差估计

是θ问题回顾：设θ

-θ ，使E θθ -θ估计量，E θ()2称为均方误差，最小均方误差就是求()2

=min 。

3）最大后验估计

使其满足条件：h θ设θ的后验密度为h (θX )，求θ( X )=max ，并称θ 为最

大后验估计量。计算过程为：

（1）当h (θX )容易计算时，解方程

（2）一般情况时，由于h (θX )=

h (θX ∂∂θh (θX )=0，或∂∂θln h (θX )=0 f (X )⋅h (θ)f X (X )∂

∂θ，f (X )⋅h (θ)的最大值与可解方程)的最大值等价，∂

∂θln f (X )+ln h (θ)=0来求最大后验估计

量。

与经典估计不同，Bayesian 估计假设所估计的参数θ是一个随机变量，这里估计的是它的一次实现。

θ p (θ)

x , θ p (x , θ)=p (θ)⋅p (x θ)=p (θx )⋅p (x ) (4.3)

约束一般得不到可实现的估计量，因与经典估计不同，在这里最小mse (θ)

为θ是确定量，它不参与概率空间的运算，即

=mse θ()⎰(

∂m se (θ)∂

∂θ= -θθ)2⋅p (x , θ)dx (4.4) ∂θ⎰( -θθ)2

⋅p (x , θ)dx

=g x , θ，估计器中包含待确定量，所以是不解上面的方程，一般得到：θ()

可实现的。

现在讨论Bayesian 估计问题，θ是随机量，是概率空间的一个分量。

=Bmse θ()⎰⎰( θ-θ)2

p (x , θ)dx ⋅d θ=f (x 0, θ) (4.5)

=g x 。具体计算如下：求导并令为0，得到θ对θ(0)

=Bmse θ

()⎰⎰( θ-θ)2p (x , θ)dx ⋅d θ⎡ =⎰⎰⎰⎰θ-θ⎢⎣()2⎤p (θx )d θ⋅p (θ)⋅dx ⎥⎦ (4.6)

最小，令因为p (x )≥0对所有x ，故使Bmse (θ)

最小，即：

得到：

∂∂θ⎰(⎰( θ-θ θ-θ)2p (θx )⋅d θ (4.7) )2 p θx ⋅d θp (θx )⋅d θ=2⎰θ-θ()() (4.8) =θ⋅p θx d θ=E θx θ()()⎰

p (x θ(4.9) 这是最小MSE Bayesian估计器，在计算时，经常利用Bayesian 关系式 p (θx )=)p (θ)

⎰p (x )p (θ)d θ (4.10)

下面对高斯概率函数的特性进行分析，为后面的滤波原理作铺垫。如果x 和y 是联合高斯分布，均值矢量为⎡分块协方差矩阵为 ⎣E (x ), E (y )⎤⎦，

⎛C xx C = ⎝C yx C xy ⎫⎪C yy ⎭T

⎧⎡1⎪⎢p (x , y )=exp -⎨1⎢22⎪2π⎡det C ⎤⎣()⎩⎣⎦1T ⎡x -E (x )⎤⎡x -E (x )⎤⎤⎫⎪-1⎥⋅C ⎢⎥⎢⎥⎬ y -E y y -E y ()⎦()⎦⎥⎣⎣⎦⎪⎭ (4.11) 则条件概率密度函数p (y x )也是高斯的，且有：

E (y x )=E (y )+C yx ⋅C xx (x -E (x )) -1(4.12) (4.13) C y x =C yy -C yx ⋅C xx C xy -1

若y 是待估计参数θ，x 是数据，(4.12)式就是Bayesian 估计，(4.13)是估计方差的表达式。具体推导见附录。

4.3 粒子滤波理论

前面说过，粒子滤波是一种基于蒙特卡罗和递推贝叶斯估计的滤波方法。因此，首先要对研究的问题进行建模，在模型的基咄上寻找求解算法。自然界中的多数系统为动态非线性系统，对这些系统的描述有很多方法，如随机过程。在进行系统建模过程中既要考虑模型建立的有效性，又要考虑模型求解的可行性。大多数模型都没有解析解，对这类模型的求解方法最好且最具操作性的就是递归算法，且随着现代计算机计算能力的不断提高，计算能力的限制已经不是主要制约因素。现在科学中对动态系统的描述是基于状态空间进行建模的，可由公式(4.14)与(4.15)说明。

x k =f k (x k -1, u k -1, v k -1) z k =h k (x k , n k )

(4.14) (4.15)

以上两公式即是对动态系统的状态空间表示法，其中(4.14)为状态方程，其中隐含了系统状态为一阶隐式马尔可夫过程，(4.15)为观测方程。此种表示法为同步估计法，文献[9]提出了一种用于处理一般状态空间模型状态估计的延迟取样估计算法，考虑到将来的观测值包含有当前状态的信息，在求取后验概率时进行了多次一阶马尔可夫概率传递，对数据的相关性研究的更加细致，但算法实现时状态估计的实时性差。后面讨论的状态方程都是一阶隐式马尔可夫过程（HMM ）。

基于状态空间模型的滤波算法有经典KF 、EKF 、UKF 、PF 、UPF 等等。拿经典KF 为例，它适用于线性时变高斯分布动态模型：EKF 则是在经典KF 基础上对状态传递函数和测量函数进行线性化处理。当动态模型为线性高斯分布时，经典KF 为最佳滤波算法；当为非线性高斯分布时，EKF 为最佳选择算法；当系统为非线性非高斯分布时，PF 及其改进UPF 为最佳选择算法。

由于自然界中的动态系统一般都为非线性非高斯分布，因此研究非线性非高斯分布情况下的滤波算法具有普遍意义。粒子滤波是针对此种情况下的滤波算法，它以贝叶斯估计理论为基础，以蒙特卡罗随机抽样算法为核心，通过抽样估计状态空间的后验概率密度分布，由时间更新与测量更新两个步骤来达到最优贝叶斯估计。粒子滤波以序贯重要性采样为核心算法（Sequential Importance Sampling ，SIS ），它是一种蒙特卡罗方法，它通过蒙特卡罗模拟实现递推贝叶斯滤波，其核心思想是利用一系列随机样本的加权和表示所需的后验概率密度，得到状态的估计值。

4.3.1序贯重要性采样[5]

在粒子滤波中，概率分布被近似为用粒子及其权值定义的离散随机测量值。定义一组粒子满足分布p (x )，可以表示为：

x ={x , w

}

N m =1

(4.16)

其中，x m 表示第m 个粒子，w m 表示第m 个粒子的归一化权重，即∑w t m =1，N

为粒子数。于是t 时刻的后验概率密度分布可以离散地加权近似为

p (x 0:t z 0:t )≈

∑w

m t

δ(x 0:t -x 0:t )

(4.17)

其中权重w 0:通过重要性采样来进行选择。注意，此处的概率离散化会导致粒子t

耗尽问题，后面会有详细的分析。

但实际应用中很难直接从p (x 0:t z 0:t )进行采样，此时引入一个重要性密度函数q (x 0:t z 0:t )，它是一个先验分布，是对p (x 0:t z 0:t )的先验估计。我们知道，当可

m 以直接从p (x 0:t z 0:t )进行采样时，w 0:=t

；当不能直接从p (x 0:t z 0:t )进行采样，

只能从先验分布函数（即重要性密度函数）进行采样时，有

w t ∝

p (x 0:t z 0:t )

q (x 0:t z 0:t )

(4.18)

重要性密度函数选为（满足HMM 条件）

q (x 0:t z 0:t )=q (x t x 0:t -1, z 0:t )q (x 0:t -1z 0:t )

(4.19)

可从x 0m :t q (x 0t :z 0t ):进行采样，假定已知t -1时刻的后验概率密度函数

p (x 0:t -1z 0:t -1)和先验重要性密度函数q (x 0:t -1z 0:t -1)，则t 时刻的后验概率密度函数

为

p (x 0:t z 0:t )=

p z t x 0:t z 0:t -1p (x 0:t z 0:t -1)

p (z t z 0:t -1)p z t x 0:t z 0:t -1p x t x 0:t -1z 0:t -1

p (z t z 0:t -1)

p (z t x t )p (x t x t -1)p (z t z 0:t -1)

((

)

)()

⨯p (x 0:t -1z 0:t -1)

(4.20)

p (x 0:t -1z 0:t -1)

∝p (z t x t )p (x t x t -1)p (x 0:t -1z 0:t -1)

将(4.20)代入(4.18)式，得

w t ∝

p z t x t

(

)p (x

m m

m t

x t -1p (x 0:t -1z 0:t -1)

m )

m t -1

q x t x 0:t -1, z 0:t q (x 0:t -1z 0:t )

m ()

p z t x

()p (x

m t

m t -1

)

(4.21)

q x t x 0:t -1, z 0:t

(

)

由(4.21)式可以更新下一时刻的权重。当N →∞时，(4.17)式将接近真实的后

验概率密度p (x t z 0:t )。当

q x t x 0:t -1, z 0:t =q x t x t -1, z t

(

)(

)

t -1

(4.22)

和z t 有关。

q x t x 0:t -1, z 0:t =p x t x t -1, z t

()(

)时，重要性密度函数只与x

可以看出，最佳重要性密度函数取为p (x t m x 0:, z 0:t )，此时 t -1

q x t x t -1, z t

()

opt

=p x t

t -1

, z t

))(

m m

p z t x t x t -1p x t x t -1

()

(4.23)

p z t x t -1

()

因分母p (z t x t m =-1)

⎰(

p z t x t

)p (x

x t -1dx t

)

出现积分项难于实现。一般地，

q (x t z 0:t )越接近p (x t z 0:t )

则近似得越好。有时也可取先验重要性密度函数

q x t x 0:t -1, z 0:t =p x t x t -1

(

)(

)，但由于它没有考虑新观测量的影响，误差较大。

在SIS 算法中，涉及MC 抽样，其具体实现见附录。 4.3.2出现的问题 1）退化现象

在SIS 粒子滤波中，经过若干次迭代后，除了一个粒子外其余粒子只有微小的权重，可忽略不计。这意味着大量的计算工作都被用来更新那些对求解几乎不起作用的粒子上。对退化现象的一个有效衡量方法是有效采样尺度N eff ，它定义为：

N eff =

N 1+Var (w

)

(4.24)

其中，Var (w t m )为w t m 的方差。但这不能精确估计，因此其近似值为

N eff =

∑(w )

m t

m =1

(4.25)

此外的w t m 为归一化权重。当N eff ≤N 时，退化现象变得恶化。减小这一不利影响的粗略方法为采用足够多的粒子数目N ，但这必然影响算法的实时性，增加了计算量。

2）重要性密度函数的选择

, z 0:t )的选择直接影响了权重的方差大小，重要性密度函数q (x t x 0:其值越接t -1

, z 0:t )，权重方差越小。则有效粒子数N eff 越接近最佳重要性密度函数p (x t m x 0:t -1

近于N ，退化现象影响越小。当状态量x t 为有限集合时，最佳重要性密度函数中的积分可以容易求出。因此适当选取重要性密度函数是减小退化现象的一个方法。

3）再采样

减小退化现象的第二个方法是再采样。再采样的目的在于减少权重小的粒子的数目，集中处理具有大权重的粒子。基本方法是通过对后验概率密度函数的离散近似表达式

p (x t z 0:t )≈

∑w

N N i =1

m t

δ(x t -x t

)

(4.26)

再采样N 次，生成一个新的粒子集合{x 被重新设置为

}

。由于再采样是独立同分布的，权值

w t =

(4.27)

再采样在一定程度上可以减小退化现象，但带来的负面作用是粒子耗尽问题，即具有较大权值的粒子被多次选取，采样结果中包含了许多重复点，从而损失了粒子的多样性。这称为粒子贫化现象（particle impoverishment）。粒子贫化现象在过程噪声小的情况下表现得尤为严重，所有的粒子经过少量的再采样步骤即塌陷为单一粒子。这不仅损失了粒子的多样性，而且一些基于粒子多样性特性的统计平滑量也会变得无效。针对这一问题，国外许多学者研究了多种方案，如增加马尔可夫链蒙特卡罗移动步骤、粒子正则再采样等。文献[8]中列出了一般粒子滤波的伪代码，现摘录如下： ⎡x i , w i N ⎤=SIS ⎡x i , w i N , z ⎤{}⎦{}⎢⎢⎣k k i =1⎥⎣k -1k -1i =1k ⎥⎦

● FOR i=1:N

--|Draw x k i ~q (x k x k i -1, z k ) --Assign the particle a weight, w k i , According to w t m ∝w t m

-1

p z t x t

q x

(

)p (x

m 0:t -1

m t

x t -1

)

(

m t

, z 0:t

)

● END FOR

⎡x j , w j , i j N ⎤=RESAM PLE ⎡x i , w i N ⎤

}j =1⎦{}⎥⎢{k k ⎥⎢⎣⎣k k i =1⎦

● Initialize the CDF: c 1=0 ● FOR i=2:N

--Construct CDF: c i =c i -1+w k i ● END FOR

● Start at the bottom of the CDF: i=1

1⎫

● Draw a starting point: u 1~U ⎛0, ⎪

⎝

N ⎭

● FOR j=1:N

--Move along the CDF: u j =u 1+--WHILE u j >c i --i =i +1

--END WHILE

--Assign sample: x k j =x k i --Assign weight: w k j =

(j -1)

--Assign parent:i j =i ● END FOR

⎡x i , w i N ⎤=PF ⎡x i , w i

, z k ⎤{}{}k k k -1k -1⎢⎢⎥i =1⎥i =1⎣⎦⎣⎦

● FOR i=1:N

--|Draw x k i ~q (x k x k i -1, z k ) --Assign the particle a weight, w k i , According to w k i ∝w k i -1

● END FOR

● Calculate total weight:t =SU M ● FOR i=1:N

--Normalize: w k i =t -1w k i ● END FOR

● Calculate N eff using N eff =

p z k x k

(

)p (x

x k -1

q x k x 0:k -1, z 0:k

(

)

(

{w k }

N i =1

)

∑(w )

i k

i =1

--Resample using algorithm 2:

N N

--⎡{x k i , w k i , -}⎤=RESAM PLE ⎡{x k i , w k i }⎤ ⎢⎥⎢⎥

⎣

i =1

⎦⎣

i =1

⎦

END IF

上述伪代码中的算法二中用到了直接抽样方法，具体推导见附录。介绍了粒子滤波的原理和一般算法后，需要对其滤波效果和执行效率有定量的认识。表 1[10]列出了各种滤波算法的适用场合。

表 1 各种滤波算法的适用场合

PF EKF/UKF/PF

PF PF PF

PF EKF/UKF/PF

PF PF PF

线性非高斯非线性高斯非线性非高斯

可以看出，当状态方程与观测方程都为高斯噪声影响时，KF 算法占优；当状态方程或观测方程为非线性高斯影响时，EKF/UKF占优；而PF 各种场合都能应用。

下面再通过例子了解粒子滤波算法的效率，如表 2[11]所示。

表 2 静基座对准精度比较

由表 2可以看出，粒子滤波算法的开销比EKF 大得多，因此在对实时性要求比较高的场合，需要对粒子滤波进行改进。

为了解决粒子滤波较高的计算开销的问题，现在有很多的改进方案，如粒子滤波与其它滤波算法结合的复合算法，也有从硬件上进行开发的方案，文献[12]中就从挖掘相邻时刻粒子滤波操作重叠性的角度，引入一种预采样和预计算权值策略，提高了粒子滤波硬件实现的实时性，结果表明量测频率提高了25%。

附录

1、插值滤波

1.1）一维Stirling 插值展开及其逼近精度将函数f (x )在x =x 处二阶Taylor 展开为

'D x f (x )≈f x +f D

D D

()f ''(x )

()(x -x )+

(x -x )))(

用中心差分（即左右和的一半）代替导数，即

'D x =f D

()()

f x +h -f x -h

((

))

()

''D x =f D

f x +h +f x -h -2f x

(

利用Stirling 插值公式，得

'D x f x +f D

()()(x -x )+

x -x +

''D x f D

()(

(x -x ))

=f x +f 'x

()()()

f ''x 2

()

x -x +

(5)⎛f (3)x ⎫f x

h +h +⋅⋅⋅⎪x -x + ⎪3! 5! ⎝⎭

()()

()

(6)⎛f (4)x ⎫f x

h +h +⋅⋅⋅⎪x -x ⎪4! 6! ⎝⎭

()()

()

采用Stirling 中心差分形式展开后，前3项与二阶Taylor 展开式相同，后两项对应高阶项，其精度由步长h 来控制。显然，Stirling 插值展开式精度高于Taylor 级数展开式。

1.2）多维Stirling 插值展开

设x ∈R n ，y =f (x )为函数向量，则在x =x 处用Stirling 插值公式展开，得

y =f

(

x +∆x ≈f

)()

∆x f +1D ∆

x +D x f

式中，差分算子

⎫1⎛n

D ∆x f = ∑∆x p μp δp ⎪f x

h ⎝p =1

⎭

()

∆x D

⎛1 n 22

f =2∑∆x p δp +

h p =1

⎝⎫⎪

∑∑∆x p ∆x q (μp δp )(μp δp )⎪f (x ) p =1q =1⎪q ≠p ⎭

其中，∆p 为第p 个偏差算子，μp 为第p 个平均算子。取上式中的线性项（前两项）作为非线性函数的近似，代入Bayes 状态估计公式可得到DD1滤波器，这里DD 代表差分算法（Divided Difference），1表示只取一阶项，即线性近似，DD1滤波精度与Unscented 滤波类似。取二阶近似可构成DD2滤波器，估计性

能进一步改善，精度高于Unscented 滤波，而且实现复杂性和计算量都很小，是Norgaard 等人重点推荐的滤波器。

2、联合高斯分布的条件期望和协方差

列出相关公式为：

E (y x )=E (y )+C yx ⋅C xx (x -E (x ))

-1

C y x =C yy -C yx ⋅C xx C xy

证明：已知x 和y 服从联合高斯分布，其联合概率密度函数为：

⎧⎡

11⎪

⎢p (x , y )=exp -⎨1

⎢22⎪2π⎡det C ⎤⎣()⎩⎣⎦

⎡x -E (x )⎤⎡x -E (x )⎤⎤⎫⎪-1

⎥⋅C ⎢⎥⎢⎥⎬

y -E y y -E y ()()⎣⎦⎣⎦⎥⎦⎪⎭

条件期望为：

E (y x )=

⎰

yf y x dy =

⎰

(x , y )

f X

⎰(y -E (y )+E (y ))

⎰(y -E (y ))

(x , y )

f X

=E (y )+

(x , y )

f X

且E (x )=μ1，E (y )=μ2，D (x )=σ12，D (y )=σ22，则

C ov (y , x )=

⎰⎰(y -μ)(x -μ)f (

x , y )dxdy

-∞

∞∞

⎰⎰(y -μ)(

x -μ)

-∞

∞∞

⎤

⎥dydx ⎥⎦

⎡⎛⎫(x -μ1)y -μ2x -μ11exp -ρ⎪-2σ2σ1⎭2σ1令t =

⎛y -μ2x -μ1⎫x -μ1

-ρ⎪，u =

σ1⎭σ1

σ2

，则有

C ov (

y , x )=

12π

⎰⎰

-∞

∞∞-∞(

σ1σ+ρσ1σ2u u e

-u /2

)

-u +t

(

)/2

dtdu

ρσ1σ22π

(⎰

∞-∞

)(

⎰

∞-∞

-t /2

dt +

)

2π

⎰

∞-∞

-u /2

)(

⎰

∞-∞

-t /2

)

=ρσ1σ2

仿照上式的求法，有

⎰(y -E (y ))

f (x , y )f X

⎛⎫x -E x f (x , y )⎡⎤()⎣⎦dy = ⎰(y -E (y ))dy ⎪⎡T ⎣x -E (x )⎤⎦

⎪x -E x x -E x f ⎡⎤⎡⎤()()X ⎣⎦⎣⎦⎝⎭

⎣x -E (x )⎤⎦⎰(y -E (y ))⎡

f (x , y )dy

⎡⎣x -E (x )⎤⎦⎡⎣x -E (x )⎤⎦

f X

⎡⎣x -E (x )⎤⎦

分子项化为：

12π

⎰

∞-∞(

σ+ρσ2u

)

-u +t

(

)

dt ==

u e u e

-u /2

2π

-u /2

⎰

∞-∞

ρσ2t e

2-t /2

ρσ2

分母项化为：

-u /2

1 C ov (x , y )D (x )

=R YX R XX

则总的结果为

ρσ2σ1

，且又有

ρσ2σ1

ρσ1σ2σ

，当x 与y 为向量时

-1

即为E (y x )=E (y )+C yx ⋅C xx (x -E (x ))。

协方差公式可类似证明。

3、从已知分布的随机抽样——直接抽样法

为便于处理，事实上，随机数是一种从单位矩形分布中的抽样。通过这种抽样，可以将概率空间与随机变量空间一一映射，两个空间的测度相同，因此，只要随机变量的抽样满足均匀性、独立性等，当抽样数目足够多时，随机变量的抽样频率趋近于概率。下面介绍比较简单的直接抽样法。

若ξ1, ξ2, ⋅⋅⋅, ξN 为随机数，则

x n =inf t

F (t )≥t n

即x F =inf t ，事实上，

F (t )≥ξ

⎛⎫

p (x F

⎝F (t )≥ξ⎭

左连续

p (ξ

=F (x )

这种方法可以直接利用来产生任意的离散型分布：

F (x )=

∑

x i

p i

当随机数ξ满足

i -1

∑

j =1

p j

∑

j =1

p j

时，取

x F =x i

此外，对存在反函数F -1(y )的连续分布函数F (x )，有

x F =inf t =F

F (t )≥ξ

-1

(ξ)

参考文献

[1] 史延春. 非线性滤波理论的发展[J].科技咨询导报.2007，23：23

[2] 柴霖，袁建平等. 非线性估计理论的最新进展[J].宇航学报.2005，26(3)：

380-384 [3] 袁泽剑，郑南宁，贾新春. 高斯-厄米特粒子滤波器[J].电子学报.2003，31(7)：

970-973 [4] Van der Merwe R，Doucet A，Freitas N，Wan E A. The Unscented Particle Filter

[R]. Technical Report CUEDPF-INFENGPTR 380，Cambridge University, 2000. [5] 范典华. 粒子滤波. 中山大学研究生学刊（自然科学、医学版）.2005，25(2)：

22-32 [6] M.Pitt ，N.Shephard. Filtering Via Simulation. Auxilary Particle Filters. J.Amer

Statist, Assde, pp. 590-599, 1999

[7] C.Musso, N.Oudjcme, F.LeGland. Improring regularized particle filters, m

Sequential Monto Carlo Method in Practice, A.Davecet, J.F.G. de Freitas, N.J.Gordan, Eds New York: Springer-Verlag, 2001

[8] M.Sanjeev Arulampalam, Simon Maskell, Maskell, Neil Gordon, Tim Clapp. A

Tutorial on Particle Filter for Online Nonliear/Non-Gaussian Bayesian Tracking. IEEE TRANSACTIONS ON SIGNAL PROCESSING. PP. 174-188, VOL 50, NO 2, 2002 [9] 余翊华. 粒子粒波是一种基于蒙特卡罗和递推贝叶斯估计的滤波方法. 应用简

报.2007：219-223

[10] 胡士强，敬忠良. 粒子滤波算法综述. 控制与决策.2005，20（4）：361-371 [11] 熊凯，张洪钺. 粒子滤波在惯导系统非线性对准中的应用. 中国惯性技术学

报.2003，11（6）：20-26

[12] 王来雄，陈养平，黄士坦. 粒子滤波高效硬件实现的预采样策略. 武汉大学学

报（工学版）2007，40（2）：125-128

非线性滤波概念和原理介绍

一、背景介绍[1]

二、非线性估计理论的发展[2]

1 非线性估计的基本问题

一般的非线性系统其状态方程和观测方程可表示为

x k +1=f (x k , u k , v k ) y k =g (x k , w k ) (2.1) (2.2)

式中，x k 为状态量，y k 为观测量，u k 为系统输入，v k 为系统噪声，且

v k ⎡v k , Q k ⎤，w k 为观测噪声，w , R ⎤且w k ⎡⎣⎦⎣k k ⎦，v k 和w k 相互独立且系统状态x 无

关。根据Bayesian 估计原理同，可按条件期望表示出系统的状态估值。

k 对非线性模EKF 是传统非线性估计的代表，其基本思想是围绕状态估值x

2 插值滤波

3 Unscented 滤波（UF ）

图 1 U变换原理

U 变换的原理如图 1所示，首先选择有限个近似高斯分布离散点（称为σ点），它们的均值为x ，方差为P x 。对每个σ点施以非线性变换（经过非线性系

（1）UF 可以准确估计均值和协方差达到Taylor 级数的四阶精度，而EKF 估计均值达到二阶精度，协方差达到四阶精度。

4 粒子滤波

5 神经网络滤波

三、粒子滤波综述[5]

20世纪50年代，Hammeraley 等人提出一种被称为SIS （Sequential Important Samplong ，序贯重要性采样）的方法。七十年代一系列改进的SIS 算法相继出现。

四、粒子滤波原理及算法描述[8]

在介绍算法之前先介绍下贝叶斯估计的思想，它是非线性滤波算法的理论基础，它也是PF 算法的其中一个假设。

4.1 贝叶斯方法

n (4.1)

∑p (B

i =1A i )p (A i )

贝叶斯公式反映了先验分布向后验分布的转化。在(4.1)中先验概率分布的选取是一个重大问题，Bayes 认为θ在其取值范围内为均匀分布。

4.2 贝叶斯估计

贝叶斯估计有三种形式：

估计θ 1）用后验期望（后验均值）θ

设θ的后验密度为h (θX )，θ的后验期望为：

=E θX =θ⋅h θX d θ θ()⎰()(4.2) =E θX 去估计θ是一个很自然的想法。用θ()

2）最小均方误差估计

是θ问题回顾：设θ

-θ ，使E θθ -θ估计量，E θ()2称为均方误差，最小均方误差就是求()2

=min 。

3）最大后验估计

使其满足条件：h θ设θ的后验密度为h (θX )，求θ( X )=max ，并称θ 为最

大后验估计量。计算过程为：

（1）当h (θX )容易计算时，解方程

（2）一般情况时，由于h (θX )=

h (θX ∂∂θh (θX )=0，或∂∂θln h (θX )=0 f (X )⋅h (θ)f X (X )∂

∂θ，f (X )⋅h (θ)的最大值与可解方程)的最大值等价，∂

∂θln f (X )+ln h (θ)=0来求最大后验估计

量。

与经典估计不同，Bayesian 估计假设所估计的参数θ是一个随机变量，这里估计的是它的一次实现。

θ p (θ)

x , θ p (x , θ)=p (θ)⋅p (x θ)=p (θx )⋅p (x ) (4.3)

约束一般得不到可实现的估计量，因与经典估计不同，在这里最小mse (θ)

为θ是确定量，它不参与概率空间的运算，即

=mse θ()⎰(

∂m se (θ)∂

∂θ= -θθ)2⋅p (x , θ)dx (4.4) ∂θ⎰( -θθ)2

⋅p (x , θ)dx

=g x , θ，估计器中包含待确定量，所以是不解上面的方程，一般得到：θ()

可实现的。

现在讨论Bayesian 估计问题，θ是随机量，是概率空间的一个分量。

=Bmse θ()⎰⎰( θ-θ)2

p (x , θ)dx ⋅d θ=f (x 0, θ) (4.5)

=g x 。具体计算如下：求导并令为0，得到θ对θ(0)

=Bmse θ

()⎰⎰( θ-θ)2p (x , θ)dx ⋅d θ⎡ =⎰⎰⎰⎰θ-θ⎢⎣()2⎤p (θx )d θ⋅p (θ)⋅dx ⎥⎦ (4.6)

最小，令因为p (x )≥0对所有x ，故使Bmse (θ)

最小，即：

得到：

∂∂θ⎰(⎰( θ-θ θ-θ)2p (θx )⋅d θ (4.7) )2 p θx ⋅d θp (θx )⋅d θ=2⎰θ-θ()() (4.8) =θ⋅p θx d θ=E θx θ()()⎰

p (x θ(4.9) 这是最小MSE Bayesian估计器，在计算时，经常利用Bayesian 关系式 p (θx )=)p (θ)

⎰p (x )p (θ)d θ (4.10)

下面对高斯概率函数的特性进行分析，为后面的滤波原理作铺垫。如果x 和y 是联合高斯分布，均值矢量为⎡分块协方差矩阵为 ⎣E (x ), E (y )⎤⎦，

⎛C xx C = ⎝C yx C xy ⎫⎪C yy ⎭T

E (y x )=E (y )+C yx ⋅C xx (x -E (x )) -1(4.12) (4.13) C y x =C yy -C yx ⋅C xx C xy -1

若y 是待估计参数θ，x 是数据，(4.12)式就是Bayesian 估计，(4.13)是估计方差的表达式。具体推导见附录。

4.3 粒子滤波理论

x k =f k (x k -1, u k -1, v k -1) z k =h k (x k , n k )

(4.14) (4.15)

4.3.1序贯重要性采样[5]

在粒子滤波中，概率分布被近似为用粒子及其权值定义的离散随机测量值。定义一组粒子满足分布p (x )，可以表示为：

x ={x , w

}

N m =1

(4.16)

其中，x m 表示第m 个粒子，w m 表示第m 个粒子的归一化权重，即∑w t m =1，N

为粒子数。于是t 时刻的后验概率密度分布可以离散地加权近似为

p (x 0:t z 0:t )≈

∑w

m t

δ(x 0:t -x 0:t )

(4.17)

其中权重w 0:通过重要性采样来进行选择。注意，此处的概率离散化会导致粒子t

耗尽问题，后面会有详细的分析。

m 以直接从p (x 0:t z 0:t )进行采样时，w 0:=t

；当不能直接从p (x 0:t z 0:t )进行采样，

只能从先验分布函数（即重要性密度函数）进行采样时，有

w t ∝

p (x 0:t z 0:t )

q (x 0:t z 0:t )

(4.18)

重要性密度函数选为（满足HMM 条件）

q (x 0:t z 0:t )=q (x t x 0:t -1, z 0:t )q (x 0:t -1z 0:t )

(4.19)

可从x 0m :t q (x 0t :z 0t ):进行采样，假定已知t -1时刻的后验概率密度函数

p (x 0:t -1z 0:t -1)和先验重要性密度函数q (x 0:t -1z 0:t -1)，则t 时刻的后验概率密度函数

为

p (x 0:t z 0:t )=

p z t x 0:t z 0:t -1p (x 0:t z 0:t -1)

p (z t z 0:t -1)p z t x 0:t z 0:t -1p x t x 0:t -1z 0:t -1

p (z t z 0:t -1)

p (z t x t )p (x t x t -1)p (z t z 0:t -1)

((

)

)()

⨯p (x 0:t -1z 0:t -1)

(4.20)

p (x 0:t -1z 0:t -1)

∝p (z t x t )p (x t x t -1)p (x 0:t -1z 0:t -1)

将(4.20)代入(4.18)式，得

w t ∝

p z t x t

(

)p (x

m m

m t

x t -1p (x 0:t -1z 0:t -1)

m )

m t -1

q x t x 0:t -1, z 0:t q (x 0:t -1z 0:t )

m ()

p z t x

()p (x

m t

m t -1

)

(4.21)

q x t x 0:t -1, z 0:t

(

)

由(4.21)式可以更新下一时刻的权重。当N →∞时，(4.17)式将接近真实的后

验概率密度p (x t z 0:t )。当

q x t x 0:t -1, z 0:t =q x t x t -1, z t

(

)(

)

t -1

(4.22)

和z t 有关。

q x t x 0:t -1, z 0:t =p x t x t -1, z t

()(

)时，重要性密度函数只与x

可以看出，最佳重要性密度函数取为p (x t m x 0:, z 0:t )，此时 t -1

q x t x t -1, z t

()

opt

=p x t

t -1

, z t

))(

m m

p z t x t x t -1p x t x t -1

()

(4.23)

p z t x t -1

()

因分母p (z t x t m =-1)

⎰(

p z t x t

)p (x

x t -1dx t

)

出现积分项难于实现。一般地，

q (x t z 0:t )越接近p (x t z 0:t )

则近似得越好。有时也可取先验重要性密度函数

q x t x 0:t -1, z 0:t =p x t x t -1

(

)(

)，但由于它没有考虑新观测量的影响，误差较大。

在SIS 算法中，涉及MC 抽样，其具体实现见附录。 4.3.2出现的问题 1）退化现象

N eff =

N 1+Var (w

)

(4.24)

其中，Var (w t m )为w t m 的方差。但这不能精确估计，因此其近似值为

N eff =

∑(w )

m t

m =1

(4.25)

2）重要性密度函数的选择

, z 0:t )的选择直接影响了权重的方差大小，重要性密度函数q (x t x 0:其值越接t -1

, z 0:t )，权重方差越小。则有效粒子数N eff 越接近最佳重要性密度函数p (x t m x 0:t -1

3）再采样

p (x t z 0:t )≈

∑w

N N i =1

m t

δ(x t -x t

)

(4.26)

再采样N 次，生成一个新的粒子集合{x 被重新设置为

}

。由于再采样是独立同分布的，权值

w t =

(4.27)

● FOR i=1:N

--|Draw x k i ~q (x k x k i -1, z k ) --Assign the particle a weight, w k i , According to w t m ∝w t m

-1

p z t x t

q x

(

)p (x

m 0:t -1

m t

x t -1

)

(

m t

, z 0:t

)

● END FOR

⎡x j , w j , i j N ⎤=RESAM PLE ⎡x i , w i N ⎤

}j =1⎦{}⎥⎢{k k ⎥⎢⎣⎣k k i =1⎦

● Initialize the CDF: c 1=0 ● FOR i=2:N

--Construct CDF: c i =c i -1+w k i ● END FOR

● Start at the bottom of the CDF: i=1

1⎫

● Draw a starting point: u 1~U ⎛0, ⎪

⎝

N ⎭

● FOR j=1:N

--Move along the CDF: u j =u 1+--WHILE u j >c i --i =i +1

--END WHILE

--Assign sample: x k j =x k i --Assign weight: w k j =

(j -1)

--Assign parent:i j =i ● END FOR

⎡x i , w i N ⎤=PF ⎡x i , w i

, z k ⎤{}{}k k k -1k -1⎢⎢⎥i =1⎥i =1⎣⎦⎣⎦

● FOR i=1:N

--|Draw x k i ~q (x k x k i -1, z k ) --Assign the particle a weight, w k i , According to w k i ∝w k i -1

● END FOR

● Calculate total weight:t =SU M ● FOR i=1:N

--Normalize: w k i =t -1w k i ● END FOR

● Calculate N eff using N eff =

p z k x k

(

)p (x

x k -1

q x k x 0:k -1, z 0:k

(

)

(

{w k }

N i =1

)

∑(w )

i k

i =1

--Resample using algorithm 2:

N N

--⎡{x k i , w k i , -}⎤=RESAM PLE ⎡{x k i , w k i }⎤ ⎢⎥⎢⎥

⎣

i =1

⎦⎣

i =1

⎦

END IF

表 1 各种滤波算法的适用场合

PF EKF/UKF/PF

PF PF PF

PF EKF/UKF/PF

PF PF PF

线性非高斯非线性高斯非线性非高斯

下面再通过例子了解粒子滤波算法的效率，如表 2[11]所示。

表 2 静基座对准精度比较

由表 2可以看出，粒子滤波算法的开销比EKF 大得多，因此在对实时性要求比较高的场合，需要对粒子滤波进行改进。

附录

1、插值滤波

1.1）一维Stirling 插值展开及其逼近精度将函数f (x )在x =x 处二阶Taylor 展开为

'D x f (x )≈f x +f D

D D

()f ''(x )

()(x -x )+

(x -x )))(

用中心差分（即左右和的一半）代替导数，即

'D x =f D

()()

f x +h -f x -h

((

))

()

''D x =f D

f x +h +f x -h -2f x

(

利用Stirling 插值公式，得

'D x f x +f D

()()(x -x )+

x -x +

''D x f D

()(

(x -x ))

=f x +f 'x

()()()

f ''x 2

()

x -x +

(5)⎛f (3)x ⎫f x

h +h +⋅⋅⋅⎪x -x + ⎪3! 5! ⎝⎭

()()

()

(6)⎛f (4)x ⎫f x

h +h +⋅⋅⋅⎪x -x ⎪4! 6! ⎝⎭

()()

()

1.2）多维Stirling 插值展开

设x ∈R n ，y =f (x )为函数向量，则在x =x 处用Stirling 插值公式展开，得

y =f

(

x +∆x ≈f

)()

∆x f +1D ∆

x +D x f

式中，差分算子

⎫1⎛n

D ∆x f = ∑∆x p μp δp ⎪f x

h ⎝p =1

⎭

()

∆x D

⎛1 n 22

f =2∑∆x p δp +

h p =1

⎝⎫⎪

∑∑∆x p ∆x q (μp δp )(μp δp )⎪f (x ) p =1q =1⎪q ≠p ⎭

能进一步改善，精度高于Unscented 滤波，而且实现复杂性和计算量都很小，是Norgaard 等人重点推荐的滤波器。

2、联合高斯分布的条件期望和协方差

列出相关公式为：

E (y x )=E (y )+C yx ⋅C xx (x -E (x ))

-1

C y x =C yy -C yx ⋅C xx C xy

证明：已知x 和y 服从联合高斯分布，其联合概率密度函数为：

⎧⎡

11⎪

⎢p (x , y )=exp -⎨1

⎢22⎪2π⎡det C ⎤⎣()⎩⎣⎦

⎡x -E (x )⎤⎡x -E (x )⎤⎤⎫⎪-1

⎥⋅C ⎢⎥⎢⎥⎬

y -E y y -E y ()()⎣⎦⎣⎦⎥⎦⎪⎭

条件期望为：

E (y x )=

⎰

yf y x dy =

⎰

(x , y )

f X

⎰(y -E (y )+E (y ))

⎰(y -E (y ))

(x , y )

f X

=E (y )+

(x , y )

f X

且E (x )=μ1，E (y )=μ2，D (x )=σ12，D (y )=σ22，则

C ov (y , x )=

⎰⎰(y -μ)(x -μ)f (

x , y )dxdy

-∞

∞∞

⎰⎰(y -μ)(

x -μ)

-∞

∞∞

⎤

⎥dydx ⎥⎦

⎡⎛⎫(x -μ1)y -μ2x -μ11exp -ρ⎪-2σ2σ1⎭2σ1令t =

⎛y -μ2x -μ1⎫x -μ1

-ρ⎪，u =

σ1⎭σ1

σ2

，则有

C ov (

y , x )=

12π

⎰⎰

-∞

∞∞-∞(

σ1σ+ρσ1σ2u u e

-u /2

)

-u +t

(

)/2

dtdu

ρσ1σ22π

(⎰

∞-∞

)(

⎰

∞-∞

-t /2

dt +

)

2π

⎰

∞-∞

-u /2

)(

⎰

∞-∞

-t /2

)

=ρσ1σ2

仿照上式的求法，有

⎰(y -E (y ))

f (x , y )f X

⎛⎫x -E x f (x , y )⎡⎤()⎣⎦dy = ⎰(y -E (y ))dy ⎪⎡T ⎣x -E (x )⎤⎦

⎪x -E x x -E x f ⎡⎤⎡⎤()()X ⎣⎦⎣⎦⎝⎭

⎣x -E (x )⎤⎦⎰(y -E (y ))⎡

f (x , y )dy

⎡⎣x -E (x )⎤⎦⎡⎣x -E (x )⎤⎦

f X

⎡⎣x -E (x )⎤⎦

分子项化为：

12π

⎰

∞-∞(

σ+ρσ2u

)

-u +t

(

)

dt ==

u e u e

-u /2

2π

-u /2

⎰

∞-∞

ρσ2t e

2-t /2

ρσ2

分母项化为：

-u /2

1 C ov (x , y )D (x )

=R YX R XX

则总的结果为

ρσ2σ1

，且又有

ρσ2σ1

ρσ1σ2σ

，当x 与y 为向量时

-1

即为E (y x )=E (y )+C yx ⋅C xx (x -E (x ))。

协方差公式可类似证明。

3、从已知分布的随机抽样——直接抽样法

若ξ1, ξ2, ⋅⋅⋅, ξN 为随机数，则

x n =inf t

F (t )≥t n

即x F =inf t ，事实上，

F (t )≥ξ

⎛⎫

p (x F

⎝F (t )≥ξ⎭

左连续

p (ξ

=F (x )

这种方法可以直接利用来产生任意的离散型分布：

F (x )=

∑

x i

p i

当随机数ξ满足

i -1

∑

j =1

p j

∑

j =1

p j

时，取

x F =x i

此外，对存在反函数F -1(y )的连续分布函数F (x )，有

x F =inf t =F

F (t )≥ξ

-1

(ξ)

参考文献

[1] 史延春. 非线性滤波理论的发展[J].科技咨询导报.2007，23：23

[2] 柴霖，袁建平等. 非线性估计理论的最新进展[J].宇航学报.2005，26(3)：

380-384 [3] 袁泽剑，郑南宁，贾新春. 高斯-厄米特粒子滤波器[J].电子学报.2003，31(7)：

970-973 [4] Van der Merwe R，Doucet A，Freitas N，Wan E A. The Unscented Particle Filter

[R]. Technical Report CUEDPF-INFENGPTR 380，Cambridge University, 2000. [5] 范典华. 粒子滤波. 中山大学研究生学刊（自然科学、医学版）.2005，25(2)：

22-32 [6] M.Pitt ，N.Shephard. Filtering Via Simulation. Auxilary Particle Filters. J.Amer

Statist, Assde, pp. 590-599, 1999

[7] C.Musso, N.Oudjcme, F.LeGland. Improring regularized particle filters, m

Sequential Monto Carlo Method in Practice, A.Davecet, J.F.G. de Freitas, N.J.Gordan, Eds New York: Springer-Verlag, 2001

[8] M.Sanjeev Arulampalam, Simon Maskell, Maskell, Neil Gordon, Tim Clapp. A

报.2007：219-223

[10] 胡士强，敬忠良. 粒子滤波算法综述. 控制与决策.2005，20（4）：361-371 [11] 熊凯，张洪钺. 粒子滤波在惯导系统非线性对准中的应用. 中国惯性技术学

报.2003，11（6）：20-26

[12] 王来雄，陈养平，黄士坦. 粒子滤波高效硬件实现的预采样策略. 武汉大学学

报（工学版）2007，40（2）：125-128

非线性滤波概念和原理介绍(legend08fda整理)

相关文章