《随机系统的滤波与控制》研究生课程教学课件（讲稿）非线性系统滤波算法——Introduction to Particle Filtering

Introduction to Particle FilteringDr.Yuan-Li CaiSpring·2022

pink Introduction to Particle Filtering Dr. Yuan-Li Cai Spring • 2022

0.Outline1引言/225贝叶斯递推滤波78/完备采样（perfectsampling）/11重要性采样与重采样/18粒子滤波算法/436改进粒子滤波

0. Outline 1 引言 / 2 2 贝叶斯递推滤波 / 5 3 完备采样（perfect sampling） / 8 4 重要性采样与重采样 / 11 5 粒子滤波算法 / 18 6 改进粒子滤波 / 43

Introductionto ParticleFiltering1引信1.引言前面介绍的扩展卡尔曼滤波方法（EKF）是非线性系统状态估计中最常用的方法，其本质是基于线性化来传递系统的均值和协方差。对于非线性十分严重的系统，实际应用EKF变得非常困难，有时给出的状态估计可能十分不可靠。而UKF可以减小线性化带来的误差，因此一般情况下可以获得比EKF要高的估计精度。对比EKF和UKF，我们不难发现EKF估计非线性系统均值的精度是1阶的，而UKF可以到达2阶以上。然而，当系统的状态方程或量测方程的非线性非常严重时，两者都避免不了滤波发散问题，只不过UKF推迟或延缓了滤波的发散。2/52Dr. Yuan-Li CaiXi'an Jiaotong University

1 引言 Introduction to Particle Filtering 1. 引言 前面介绍的扩展卡尔曼滤波方法（EKF）是非线性系统状态估计中最常 用的方法，其本质是基于线性化来传递系统的均值和协方差。对于非线性 十分严重的系统，实际应用 EKF 变得非常困难，有时给出的状态估计可能 十分不可靠。而 UKF 可以减小线性化带来的误差，因此一般情况下可以获 得比 EKF 要高的估计精度。对比 EKF 和 UKF，我们不难发现 EKF 估计非 线性系统均值的精度是 1 阶的，而 UKF 可以到达 2 阶以上。然而，当系 统的状态方程或量测方程的非线性非常严重时，两者都避免不了滤波发散 问题，只不过 UKF 推迟或延缓了滤波的发散。 Dr. Yuan-Li Cai 2/52 Xi’an Jiaotong University

Introductionto Particle Filtering1引信言这里我们将介绍粒子滤波（particalfilter，PF）方法，可以有效地克服EKF或UKF可能遇到的滤波发散问题。粒子滤波的最初研究可以追溯到20世纪40年代，当时Metropolis和NorbertWiener就提出了类似的设想。但是，实现粒子滤波离不开强大的计算资源，即使是计算机技术高度发展的今天，粒子滤波所需要的计算量仍然是限制其广泛应用的瓶颈。在有的文献中，粒子滤波又称为序贯重要性采样、蒙特-卡洛滤波等。粒子滤波是一种“暴力”滤波方法，它以计算代价换取滤波性能。对于有些问题而言，这是值得或必须的。而对另外一些问题而言，如果计算资源受到较大的限制，那么粒子滤波则是不可取的。简言之，世上没有免费的午餐，要不要采用粒子滤波，或者如何采用粒子滤波，要在计算资源与估计性能之间寻找合理的折裹，完全取决于问题本身。3/52Dr.Yuan-LiCaiXi'an JiaotongUniversity

1 引言 Introduction to Particle Filtering 这里我们将介绍粒子滤波（partical filter, PF）方法，可以有效地克服 EKF 或 UKF 可能遇到的滤波发散问题。粒子滤波的最初研究可以追溯到 20 世纪 40 年代，当时 Metropolis 和 Norbert Wiener 就提出了类似的设想。 但是，实现粒子滤波离不开强大的计算资源，即使是计算机技术高度发展 的今天，粒子滤波所需要的计算量仍然是限制其广泛应用的瓶颈。在有的 文献中，粒子滤波又称为序贯重要性采样、蒙特-卡洛滤波等。 粒子滤波是一种“暴力”滤波方法，它以计算代价换取滤波性能。对于有 些问题而言，这是值得或必须的。而对另外一些问题而言，如果计算资源 受到较大的限制，那么粒子滤波则是不可取的。简言之，世上没有免费的 午餐，要不要采用粒子滤波，或者如何采用粒子滤波，要在计算资源与估 计性能之间寻找合理的折衷，完全取决于问题本身。 Dr. Yuan-Li Cai 3/52 Xi’an Jiaotong University

Introduction to Particle Filtering1引信由于PF属于随机滤波算法，因此我们首先简要回顾一下贝叶斯递推滤波算法（第2节）。PF的本质是蒙特-卡洛仿真，我们将在第3节、第4节分别介绍其中的完备采样、重要性采样及重采样等技术。然后，在第5节介绍PF的基本原理和算法。在基本粒子滤波算法的基础上，已经发展起来许多改进算法，我们将在第6节简要讨论两个重要的改进算法，分别是正则化粒子滤波和组合粒子滤波。此外，我们还给出了几个算例（附有了MATLAB代码），由此可以帮助大家对基本概念的理解和掌握。4/52Dr.Yuan-LiCaiXi'anJiaotongUniversity

