这段代码实现了一个One Euro Filter（一欧元滤波器）。
1. 原理简述
One Euro Filter是一种主要用于人机交互（如 VR 手柄追踪、光标移动、手势识别）的算法。它解决了一个核心矛盾：抖动（Jitter）与延迟（Lag/Latency）之间的权衡。
问题：
如果你为了让信号平滑（去抖动）而使用强滤波，会导致画面跟不上手的移动（高延迟）。
如果你为了响应快（低延迟）而减少滤波，画面会不停晃动（高抖动）。
解决方案：
动态调整截止频率。
当速度慢时（手停在半空）：认为用户想精确定位，增加滤波强度（降低截止频率），消除抖动。
当速度快时（手快速挥动）：认为用户关注的是速度和方向，减少滤波强度（提高截止频率），消除延迟。
import numpy as np class OneEuroFilter: def __init__( self, min_cutoff=1.0, # 最小截止频率 (Hz)。值越小，静止时越平滑（抖动越少），但迟滞感越强。 beta=0.0, # 速度系数。值越大，快速移动时响应越快（延迟越低），但可能引入高频噪声。 d_cutoff=1.0, # 导数（速度）的截止频率 (Hz)。用于平滑速度的计算，通常设为 1.0Hz。 ): """初始化 One Euro Filter，支持 N 维 Numpy 数组（注释中提到的 14 维只是示例）。""" self.min_cutoff = float(min_cutoff) self.beta = float(beta) self.d_cutoff = float(d_cutoff) # 用于存储数据的形状，确保后续输入数据维度一致 self.data_shape = None # 初始化上一帧的状态：时间、信号值、导数（速度） self.t_prev = None # 上一次的时间戳 self.x_prev = None # 上一次过滤后的信号值 self.dx_prev = None # 上一次过滤后的导数（速度） # 指定平滑函数，通常是指数平滑算法 self.smoothing_fn = exponential_smoothing def next(self, t, x, dx0=None): """ 计算下一帧的过滤信号。 参数: t: 当前时间戳 (秒) x: 当前原始信号值 (Numpy array) dx0: (可选) 初始速度 """ # --- 1. 初始化阶段 --- # 如果是第一次调用（没有上一帧数据），直接返回当前原始值作为初始状态 if self.t_prev is None: self.data_shape = x.shape # 记录数据维度 self.t_prev = float(t) # 记录当前时间 self.x_prev = np.array(x, dtype=float) # 记录当前值 # 初始化速度（导数），如果没有提供 dx0，则默认为 0 if dx0 is None: self.dx_prev = np.zeros_like(x) else: self.dx_prev = np.array(dx0, dtype=float) return x # --- 2. 数据检查 --- # 确保传入的数据维度没有发生改变 if x.shape != self.data_shape: raise ValueError("Unexpected data shape") # --- 3. 计算时间间隔 --- # t_e (Time Elapsed): 当前帧与上一帧的时间差 t_e = t - self.t_prev # --- 4. 计算并平滑速度 (Derivative) --- # 这一步是为了获取平滑的“速度”，用来动态调整后续的截止频率。 # 计算速度滤波的 alpha 值 (平滑因子) # d_cutoff 是固定的，通常设为 1Hz，用于避免速度计算本身抖动过大 a_d = smoothing_factor(t_e, self.d_cutoff) # 计算原始速度：(当前位置 - 上次位置) / 时间间隔 dx = (x - self.x_prev) / t_e # 对速度进行指数平滑 # dx_hat 即为“估计速度” dx_hat = self.smoothing_fn(a_d, dx, self.dx_prev) # --- 5. 计算并平滑主信号 (Signal) --- # 这是 One Euro Filter 的核心：动态计算 cutoff (截止频率) # 公式：cutoff = min_cutoff + beta * |速度| # 速度越快，cutoff 越高，滤波越弱，延迟越低。 # 速度越慢，cutoff 越接近 min_cutoff，滤波越强，越稳。 cutoff = self.min_cutoff + self.beta * np.abs(dx_hat) # 根据动态计算出的 cutoff 计算主信号的 alpha 值 a = smoothing_factor(t_e, cutoff) # 对主信号进行指数平滑 x_hat = self.smoothing_fn(a, x, self.x_prev) # --- 6. 更新状态 --- # 将当前计算出的平滑值和时间保存，供下一帧使用 self.x_prev = x_hat self.dx_prev = dx_hat self.t_prev = t return x_hat补充缺失的辅助函数
代码片段中调用了 smoothing_factor 和 exponential_smoothing，在标准的 One Euro Filter 实现中，它们的逻辑如下：
import numpy as np import math def smoothing_factor(t_e, cutoff): """ 计算指数平滑的 alpha 值 (0 <= alpha <= 1)。 alpha 决定了新数据在结果中的占比。 原理公式: tau = 1 / (2 * pi * cutoff) alpha = 1 / (1 + tau / t_e) """ r = 2 * math.pi * cutoff * t_e return r / (r + 1) def exponential_smoothing(a, x, x_prev): """ 执行指数移动平均 (Exponential Moving Average)。 公式: x_hat = alpha * x_raw + (1 - alpha) * x_prev a: alpha (平滑因子) x: 当前原始输入 x_prev: 上一次的平滑输出 """ return a * x + (1 - a) * x_prev总结：参数如何调节？
在使用这个类时，调节 min_cutoff 和 beta 是关键：
先调 min_cutoff (保持 beta = 0)：
将物体保持静止，观察输出数据。
如果数据还在抖动，减小min_cutoff。
直到静止时的抖动在你可接受的范围内（此时延迟会比较大，但在下一步解决）。
再调 beta：
快速移动物体。
如果感觉跟随有明显的滞后（延迟），增加beta。
增加 beta 会让高速运动时的截止频率变大，从而减少延迟。但如果 beta 太大，高速运动结束时可能会有“过冲”或微小抖动。
这个算法非常适合处理鼠标光标、VR 头显、Kinect 骨骼数据等实时连续信号。
这段代码定义了一个LPRotationFilter (低通旋转滤波器)类。
它的主要作用是对旋转数据（四元数或旋转矩阵）进行平滑处理，消除传感器（如 VR 手柄、机械臂末端、摄像头）带来的高频抖动，使旋转动作看起来更平稳。
import numpy as np from scipy.spatial.transform import Rotation as R # 假设外部定义的旋转平滑函数（通常是球面线性插值 SLERP 或归一化线性插值 NLERP） # def rotational_exponential_smoothing(alpha, current, previous): ... class LPRotationFilter: """ 来源: https://github.com/Dingry/bunny_teleop_server/... 这是一个针对旋转数据的低通滤波器 (Low Pass Filter)。 """ def __init__(self, alpha): """ 初始化滤波器。 参数: alpha: 平滑系数 (0 < alpha <= 1)。 alpha 越小，平滑程度越高，抗抖动越强，但延迟越大。 alpha 越大，响应越快，延迟越小，但抖动可能保留。 """ self.alpha = alpha # 保存平滑系数 self.is_init = False # 标记是否已经初始化（是否接收过第一帧数据） self.y = None # 保存上一帧的滤波结果 (y 代表 output) def next(self, x: np.ndarray): """ 输入当前的四元数，返回平滑后的四元数。 参数: x: 当前帧的旋转四元数，形状必须是 (4,)，即 [x, y, z, w] 或 [w, x, y, z] """ # 强制检查输入形状是否为 4 维向量（四元数标准形状） assert x.shape == (4,) # --- 初始化逻辑 --- # 如果是第一次运行，没有“上一帧”，直接把当前帧作为初始状态 if not self.is_init: self.y = x # 记录当前值 self.is_init = True # 标记已初始化 return self.y.copy() # 返回副本，防止外部修改影响内部状态 # --- 滤波逻辑 --- # 调用外部的旋转指数平滑函数。 # 原理类似：y_new = alpha * x_current + (1 - alpha) * y_prev # 但因为是四元数，不能直接加减，需要用球面插值 (SLERP/NLERP) 处理。 self.y = rotational_exponential_smoothing(self.alpha, x, self.y) # 返回平滑后的四元数副本 return self.y.copy() def next_mat(self, x: np.ndarray): """ 输入旋转矩阵，返回平滑后的旋转矩阵。 这是一个 wrapper (包装器)，方便直接处理矩阵格式的数据。 参数: x: 3x3 旋转矩阵 或 4x4 变换矩阵 """ # 检查输入形状是否合法 assert x.shape == (3, 3) or x.shape == (4, 4) # 如果输入是 4x4 矩阵（通常是齐次变换矩阵），只取左上角 3x3 的旋转部分 if x.shape == (4, 4): x = x[:3, :3] # 1. 转换：矩阵 (Matrix) -> 四元数 (Quaternion) # 使用 scipy 的 Rotation 库进行转换，因为四元数更适合做插值平滑 x = R.from_matrix(x).as_quat() # 2. 滤波：调用上面的 next() 方法对四元数进行平滑 next_x_quat = self.next(x) # 3. 还原：四元数 (Quaternion) -> 矩阵 (Matrix) # 将平滑后的四元数转回旋转矩阵返回 return R.from_quat(next_x_quat).as_matrix() def reset(self): """重置滤波器状态，清除历史数据""" self.y = None self.is_init = False这段代码定义了一个LPRotationFilter (低通旋转滤波器)类。
它的主要作用是对旋转数据（四元数或旋转矩阵）进行平滑处理，消除传感器（如 VR 手柄、机械臂末端、摄像头）带来的高频抖动，使旋转动作看起来更平稳。
1. 代码逐行注释与解释
为了代码能运行，我们假设代码中隐含引用了 scipy.spatial.transform.Rotation (简称 R) 以及一个外部函数 rotational_exponential_smoothing。
import numpy as np from scipy.spatial.transform import Rotation as R # 假设外部定义的旋转平滑函数（通常是球面线性插值 SLERP 或归一化线性插值 NLERP） # def rotational_exponential_smoothing(alpha, current, previous): ... class LPRotationFilter: """ 来源: https://github.com/Dingry/bunny_teleop_server/... 这是一个针对旋转数据的低通滤波器 (Low Pass Filter)。 """ def __init__(self, alpha): """ 初始化滤波器。 参数: alpha: 平滑系数 (0 < alpha <= 1)。 alpha 越小，平滑程度越高，抗抖动越强，但延迟越大。 alpha 越大，响应越快，延迟越小，但抖动可能保留。 """ self.alpha = alpha # 保存平滑系数 self.is_init = False # 标记是否已经初始化（是否接收过第一帧数据） self.y = None # 保存上一帧的滤波结果 (y 代表 output) def next(self, x: np.ndarray): """ 输入当前的四元数，返回平滑后的四元数。 参数: x: 当前帧的旋转四元数，形状必须是 (4,)，即 [x, y, z, w] 或 [w, x, y, z] """ # 强制检查输入形状是否为 4 维向量（四元数标准形状） assert x.shape == (4,) # --- 初始化逻辑 --- # 如果是第一次运行，没有“上一帧”，直接把当前帧作为初始状态 if not self.is_init: self.y = x # 记录当前值 self.is_init = True # 标记已初始化 return self.y.copy() # 返回副本，防止外部修改影响内部状态 # --- 滤波逻辑 --- # 调用外部的旋转指数平滑函数。 # 原理类似：y_new = alpha * x_current + (1 - alpha) * y_prev # 但因为是四元数，不能直接加减，需要用球面插值 (SLERP/NLERP) 处理。 self.y = rotational_exponential_smoothing(self.alpha, x, self.y) # 返回平滑后的四元数副本 return self.y.copy() def next_mat(self, x: np.ndarray): """ 输入旋转矩阵，返回平滑后的旋转矩阵。 这是一个 wrapper (包装器)，方便直接处理矩阵格式的数据。 参数: x: 3x3 旋转矩阵 或 4x4 变换矩阵 """ # 检查输入形状是否合法 assert x.shape == (3, 3) or x.shape == (4, 4) # 如果输入是 4x4 矩阵（通常是齐次变换矩阵），只取左上角 3x3 的旋转部分 if x.shape == (4, 4): x = x[:3, :3] # 1. 转换：矩阵 (Matrix) -> 四元数 (Quaternion) # 使用 scipy 的 Rotation 库进行转换，因为四元数更适合做插值平滑 x = R.from_matrix(x).as_quat() # 2. 滤波：调用上面的 next() 方法对四元数进行平滑 next_x_quat = self.next(x) # 3. 还原：四元数 (Quaternion) -> 矩阵 (Matrix) # 将平滑后的四元数转回旋转矩阵返回 return R.from_quat(next_x_quat).as_matrix() def reset(self): """重置滤波器状态，清除历史数据""" self.y = None self.is_init = False2. 原理解析
这个类的核心原理结合了指数移动平均 (EMA)和四元数几何 (Quaternion Geometry)。
A. 为什么要用这个类？(One Euro Filter vs LP Filter)
这只是一个简单的低通滤波器 (Low Pass Filter, LPF)，它的 alpha 是固定的。
对比上一条的 One Euro Filter：One Euro Filter 会根据速度动态调整 alpha。而这个 LPRotationFilter 的 alpha 是恒定的。
适用场景：适合噪声比较稳定，或者对延迟要求不是极其苛刻的场景。
因此，代码中调用的 rotational_exponential_smoothing 内部通常实现的是NLERP (Normalized Linear Interpolation)或SLERP (Spherical Linear Interpolation)。
C. next_mat 的工作流
很多机器人学库（如 numpy, eigen）习惯用 3x3 矩阵或者是 4x4 变换矩阵来表示位姿。
但是矩阵很难直接进行平滑插值（直接对矩阵元素做平均会破坏正交性，导致矩阵不再是旋转矩阵）。
所以标准流程是：
Matrix -> Quaternion: 转成四元数。
Filter: 在四元数空间进行平滑（数学上更健壮）。
Quaternion -> Matrix: 转回矩阵供其他程序使用。
完整代码如下：
import math import numpy as np from numba import jit from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp @jit def smoothing_factor(t_e, cutoff): r = 2 * math.pi * cutoff * t_e return r / (r + 1) @jit def exponential_smoothing(a, x, x_prev): return a * x + (1 - a) * x_prev def rotational_exponential_smoothing(a, x, x_prev): s = Slerp([0, 1], R.from_quat([x_prev, x])) x_hat = s(a) return x_hat.as_quat() class OneEuroFilter: def __init__( self, min_cutoff=1.0, beta=0.0, d_cutoff=1.0, ): """Initialize the one euro filter for a 14-dimensional numpy array.""" self.min_cutoff = float(min_cutoff) self.beta = float(beta) self.d_cutoff = float(d_cutoff) self.data_shape = None self.t_prev = None self.x_prev = None self.dx_prev = None self.smoothing_fn = exponential_smoothing def next(self, t, x, dx0=None): """Compute the filtered signal for a 14-dimensional numpy array.""" if self.t_prev is None: self.data_shape = x.shape self.t_prev = float(t) self.x_prev = np.array(x, dtype=float) if dx0 is None: self.dx_prev = np.zeros_like(x) else: self.dx_prev = np.array(dx0, dtype=float) return x if x.shape != self.data_shape: raise ValueError("Unexpected data shape") t_e = t - self.t_prev # The filtered derivative of the signal a_d = smoothing_factor(t_e, self.d_cutoff) dx = (x - self.x_prev) / t_e dx_hat = self.smoothing_fn(a_d, dx, self.dx_prev) # The filtered signal cutoff = self.min_cutoff + self.beta * np.abs(dx_hat) a = smoothing_factor(t_e, cutoff) x_hat = self.smoothing_fn(a, x, self.x_prev) # Memorize the previous values self.x_prev = x_hat self.dx_prev = dx_hat self.t_prev = t return x_hat class LPRotationFilter: """https://github.com/Dingry/bunny_teleop_server/blob/main/bunny_teleop_server/utils/robot_utils.py""" def __init__(self, alpha): self.alpha = alpha self.is_init = False self.y = None def next(self, x: np.ndarray): assert x.shape == (4,) if not self.is_init: self.y = x self.is_init = True return self.y.copy() self.y = rotational_exponential_smoothing(self.alpha, x, self.y) return self.y.copy() def next_mat(self, x: np.ndarray): """take and return rotation matrix instead of quat""" assert x.shape == (3, 3) or x.shape == (4, 4) if x.shape == (4, 4): x = x[:3, :3] x = R.from_matrix(x).as_quat() next_x_quat = self.next(x) return R.from_quat(next_x_quat).as_matrix() def reset(self): self.y = None self.is_init = False调用例子：
初始化：
self.left_positiion_filter = OneEuroFilter(
min_cutoff=self.config.position_filter.min_cutoff,
beta=self.config.position_filter.beta,
)
self.left_orientation_filter = LPRotationFilter(self.config.orientation_filter.alpha)
调用：
xyzquat = pin.SE3ToXYZQUAT(pose)
t = time.time()
xyzquat[:3] = self.left_positiion_filter.next(t, xyzquat[:3])
xyzquat[3:] = self.left_orientation_filter.next(xyzquat[3:])
