Kalman filtering卡尔曼滤波和Particle Filter粒子滤波及其MATLAB实现

---

前言

通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。如有错误还望指出。参考自：机器学习-白板推导系列(十六)-粒子滤波（Particle Filter）

---

一、基本概念和铺垫

1.两个假设

齐次Markov假设：状态变量z_t只与上一个时刻的z_t-1有关。
观测独立假设：观测变量x_t只与对应z_t有关。

2.卡尔曼滤波（线性）

公式：
$$z_t=g(z_{t-1},u,\epsilon )=A\cdot z_{t-1}+B+\epsilon ，$$
其中，$A$、$B$为矩阵，$\epsilon$为符合高斯分布的随机变量$\epsilon \sim N(0,Q)$，$u$为外部输入，根据情况可有也可以没有。
$$x_t=h(z_t,u,\delta )=C\cdot z_t+D+\delta .$$
其中，$C$、$D$为矩阵，$\delta$为符合高斯分布的随机变量$\delta \sim N(0,R)$。
问题：求解$p(z_t|x_1,...,x_t)$

Step1:Prediction预测——给$z_t$一个先验
$$p(z_t|x_1,...,x_{t-1})={\int }_{z_{t-1}}p(z_t|z_{t-1})\cdot p(z_{t-1}|x_1,...,x_{t-1})d{z}_{t-1}$$
Step2:Update更新——求$z_t$的后验
$$p(z_t|x_1,...,x_t)\propto p(x_t|z_t)\cdot p(z_t|x_1,...,x_{t-1})$$

3.粒子滤波（非线性）

由于其非线性特性，难以求出解析解，因此通过采样求出后验。

二、粒子滤波具体步骤

1.重要性采样Importance Sampling&SIS

Monte Carlo Method:

由大数定理可有求得期望，
$$E_{z|x}[f(z)]=\int f(z)\cdot p(z|x)dx\approx \frac{1}{N}\sum _{i=1}^{N}f(z^i)$$
其中$N$个样本，$z^i\sim p(z|x)$

Importance Sampling:

引入一个新的可行的提议分布（proposed distribution）$q(z|x)$，同样利用大数分布，可得，
$$E_{z|x}[f(z)]=\int f(z)\cdot \frac{p(z|x)}{q(z|x)}\cdot q(z|x)dx\approx \frac{1}{N}\sum _{i=1}^{N}f(z^i)\cdot \frac{p(z^i|x)}{q(z^i|x)}=\frac{1}{N}\sum _{i=1}^{N}f(z^i)\cdot w^i$$
Filtering:
下文将$x_1,x_2,...,x_t$简写为$x_{1:t}$，要求$p(z_t|x_{1:t})$，由上文的重采样可有得到权重（weight），
$$w_t^i=\frac{p(z_t^i|x_{1:t})}{q(z_t^i|x_{1:t})}$$

Sequential Importance Sampling:

由上述提出，为解决由$w_t^i$推算出$w_{t-1}^i$，以此降低计算量。用于求$p(z_{1:t}|x_{1:t})$，而非边缘后验$p(z_t|x_{1:t})$。
推导过程：
$$p(z_{1:t}|x_{1:t})$$
根据贝叶斯公式：
$$\begin{gathered} =\frac{p(z_{1:t},x_{1:t})}{p(x_{1:t})} \\ =\frac{p(z_{1:t},x_{1:t})}{C} \end{gathered}$$
其中，$p(x_{1:t})$可以视为常量$C$，继续根据贝叶斯公式分解得，
$$=\frac{1}{C}\cdot p(x_t|z_{1:t},x_{1:t-1})\cdot p(z_{1:t},x_{1:t-1})$$
根据观测独立假设，得到，
$$=\frac{1}{C}\cdot p(x_t|z_t)\cdot p(z_{1:t},x_{1:t-1})$$
再次利用贝叶斯公式分解，这次分解状态变量，得，
$$=\frac{1}{C}\cdot p(x_t|z_t)\cdot p(z_t|z_{1:t-1},x_{1:t-1})\cdot p(z_{1:t-1},x_{1:t-1})$$
根据齐次Markov假设，简化得到，
$$\begin{gathered} =\frac{1}{C}\cdot p(x_t|z_t)\cdot p(z_t|z_{t-1})\cdot p(z_{1:t-1},x_{1:t-1}) \\ =\frac{1}{C}\cdot p(x_t|z_t)\cdot p(z_t|z_{t-1})\cdot p(z_{1:t-1}|x_{1:t-1})\cdot p(x_{1:t-1}) \end{gathered}$$
其中$p(x_{1:t-1})$可以视为常量$D$，
$$=\frac{D}{C}\cdot p(x_t|z_t)\cdot p(z_t|z_{t-1})\cdot p(z_{1:t-1}|x_{1:t-1})$$
假定有，
$$q(z_{1:t}|x_{1:t})=q(z_t|z_{1:t-1},x_{1:t})\cdot q(z_{1:t-1}|x_{1:t-1})$$
设定$q$也符合两个假设，既，
$$q(z_{1:t}|x_{1:t})=q(z_t|z_{t-1},x_{1:t})\cdot q(z_{1:t-1}|x_{1:t-1})$$
带入权重中可得，
$$\begin{gathered} w_t^i\propto \frac{p(z_t^i|x_{1:t})}{q(z_t^i|x_{1:t})}\propto \frac{p(x_t|z_t)\cdot p(z_t|z_{t-1})\cdot p(z_{1:t-1}|x_{1:t-1})}{q(z_t|z_{t-1})\cdot q(z_{1:t-1}|x_{1:t-1})} \\ =\frac{p(x_t|z_t)\cdot p(z_t|z_{t-1})}{q(z_t|z_{t-1},x_{1:t})}\cdot w_{t-1}^i \end{gathered}$$
算法过程：