1 引言 Introduction to Particle Filtering 由于 PF 属于随机滤波算法，因此我们首先简要回顾一下贝叶斯递推 滤波算法 (第2节)。PF 的本质是蒙特-卡洛仿真，我们将在第3节、第4节分 别介绍其中的完备采样、重要性采样及重采样等技术。然后，在第5节介绍 PF 的基本原理和算法。在基本粒子滤波算法的基础上，已经发展起来许多 改进算法，我们将在第6节简要讨论两个重要的改进算法，分别是正则化粒 子滤波和组合粒子滤波。此外，我们还给出了几个算例（附有了 MATLAB 代码），由此可以帮助大家对基本概念的理解和掌握。 Dr. Yuan-Li Cai 4/52 Xi’an Jiaotong University

Introduction to Particle Filtering2贝叶斯递推滤波2.贝叶斯递推滤波考虑如下一般的非线性系统(1)rk+1= fk(ck,Wk)(2)yk= hk(ak, Uk)其中，kERnykERm；fWkl和wl是相互独立的纯随机序列co~pro(ro）=p(ro）=p(colyo)，而且与（wk和{uk不相关。5/52Dr.Yuan-Li CaiXi'an Jiaotong University

2 贝叶斯递推滤波 Introduction to Particle Filtering 2. 贝叶斯递推滤波 考虑如下一般的非线性系统： xk+1 = fk(xk, wk) (1) yk = hk(xk, vk) (2) 其中，xk ∈ Rn，yk ∈ Rm；{wk} 和 {wk} 是相互独立的纯随机序列； x0 ∼ px0 (x0) = p(x0) = p(x0|y0)，而且与 {wk} 和 {vk} 不相关。 Dr. Yuan-Li Cai 5/52 Xi’an Jiaotong University

IntroductiontoParticleFiltering2贝叶斯递推滤波记Y=（1,2,，yk)，那么p(+1|) = / p(+1)p()dc(3)(4)p(yk+i[Yk) =/ p(yk+1|+1)p(rk+1|Yx)dk+1p(+k+1/Y+1) = P(yk+1/++1)p(tk+1/Y)(5)p(yk+1/Yk)以上三式便构成了递推求解p(kY）的核心公式，称为贝叶斯递推滤波算法。一般情况下，通常无法获得解析解。6/52Dr. Yuan-Li CaiXi'an Jiaotong University

2 贝叶斯递推滤波 Introduction to Particle Filtering 记 Yk = (y1, y2, · · · , yk), 那么 p(xk+1|Yk) = ∫ p(xk+1|xk)p(xk|Yk)dxk (3) p(yk+1|Yk) = ∫ p(yk+1|xk+1)p(xk+1|Yk)dxk+1 (4) p(xk+1|Yk+1) = p(yk+1|xk+1)p(xk+1|Yk) p(yk+1|Yk) (5) 以上三式便构成了递推求解 p(xk|Yk) 的核心公式，称为贝叶斯递推滤 波算法。一般情况下，通常无法获得解析解。 Dr. Yuan-Li Cai 6/52 Xi’an Jiaotong University

Introduction to Particle Filtering2贝叶斯递推滤波在MonteCarlo（MC）仿真中，以若干足够多的离散样本（称为粒子）对期望的概率密度进行近似，即NP(ra/Y) ~p(rx/Y) =w@8(r -2(6)i=1NEw()()≥0，而且其中，=1。=1问题：如果生成粒子{ri=1,,N并确定【wi=1,2….,?7/52Dr.Yuan-Li CaiXian JiaotongUniversity

2 贝叶斯递推滤波 Introduction to Particle Filtering 在 Monte Carlo (MC) 仿真中，以若干足够多的离散样本（称为粒子）对 期望的概率密度进行近似，即 p(xk|Yk) ≈ pˆ(xk|Yk) = ∑ N i=1 w (i) k δ(x − x (i) k ) (6) 其中，w (i) k ≥ 0，而且 ∑ N i=1 w (i) k = 1。 问题：如果生成粒子 {x (i) k }i=1,2,··· ,N 并确定 {w (i) k }i=1,2,··· ,N ？ Dr. Yuan-Li Cai 7/52 Xi’an Jiaotong University

3完备采样（PERFECTSAMPLING）Introduction to Particle Filtering3.完备采样（perfectsampling）对于目标密度函数p(a)，设【ar(),i=1,2,...,N)是根据p(r）采样到的独立同分布粒子，那么p(a)(m)=(a20)(7)-8/52Dr.Yuan-LiCaiXi'an JiaotongUniversity

3 完备采样（PERFECT SAMPLING） Introduction to Particle Filtering 3. 完备采样（perfect sampling） 对于目标密度函数 p(x)，设 {x (i) , i = 1, 2, · · · , N} 是根据 p(x) 采样到 的独立同分布粒子，那么 p(x) ≈ pˆ(x) = 1 N ∑ N i=1 δ(x − x (i) ) (7) Dr. Yuan-Li Cai 8/52 Xi’an Jiaotong University

Introduction to Particle Filtering3完备采样（PERFECTSAMPLING）例如：N>rip(r)dr~公i=1NN(r() -)(r() -)T(-a)(-a)Tp(r)dr~var(r)Ni=l更加一般地Eg(a) ~Eng(a) =g(r()(8)AT9/52Dr.Yuan-LiCaiXi'an JiaotongUniversity

3 完备采样（PERFECT SAMPLING） Introduction to Particle Filtering 例如： x¯ = ∫ xp(x)dx ≈ 1 N ∑ N i=1 x (i) var(x) = ∫ (x − x¯)(x − x¯) T p(x)dx ≈ 1 N ∑ N i=1 (x (i) − x¯)(x (i) − x¯) T 更加一般地 Eg(x) ≈ EˆN g(x) = 1 N ∑ N i=1 g(x (i) ) (8) Dr. Yuan-Li Cai 9/52 Xi’an Jiaotong University