粒子滤波代码pytorch

以下一个使用PyTorch实现粒子滤波的示例代码：

import torch
import numpy as np

# 初始化粒子
def init_particles(num_particles):
    particles = torch.randn(num_particles, 1)
    weights = torch.ones(num_particles) / num_particles
    return particles, weights

# 更新粒子
def update_particles(particles, weights, measurement_noise):
    particles += measurement_noise * torch.randn_like(particles)
    weights.fill_(1.0)

# 计算权重
def compute_weights(particles, weights, measurement, measurement_noise):
    likelihood = torch.exp(-0.5 * ((particles - measurement) / measurement_noise) ** 2)
    weights *= likelihood / torch.sum(likelihood)

# 重采样
def resample(particles, weights):
    indices = torch.multinomial(weights, len(weights), replacement=True)
    particles.copy_(particles[indices])
    weights.fill_(1.0 / len(weights))

# 粒子滤波主程序
def particle_filter(num_particles, T, measurement_noise):
    particles, weights = init_particles(num_particles)

    for t in range(T):
        # 测量
        measurement = torch.tensor([t], dtype=torch.float32)

        # 更新粒子状态
        update_particles(particles, weights, measurement_noise)

        # 计算权重
        compute_weights(particles, weights, measurement, measurement_noise)

        # 重采样
        resample(particles, weights)

        # 输出估计值
        estimated_state = torch.mean(particles)
        print("时刻 {}: 估计值 = {}".format(t, estimated_state.item()))

# 运行粒子滤波
num_particles = 1000  # 粒子数
T = 10  # 时间步数
measurement_noise = 0.1  # 测量噪声
particle_filter(num_particles, T, measurement_noise)