粒子滤波器简介与Python实现(1)

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

​1​exp(−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​

• 权重更新：更新每个粒子的权重以反映其与新观测的匹配程度：

w

i

k

=

1

2

π

R

exp

⁡

(

−

∥

P

i

k

−

Z

k

∥

2

2

R

)

w_i^k = \frac{1}{\sqrt{2\pi R}} \exp\left(-\frac{|\mathbf{P}_i^k - \mathbf{Z}_k|^2}{2R}\right)

wik​=2πR

​1​exp(−2R∥Pik​−Zk​∥2​)

• 权重归一化：确保所有权重之和为 1：

w

i

k

=

w

i

k

∑

j

=

1

N

w

j

k

w_i^k = \frac{w_i^{k}{\sum_{j=1}}N w_j^k}

wik​=∑j=1N​wjk​wik​​

• 重采样：根据权重 (w_i^k) 从当前粒子集中选择 (N) 个粒子，高权重的粒子有更高概率被选中多次。

5. 状态估计

粒子群中心，即所有粒子的加权平均位置，给出了对系统状态的估计：

X

^

k

=

∑

i

=

1

N

w

i

k

P

i

k

\mathbf{\hat{X}}_k = \sum_{i=1}^N w_i^k \mathbf{P}_i^k

X^k​=∑i=1N​wik​Pik​

通过这一序列的步骤，粒子滤波利用观测数据来连续更新对系统状态的估计，同时考虑了过程和观测的不确定性。

结论

粒子滤波器是一种非常灵活和强大的工具，尤其适用于处理非线性和非高斯问题。通过本例的模拟，我们可以看到，即使在存在噪声的情况下，粒子滤波器也能有效地跟踪和估计系统的状态。随着粒子数量的增加，估计的准确性通常会提高，但同时也会增加计算的复杂性。因此，在实际应用中，需要根据问题的特性和计算资源，适当选择粒子数量。

如何应用

要整合WiFi定位和PDR（Pedestrian Dead Reckoning）数据使用粒子滤波进行室内定位，整个过程可以概括为以下几个步骤：

1. 导入必要的库

import numpy as np
import matplotlib.pyplot as plt
from math import cos, sin, sqrt, pi

2. 定义获取WiFi位置和PDR步长与方向的函数

def get\_wifi\_position():
    # 此函数应返回基于WiFi定位的当前位置估计（x, y）
    # 例如，返回 (10, 20)，在实际应用中，这里会根据WiFi信号强度来计算
    return 10, 20

def get\_pdr\_step():
    # 此函数应返回基于PDR的步长和方向（距离, 方向角度）
    # 例如，返回步长为0.5米，方向为0度（向北）
    return 0.5, 0

3. 初始化参数和状态

N = 200  # 粒子总数
Q = np.diag([0.1, 0.1])\*\*2  # 过程噪声协方差矩阵，调整以匹配PDR步进的不确定性
R = np.diag([2, 2])\*\*2  # 测量噪声协方差矩阵，调整以匹配WiFi定位精度
T = 50  # 测量时间步数
WorldSize = 100  # 模拟环境的大小

X = np.zeros((2, T))  # 系统状态
Z = np.zeros((2, T))  # 观测状态
P = np.random.rand(2, N) \* WorldSize  # 粒子位置
w = np.zeros(N)  # 粒子权重

4. 粒子滤波主循环

for k in range(1, T):
    wifi_x, wifi_y = get_wifi_position()
    step_length, step_direction = get_pdr_step()
    
    # 更新粒子群
    for i in range(N):
        dx = step_length \* cos(step_direction) + np.random.normal(0, sqrt(Q[0,0]))
        dy = step_length \* sin(step_direction) + np.random.normal(0, sqrt(Q[1,1]))
        P[:, i] += np.array([dx, dy])
        
        # 计算每个粒子与WiFi位置的距离
        dist = sqrt((P[0, i] - wifi_x)\*\*2 + (P[1, i] - wifi_y)\*\*2)
        w[i] = (1 / sqrt(2 \* pi \* R[0,0])) \* np.exp(-dist\*\*2 / (2 \* R[0,0]))
        
    # 归一化权重
    w /= np.sum(w)
    
    # 重采样
    indices = np.random.choice(N, N, p=w)
    P = P[:, indices]
    
    # 计算并保存粒子群中心为系统状态的估计
    X[:, k] = np.average(P, axis=1, weights=w)

5. 结果可视化（可选）

plt.figure()
plt.plot(X[0, :], X[1, :], label='Estimated Trajectory')
plt.scatter(wifi_x, wifi_y, c='r', label='WiFi Position')
plt.title('Particle Filter for WiFi-PDR Fusion Indoor Positioning')
plt.xlabel('X position')
plt.ylabel('Y position')
plt.legend()
plt.show()

请注意，这里的get_wifi_position()和get_pdr_step()函数需要根据实际情况来实现，比如，从传感器或之前处理好的数据中获取信息。此外，过程噪声Q和测量噪声R的设定需要根据实际应用中PDR和WiFi定位的精确度来调整。

自我介绍一下，小编13年上海交大毕业，曾经在小公司待过，也去过华为、OPPO等大厂，18年进入阿里一直到现在。

深知大多数Python工程师，想要提升技能，往往是自己摸索成长或者是报班学习，但对于培训机构动则几千的学费，着实压力不小。自己不成体系的自学效果低效又漫长，而且极易碰到天花板技术停滞不前！

因此收集整理了一份《2024年Python开发全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升又不知道该从何学起的朋友，同时减轻大家的负担。

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，基本涵盖了95%以上前端开发知识点，真正体系化！

由于文件比较大，这里只是将部分目录大纲截图出来，每个节点里面都包含大厂面经、学习笔记、源码讲义、实战项目、讲解视频，并且后续会持续更新

如果你觉得这些内容对你有帮助，可以扫码获取！！！（备注Python）

好的数据中获取信息。此外，过程噪声Q和测量噪声R的设定需要根据实际应用中PDR和WiFi定位的精确度来调整。

自我介绍一下，小编13年上海交大毕业，曾经在小公司待过，也去过华为、OPPO等大厂，18年进入阿里一直到现在。

深知大多数Python工程师，想要提升技能，往往是自己摸索成长或者是报班学习，但对于培训机构动则几千的学费，着实压力不小。自己不成体系的自学效果低效又漫长，而且极易碰到天花板技术停滞不前！

因此收集整理了一份《2024年Python开发全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升又不知道该从何学起的朋友，同时减轻大家的负担。

[外链图片转存中…(img-mXYm62Z7-1713256116313)]

[外链图片转存中…(img-LUx7rtp7-1713256116314)]

[外链图片转存中…(img-DzYQ6Kjd-1713256116314)]

[外链图片转存中…(img-Icmq8GwZ-1713256116314)]

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，基本涵盖了95%以上前端开发知识点，真正体系化！

由于文件比较大，这里只是将部分目录大纲截图出来，每个节点里面都包含大厂面经、学习笔记、源码讲义、实战项目、讲解视频，并且后续会持续更新

如果你觉得这些内容对你有帮助，可以扫码获取！！！（备注Python）