粒子滤波PF—从贝叶斯滤波到粒子滤波PF——Part-II（蒙特卡洛方法）
粒子滤波PF—从贝叶斯滤波到粒子滤波PF——Part-II（蒙特卡洛方法）
原创不易，路过的各位大佬请点个赞
机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码WX: ZB823618313
粒子滤波PF—从贝叶斯滤波到粒子滤波PF——Part-II（蒙特卡洛方法）
- 粒子滤波PF—从贝叶斯滤波到粒子滤波PF——Part-II（蒙特卡洛方法）
-
- 1、贝叶斯滤波
- 2、 基于 p ( x k ∣ z k )p(x_k |z^{k}) p(xk∣zk)的各种滤波器
- 3、 大数定理（为了方便理解MC方法）
- 4、 蒙特卡洛方法MC
- 5. PF的目标跟踪应用：
-
- 5.1. 仿真参数
- 5.2. 跟踪轨迹
- 5.3. 跟踪误差（RMSE）
本博客是前一个该系列博客的延续，对前博客的内容这里不做重复，嘻嘻嘻嘻嘻嘻嘻。
为什么会这样呢，这样博客就比较短，看起来不废劲、压迫感骤减，哈哈哈哈哈，当然了主要是为了刷数量、且一次写不完，哈哈哈哈
在非线性条件下，贝叶斯滤波面临一个重要问题是状态分布的表达和积分式的求解，由前面章节中的分析可知,对于一般的非线性/非高斯系统,解析求解的途径是行不通的。在数值近似方法中，蒙特卡罗仿真是一种最为通用、有效的手段，粒子滤波就是建立在蒙特卡罗仿真基础之上的，它通过利用一组带权值的系统状态采样来近似状态的统计分布。由于蒙特卡罗仿真方法具有广泛的适用性，由此得到的粒子滤波算法也能适用于一般的非线性/非高斯系统。但是，这种滤波方法也面临几个重要问题，如有效采样(粒子)如何产生、粒子如何传递以及系统状态的序贯估计如何得到等。
简单的理解，粒子滤波就是使用了大量的随机样本，采用蒙特卡洛(MonteCarlo，MC)仿真技术完成贝叶斯递推滤波(Recursive Bayesian Filter)过程。因此本博客从贝叶斯滤波出发，简单介绍粒子滤波PF的出生、即应用
核心思想：是使用一-组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取- -个重要性概率密度并从中进行随机抽样，得到一些带有相应权值的随机样本后，在状态观测的基础上调节权值的大小.和粒子的位置，再使用这些样本来逼近状态后验分布，最后将这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束，采用样本形式而不是函数形式对状态概率密度进行描述，使其不需要对状态变量的概率分布进行过多的约束，因而在非线性非高斯动态系统中广泛应用。尽管如此，粒子滤波目前仍存在计算量过大、粒子退化等关键问题亟待突破。
1、贝叶斯滤波
考虑离散时间非线性系统动态模型，
x k = f ( x k − 1 , w k − 1 ) z k = h ( x k , v k ) (1)x_k=f(x_{k-1},w_{k-1}) \\ z_k=h(x_k,v_k ) \tag{1} xk=f(xk−1,wk−1)zk=h(xk,vk)(1)
其中 x k x_k xk为 k k k时刻的目标状态向量， z k z_k zk为 k k k时刻量测向量（传感器数据）。这里不考虑控制器 u k u_k uk。 w k {w_k} wk和 v k {v_k} vk分别是过程噪声序列和量测噪声序列。 w k w_k wk和 v k v_k vk为零均值高斯白噪声。
由于贝叶斯滤波的递推形式是基于非线性系统的后验概率密度，因此这里并不需要假设 wk w_k wk和 vk v_k vk为零均值高斯白噪声。而KF、EKF、CKF、QKF等需要假设过程、测量噪声为高斯白噪声。
因此基于贝叶斯滤波的粒子滤波可以处理非线性非高斯的状态估计问题。
定义 1 1 1 ~ k k k时刻对状态 x k x_k xk的所有测量数据为
zk = [z1T ,z2T , ⋯ ,zkT ]T z^k=[z_1^T,z_2^T,\cdots,z_k^T]^T zk=[z1T,z2T,⋯,zkT]T
2、 基于 p ( x k ∣ z k ) p(x_k |z^{k}) p(xk∣zk)的各种滤波器
说实话，你关于状态 xk x_k xk的概率密度函数（分布）都得到了，难道确定他们的各种估计还难吗？
这里给所有有缘人提醒下，实际上各种滤波、估计就是求 p( x k ∣ z k ) p(x_k |z^{k})p(xk∣zk)的一阶矩（ x k x_kxk的估计）以及二阶矩（估计的协方）。（基操、勿6）
从后验概率密度p (xk ∣zk ) p(x_k |z^{k}) p(xk∣zk)出发，得到各个滤波器见系列博客Part-I。
3、 大数定理（为了方便理解MC方法）
假设存在某一连续型随机变量 X X X，其概率密度函数为 p ( x ) p ( x ) p(x)，则 X X X 的数学期望为：
E ( X ) = ∫ x ⋅ p ( x ) d x E ( X ) = ∫ x ⋅ p ( x ) d x E(X)=∫x⋅p(x)dx
正如我们所知道的，上式中的 E [ X ] E[X] E[X]可能很难得到解析解。蒙特卡罗（Monte Carlo）方法告诉我们，可以通过对随机变量的概率密度进行随机采样，并对样本进行加权求和来近似随机变量的期望，如此一来，积分问题便转化为有限样本点的求和问题。
假设 X ( i ) \mathcal{X}^(i) X(i) 是从 p ( x ) p( x ) p(x) 中进行随机采样得到的样本集合，样本数量为 N N N，每一个样本（粒子) X(i) \mathcal{X}^{(i)} X(i)代表 X X X 的一种可能状态，即
X ( i ) ∼ p ( x ) \mathcal{X}^{(i)} ∼ p ( x ) X(i)∼p(x)
根据辛钦大数定律， X ( i ) \mathcal{X}^(i) X(i) 的样本均值依概率 1 收敛于数学期望，即对 ∀ ϵ > 0 ∀ ϵ > 0 ∀ϵ>0,有
上式意味着，当样本数量 N N N 足够大时，有
E ( X ) ≈1N ∑ i = 1N − 1 X ( i )E ( X ) ≈ \frac{1}{N}\sum_{i=1}^{N-1}\mathcal{X}^{(i)} E(X)≈N1i=1∑N−1X(i)
这就是大数定理，也是MC方法的基础。
实际上大数定理是：概率论理论 与 数理统计理论 的桥梁， 没有 大数定理， 这两个理论就是独立的学科。
4、 蒙特卡洛方法MC
说实话，如果只是单纯的用粒子滤波、蒙特卡洛方法并不需要了解。
但是不可否认的是，蒙特卡洛近似方法是贝叶斯滤波和粒子滤波的桥梁，没用MC方法，就没有粒子滤波。换句话说，粒子滤波就是贝叶斯滤波的基于MC近似方法一种实现。
没有了解蒙特卡罗近似方法，就不知道粒子滤波的妈妈是谁，会产生一种错觉：粒子滤波是一个人凭空提出来的算法。实际上他是贝叶斯滤波的一种近似实现方法。贝叶斯滤波还有很多种近似实现方法。
以上仅仅是本人的一些愚昧见解，非权威理解，哈哈啊哈哈哈哈哈。
不知道为什么，此处放个雏田，就有助于思想抛锚，嘻嘻嘻嘻嘻
如前面所述，贝叶斯方法求解状态估计问题是通过已知量测建立当前状态 x k x_k xk,的后
验概率密度函数来实现的，一旦获得 p ( x k ∣ z k ) p(x_k |z^{k}) p(xk∣zk)，也就得到了关于状态 x k x_k xk,的某一映射函数 g ( x k ) g(x_k) g(xk)的估计，即
E [ g ( x k ) ∣ z k ] = ∫ g ( x k ) p ( x k ∣ z k ) d x k , (1)E[g(x_k)|z^{k}]=\int g(x_k)p(x_k |z^{k})dx_k, \tag{1} E[g(xk)∣zk]=∫g(xk)p(xk∣zk)dxk,(1)
一般情况下，上式很难求得解析解。
蒙特卡洛方法又称为随机采样法或统计模拟法，它是以概率和统计理论方法为基础的一种计算方法，其基本思想是将所求解的问题描述为某种概率统计模型化的随机变量，利用计算机进行模拟采样，从而获得问题的近似解。粒子滤波就是基于蒙特卡洛仿真的一种滤波方法。
假设 X k i , i = 1 , 2 , ⋯ , N \mathcal{X}_k^i, i=1,2,\cdots,N Xki,i=1,2,⋯,N表示从后验概率分布函数 p ( x k ∣ z k ) p(x_k |z^{k}) p(xk∣zk)采样得到的 N N N个独立同分布的样本，则状态的后验概率密度可以通过如下经验公式近似得到:
p (xk ∣zk ) =1N δ (xk −Xki ) p(x_k |z^{k})=\frac{1}{N}\delta(x_k-\mathcal{X}_k^i) p(xk∣zk)=N1δ(xk−Xki)
式中， δ \delta δ表示狄拉克函数。
为此，根据蒙特卡洛原理，式(1)积分可近似表示为
E ^ [ g ( x k ) ∣ z k ] ≈ 1 N ∑ i = 1 N g ( X k i ) , (2)\hat{E}[g(x_k)|z^{k}]\approx \frac{1}{N}\sum_{i=1}^Ng(\mathcal{X}_k^i), \tag{2} E^[g(xk)∣zk]≈N1i=1∑Ng(Xki),(2)
当样本数 N N N增加时，根据大数定理，期望的估计几乎总是收敛
于真实期望，即
E^ [ g (xk ) ∣zk ] ⟶N → ∞ E [ g (xk ) ∣zk ] \hat{E}[g(x_k)|z^{k}] \stackrel{N\rightarrow\infty}{\longrightarrow} E[g(x_k)|z^{k}] E^[g(xk)∣zk]⟶N→∞E[g(xk)∣zk]
需要指出的是,从后验概率分布 p ( x k ∣ z k ) p(x_k |z^{k}) p(xk∣zk)进行采样近似只是蒙特卡洛方法的一种
特殊形式，实际上还有很多种形式。
这里我来解释解释上面用MC方法来实现后验概率分布函数p (xk ∣zk ) p(x_k |z^{k}) p(xk∣zk)的近似。
实际上只要学过数理统计都知道：只要样本足够大时，样本均值逼近理论均值。这也是著名的大数定理的应用之一。
因此，蒙特卡洛方法就是通过采样近似了 后验概率分布函数p (xk ∣zk ) p(x_k |z^{k}) p(xk∣zk)， 进而实现了后验期望估计E [ g (xk ) ∣zk ] E[g(x_k)|z^{k}] E[g(xk)∣zk]。
5. PF的目标跟踪应用：
5.1. 仿真参数
**一、目标模型：CV（细节见另一个博客） **
X k + 1 =[ 1 T 0 0 0 1 0 0 0 0 1 T 0 0 0 1 ] Xk +[ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] Wk X_{k+1}=\begin{bmatrix}1&T&0&0\\0&1&0&0\\0&0&1&T\\0&0&0&1 \end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix}W_k Xk+1=⎣⎢⎢⎡1000T100001000T1⎦⎥⎥⎤Xk+⎣⎢⎢⎡T2/2T0000T2/2T⎦⎥⎥⎤Wk
CV CT 模型的具体方程形式见另一个博客
二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
rkm =rk + r ~ k bkm =bk + b ~ k {r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k rkm=rk+r~kbkm=bk+b~k
其中
rk = ( x k− x 0 ) +( y k− y 0 ) 2) bk = tan − 1y k− y 0 x k− x 0 r_k=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\ rk=(xk−x0)+(yk−y0)2)bk=tan−1xk−x0yk−y0
[ x 0 , y 0 ] [x_0,y_0] [x0,y0]为雷达坐标，一般情况为0。雷达量测为 z k = [ r k , b k ] ′ z_k=[r_k,b_k]' zk=[rk,bk]′。雷达量测方差为
Rk = cov (vk ) =[ σ r 2 0 0 σ b 2 ] R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix} Rk=cov(vk)=[σr200σb2]
5.2. 跟踪轨迹
5.3. 跟踪误差（RMSE）
PF的妈妈贝叶斯滤波、以及基于标准的粒子滤波见Part-I和Part-III
原创不易，路过的各位大佬请点个赞