粒子滤波器简介与Python实现
Z[:, 0] = X[:, 0] + np.random.normal(0, np.sqrt®, 2)
初始化粒子群
for i in range(N):
P[:, i] = np.random.rand(2) * WorldSize
dist = np.linalg.norm(P[:, i] - Z[:, 0])
w[i] = (1 / (np.sqrt(2 * np.pi) * R)) * np.exp(-dist**2 / (2 * R))
PCenter[:, 0] = np.mean(P, axis=1)
绘制初始状态
plt.figure(figsize=(10, 5))
plt.scatter(X[0, 0], X[1, 0], color=‘r’, s=30, label=‘真实位置’)
plt.scatter(P[0, :], P[1, :], color=‘k’, s=5, label=‘粒子群’)
plt.scatter(PCenter[0, 0], PCenter[1, 0], color=‘b’, s=25, label=‘粒子群中心’)
plt.legend()
plt.title(‘初始状态’)
plt.xlim(0, WorldSize)
plt.ylim(0, WorldSize)
plt.show()
运动和滤波过程
for k in range(1, T):
X[:, k] = X[:, k-1] + distance * np.array([-np.cos(k * theta), np.sin(k * theta)]) + np.random.normal(0, np.sqrt(Q), 2)
Z[:, k] = X[:, k] + np.random.normal(0, np.sqrt®, 2)
for i in range(N):
P[:, i] = P[:, i] + distance * np.array([-np.cos(k * theta), np.sin(k * theta)]) + np.random.normal(0, np.sqrt(Q), 2)
dist = np.linalg.norm(P[:, i] - Z[:, k])
w[i] = (1 / (np.sqrt(2 * np.pi) * R)) * np.exp(-dist**2 / (2 * R))
w /= np.sum(w)
indices = np.random.choice(N, N, p=w)
P = P[:, indices]
PCenter[:, k] = np.mean(P, axis=1)
绘制轨迹
plt.figure(figsize=(10, 5))
plt.plot(X[0, :], X[1, :], ‘r.-’, label=‘真实轨迹’)
plt.plot(Z[0, :], Z[1, :], ‘g.-’, label=‘测量轨迹’)
plt.plot(PCenter[0, :], PCenter[1, :], ‘b.-’, label=‘粒子群中心轨迹’)
plt.legend()
plt.title(‘运动轨迹’)
plt.xlim(0, WorldSize)
plt.ylim(0, WorldSize)
plt.show()
计算并绘制误差
err[0, :] = np.linalg.norm(X - PCenter, axis=0)
err[1, :] = np.linalg.norm(X - Z, axis=0)
plt.figure(figsize=(10, 5))
plt.plot(err[0, :], ‘b.-’, label=‘粒子群误差’)
plt.plot(err[1, :], ‘r.-’, label=‘测量误差’)
plt.xlabel(‘时间步’)
plt.ylabel(‘误差’)
plt.title(‘误差分析’)
plt.legend()
plt.show()
绘制最后一步的粒子群状态
plt.figure(figsize=(10, 5))
plt.scatter(X[0, -1], X[1, -1], color=‘r’, s=30, label=‘真实位置 (最终)’)
plt.scatter(P[0, :], P[1, :], color=‘k’, s=5, label=‘粒子群 (最终)’)
plt.scatter(PCenter[0, -1], PCenter[1, -1], color=‘b’, s=25, label=‘粒子群中心 (最终)’)
plt.legend()
plt.title(‘最终状态的粒子群’)
plt.xlim(0, WorldSize)
plt.ylim(0, WorldSize)
plt.show()
#### 初始化
我们初始化系统的真实状态、观测状态和粒子群，并为每个粒子分配一个初始权重。
##### 初始化粒子群解释
这部分代码的目的是初始化粒子滤波过程中使用的粒子群。每个粒子代表了对系统状态（在这个案例中是二维位置）的一个假设或猜测。
初始化粒子群
for i in range(N):
P[:, i] = np.random.rand(2) * WorldSize # 随机初始化粒子位置
dist = np.linalg.norm(P[:, i] - Z[:, 0]) # 计算粒子与初始观测位置的欧几里得距离
w[i] = (1 / (np.sqrt(2 * np.pi) * R)) * np.exp(-dist**2 / (2 * R)) # 根据距离计算粒子的权重
PCenter[:, 0] = np.mean(P, axis=1) # 计算所有粒子位置的平均值，作为粒子群的中心位置
* 每个粒子的位置`P[:, i]`是在二维空间内随机生成的，其中每个维度的值介于`0`到`WorldSize`之间。
* 然后计算每个粒子与初始观测位置`Z[:, 0]`之间的距离。这个距离用于评估粒子的假设状态与初始观测状态的一致性。
* 粒子的权重`w[i]`根据其到观测位置的距离计算，使用高斯分布公式。距离越小，权重越大，表示该粒子代表的状态与观测数据越吻合。
* `PCenter[:, 0]`是所有粒子位置的平均值，代表了初始时刻粒子群的中心位置，可以被视为所有粒子假设的“平均”状态。
#### 运动和滤波过程
对于每一个时间步，我们根据设定的运动模型更新系统的真实状态和观测状态。然后，我们更新每个粒子的位置，根据新的观测数据调整粒子的权重，并进行重采样。
这部分代码模拟了系统状态的更新（基于设定的运动模型）和观测过程，然后根据新的观测数据更新粒子群。
for k in range(1, T): # 循环遍历每一个时间步
# 系统状态更新：基于前一时刻的状态、预定的运动方向和距离，以及过程噪声Q来更新当前状态
X[:, k] = X[:, k-1] + distance * np.array([-np.cos(k * theta), np.sin(k * theta)]) + np.random.normal(0, np.sqrt(Q), 2)
# 观测更新：在当前的真实状态基础上添加测量噪声R来生成观测值
Z[:, k] = X[:, k] + np.random.normal(0, np.sqrt(R), 2)
# 更新粒子群：对每个粒子，模拟相同的运动方向和距离，并加入过程噪声Q
for i in range(N): # 遍历所有粒子
P[:, i] = P[:, i] + distance \* np.array([-np.cos(k \* theta), np.sin(k \* theta)]) + np.random.normal(0, np.sqrt(Q), 2)
# 计算粒子与当前观测值的距离
dist = np.linalg.norm(P[:, i] - Z[:, k])
# 根据粒子与观测值的距离更新粒子权重，使用高斯分布公式
w[i] = (1 / (np.sqrt(2 \* np.pi) \* R)) \* np.exp(-dist\*\*2 / (2 \* R))
# 权重归一化：确保所有粒子的权重之和为1
w /= np.sum(w)
# 重采样：基于归一化后的权重随机选择粒子，以便用有较高权重的粒子替换权重低的粒子
indices = np.random.choice(N, N, p=w)
P = P[:, indices]
# 更新粒子群中心：计算重采样后的粒子位置平均值，代表粒子群对系统状态的估计
PCenter[:, k] = np.mean(P, axis=1)
* 系统状态`X[:, k]`根据预定义的运动模型更新。在此案例中，模型包括固定的移动距离和旋转角度，以及添加的过程噪声`Q`。
* 观测`Z[:, k]`是在新的系统状态基础上加上观测噪声`R`得到的。
* 每个粒子`P[:, i]`也根据相同的运动模型更新，以模拟可能的系统状态。
* 根据每个粒子与新观测位置的距离，重新计算每个粒子的权重。粒子与观测位置越接近，表明该粒子所代表的状态假设与实际观测数据越吻合，因此其权重越大。权重的计算同样使用高斯分布公式，体现了粒子位置与观测位置之间的关系。
* 通过归一化处理（`w /= np.sum(w)`），确保所有粒子的权重之和为1，这样权重可以作为粒子被选中进行下一代采样的概率。
* 重采样步骤（`indices = np.random.choice(N, N, p=w)`）根据每个粒子的权重随机选择N个粒子组成新的粒子群。这一步骤是粒子滤波的核心，它使得高权重的粒子（即与观测数据匹配度高的状态假设）有更高的机会被保留，而低权重的粒子可能被丢弃。这样做可以使粒子群更集中于高概率区域，提高估计的准确性。
* 更新粒子群中心`PCenter[:, k]`为所有粒子的平均位置，这一步相当于得到了该时刻所有粒子位置的统计中心，可以视为当前所有粒子假设的综合预测状态。
这个过程在每个时间步`k`重复执行，模拟了整个观测周期内系统状态的演化及其与粒子滤波估计之间的动态交互。通过不断更新粒子群，粒子滤波算法能够在有限的信息下对系统状态进行有效估计，并适应系统动态变化和观测噪声。这使得粒子滤波特别适合处理非线性和/或非高斯噪声的系统状态估计问题。
##### P = P[:, indices]作用
在粒子滤波算法中，`P = P[:, indices]`这一步执行的是重采样过程的最后一部分，即根据每个粒子的权重来更新（重采样）粒子集合。在这一步骤中，一些权重较小的粒子将被丢弃，而一些权重较大的粒子可能会被复制一次或多次。这样做的目的是为了聚焦于那些与观测数据更吻合的假设（粒子），以提高滤波器的估计精度。
在执行`indices = np.random.choice(N, N, p=w)`之后，我们得到了一个新的索引数组`indices`，这个数组的长度仍然是`N`，但里面的元素是根据权重`w`随机选择出来的粒子索引。这意味着，如果某个粒子的权重较大，它在`indices`数组中出现的次数就可能比较多；反之，如果某个粒子的权重较小，它在`indices`数组中出现的次数就可能较少或者根本不出现。
当执行`P = P[:, indices]`时，我们根据`indices`数组中的索引重新组织粒子集合。具体来说，我们从原始的粒子集合`P`中选出对应`indices`里面索引的那些粒子，形成新的粒子集合。这一过程可能会导致一些粒子被复制，而一些其他的粒子被丢弃。
##### 举例
假设我们有一个粒子集合`P`，它包含5个粒子，分别标记为`A, B, C, D, E`。我们进行一次重采样，并且基于某种方式计算得到的权重，结果选择了如下的索引：
indices = [0, 2, 2, 3, 4]
这表示：
* 粒子`A (索引0)`被选择了1次。
* 粒子`B (索引1)`没有被选择。
* 粒子`C (索引2)`被选择了2次，也就是说它被复制了。
* 粒子`D (索引3)`和`E (索引4)`各被选择了1次。
根据`indices`数组，新的粒子集合`P`将会是`[A, C, C, D, E]`。这样，我们就根据每个粒子的权重完成了一次重采样，更新了粒子集合，以便它更好地反映了观测数据和系统状态的后验概率分布。通过重采样，算法可以把注意力集中在那些更有可能代表系统真实状态的粒子上，从而提高状态估计的准确性。
#### 结果展示
通过matplotlib绘图，我们可以直观地看到系统的真实轨迹、测量轨迹、粒子群中心轨迹，以及随时间变化的误差分析。这些图表帮助我们理解粒子滤波器在状态估计中的作用和效果。
这段代码完成了以下任务：
1. 初始化参数和状态。
2. 在初始状态绘制了真实位置、粒子群和粒子群中心的图。
3. 模拟了物体的运动，根据粒子滤波算法更新了粒子的状态。
4. 绘制了整个运动过程中的真实轨迹、测量轨迹和粒子群中心的轨迹。
5. 计算并绘制了真实位置与粒子群中心的误差以及真实位置与测量位置的误差。
6. 在最后一步，展示了真实位置、粒子群和粒子群中心的最终状态。
通过这段代码，您可以直观地看到粒子滤波算法在定位和跟踪运动物体时的效果，包括粒子群如何随时间演化以及其如何逼近真实的运动轨迹。
#### 数学公式
让我们使用数学公式和计算步骤来具体解释粒子滤波的过程。粒子滤波是一种顺序蒙特卡洛方法，用于估计动态系统的状态，特别是在只能间接观测到系统状态的情况下。这里，我们以一个简化的二维空间模型为例，系统的状态由位置坐标表示。
##### 1. 初始状态与粒子初始化
* 系统的真实初始状态为
X
0
=
[
x
0
,
y
0
]
T
\mathbf{X}\_0 = [x\_0, y\_0]^T
X0=[x0,y0]T。
* 观测状态为
Z
0
=
X
0
+
n
R
\mathbf{Z}\_0 = \mathbf{X}\_0 + \mathbf{n}\_R
Z0=X0+nR，其中
n
R
∼
N
(
0
,
R
)
\mathbf{n}\_R \sim \mathcal{N}(0, R)
nR∼N(0,R) 表示测量噪声。
* 初始化
N
N
N 个粒子
P
i
0
\mathbf{P}\_i^0
Pi0，每个粒子随机分布在定义的空间内。对于每个粒子，计算其权重
w
i
0
w\_i^0
wi0 以反映粒子与初始观测的匹配度：
w
i
0
=
1
2
π
R
exp
(
−
∥
P
i
0
−
Z
0
∥
2
2
R
)
w\_i^0 = \frac{1}{\sqrt{2\pi R}} \exp\left(-\frac{\|\mathbf{P}\_i^0 - \mathbf{Z}\_0\|^2}{2R}\right)
wi0=2πR
1exp(−2R∥Pi0−Z0∥2)
##### 2. 系统状态更新
对于每一个时间步 (k)，系统的真实状态更新为：
X
k
=
X
k
−
1
+
d
k
+
n
Q
\mathbf{X}\_k = \mathbf{X}\_{k-1} + \mathbf{d}\_k + \mathbf{n}\_Q
Xk=Xk−1+dk+nQ
* d
k
\mathbf{d}\_k
dk 表示根据运动模型预计的状态变化，例如距离
distance
\text{distance}
distance 乘以方向
[
−
cos
(
k
θ
)
,
sin
(
k
θ
)
]
T
[-\cos(k\theta), \sin(k\theta)]^T
[−cos(kθ),sin(kθ)]T。
* n
Q
∼
N
(
0
,
Q
)
\mathbf{n}\_Q \sim \mathcal{N}(0, Q)
nQ∼N(0,Q) 表示过程噪声。
##### 3. 观测更新
观测值
Z
k
\mathbf{Z}\_k
Zk 为真实状态加上观测噪声：
Z
k
=
X
k
+
n
R
\mathbf{Z}\_k = \mathbf{X}\_k + \mathbf{n}\_R
Zk=Xk+nR
##### 4. 粒子更新与重采样
* **粒子更新**：对于每个粒子，根据相同的运动模型和过程噪声进行更新：
P
i
k
=
P
i
k
−
1
+
d
k
+
n
Q
\mathbf{P}\_i^k = \mathbf{P}\_i^{k-1} + \mathbf{d}\_k + \mathbf{n}\_Q
Pik=Pik−1+dk+nQ
* **权重更新**：更新每个粒子的权重以反映其与新观测的匹配程度：
**自我介绍一下，小编13年上海交大毕业，曾经在小公司待过，也去过华为、OPPO等大厂，18年进入阿里一直到现在。**
**深知大多数Python工程师，想要提升技能，往往是自己摸索成长或者是报班学习，但对于培训机构动则几千的学费，着实压力不小。自己不成体系的自学效果低效又漫长，而且极易碰到天花板技术停滞不前！**
**因此收集整理了一份《2024年Python开发全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升又不知道该从何学起的朋友，同时减轻大家的负担。**
**既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，基本涵盖了95%以上前端开发知识点，真正体系化！**
**由于文件比较大，这里只是将部分目录大纲截图出来，每个节点里面都包含大厂面经、学习笔记、源码讲义、实战项目、讲解视频，并且后续会持续更新**
**如果你觉得这些内容对你有帮助，可以扫码获取！！！（备注Python）**
，也去过华为、OPPO等大厂，18年进入阿里一直到现在。**
**深知大多数Python工程师，想要提升技能，往往是自己摸索成长或者是报班学习，但对于培训机构动则几千的学费，着实压力不小。自己不成体系的自学效果低效又漫长，而且极易碰到天花板技术停滞不前！**
**因此收集整理了一份《2024年Python开发全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升又不知道该从何学起的朋友，同时减轻大家的负担。**
[外链图片转存中...(img-G3qZSgmV-1712826950303)]
[外链图片转存中...(img-CqRKGQf6-1712826950303)]
**既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，基本涵盖了95%以上前端开发知识点，真正体系化！**
**由于文件比较大，这里只是将部分目录大纲截图出来，每个节点里面都包含大厂面经、学习笔记、源码讲义、实战项目、讲解视频，并且后续会持续更新**
**如果你觉得这些内容对你有帮助，可以扫码获取！！！（备注Python）**
<img src="https://img-community.csdnimg.cn/images/fd6ebf0d450a4dbea7428752dc7ffd34.jpg" alt="img" style="zoom:50%;" />
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
22
0- 0
已为社区贡献1条内容
所有评论(0)