粒子滤波python代码

以下是一个简单的粒子滤波Python代码示例：

import numpy as np
import matplotlib.pyplot as plt

# 真实状态方程
def f(x):
    return 0.5*x + 25*x/(1+x**2) + 8*np.cos(1.2*x)

# 观测方程
def h(x):
    return x**2/20

# 生成观测数据
def generate_data(N, std):
    xs = np.empty(N)
    ys = np.empty(N)
    for i in range(N):
        xs[i] = i+1
        ys[i] = h(f(xs[i])) + np.random.normal(0, std)
    return xs, ys

# 粒子滤波
def particle_filter(N, ys, std):
    # 初始化粒子权重和状态
    particles = np.random.uniform(-10, 10, size=N)
    weights = np.ones(N) / N
    xs = np.empty(len(ys))
    
    for i, y in enumerate(ys):
        # 重采样
        idx = np.random.choice(N, size=N, p=weights)
        particles = particles[idx]
        weights = np.ones(N) / N
        
        # 状态转移
        particles = f(particles) + np.random.normal(0, std, size=N)
        
        # 权重更新
        weights *= np.exp(-0.5*(y-h(particles))**2/std**2)
        weights /= np.sum(weights)
        
        # 估计状态
        xs[i] = np.sum(weights*particles)
    
    return xs

# 生成观测数据
xs, ys = generate_data(50, 10)

# 粒子滤波
xs_pf = particle_filter(1000, ys, 10)

# 绘制结果
plt.plot(xs, ys, 'ro', label='Observations')
plt.plot(xs, f(xs), 'g-', label='True State')
plt.plot(xs, xs_pf, 'b-', label='Particle Filter')
plt.legend()
plt.show()

在上面的代码中，我们首先定义了真实状态方程和观测方程。然后使用这些方程来生成观测数据。接下来，我们使用粒子滤波算法对这些观测数据进行滤波，并计算出每个时刻的状态估计。最后，我们将结果可视化。