这段代码实现了一个 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_smoothingdef 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，则默认为 0if 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_hatself.dx_prev = dx_hatself.t_prev = treturn 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_ereturn r / (r + 1) def exponential_smoothing(a, x, x_prev):"""执行指数移动平均 (Exponential Moving Average)。公式:x_hat = alpha * x_raw + (1 - alpha) * x_preva: 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 = Noneself.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 = Noneself.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_ereturn 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 = Noneself.t_prev = Noneself.x_prev = Noneself.dx_prev = Noneself.smoothing_fn = exponential_smoothingdef 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.shapeself.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 xif x.shape != self.data_shape:raise ValueError("Unexpected data shape")t_e = t - self.t_prev# The filtered derivative of the signala_d = smoothing_factor(t_e, self.d_cutoff)dx = (x - self.x_prev) / t_edx_hat = self.smoothing_fn(a_d, dx, self.dx_prev)# The filtered signalcutoff = 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 valuesself.x_prev = x_hatself.dx_prev = dx_hatself.t_prev = treturn 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 = alphaself.is_init = Falseself.y = Nonedef next(self, x: np.ndarray):assert x.shape == (4,)if not self.is_init:self.y = xself.is_init = Truereturn 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 = Noneself.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:])
