 # 粒子滤波概述

1. 初始化：用gps初始化粒子群的位置
2. prediction：根据motion model与物体的控制信息$u$预测下个时刻粒子群中粒子的位置
3. update：根据物体的观测值$z$与地图值$z_l$计算出每个粒子的权重$w$
4. resample：根据粒子的$w$重新采样粒子  # Motion Model Bicycle Model假定汽车的四个轮子可以简化成一个轮子，类似自行车故得此名。

# Prediction

Prediction阶段，即：

$$x_{t+1} = f(x_t, u_t)$$  # Update

1. 粒子观测值的坐标系是以粒子为中心的，粒子前进方向为x轴的坐标系，不同与地图标志物的坐标系。需要统一到地图坐标系。
2. 地图的标志物太多，只需要考虑粒子传感器范围内的标志物即可。
3. 粒子观测值与地图标志物需要建立一一匹配的关系。
4. 更新粒子的权重。

## 坐标变换

$$(x_{map},y_{map}) = f(x_{particle}, y_{particle}, \theta_{particle}, x_{obs}, y_{obs})$$ $$\begin{split} x’ &= x\cos(\theta) +y\sin(\theta) \\ y’ &= -x\sin(\theta) +y\cos(\theta) \end{split}$$

$$\begin{split} x’ &= x\cos(\theta) - y\sin(\theta) \\ y’ &= x\sin(\theta) +y\cos(\theta) \end{split}$$

$$\begin{split} x’ &= x\cos(\theta) - y\sin(\theta) + x_p\\ y’ &= x\sin(\theta) +y\cos(\theta) + y_p \end{split}$$ ## Association ## Update

$$P(x,y) = \frac{1}{2\pi \sigma_x \sigma_y} \exp^{-(\frac{(x-\mu_x)^2}{2\sigma_x^2}+\frac{(y-\mu_y)^2}{2\sigma_y^2})}$$

$$w = \prod P(x,y)$$

# Resample

Update阶段获得每个粒子的权重后，根据权重重新筛选粒子即可，代码如下：