前提：$t-1$时刻采样已完成→$w_{t-1}^i$已知
$t$时刻：
$fori=1,...,N$
$$z_t^i\sim q(z_t|z_t-1,x_{1:t})$$
$$w_t^i\propto \frac{p(x_t|z_t)\cdot p(z_t|z_{t-1})}{q(z_t|z_{t-1},x_{1:t})}\cdot w_{t-1}^i$$
$end$
$$w_t^i归一化$$

存在问题： 权值退化，个别权重接近一，其余权重很小
解决方案：（1）重采样。（2）选择一个合适的$q(z)$

2.重采样Re-sampling

轮盘赌法

根据权重进行轮盘赌，权重对应被采样概率。如$x^1,x^2,x^3$的权重为分别为0.4,0.2,0.3，那么它们各自被采样的概率就是40%,20%,30%。

其余方法

多项式采样、系统重采样、残差重采样法

基本粒子滤波Basic Particle Filter

SIS+Re-sampling

3.SIR Filter

重新设定$q(z)$为，
$$p(z_t|z_{1:t},x_{1:t})=p(z_t|z_{t-1})$$
带入权重中得，
$$w_t^i=p(x_t|z_t)\cdot w_{t-1}^i$$

三、MATLAB程序实现

clc;clear all;close all;

%初始参数设置
z_delta = 1; %状态变量的随机变量的方差
x_delta = 1; %观测变量的随机变量的方差
z = 0; %状态变量初始值
x = z^2/5+sqrt(x_delta)*rand;%观测变量初始值，自定函数，用于生成样本
T = 100; %跟踪时长
N = 100; %粒子个数

%生成粒子
N_delta=2; %粒子的高斯分布方差
N_mu=x; %粒子的高斯分布均值
Particle=zeros(1,N); %初始化粒子
for i=1:N %产生符合高斯分布的粒子
    Particle(i)=N_mu+sqrt(N_delta)*randn;
end

%实际的样本数据
z_real=[z];%用于记录实际状态变量
x_real=[x];%用于记录实际观测变量
for t=2:T
    z=randn*sqrt(z_delta)+cos(z^2)*z/(2+z^2)+sin(t-1)*log(t);%随意设置的一个函数，z(t)只与z(t-1)有关
    x = z^2/5+sqrt(x_delta)*rand;%x(t)只与z(t)有关
    z_real=[z_real,z];%用于记录实际状态变量
    x_real=[x_real,x];%用于记录实际观测变量
end

%粒子滤波
for t=1:T
    
    for i=1:N
        %状态变量z从先验p(z(t)|z(t-1))中进行采样
        z_update(i)=randn*sqrt(z_delta)+cos(Particle(i)^2)*Particle(i)/(2+Particle(i)^2)+sin(t-1)*log(t);
        %计算对应的观测变量，为权重计算做准备
        x_update(i) = z_update(i)^2/5+sqrt(x_delta)*rand;
        %采用SIR方法，w(t)=p(x(t)|z(t))*w(t-1)，采用噪声符合高斯分布
        w(i)= (1/sqrt(2*pi*x_delta)) * exp(-(x_real(t) - x_update(i))^2/(2*x_delta));
    end
    %将权重归一化
    w=w./(sum(w));%防止出现除0的情况
   
    %重采样
    for i = 1 : N    
        Particle(i) = z_update(find(rand <= cumsum(w),1));   % 粒子权重大的将多得到后代    
    end   
    
    %状态估计，重采样以后，每个粒子的权重都变成了1/N 
    z_estimate(t)=mean(Particle);
    
end


%绘图
figure;
plot(1:T,z_real,'.-b', 1:T, z_estimate, '-.r','linewidth',3)
xlabel('time step'); ylabel('flight position');    
legend('真实值', '粒子估算值');  

总结

在经济学领域，粒子滤波被应用在经济数据预测；在军事领域已经被应用于雷达跟踪空中飞行物，空对空、空对地的被动式跟踪；在自动驾驶中，它被应用于目标检测；交通管制领域它被应用在对车或人视频监控；它还用于机器人的全局定位。引用自：百度百科