粒子滤波 particle filter —从贝叶斯滤波到粒子滤波——Part-I（贝叶斯滤波）

原创不易，路过的各位大佬请点个赞

机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313

在非线性条件下，贝叶斯滤波面临一个重要问题是状态分布的表达和积分式的求解，由前面章节中的分析可知,对于一般的非线性/非高斯系统,解析求解的途径是行不通的。在数值近似方法中，蒙特卡罗仿真是一种最为通用、有效的手段，粒子滤波就是建立在蒙特卡罗仿真基础之上的，它通过利用一组带权值的系统状态采样来近似状态的统计分布。由于蒙特卡罗仿真方法具有广泛的适用性，由此得到的粒子滤波算法也能适用于一般的非线性/非高斯系统。但是，这种滤波方法也面临几个重要问题，如有效采样(粒子)如何产生、粒子如何传递以及系统状态的序贯估计如何得到等。

简单的理解，粒子滤波就是使用了大量的随机样本，采用蒙特卡洛(MonteCarlo，MC)仿真技术完成贝叶斯递推滤波(Recursive Bayesian Filter)过程。因此本博客从贝叶斯滤波出发，简单介绍粒子滤波PF的出生、即应用

核心思想：是使用一组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度并从中进行随机抽样，得到一些带有相应权值的随机样本后，在状态观测的基础上调节权值的大小。和粒子的位置，再使用这些样本来逼近状态后验分布，最后将这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束，采用样本形式而不是函数形式对状态概率密度进行描述，使其不需要对状态变量的概率分布进行过多的约束，因而在非线性非高斯动态系统中广泛应用。尽管如此，粒子滤波目前仍存在计算量过大、粒子退化等关键问题亟待突破。

1、贝叶斯滤波

考虑离散时间非线性系统动态模型，
$$\begin{gathered} x_k=f(x_{k-1},w_{k-1}) \\ {z}_k=h(x_k,v_k)\text{\hspace{1em}(1)} \end{gathered}$$
其中$x_k$为$k$时刻的目标状态向量，$z_k$为$k$时刻量测向量（传感器数据）。这里不考虑控制器$u_k$。$w_k$和$v_k$分别是过程噪声序列和量测噪声序列。$w_k$和$v_k$为零均值高斯白噪声。

由于贝叶斯滤波的递推形式是基于非线性系统的后验概率密度，因此这里并不需要假设$w_k$和$v_k$为零均值高斯白噪声。而KF、EKF、CKF、QKF等需要假设过程、测量噪声为高斯白噪声。

因此基于贝叶斯滤波的粒子滤波可以处理非线性非高斯的状态估计问题。

定义$1$ ~ $k$时刻对状态$x_k$的所有测量数据为
$$z^k=[z_1^T,z_2^T,\cdots ,z_k^T{]}^T$$

贝叶斯滤波问题就是计算对$k$时刻状态$x$估计的置信程度，为此构造概率密度函数$p(x_k|z^k)$，在给定初始分布$p(x_0|z_0)=p(x_0)$后，从理论上看，可以通过预测和更新两个步骤递推得到概率密度函数$p(x_k|z^k)$的值。

是不是卡尔曼滤波的雏形出现了，哈哈哈，预测、更新也存在KF中。

1.1、 预测

现假定$k-1$时刻的概率密度函数已知，则通过将Chapman-Kolmogorov等式应用
于动态方程(1)，即可预测$k$时刻状态的先验概率密度函数为
$$p(x_k|z^{k-1})=\int p(x_k|x_{k-1})p(k-1|z^{k-1})d{x}_{k-1})\text{\hspace{1em}(2)}$$

实际上，状态转移方程写为概率密度的形式即为：$$x_k=f(x_{k-1},w_{k-1})\underset{\text{等价}}{=}p(x_k|x_{k-1})$$
式(2)中隐含假定了$p(x_k|x_{k-1})=p(x_k|x_{k-1},z^{k-1})$，实际上这本身在这里就是成立的，基于(1)式的马尔可夫过程。

1.2、 更新

在获得$p(x_k|z^{k-1})$的基础上，结合$k$时刻得到的新的量测值，基于贝叶斯公式，可以计算$k$时刻状态的后验概率密度函数:
$$p(x_k|z^k)=\frac{p(z_k|x_k)p(x_k|z^{k-1})}{p(z_k|z^{k-1})}\text{\hspace{1em}(3)}$$
式中分子$p(z_k|z^{k-1})$有全概率公式得到
$$p(z_k|z^{k-1})=\int p(z_k|x_k)p(x_k|z^{k-1})d{x}_k\text{\hspace{1em}(4)}$$

