Python 粒子滤波算法剔除轨迹异常点

下面是使用粒子滤波算法找出并剔除异常点的 Python 代码：

import numpy as np

def particle_filter(x, y, t, num_particles=100, threshold=3):
    # 初始化粒子的权重
    weights = np.ones(num_particles) / num_particles
    
    # 初始化粒子的状态
    particles = np.column_stack((x, y))
    
    # 初始化滤波结果
    filtered_x = []
    filtered_y = []
    filtered_t = []
    
    for i in range(len(x)):
        # 计算每个粒子的观测值和实际值之间的误差
        errors = np.linalg.norm(particles - [x[i], y[i]], axis=1)
        
        # 计算粒子的权重
        weights *= np.exp(-0.5 * (errors / threshold) ** 2)
        
        # 归一化粒子的权重
        weights /= np.sum(weights)
        
        # 重采样
        indices = np.random.choice(num_particles, size=num_particles, replace=True, p=weights)
        particles = particles[indices]
        
        # 更新滤波结果
        filtered_x.append(np.mean(particles[:, 0]))
        filtered_y.append(np.mean(particles[:, 1]))
        filtered_t.append(t[i])
    
    return filtered_x, filtered_y, filtered_t

# 输入数据
x = [0.0, 1.0, 3.0, 4.0, 7.0, 8.0, 13.0, 9.0, 11.0, 11.0]
y = [0.0, 2.0, 5.0, 5.0, 5.0, 4.0, 20.0, 3.0, 1.0, 0.0]
t = [0.0, 0.1, 0.3, 0.4, 0.7, 0.8, 0.9, 1.0, 1.2, 1.3]

# 使用粒子滤波算法剔除异常点
filtered_x, filtered_y, filtered_t = particle_filter(x, y, t)

# 输出滤波结果
print('Filtered X:', filtered_x)
print('Filtered Y:', filtered_y)
print('Filtered T:', filtered_t)

上述代码使用粒子滤波算法对给定的轨迹点数据进行滤波，剔除了可能的异常点。其中，num_particles参数表示粒子的数量，threshold参数表示异常点的阈值。输出结果为滤波后的x坐标、y坐标和时间。