一维粒子滤波python代码
时间: 2023-10-13 09:27:58 浏览: 50
以下是一维粒子滤波的python代码示例:
```python
import numpy as np
import matplotlib.pyplot as plt
def motion_model(x, u):
# 一维运动模型
return x + u + np.random.normal(0, 1)
def sensor_model(x, z):
# 一维测量模型
return np.exp(-(x-z)**2/2) / np.sqrt(2*np.pi)
def resampling(particles, weights):
# 重采样
N = len(particles)
indexes = np.zeros(N, dtype=int)
cumulative_sum = np.cumsum(weights)
i, j = 0, 0
while i < N and j < N:
if np.random.rand() < cumulative_sum[j]:
indexes[i] = j
i += 1
else:
j += 1
return particles[indexes], np.ones(N) / N
def main():
# 初始化粒子
N = 100
particles = np.random.normal(0, 1, N)
weights = np.ones(N) / N
# 运动和测量模型
u = 1
z = 2
# 粒子滤波
for i in range(10):
# 运动模型
particles = motion_model(particles, u)
# 权重计算
weights *= sensor_model(particles, z)
weights /= np.sum(weights)
# 重采样
particles, weights = resampling(particles, weights)
# 结果可视化
plt.hist(particles, bins=20, weights=weights)
plt.show()
if __name__ == '__main__':
main()
```
在这个示例中,我们使用了一维的运动模型和测量模型,并且每次迭代都进行一次重采样。最后,我们将粒子的分布可视化为一个直方图。