我就说吧，上述过程实际上贝叶斯后燕推断的公式，哈哈哈哈啊哈

实际上这也是卡尔曼滤波的更新思想：在$k$时刻得到测量$z_k$后，利用测量$z_k$修正先验概率，进而获得当前时刻状态的后验概率。我正是太机智了，哈哈啊哈

2、 基于$p(x_k|z^k)$的各种滤波器

说实话，你关于状态$x_k$的概率密度函数（分布）都得到了，难道确定他们的各种估计还难吗？

这里给所有有缘人提醒下，实际上各种滤波、估计就是求$p(x_k |z^{k})$的一阶矩（$x_k$的估计）以及二阶矩（估计的协方）。（基操、勿6）

式(3)描述了一个由$k-1$时刻后验概率密度函数向$k$时刻后验概率密度函数递推的完整过程，从而构成了贝叶斯估计最优解的通用表示形式。进而通过后验分布$p(x_k|z^k)$可以得到不同准则条件下$x$的最优估计划。

如最小均方误差(MMSE)估计为：
$${\hat{x}}_k=E[x_k|z_k]=\int x_{k}p(x_k|z^k)d{x}_k$$

最大后验(MAP)估计为：
$${\hat{x}}_k=\arg \underset{x_k}{\min }p(x_k|z^k)$$

实际上粒子滤波就是基于蒙特卡洛技术、将上述递推过程用大量采样的方式实现了。

是不是看到这里，有一种豁然开朗的感觉，实际上上面的介绍及推导都是简化版本，也加了本人一些愚钝的理解在里面，因此也是有一些误区、对高水平之人、还是建议看经典的著作最为有用和严谨。

3. PF的目标跟踪应用：

3.1. 仿真参数

一、目标模型：CV CT CV
第一阶段：1:39s，匀速运动CV
第二阶段：40:91s，匀速圆周运动CT，角速度：$6\ast \pi /180;$
第三阶段：92:99s，匀速运动CV
第四阶段：100:131s，匀速圆周运动CT，角速度：$-3\ast \pi /180;$
第五阶段：132:150s，匀速运动CV

CV模型：（细节见另一个博客）
X k + 1 = [ 1 T 0 0 0 1 0 0 0 0 1 T 0 0 0 1 ] X k + [ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] W k X_{k+1}=

X_{k} + W_k Xk+1​=⎣ ⎡​1000​T100​0010​00T1​⎦ ⎤​Xk​+⎣ ⎡​T2/2T00​00T2/2T​⎦ ⎤​Wk​
CT模型：（细节见另一个博客）
X k + 1 = [ 1 sin ⁡ ( ω T ) ω 0 − 1 − cos ⁡ ( ω T ) ω 0 cos ⁡ ( ω T ) 0 − sin ⁡ ( ω T ) 0 1 − cos ⁡ ( ω T ) ω 1 sin ⁡ ( ω T ) ω 0 sin ⁡ ( ω T ) 0 cos ⁡ ( ω T ) ] X k + [ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] W k X_{k+1}=

[1sin⁡(ωT)ω0−1−cos⁡(ωT)ω0cos⁡(ωT)0−sin⁡(ωT)01−cos⁡(ωT)ω1sin⁡(ωT)ω0sin⁡(ωT)0cos⁡(ωT)]" role="presentation" style="position: relative;">⎡⎣⎢⎢⎢⎢⎢1000sin(ωT)ωcos(ωT)1−cos(ωT)ωsin(ωT)0010−1−cos(ωT)ω−sin(ωT)sin(ωT)ωcos(ωT)⎤⎦⎥⎥⎥⎥⎥$$\left[\begin{array}{cccc}1& \frac{\sin (\omega T)}{\omega }& 0& -\frac{1-\cos (\omega T)}{\omega }\\ 0& \cos (\omega T)& 0& -\sin (\omega T)\\ 0& \frac{1-\cos (\omega T)}{\omega }& 1& \frac{\sin (\omega T)}{\omega }\\ 0& \sin (\omega T)& 0& \cos (\omega T)\end{array}\right]$$

X_{k} +

[T2/20T00T2/20T]" role="presentation" style="position: relative;">⎡⎣⎢⎢⎢⎢T2/2T0000T2/2T⎤⎦⎥⎥⎥⎥$$\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]$$

