如何使用Savitzky-Golay滤波器进行轨迹平滑

一、Savitzky-Golay滤波器介绍

Savitzky-Golay滤波器是一种数字滤波器，用于平滑数据，特别是在信号处理中。它基于最小二乘法的思想，通过拟合数据到一个滑动窗口内的低阶多项式来实现平滑。这种滤波器的优点是它可以保留数据的高频信息（即峰值），这在许多应用中是非常重要的。

滤波器的工作原理

1. 选择一个窗口大小（通常是奇数），以及要拟合的多项式的阶数。
2. 对于数据中的每个点，将该点及其窗口内的邻居拟合到一个多项式（例如线性、二次、三次等）。
3. 使用该多项式在窗口中心的值替换原始数据点的值。
4. 将窗口向前移动一个数据点，并重复步骤2和3，直到处理完所有的数据点。

Savitzky-Golay滤波器的主要优点

• 它可以有效地平滑噪声数据，同时保留数据的形状特征，如峰值和宽度。
• 它是一种线性滤波器，计算效率高。
• 它的结果可以很好地逼近原始信号的导数，这对于找出数据的最大值和最小值等特征非常有用。

然而，需要注意的是，Savitzky-Golay滤波器对于数据边缘的处理可能会产生不理想的效果，因为在数据边缘，滤波器窗口内可能没有足够的点来拟合多项式。此外，如果数据中有大的突变或异常值，滤波器可能也会受到影响。

二、如何使用Savitzky-Golay滤波器进行轨迹平滑

import numpy as np
from scipy.signal import savgol_filter

# 读取轨迹文件
data = np.loadtxt('trajectory.txt', delimiter=',')

# 提取index, x, y, theta, default
index = data[:, 0]
x = data[:, 1]
y = data[:, 2]
theta = data[:, 3]
default = data[:, 4]

# 定义滤波器窗口大小和多项式阶数
window_size, poly_order = 51, 3

# 对x, y, theta进行滤波
x_smooth = savgol_filter(x, window_size, poly_order)
y_smooth = savgol_filter(y, window_size, poly_order)
theta_smooth = savgol_filter(theta, window_size, poly_order)

# 输出平滑后的轨迹
smooth_data = np.column_stack((index, x_smooth, y_smooth, theta_smooth, default))
np.savetxt('smooth_trajectory.txt', smooth_data, delimiter=',')

上述代码读取trajectory.txt轨迹文件，然后提取出了每个点的x, y, theta值。然后，它使用Savitzky-Golay滤波器对x, y, theta进行了平滑，并将平滑后的结果保存在smooth_trajectory.txt中。