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

最新推荐文章于 2024-08-17 13:43:38 发布

2401_83817024

最新推荐文章于 2024-08-17 13:43:38 发布

阅读量913

点赞数 17

分类专栏：程序员文章标签： python 线性代数机器学习

本文链接：https://blog.csdn.net/2401_83817024/article/details/137645903

版权

程序员专栏收录该内容

380 篇文章 1 订阅

订阅专栏

\mathbf{P}_i^0

Pi0，每个粒子随机分布在定义的空间内。对于每个粒子，计算其权重

w_i^0

wi0 以反映粒子与初始观测的匹配度：

exp

⁡

(

−

∥

−

∥

)

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)，系统的真实状态更新为：

−

\mathbf{X}_k = \mathbf{X}_{k-1} + \mathbf{d}_k + \mathbf{n}_Q

Xk=Xk−1+dk+nQ

\mathbf{d}_k

dk 表示根据运动模型预计的状态变化，例如距离

distance

\text{distance}

distance 乘以方向

[

−

cos

⁡

(

)

sin

⁡

(

)

]

[-\cos(k\theta), \sin(k\theta)]^T

[−cos(kθ),sin(kθ)]T。

∼

(

)

\mathbf{n}_Q \sim \mathcal{N}(0, Q)

nQ∼N(0,Q) 表示过程噪声。

3. 观测更新

观测值

\mathbf{Z}_k

Zk 为真实状态加上观测噪声：

\mathbf{Z}_k = \mathbf{X}_k + \mathbf{n}_R

Zk=Xk+nR

4. 粒子更新与重采样

粒子更新：对于每个粒子，根据相同的运动模型和过程噪声进行更新：

−

\mathbf{P}_i^k = \mathbf{P}_i^{k-1} + \mathbf{d}_k + \mathbf{n}_Q

Pik=Pik−1+dk+nQ

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

exp

⁡

(

−

∥

−

∥

)

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

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

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

∑

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

wik=∑j=1Nwjkwik

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

5. 状态估计

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

∑

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

X^k=∑i=1NwikPik

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

结论

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

如何应用

要整合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开发全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升又不知道该从何学起的朋友，同时减轻大家的负担。