W_k Xk+1​=⎣ ⎡​1000​ωsin(ωT)​cos(ωT)ω1−cos(ωT)​sin(ωT)​0010​−ω1−cos(ωT)​−sin(ωT)ωsin(ωT)​cos(ωT)​⎦ ⎤​Xk​+⎣ ⎡​T2/2T00​00T2/2T​⎦ ⎤​Wk​

初始状态：
$$\begin{gathered} x_0=[30000,80,20000,50{]}^T \\ {P}_0=\text{diag}([1e6,1e2,1e6,1e2]) \end{gathered}$$

CV CT 模型的具体方程形式见另一个博客

二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} 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} \end{gathered}$$
$[x_0,y_0]$为雷达坐标。雷达量测为$z_k=[r_k,b_k{]}^{\prime }$。雷达量测方差为
R k = cov ( v k ) = [ σ r 2 0 0 σ b 2 ] R_k=\text{cov}(v_k)=

Rk​=cov(vk​)=[σr2​0​0σb2​​]

3.2. 跟踪轨迹

3.3. 跟踪误差

3.4.部分代码

close all;
clear all;
clc;
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%系统参数设置
runs=30; %蒙特卡洛实验次数，滤波将进行100次
steps=150; %每次滤波进行80次采样

% % N=1000;%粒子滤波例子点个数
%模型1的动态方程参数设置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CV模型
T=1;
Fk1=[1 T 0 0;
    0 1 0 0;
    0 0 1 T;
    0 0 0 1;];

q1=0.1; % 目标运动学标准差，过程噪声
Qk1=q1^2*[T^3/3, T^2/2,        0,     0;
    T^2/2, T^2,            0,     0;
    0,     0,     T^3/3,   T^2/2;
    0,     0,     T^2/2,   T^2  ;];% 过程噪声协方差
Gk= eye(4); %过程噪声增益矩阵

%模型2的动态方程参数设置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CT模型
w1 = 6*pi/180;
Fk2=[1       sin(w1*T)/w1        0     -(1-cos(w1*T))/w1         ;
    0         cos(w1*T)        0            -sin(w1*T)        ;
    0   (1-cos(w1*T))/w1        1           sin(w1*T)/w1        ;
    0         sin(w1*T)        0             cos(w1*T)        ;];
q2 = 0.01;

% % Qk2= q2^2*[2*(w1*T-sin(w1*T))/w1^3       (1-cos(w1*T))/w1^2                    0           (w1*T-sin(w1*T))/w1^2     ;
% %     (1-cos(w1*T))/w1^2                     T       -(w1*T-sin(w1*T))/w1^2            0        ;
% %     0         -(w1*T-sin(w1*T))/w1^2       2*(w1*T-sin(w1*T))/w1^3           (1-cos(w1*T))/w1^2  ;
% %     (w1*T-sin(w1*T))/w1^2                     0             (1-cos(w1*T))/w1^2                         T;];
Qk2=q1^2*[T^3/3, T^2/2,        0,     0;
    T^2/2, T^2,            0,     0;
    0,     0,     T^3/3,   T^2/2;
    0,     0,     T^2/2,   T^2  ;];% 过程噪声协方差
%模型3的动态方程参数设置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CT模型
w = -5*pi/180;
Fk3=[1       sin(w*T)/w        0     -(1-cos(w*T))/w         ;
    0         cos(w*T)        0            -sin(w*T)        ;
    0   (1-cos(w*T))/w        1           sin(w*T)/w        ;
    0         sin(w*T)        0             cos(w*T)        ;];
% % Qk3= q2^2*[2*(w*T-sin(w*T))/w^3       (1-cos(w*T))/w^2                    0           (w*T-sin(w*T))/w^2     ;
% %     (1-cos(w*T))/w^2                     T       -(w*T-sin(w*T))/w^2            0        ;
% %     0         -(w*T-sin(w*T))/w^2       2*(w*T-sin(w*T))/w^3           (1-cos(w*T))/w^2  ;
% %     (w*T-sin(w*T))/w^2                     0             (1-cos(w*T))/w^2                         T;];
Qk3=q1^2*[T^3/3, T^2/2,        0,     0;
    T^2/2, T^2,            0,     0;
    0,     0,     T^3/3,   T^2/2;
    0,     0,     T^2/2,   T^2  ;];% 过程噪声协方差
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

蒙特卡洛近似技术、以及基于MC方法的粒子滤波见Part-II和Part-III

原创不易，路过的各位大佬请点个赞