python粒子滤波_深入理解粒子滤波（Particle Filter)#附MATLAB代码

1. 粒子滤波算法简介

粒子滤波通过非参数化的蒙特卡洛(Monte Carlo)模拟方法和重要性采样来实现递推贝叶斯滤波，适用于任何能用状态空间模型描述的非线性系统，精度可以逼近最优估计。该方法的思想是用一组粒子来近似表示系统的后验概率分布，然后来估计非线性系统的状态。

2. 贝叶斯滤波

贝叶斯滤波方法是很多方法的基础，例如Kalman滤波，扩展Kalman滤波以及粒子滤波。它的工作就是根据不断接收到的新信息和已经提供的一些已经知道的信息，来不断更新概率。更新概率值的方法是根据概率论中的条件概率计算公式来做的。首先我们来回顾一下概率论的一些基础知识吧~

2.1 概率论基础

X: 表示一个随机变量，如果它有有限个可能的取值{x1,x2,⋯,xn}.

p(X = xi): 表示变量X的值为 xi的概率。

p(⋅):称为概率质量函数(probability mass function).

例：一个家里有3个房间，机器人在各个房间的概率为 p(room)={0.1,0.3,0.6}.

如果X在连续空间取值，p(x)称为概率密度函数(probability density function)

联合概率：p(X=x and Y=y)=p(x,y)，称为联合概率密度分布。如果X和Y是相互独立的随机变量，p(x,y)=p(x)p(y)。

条件概率：p(X=x|Y=y) 是在已知Y=y的条件下，计算X=x的概率。

如果x和y相互独立，则：

全概率公式：

离散情况下:

连续情况下：

贝叶斯公式：

基于条件概率公式和全概率公式，我们可以导出贝叶斯公式：

这里面x一般是某种状态；y一般是代表某种观测。P(x)叫做先验概率，P(y|x)也叫做似然。

2.2 贝叶斯融合多种观测

在很多应用问题中，我们会用多种观测信息对一个状态进行猜测和推理

所以有

2.2 贝叶斯融合动作

我们用u来描述动作，在x′状态下，执行了动作u之后，对象状态改变为x的概率表述为：

执行某一动作后，计算动作后的状态概率，需要考虑动作之前的各种状态情况，把所有情况用全概率公式计算：

连续情况下：

离散情况下：

2.3 贝叶斯滤波算法

2.3.1 系统输入

1到t时刻的状态观测和动作：dt={u1,z1…,ut,zt}

观测模型

动作的状态转移模型：P(x(t)|u,x(t-1))

系统状态的先验概率分布P(x)

2.3.2 期望输出

计算状态的后验概率，称为状态的置信概率：Bel(xt)=P(xt|u1,z1…,ut,zt)

2.3.3 模型假设

Markov性假设: t时刻的状态由t−1时刻的状态和t时刻的动作决定。t时刻的观测仅同t时刻的状态相关。

静态环境，即对象周边的环境假设是不变的

观测噪声、模型噪声等是相互独立的

2.3.3 Bayes滤波算法

基于上述设定和假设，我们给出贝叶斯滤波算法的推导过程：