第 27 章网球感知与轨迹预测——从 Ground Truth 到 EKF 再到残差学习

本章定位：Ch26 建立了网球场的物理底座——坐标系、球体模型、发球命令、弹道空气动力学。但那些都是"仿真侧的物理真相"，控制器不能直接使用。真实部署中，机器人看到的不是 ground truth 状态向量，而是有噪声、有延迟、可能遗漏的传感器观测。本章在 Ch26 的物理底座之上构建感知与轨迹预测管线——从"仿真真值"到"可消费的预测接口"。核心产出不是一个 detector 或一个 filter，而是一个可被 Ch28 控制器直接调用的 PredictorOutput 数据结构：击球点在哪、还剩多少时间、预测有多可信。

前置依赖：Ch26（court-local 坐标系 / 八维发球命令 / BallAerodynamics / 接触物理）、Ch09（Teacher-Student 特权学习）、线性代数（矩阵求逆 / 特征值）、概率论（高斯分布 / 条件概率 / 贝叶斯更新）

关键参考：Kalman 1960（ASME Trans.）· ✅ LATENT（arXiv:2603.12686，dynamics randomization + observation noise 消融）· ✅ HITTER（arXiv:2508.21043，OptiTrack 360 Hz + 解析弹道预测）· ✅ PACE（arXiv:2509.21690，learned predictor + physics predictor 双通道）· 📄 ETH 羽毛球（Science Robotics, 2025，perception-aware training）· 📄 Phybot 人形羽毛球（arXiv:2511.11218，EKF + prediction-free 变体）· 📄 Gray-box 乒乓球预测（arXiv:2305.15189，EKF + 神经旋转估计）· Cho et al. 2014（GRU/Encoder-Decoder, arXiv:1406.1078）

前置自测

📋 答不出 $\ge$ 3 题 → 先回 Ch26 和概率论基础复习

[Ch26] 当前 tennis observation 包含哪些项？各自的 frame 是什么（world vs body）？
[Ch26] 当前 tennis task 有 camera sensor 吗？actions 字典的内容是什么？
[概率] 两个高斯分布的乘积（归一化后）还是高斯吗？均值和方差如何计算？
[线代] Jacobian 矩阵在非线性函数局部线性化中的作用是什么？
[信号] 如果观测延迟 30 ms，球速 30 m/s，延迟导致的位置误差是多少？这个误差相对于球拍有效击球面（直径约 30 cm）意味着什么？

本章目标

学完本章后，你应该能够：

推导 6D 抛体模型的 EKF 预测步和更新步的完整公式，包括含空气阻力的非线性 Jacobian
实现一个完整的 Python EKF 类，能处理漏检帧和弹跳模式切换
解释 Kalman 增益 $K$ 的物理含义——它在"相信预测"和"相信观测"之间自动权衡
设计从 ground truth 到 camera observation 的分层验证路线（信息退化阶梯）
写出 GRU 残差预测器的完整架构和训练流程，解释为什么用物理模型做 baseline 而非端到端学习
实现延迟补偿的数学方法和工程代码
定义给 Ch28 控制器的 PredictorOutput 接口，包含 impact_pos_c、time_to_impact、uncertainty、validity 四个核心字段

27.1 感知问题的本质：为什么"看见球"远远不够 ⭐⭐

这一节解决什么问题：建立对网球感知问题复杂度的正确认知，避免"只要检测到球就能打"的天真假设。

动机：六个让天真方案失败的原因

一个天真的感知方案是：每帧检测球心，相邻两帧差分得速度，线性外推到球拍平面，然后让机器人挥拍。这个方案在慢速桌面物体上可能勉强工作，在网球上几乎一定失败。原因有六个，每一个都与 Ch26 的物理设计直接相关。

原因一：球极小。 回顾 Ch26：网球半径 0.033 m，在距离 10 m 的相机图像中可能只有几个像素。图像检测抖动一个像素，反投影到三维空间可能就是几厘米甚至十几厘米的误差。这与工业视觉中检测大型工件的情况完全不同——工件占据大量像素，亚像素精度就能达到毫米级定位。

原因二：球极快。 Ch26 中发射速度范围是 20-45 m/s。10 ms 的时间误差对应 0.20-0.45 m 的位移——这已经超过了球拍有效击球面的尺度（直径约 30 cm）。相邻帧之间的球位移很大，如果帧率低（比如 30 fps），差分速度估计会极其嘈杂。

原因三：球有抛物线。 重力让 $z$ 方向速度持续变化（每秒增加 9.81 m/s 的向下速度）。线性外推假设速度恒定，会系统性地错过击球高度——预测点比真实点偏高。飞行 0.5 秒后，纯线性外推的 $z$ 方向误差 $= \frac{1}{2} \times 9.81 \times 0.5^2 \approx 1.23$ m——整个身高都差出来了。

原因四：球会弹跳。 Ch26 验证了球落地后速度方向突变（COR 约 0.73-0.76）。纯平滑模型（如简单移动平均或一阶 EKF）会把弹跳当异常点，在弹跳前后给出完全错误的预测。

原因五：空气动力学残差。 Ch26 的三层空气动力学模型（drag + Magnus）不是精确标定的真实空气动力学——$C_d$ 和 $C_L$ 只是近似值，旋转衰减被忽略。predictor 不能假设模型永远精确——必须有处理建模误差的机制。LATENT 的消融实验直接证明了这一点：去掉 dynamics randomization 后真机成功率从 91% 骤降到 14-29%。

原因六：观测有延迟。 你"看到"的球是过去的球。相机曝光、图像传输、检测推理、滤波更新都需要时间。HITTER 使用 OptiTrack @ 360 Hz 仍然有约 3-5 ms 的感知延迟；使用 RGB 相机的系统延迟可达 30-100 ms。30 m/s 球速 + 30 ms 延迟 = 0.9 m 位置误差——整个球拍都够不到。如果直接把旧观测当当前状态，系统会稳定地"慢半拍"，而且这种"慢"不会在 detector loss 上显现——detector 可以非常准，但它准的是过去。

类比理解：感知问题就像在漆黑中打网球，你只有一个每秒闪几十次的频闪灯。每次灯闪时你看到球在某个位置（观测），但等你处理完这个信息时灯已经闪过了（延迟），而且因为闪光太短你看得不太清（噪声）。更糟的是，球在你没看到的时间里可能弹了一下（模式切换）。你需要的不是"更亮的灯"（更好的 detector），而是"根据灯闪的瞬间推算球现在在哪"（状态估计 + 预测）。这个类比的边界在于：频闪灯的观测是瞬时的，而真实相机的曝光时间造成运动模糊，这是额外的噪声源。

本质洞察：网球感知的产物不是图像框，也不是当前球心，而是带时间戳、带不确定性、可外推到击球窗口的状态估计。如果输出不能被控制器消费，检测精度再好也只是中间指标。控制器需要的是 (impact_pos_c, time_to_impact, uncertainty)，不是 (pixel_x, pixel_y, confidence)。

感知链条的五层分离

将感知问题分成五层，每层有明确的输入输出，层与层之间通过定义良好的接口连接。这种分层的核心价值是：当系统出问题时，你可以逐层定位误差来源——而不是在一个端到端黑箱里猜测"到底哪里错了"。

层	输入	输出	典型误差来源	对应技术
1. 真实状态	MuJoCo/mjlab state	球的位置/速度/角速度	无（ground truth）	`ball.data.root_pos_w`
2. 传感器成像	3D 世界	RGB/depth/segmentation 图像	分辨率、遮挡、噪声	CameraSensorCfg
3. 感知模型	图像	2D/3D 球心、confidence	漏检、误检、定位误差	YOLO / PointTrack
4. 状态估计	多帧观测	位置/速度/协方差	模型误差、延迟	EKF（本章核心）
5. 轨迹预测	当前状态估计	未来击球窗口	空气动力学残差、弹跳	物理模型 + GRU 残差

这种分层的工程价值在于：你可以在任意两层之间插入 ground truth来隔离误差。例如： - 用 ground truth 替代层 1-3 → 直接用真实球状态做 EKF 更新 → 如果 EKF 仍然不准，问题在 EKF 模型 - 用 ground truth 替代层 1-4 → 直接用真实状态做轨迹预测 → 如果预测不准，问题在预测模型 - 这就是 Ch09 Teacher-Student 架构的核心思想——先用 privileged information 建立性能上限，再逐步移除

反事实推理：如果第一天就训练端到端视觉控制（RGB 图像 → 关节动作），当系统打不到球时你无法诊断：是 detector 没检测到球？还是 EKF 估计不准？还是预测模型有误？还是控制器执行不到位？五层分离让你可以逐层替换 ground truth，精确定位是哪一层出了问题。在 PACE（arXiv:2509.21690）的设计中，作者明确使用了"learned predictor for observation augmentation + physics predictor for reward construction"的双通道设计——这正是分层思想的工程实例。

前沿系统的感知方案对比

在进入技术细节之前，先对比前沿系统选择了什么样的感知方案——这些选择背后的权衡值得仔细理解：

系统	感知方案	感知频率	延迟	优势	劣势
LATENT	外部动捕（OptiTrack）	100+ Hz	~5 ms	精度最高	需要外部设备，不可移动
HITTER	外部动捕（OptiTrack $\times$ 9）	360 Hz	~3 ms	毫米级精度	9 台相机，成本极高
ETH 羽毛球	机载立体相机	~30 Hz	~30 ms	自主部署	精度较低，需 perception-aware 训练
PACE	近期球位置历史 → learned predictor（训练侧另有 physics predictor 监督/构造 reward）	—	—	预测器轻量、可 zero-shot 部署	部署时仍依赖外部球位置测量（论文在 Booster T1 上 zero-shot 部署）
Phybot	外部动捕 + EKF	100+ Hz	~5 ms	有滤波平滑	依赖外部设备

关键观察：除了 ETH 羽毛球外，其他系统都依赖外部动捕系统——这在实验室可行但不可移动。ETH 的 perception-aware training 是目前唯一实现了机载感知部署的球类运动系统，但其对象是羽毛球（速度 $\le$ 12 m/s，远慢于网球）。网球的高速（20-45 m/s）对机载感知提出了更高要求——更高帧率、更低延迟、更精确的检测。

对教学项目来说，推荐的路线是：先用 ground truth（仿真真值）验证控制上限 → 再逐步引入噪声和延迟 → 最后考虑机载视觉。本章的大部分内容聚焦于前两个阶段——用 ground truth 和 EKF 建立感知管线，为 Ch28 的控制训练提供可靠的球状态估计。

⚠️ 常见陷阱

⚠️ 思维陷阱：认为"只要 detector mAP 足够高就能打好球" - mAP 衡量的是图像级检测质量，不关心时间连续性、延迟补偿、未来预测 - 一个 mAP 0.95 的 detector 如果有 30 ms 延迟且不做补偿，在 30 m/s 球速下等效于 0.9 m 的位置误差——整个球拍都够不到 - 正确做法：从控制需求反推感知指标——Ch28 需要的是 impact position error < 5 cm 和 time-to-impact error < 10 ms

⚠️ 概念误区：认为 EKF 是万能滤波器 - EKF 依赖过程模型和观测模型的正确性。如果过程模型不含弹跳，EKF 会在弹跳时刻给出完全错误的预测 - 正确做法：先明确 EKF 的过程模型覆盖了哪些物理、忽略了什么，再针对遗漏设计补偿

⚠️ 编程陷阱：混用 world frame 和 body frame 的速度 - Ch26 已经指出 ball_velocity 返回 body frame，而 ball_position 返回 world frame - 后果：body frame 速度随四元数旋转，predictor 会产生系统性偏差 - 正确做法：统一转换为 world-aligned frame（root_lin_vel_w 而非 root_lin_vel_b）

⚠️ 思维陷阱：第一天就训练 RGB-only 端到端控制 - 所有误差混在一起，检测、预测、控制无法分离 - 正确做法：分层验证——先 ground truth 再逐步降级

练习

[估算题] 球速 35 m/s，相机帧率 60 fps。相邻两帧之间球移动了多少米？如果 3D 检测精度为 $\pm$ 2 cm，差分速度估计 $v = \Delta p / \Delta t$ 的绝对误差是多少？相对于 35 m/s 的球速是百分之多少？
[思考题] 为什么本章建议先用 ground truth predictor 再逐步降级到视觉输入，而不是直接训练端到端视觉控制？从 debug 和 Sim2Real 两个角度分析。
[设计题] 参考 PACE 的双通道 predictor 设计（learned predictor + physics predictor），解释为什么训练阶段用 physics predictor 构造 reward 而非用 learned predictor？提示：考虑 reward 的稳定性和准确性需求。

知道了"为什么不能只看球"之后，下一步是定义"看球"的数据格式——什么样的 ground truth 可以作为训练标签，frame 如何统一，时间戳如何关联。

27.2 Ground Truth Label 设计与 Frame 统一 ⭐⭐

这一节解决什么问题：把仿真的 ground truth 转化为结构化的训练 label，统一 frame 约定。

动机：仿真最宝贵的资源是 ground truth

仿真环境最大的优势不是渲染质量或物理精度——而是完美的 ground truth。真实世界中，你永远不知道球的精确位置（动捕系统有噪声、有遮挡、有延迟）。但在仿真中，ball.data.root_pos_w 就是真实位置，精度到浮点数精度。这个 ground truth 是训练感知模型的金矿——但需要正确的格式化才能被使用。

ground truth 不等于可训练的 label——它需要 frame 统一、timestamp 关联、episode 条件标注。

Label 数据规格

字段	类型	含义	是否 privileged	来源
`t`	float	仿真时间（秒）	否	`env.sim_time`
`env_id`	int	并行环境 id	否	索引
`ball_pos_c`	float3	court-local 球心位置	是	`root_pos_w - env_origins`
`ball_vel_w`	float3	world-aligned 线速度	是	`root_lin_vel_w`
`ball_ang_w`	float3	world-aligned 角速度	是	`root_ang_vel_w`
`launch_cmd`	float8	八维发球命令（Ch26）	是（仿真条件）	`LaunchCommand.launch_params`
`is_airborne`	bool	是否在空中	是	`ball_pos_c[2] > threshold`
`has_bounced`	bool	是否已弹跳	是	弹跳检测器
`timestamp`	float	采样时间戳	否	`env.sim_time`

为什么 timestamp 如此重要？ 没有 timestamp 的观测不能补偿延迟。这就像银行交易记录中的时间戳——没有时间信息的交易记录无法对账。如果相机每 20 ms 出一帧，控制每 10 ms 更新一次，predictor 必须知道每个观测对应哪个仿真时间。

Frame 统一

Ch26 已经指出：当前源码中 ball_velocity 返回 body frame，而 ball_position 返回 world frame。这种不一致在 label 生成时必须修正。

# Frame 统一的核心代码
def collect_ground_truth_label(env, env_ids=None):
    """收集 ground truth label，统一到 court-local / world-aligned frame。"""
    if env_ids is None:
        env_ids = torch.arange(env.num_envs, device=env.device)

    ball = env.scene["ball"]

    # 位置：world frame → court-local（减 env_origins）
    ball_pos_w = ball.data.root_pos_w[env_ids, :3]
    ball_pos_c = ball_pos_w - env.scene.env_origins[env_ids]

    # 速度：确保使用 world frame（不是 body frame！）
    ball_vel_w = ball.data.root_lin_vel_w[env_ids, :3]  # ✅ world frame
    # ball_vel_b = ball.data.root_lin_vel_b[env_ids, :3]  # ❌ body frame

    # 角速度：world frame
    ball_ang_w = ball.data.root_ang_vel_w[env_ids, :3]

    # 弹跳检测
    is_airborne = ball_pos_c[:, 2] > 0.05  # 5 cm 以上视为空中
    ball_speed = ball_vel_w.norm(dim=-1)
    has_bounced = ~is_airborne & (ball_speed > 0.5)

    # 时间戳
    timestamp = torch.full((len(env_ids),), env.sim_time, device=env.device)

    return {
        "ball_pos_c": ball_pos_c,
        "ball_vel_w": ball_vel_w,
        "ball_ang_w": ball_ang_w,
        "is_airborne": is_airborne,
        "has_bounced": has_bounced,
        "timestamp": timestamp,
    }

Body frame 到 world frame 的转换：如果你只有 body frame 速度（root_lin_vel_b），需要用 root link 的四元数做旋转：

def body_to_world_velocity(vel_b, quat_wxyz):
    """将 body frame 速度转换为 world frame。

    Args:
        vel_b: [N, 3] body frame velocity
        quat_wxyz: [N, 4] quaternion [w, x, y, z]（MuJoCo 约定）

    Returns:
        vel_w: [N, 3] world frame velocity
    """
    # 从四元数构造旋转矩阵
    w, x, y, z = quat_wxyz[:, 0], quat_wxyz[:, 1], quat_wxyz[:, 2], quat_wxyz[:, 3]
    R = torch.stack([
        1 - 2*(y*y + z*z), 2*(x*y - w*z), 2*(x*z + w*y),
        2*(x*y + w*z), 1 - 2*(x*x + z*z), 2*(y*z - w*x),
        2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x*x + y*y),
    ], dim=-1).reshape(-1, 3, 3)

    # 旋转：v_w = R @ v_b
    vel_w = torch.bmm(R, vel_b.unsqueeze(-1)).squeeze(-1)
    return vel_w

对球体来说，body frame 和 world frame 的差异在视觉上不明显（球是各向同性的），但在数值上——如果球有显著旋转（topspin 200+ rad/s），body frame 速度每步都在旋转坐标系中变化，即使 world frame 速度平滑。predictor 如果不知道这种旋转，会产生看似"随机"但实际有规律的估计误差。

观测历史 buffer 的设计

前沿系统（PACE、ETH 羽毛球）将最近 N 帧球位置作为观测输入。这需要在环境层实现一个 observation history buffer：

class BallObsHistoryBuffer:
    """维护最近 N 帧球状态的滑动窗口。"""

    def __init__(self, num_envs, history_length, obs_dim, device):
        self.history_length = history_length
        self.buffer = torch.zeros(num_envs, history_length, obs_dim, device=device)
        self.timestamps = torch.zeros(num_envs, history_length, device=device)
        self.valid = torch.zeros(num_envs, history_length, dtype=torch.bool, device=device)

    def push(self, obs, timestamp):
        """推入新观测，弹出最旧观测。"""
        # 左移（丢弃最旧）
        self.buffer[:, :-1] = self.buffer[:, 1:].clone()
        self.timestamps[:, :-1] = self.timestamps[:, 1:].clone()
        self.valid[:, :-1] = self.valid[:, 1:].clone()
        # 填入最新
        self.buffer[:, -1] = obs
        self.timestamps[:, -1] = timestamp
        self.valid[:, -1] = True

    def reset(self, env_ids):
        """episode reset 时清空指定环境的历史。"""
        self.buffer[env_ids] = 0.0
        self.timestamps[env_ids] = 0.0
        self.valid[env_ids] = False

    def get_flat(self):
        """返回展平的历史向量 [N, history_length × obs_dim]。"""
        return self.buffer.reshape(self.buffer.shape[0], -1)

PACE 等系统的 learned predictor 都以近期球位置历史作为输入。本书教学项目采用"最近 10 帧球位置（每帧 3D = 30 维输入）→ 预测未来若干步"的设定（这些具体维度是教学选择，PACE 论文的预测器更轻量、输出目标击球点）。这个 buffer 需要在环境的 step() 方法中每步更新，在 reset() 时清空。

⚠️ 常见陷阱

⚠️ 编程陷阱：把 reward 区域当 detector label - Ch26 的 ball_landed_in_service_box 是 Phase 1 merged service-area reward，不是精确空间 label - 正确做法：label 用 ground truth 坐标直接计算几何判定

⚠️ 概念误区：认为 privileged 数据不能用于训练 - privileged 数据是 teacher label。student 只在输入端受限，target 端可以用 privileged - 正确做法：明确标注每个字段是否 privileged——teacher 看 ball_pos_c（精确），student 看 detected_ball_pos（有噪声）

⚠️ 编程陷阱：history buffer 在 reset 时未清空 - 后果：新 episode 开始时 buffer 中残留上个 episode 的球位置 → predictor 基于错误的历史做预测 - 正确做法：在 env.reset() 回调中调用 buffer.reset(env_ids)

练习

[编程题] 用 Python 写 body_to_world_velocity 函数，验证：对单位四元数 $q = (1, 0, 0, 0)$，body frame 和 world frame 速度应该相同。对 $q = (0.707, 0, 0.707, 0)$（绕 y 轴旋转 90°），body frame 的 $(1, 0, 0)$ 应该变成 world frame 的 $(0, 0, -1)$。
[设计题] 扩展 label 表，增加 first_bounce_pos 和 net_crossing_height 字段。说明它们对 Ch28 的 reward 有什么用途。

Isaac Lab 中的 Label 收集对照

在 Isaac Lab 中收集 ground truth label 使用完全相同的 API 接口——root_pos_w、root_lin_vel_w 等属性名称在 mjlab 和 Isaac Lab 中是统一的（这是 manager-based 架构的设计优势）。唯一的差异在于：

# mjlab 中获取 env_origins
env_origins = env.scene.env_origins  # [N, 3]

# Isaac Lab 中获取 env_origins
env_origins = env.scene.env_origins  # 同名同义！

# 两个框架中 label 收集代码完全相同
# 这意味着同一套感知管线代码可以在两个框架中无修改运行

这种统一性是刻意设计的——Ch04 讲解了 manager-based 架构的核心价值之一就是跨框架代码复用。感知管线（EKF、GRU predictor、延迟补偿）完全不依赖物理引擎——它们只接收 (position, velocity, timestamp) 元组，不关心这些数据来自 MuJoCo 还是 PhysX。

label 格式和 frame 约定统一后，接下来对比不同的球状态获取方案——从仿真真值到机载视觉，每种方案的精度、延迟和工程复杂度有什么权衡。

27.3 球状态观测方案对比 ⭐⭐

这一节解决什么问题：系统对比三种观测方案（直接状态 / 外部动捕 / 机载视觉），为教学项目选择合适的方案。

三种观测方案

方案	精度	延迟	硬件需求	适用阶段
直接状态（ground truth）	完美	0	无（仿真内置）	第一阶段：验证控制上限
外部动捕（OptiTrack/Vicon）	亚毫米	3-5 ms	6-9 台高速相机	实验室部署
机载视觉（RGB/depth 相机）	厘米级	20-100 ms	1-2 台相机	自主部署

方案 A：直接状态（仿真阶段推荐）

直接从 ball.data.root_pos_w 读取球的精确状态。这是训练初期唯一推荐的方案——因为它消除了感知层的所有误差，让你专注于验证控制策略本身。

# 方案 A 的 observation term（ground truth）
def ball_state_privileged(env) -> torch.Tensor:
    """返回球的完美状态向量 [pos3 + vel3 + ang_vel3 = 9D]。"""
    ball = env.scene["ball"]
    pos_c = ball.data.root_pos_w[:, :3] - env.scene.env_origins
    vel_w = ball.data.root_lin_vel_w[:, :3]
    ang_w = ball.data.root_ang_vel_w[:, :3]
    return torch.cat([pos_c, vel_w, ang_w], dim=-1)  # [N, 9]

为什么不加噪声？ 因为第一阶段的目标是建立控制上限——在完美感知下，策略能达到什么成功率？如果完美感知下成功率只有 50%，问题在控制而非感知，你不应该去改 detector。这就是 Ch09 Teacher-Student 架构的核心理念——先用 teacher（privileged）确定上限。

方案 B：外部动捕（实验室阶段）

HITTER 使用 9 台 OptiTrack 相机 @ 360 Hz。动捕系统提供球的 3D 位置（通过球上的反光标记），但不直接提供速度——速度需要从位置差分或 EKF 估计。

# 方案 B 的 observation term（模拟动捕观测）
def ball_state_mocap(env, noise_std=0.001) -> torch.Tensor:
    """模拟动捕系统的观测：位置精确 + 小噪声，速度需差分。"""
    ball = env.scene["ball"]
    pos_c = ball.data.root_pos_w[:, :3] - env.scene.env_origins
    # 添加动捕噪声（亚毫米级）
    pos_noisy = pos_c + torch.randn_like(pos_c) * noise_std
    # 动捕不直接提供速度，需要 EKF 估计
    return pos_noisy  # [N, 3]，速度由 EKF 估计

方案 C：机载视觉（自主部署阶段）

ETH 羽毛球是唯一使用机载立体相机的球类运动系统。其核心创新是 perception-aware training——训练时注入模拟真实视觉误差的噪声模型，让策略学会在有噪声观测下工作。

# 方案 C 的 perception-aware noise model（参考 ETH badminton）
def ball_state_vision(env, robot_state) -> torch.Tensor:
    """模拟机载视觉的观测：噪声随距离和机器人运动增大。"""
    ball = env.scene["ball"]
    pos_c = ball.data.root_pos_w[:, :3] - env.scene.env_origins

    # 1. 距离依赖的噪声（远处目标检测更不准）
    robot_head_pos = robot_state[:, :3]  # 假设机器人头部位置
    distance = (pos_c - robot_head_pos).norm(dim=-1, keepdim=True)
    pos_noise_std = 0.01 + 0.05 * (distance / 10.0)  # 近处 1cm，10m 处 6cm

    # 2. 运动模糊噪声（机器人快速运动时相机图像模糊）
    robot_speed = robot_state[:, 3:6].norm(dim=-1, keepdim=True)
    blur_factor = 1.0 + 0.3 * robot_speed  # 速度越快噪声越大

    # 3. 遮挡/漏检（以概率 miss_rate 返回上一帧的观测）
    miss_rate = 0.05  # 5% 漏检率
    detected = torch.rand(env.num_envs, device=env.device) > miss_rate

    # 最终噪声
    total_noise_std = pos_noise_std * blur_factor
    pos_noisy = pos_c + torch.randn_like(pos_c) * total_noise_std

    return pos_noisy, detected  # [N, 3], [N] bool

从 2D 图像到 3D 球心的四条路线

如果最终使用机载视觉，需要把 2D 图像中的检测结果转化为 3D 球心坐标。有四种方法，复杂度递增：

路线	方法	优点	缺点	适用阶段
Depth 反投影	球 mask 内深度 + 相机内参	直接	小球 depth 噪声大	阶段 2-3
多相机三角化	2D ray 求最近点	不需 depth	需同步和标定	阶段 3-4
单目 + EKF	2D center + 运动模型	硬件最简	可观测性差	阶段 4-5
端到端 latent	CNN/GRU → 轨迹	灵活	debug 困难	研究阶段

推荐路线（教学项目）：ground truth → segmentation+depth → RGB detector+EKF。每一步只移除一个 privileged component。

当前 tennis task 没有 camera sensor（Ch26 确认的事实边界）。如果要添加，mjlab 的 CameraSensorCfg 支持三种数据类型：RGB [B, H, W, 3]（球检测输入）、depth [B, H, W, 1]（3D 反投影）、semantic_segmentation [B, H, W, 2]（teacher label）。其中 segmentation 的通道 0 是 object id（球的 id），通道 1 是 MuJoCo object type，背景为 (-1, -1)。segmentation 在仿真中提供完美的球像素 mask，但部署时不可用——因此只作为 teacher signal（Ch09 Teacher-Student 架构）。

# 添加 camera sensor 到 tennis scene（预规划代码）
class TennisVisionSceneCfg(TennisSceneCfg):
    """在球场场景中添加相机。"""

    # 固定相机（裁判视角，俯视球场）
    overhead_camera = CameraSensorCfg(
        prim_path="/World/camera_overhead",
        update_period=0.02,  # 50 Hz
        height=240,
        width=320,
        data_types=["rgb", "depth", "semantic_segmentation"],
        spawn=PinholeCameraCfg(
            focal_length=5.0,
            focus_distance=15.0,
            horizontal_aperture=20.955,
        ),
        offset=CameraSensorCfg.OffsetCfg(
            pos=(0.0, 0.0, 12.0),   # 球场上方 12 m
            rot=(0.0, 0.0, 0.0, 1.0),  # 朝下
        ),
    )

    # 机器人头部相机（机载视觉）
    head_camera = CameraSensorCfg(
        prim_path="/World/robot/head_camera",
        update_period=0.033,  # 30 Hz（真机典型帧率）
        height=480,
        width=640,
        data_types=["rgb", "depth"],
        spawn=PinholeCameraCfg(
            focal_length=3.0,
            horizontal_aperture=20.955,
        ),
        offset=CameraSensorCfg.OffsetCfg(
            pos=(0.05, 0.0, 0.1),  # 头部偏前偏上
        ),
    )

球在图像中有多大？ 一个快速估算：球距相机 15 m，相机 fovy = 45°，图像宽 640 px。视场宽度 = $2 \times 15 \times \tan(22.5°) \approx 12.4$ m。球直径 0.066 m，在图像中占 $0.066/12.4 \times 640 \approx 3.4$ 像素。这意味着球在远距离的图像中只有几个像素——标准的 YOLO 等目标检测器对这么小的目标效果很差。可能需要专门的小目标检测方法（如 event camera，arXiv:2506.07860 使用事件相机实现了 4.5 ms 延迟的乒乓球检测）。

信息退化阶梯

将三种方案排列成"信息退化阶梯"——每一步移除一个 privileged 信号源，观察性能下降：

阶段	输入	移除了什么	预期性能	目的
1	ground truth 球状态	无	upper bound	控制上限
2	ground truth 位置 + 差分速度	直接速度 → 差分速度	轻微下降	速度估计影响
3	ground truth 位置 + 延迟	零延迟 → 有延迟	明显下降	延迟影响量化
4	ground truth 位置 + 噪声 + 延迟	完美定位 → 有噪声	进一步下降	噪声影响量化
5	动捕位置 + EKF 速度	ground truth → 动捕	接近阶段 4	动捕足够吗
6	RGB 视觉 + EKF	动捕 → 机载视觉	显著下降	视觉方案可行性

每退一步记录性能下降。如果阶段 3 的性能下降 > 30%（说明延迟是主要瓶颈），你应该投入精力在延迟补偿上，而非改进 detector。如果阶段 4 和阶段 3 差不多（说明噪声不是瓶颈），你不需要更好的 detector。

本质洞察：仿真中的 privileged signal 不是作弊，而是把问题拆开的工具。先用真值建立上限，再逐步移除真值，才能知道性能掉在哪里。这和 Ch25 中"Phase 0-2 验证流程"的思想完全一致。

⚠️ 常见陷阱

⚠️ 思维陷阱：第一天就集成 RGB 视觉 - 后果：所有误差混在一起，无法定位问题 - 正确做法：从 ground truth 开始，按信息退化阶梯逐步降级

⚠️ 编程陷阱：仿真中的 PTZ 相机瞬间跟踪球心 - 后果：等于给了不真实的 privileged tracking——真实相机云台有转速和延迟 - 正确做法：第一版不做 PTZ；如果做必须加转速限制和延迟

练习

[设计题] 设计退化阶梯实验方案。每步移除什么 privileged signal？用什么指标（不是 reward，而是 impact point error 和 time-to-impact error）衡量下降？
[估算题] 球距相机 15 m，fovy=45 度，图像宽 160 px。球在图像中占多少像素？提示：视场宽度 = 2 $\times$ 15 $\times$ tan(22.5°)。

对比完方案后，我们进入本章的技术核心——EKF 状态估计。这是从嘈杂观测中提取可靠球状态的数学工具。

27.4 扩展卡尔曼滤波器（EKF）：从贝叶斯到工程实现 ⭐⭐⭐

这一节解决什么问题：从贝叶斯滤波的第一性原理出发，推导网球轨迹估计的 EKF 公式，并给出完整的 Python 实现。

为什么需要滤波而非直接观测

如果观测完美无噪声，直接用观测值即可。但现实中检测有抖动（像素级噪声反投影为厘米级误差）、有遗漏（漏检帧）、有延迟。EKF 的本质是在有噪声的观测和不完美的模型之间做最优权衡。

类比理解：你同时有两个不太靠谱的朋友在电话里给你指路——一个看着地图（模型预测，知道大方向但有偏差），一个站在路口看（观测，能看到实时情况但有时说错）。EKF 就是那个根据"谁更可信"来综合两人意见的导航系统。Kalman 1960 年的原始论文证明了在线性高斯假设下，Kalman 滤波器给出的是均方误差意义下的最优估计。

状态空间定义

最小状态向量包含位置和速度（6D）：

\[\mathbf{x} = \begin{pmatrix} p_x \\ p_y \\ p_z \\ v_x \\ v_y \\ v_z \end{pmatrix} \in \mathbb{R}^6\]

为什么不包含角速度？ 如果后续需要估计旋转（用于 Magnus 效应建模），可以扩展为 9D（加 $\omega_x, \omega_y, \omega_z$）。但第一版不要过大——状态越大，观测越难约束，EKF 的数值稳定性越差。先用 6D 验证基本功能，再根据需要扩展。

为什么不估计空气动力学参数？ 如果在线估计 $C_d$ 和 $C_L$，状态向量扩展为 8D。这在理论上可行，但需要足够长的观测序列才能可观测——球飞行时间只有 0.5-1 秒，观测帧数可能不足以同时估计位置、速度和空气动力学参数。Gray-box 乒乓球预测（arXiv:2305.15189）的做法是：用神经网络从 launcher 参数预测初始旋转，然后在 EKF 中使用这个预测值而非在线估计——这是一个值得参考的工程折中。

过程模型推导

Step 1：连续时间动力学——纯重力版（基础）。

\[\dot{\mathbf{x}} = \begin{pmatrix} \mathbf{v} \\ \mathbf{g} \end{pmatrix}, \quad \mathbf{g} = (0, 0, -9.81)^T\]

Step 2：离散化。 使用 Euler 法（对 $\Delta t$ 较小时足够精确）：

\[\mathbf{x}_{k+1} = f(\mathbf{x}_k, \Delta t) = \begin{pmatrix} p_x + v_x \Delta t \\ p_y + v_y \Delta t \\ p_z + v_z \Delta t + \frac{1}{2}g_z\Delta t^2 \\ v_x \\ v_y \\ v_z + g_z\Delta t \end{pmatrix}, \quad g_z = -9.81\]

（注意 $g_z=-9.81$ 已含负号，故位置/速度项用 $+$ 号即可得到向下的重力效果，与下文代码 += 0.5*self.g*dt**2、+= self.g*dt 一致。）

Step 3：Jacobian 矩阵。 对 $\mathbf{x}_k$ 求偏导：

\[F = \frac{\partial f}{\partial \mathbf{x}} = \begin{pmatrix} I_3 & \Delta t \cdot I_3 \\ 0_3 & I_3 \end{pmatrix} \in \mathbb{R}^{6 \times 6}\]

对于这个线性过程模型，Jacobian 就是转移矩阵本身——EKF 退化为标准 Kalman Filter。当加入非线性项（空气阻力）时，Jacobian 才变得重要。

EKF 预测步

\[\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \Delta t)$$ $$P_{k|k-1} = F P_{k-1|k-1} F^T + Q\]

$P$ 是状态协方差矩阵（$6\times6$），衡量"我们对状态估计有多不确定"。$Q$ 是过程噪声协方差矩阵，衡量"模型有多不准"。

$Q$ 的物理含义与设置方法：$Q$ 的对角元素应该反映过程模型遗漏的加速度量级。纯重力模型忽略了 drag（约 1-20 m/s²，取决于速度）和 Magnus（约 1-5 m/s²）。经验规则：

\[Q = \begin{pmatrix} \frac{1}{4}\sigma_a^2 \Delta t^4 I_3 & \frac{1}{2}\sigma_a^2 \Delta t^3 I_3 \\ \frac{1}{2}\sigma_a^2 \Delta t^3 I_3 & \sigma_a^2 \Delta t^2 I_3 \end{pmatrix}\]

其中 $\sigma_a$ 是遗漏加速度的标准差。对纯重力模型（遗漏 drag），$\sigma_a \approx 5$ m/s²。对含 drag 模型（遗漏 Magnus），$\sigma_a \approx 2$ m/s²。

观测模型与 EKF 更新步

最简单的观测模型——直接观测 3D 位置：

\[\mathbf{z}_k = H \mathbf{x}_k + \mathbf{r}_k, \quad H = \begin{pmatrix} I_3 & 0_3 \end{pmatrix}, \quad \mathbf{r}_k \sim \mathcal{N}(0, R)\]

$R$ 是观测噪声协方差矩阵，对动捕系统 $R \approx \text{diag}(0.001^2, 0.001^2, 0.001^2)$ m²（毫米级），对 RGB 视觉 $R \approx \text{diag}(0.03^2, 0.03^2, 0.05^2)$ m²（厘米级，z 方向更差）。

EKF 更新步：

\[\mathbf{y}_k = \mathbf{z}_k - H \hat{\mathbf{x}}_{k|k-1} \quad \text{(innovation，新息)}$$ $$S_k = H P_{k|k-1} H^T + R \quad \text{(新息协方差)}$$ $$K_k = P_{k|k-1} H^T S_k^{-1} \quad \text{(Kalman 增益)}$$ $$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k \mathbf{y}_k$$ $$P_{k|k} = (I - K_k H) P_{k|k-1}\]

Kalman 增益的物理意义

Kalman 增益 $K$ 是 EKF 最核心的量。它自动权衡"信模型"还是"信观测"：

情况	物理含义	$K$ 的行为	滤波器表现
$R \to \infty$	观测完全不可信	$K \to 0$	纯靠模型预测
$P \to \infty$	模型完全不可信	位置子空间增益趋于单位映射（$H$ 为 $3\times6$ 非方阵，无普通逆），未观测速度由 $P$ 的位置-速度交叉协方差修正	完全相信观测
$R \approx 0$	观测非常精确	$K$ 较大	每次更新大幅修正
$Q \approx 0$	模型非常精确	$P$ 缓慢增长，$K$ 较小	观测影响有限

这个自适应机制让 EKF 能处理不同质量的观测：检测可信时多听观测，模型可信时多听预测。漏检帧时只做预测不更新，$P$ 逐步增大；一旦重新检测到球，$K$ 自动增大以快速纠偏。

反事实推理：如果不用 EKF，只做差分速度估计会怎样？ $v_k = (p_k - p_{k-1})/\Delta t$，观测噪声 $\sigma_p$ 被放大为 $\sigma_v = \sqrt{2}\sigma_p / \Delta t$。对 $\sigma_p = 2$ cm、$\Delta t = 1/60$ s，$\sigma_v \approx 1.7$ m/s（球速的 5-10%）——完全不可用于精确击球预测。EKF 通过融合模型预测来平滑噪声，同时保持对真实变化的响应。

完整 Python EKF 实现

import torch
import math

class BallEKF:
    """6D 网球轨迹 EKF（批量处理多环境）。"""

    def __init__(self, num_envs, device, sigma_a=5.0, sigma_obs=0.02):
        self.num_envs = num_envs
        self.device = device
        self.state_dim = 6
        self.obs_dim = 3

        # 状态估计 [N, 6]：(px, py, pz, vx, vy, vz)
        self.x = torch.zeros(num_envs, self.state_dim, device=device)
        # 协方差 [N, 6, 6]
        self.P = torch.eye(self.state_dim, device=device).unsqueeze(0).repeat(num_envs, 1, 1) * 10.0

        # 过程噪声参数
        self.sigma_a = sigma_a

        # 观测噪声
        self.R = torch.eye(self.obs_dim, device=device) * sigma_obs**2

        # 观测矩阵 H = [I3 | 03]
        self.H = torch.zeros(self.obs_dim, self.state_dim, device=device)
        self.H[:3, :3] = torch.eye(3)

        # 重力
        self.g = torch.tensor([0.0, 0.0, -9.81], device=device)

    def predict(self, dt):
        """EKF 预测步：使用纯重力模型。"""
        # 状态转移
        x_new = self.x.clone()
        x_new[:, :3] += self.x[:, 3:6] * dt + 0.5 * self.g * dt**2
        x_new[:, 3:6] += self.g * dt
        self.x = x_new

        # Jacobian
        F = torch.eye(self.state_dim, device=self.device).unsqueeze(0).repeat(self.num_envs, 1, 1)
        F[:, :3, 3:6] = torch.eye(3, device=self.device) * dt

        # 过程噪声
        Q = self._compute_Q(dt)

        # 协方差传播
        self.P = torch.bmm(torch.bmm(F, self.P), F.transpose(1, 2)) + Q

    def update(self, z, detected):
        """EKF 更新步：只对检测到球的环境更新。

        Args:
            z: [N, 3] 观测位置
            detected: [N] bool 是否检测到
        """
        if not detected.any():
            return

        env_ids = detected.nonzero(as_tuple=True)[0]

        # Innovation
        H = self.H.unsqueeze(0)  # [1, 3, 6]
        z_pred = torch.bmm(H.expand(len(env_ids), -1, -1),
                           self.x[env_ids].unsqueeze(-1)).squeeze(-1)
        y = z[env_ids] - z_pred  # [M, 3]

        # Innovation covariance
        P_sub = self.P[env_ids]  # [M, 6, 6]
        S = torch.bmm(torch.bmm(H.expand(len(env_ids), -1, -1), P_sub),
                       H.expand(len(env_ids), -1, -1).transpose(1, 2))
        S = S + self.R.unsqueeze(0)  # [M, 3, 3]

        # Kalman gain
        K = torch.bmm(torch.bmm(P_sub, H.expand(len(env_ids), -1, -1).transpose(1, 2)),
                       torch.linalg.inv(S))  # [M, 6, 3]

        # State update
        self.x[env_ids] = self.x[env_ids] + torch.bmm(K, y.unsqueeze(-1)).squeeze(-1)

        # Covariance update: Joseph form for numerical stability
        # 标准形式 P = (I-KH)P 在数值上可能导致 P 非对称甚至非正定
        # Joseph form: P = (I-KH)P(I-KH)^T + KRK^T 保证对称正定
        I_KH = (torch.eye(self.state_dim, device=self.device).unsqueeze(0)
                - torch.bmm(K, H.expand(len(env_ids), -1, -1)))
        R_batch = self.R.unsqueeze(0).expand(len(env_ids), -1, -1)
        self.P[env_ids] = (
            torch.bmm(torch.bmm(I_KH, P_sub), I_KH.transpose(1, 2))
            + torch.bmm(torch.bmm(K, R_batch), K.transpose(1, 2))
        )

    def reset(self, env_ids, init_pos, init_vel=None):
        """重置指定环境的 EKF 状态。"""
        self.x[env_ids, :3] = init_pos
        if init_vel is not None:
            self.x[env_ids, 3:6] = init_vel
        else:
            self.x[env_ids, 3:6] = 0.0
        self.P[env_ids] = torch.eye(self.state_dim, device=self.device) * 10.0

    def propagate_to(self, dt_future):
        """从当前状态前推到未来时间（不更新 self.x/P）。"""
        x_future = self.x.clone()
        x_future[:, :3] += self.x[:, 3:6] * dt_future + 0.5 * self.g * dt_future**2
        x_future[:, 3:6] += self.g * dt_future
        return x_future

    def _compute_Q(self, dt):
        """计算过程噪声协方差矩阵（连续白噪声加速度模型）。"""
        sa2 = self.sigma_a ** 2
        Q = torch.zeros(self.num_envs, self.state_dim, self.state_dim, device=self.device)
        # 位置-位置块
        Q[:, :3, :3] = torch.eye(3, device=self.device) * (sa2 * dt**4 / 4)
        # 位置-速度块
        Q[:, :3, 3:6] = torch.eye(3, device=self.device) * (sa2 * dt**3 / 2)
        Q[:, 3:6, :3] = torch.eye(3, device=self.device) * (sa2 * dt**3 / 2)
        # 速度-速度块
        Q[:, 3:6, 3:6] = torch.eye(3, device=self.device) * (sa2 * dt**2)
        return Q

    @property
    def estimated_pos(self):
        return self.x[:, :3]

    @property
    def estimated_vel(self):
        return self.x[:, 3:6]

    @property
    def position_uncertainty(self):
        """返回位置估计的不确定性（标准差的 trace）。"""
        return self.P[:, :3, :3].diagonal(dim1=-2, dim2=-1).sum(dim=-1).sqrt()

# === 使用示例：在 mjlab 环境中运行 EKF ===

def run_ekf_demo(env, num_steps=500):
    """在 Tennis Launcher 环境中运行 EKF 并记录性能。"""
    ekf = BallEKF(num_envs=env.num_envs, device=env.device, sigma_a=5.0, sigma_obs=0.02)

    # 记录轨迹
    gt_positions = []
    ekf_positions = []
    raw_observations = []
    uncertainties = []

    env.reset()
    # 初始化 EKF
    init_pos = env.scene["ball"].data.root_pos_w[:, :3] - env.scene.env_origins
    init_vel = env.scene["ball"].data.root_lin_vel_w[:, :3]
    ekf.reset(torch.arange(env.num_envs, device=env.device), init_pos, init_vel)

    for step in range(num_steps):
        # 环境步进
        obs, reward, done, info = env.step(torch.zeros(env.num_envs, 0, device=env.device))

        # Ground truth
        gt_pos = env.scene["ball"].data.root_pos_w[:, :3] - env.scene.env_origins

        # 模拟观测（ground truth + 噪声）
        noise = torch.randn_like(gt_pos) * 0.02  # 2 cm 噪声
        obs_pos = gt_pos + noise

        # EKF 预测 + 更新
        ekf.predict(dt=0.01)  # 100 Hz
        detected = torch.ones(env.num_envs, dtype=torch.bool, device=env.device)
        ekf.update(obs_pos, detected)

        # 记录
        gt_positions.append(gt_pos[0].cpu())
        ekf_positions.append(ekf.estimated_pos[0].cpu())
        raw_observations.append(obs_pos[0].cpu())
        uncertainties.append(ekf.position_uncertainty[0].cpu())

        # 处理 reset
        if done.any():
            reset_ids = done.nonzero(as_tuple=True)[0]
            new_pos = env.scene["ball"].data.root_pos_w[reset_ids, :3] - env.scene.env_origins[reset_ids]
            new_vel = env.scene["ball"].data.root_lin_vel_w[reset_ids, :3]
            ekf.reset(reset_ids, new_pos, new_vel)

    # 转为张量
    gt = torch.stack(gt_positions)       # [T, 3]
    est = torch.stack(ekf_positions)     # [T, 3]
    raw = torch.stack(raw_observations)  # [T, 3]
    unc = torch.stack(uncertainties)     # [T]

    # 计算误差
    ekf_error = (est - gt).norm(dim=-1)
    raw_error = (raw - gt).norm(dim=-1)

    print(f"EKF position RMSE: {ekf_error.mean():.4f} m")
    print(f"Raw observation RMSE: {raw_error.mean():.4f} m")
    print(f"EKF improvement: {(1 - ekf_error.mean()/raw_error.mean())*100:.1f}%")
    print(f"Mean uncertainty: {unc.mean():.4f} m")

    return gt, est, raw, unc


def plot_ekf_results(gt, est, raw, unc):
    """绘制 EKF 估计 vs 真实 vs 原始观测的对比图。"""
    import matplotlib.pyplot as plt
    import numpy as np

    fig, axes = plt.subplots(4, 1, figsize=(12, 10), sharex=True)

    t = np.arange(len(gt))

    for i, axis_name in enumerate(['x', 'y', 'z']):
        axes[i].plot(t, gt[:, i].numpy(), 'g-', label='Ground Truth', alpha=0.8)
        axes[i].plot(t, raw[:, i].numpy(), 'r.', label='Raw Obs', alpha=0.2, markersize=1)
        axes[i].plot(t, est[:, i].numpy(), 'b-', label='EKF Estimate', alpha=0.8)
        axes[i].set_ylabel(f'{axis_name} (m)')
        axes[i].legend(loc='upper right')
        axes[i].grid(True, alpha=0.3)

    axes[3].plot(t, unc.numpy(), 'k-', label='Uncertainty')
    axes[3].set_ylabel('Uncertainty (m)')
    axes[3].set_xlabel('Step')
    axes[3].legend()
    axes[3].grid(True, alpha=0.3)

    plt.tight_layout()
    plt.savefig('ekf_results.png', dpi=150)
    print("Saved to ekf_results.png")

$Q$ 和 $R$ 参数调优指南

EKF 的性能高度依赖 $Q$ 和 $R$ 的设置。以下是一个系统性的调参流程：

Step 1：从 $R$ 开始。 $R$ 由观测噪声决定——这是可以直接测量的。方法：收集 ground truth 和 noisy observation，计算差异的方差。

# 从数据中估计 R
obs_errors = raw_observations - gt_positions  # [T, 3]
R_estimated = torch.diag(obs_errors.var(dim=0))
print(f"Estimated R:\n{R_estimated}")
# 典型值：diag(0.0004, 0.0004, 0.0004) 对应 2cm std

Step 2：从 innovation 调 $Q$（用 NIS 一致性检验）。 如果 EKF 工作良好，innovation $\mathbf{y}_k$ 应该是白噪声（均值为零，方差为预测的 $S_k$），其归一化新息平方 NIS $=\mathbf{y}_k^T S_k^{-1}\mathbf{y}_k$ 应服从 $\chi^2$ 分布、长期均值约等于观测维度。如果 innovation 有系统性偏移——存在未建模的确定性项（如漏掉 drag/Magnus）。如果实际 innovation 方差长期远大于预测的 $S_k$（NIS 长期偏大）——说明滤波器低估了不确定性，$Q$（或 $R$）太小，应增大 $\sigma_a$；反之 NIS 长期偏小则说明 $Q/R$ 太大，应减小。

# innovation 分析
innovations = []
for step in range(num_steps):
    ekf.predict(dt)
    z_pred = ekf.x[:, :3]
    y = obs - z_pred  # innovation
    innovations.append(y[0].cpu())
    ekf.update(obs, detected)

innovations = torch.stack(innovations)
print(f"Innovation mean: {innovations.mean(dim=0)}")  # 应接近零
print(f"Innovation std: {innovations.std(dim=0)}")    # 应接近 sqrt(diag(S))
# 如果 mean 显著非零 → 有未建模的确定性项（漏掉 drag/Magnus 等）
# 如果 std >> sqrt(S)（NIS 长期偏大）→ 滤波器低估不确定性，Q/R 太小，增大 sigma_a
# 如果 std << sqrt(S)（NIS 长期偏小）→ Q/R 太大，减小 sigma_a

Step 3：按球速分档调 $Q$。 高速球的 drag 效应更强（Ch26：drag $\propto$ $v^2$），模型误差更大，因此 $Q$ 应该更大。

# 自适应 Q：根据当前估计速度调整
speed = ekf.estimated_vel.norm(dim=-1)
adaptive_sigma_a = 2.0 + 0.1 * speed  # 慢球 2 m/s², 快球 6+ m/s²
# 然后用 adaptive_sigma_a 重新计算 Q

Isaac Lab 中的 EKF 对照

Isaac Lab 本身不提供 EKF 实现——但你可以在 Isaac Lab 环境中使用完全相同的 PyTorch EKF 代码。关键差异在于观测的获取方式：

# Isaac Lab 中获取球状态的方式
# (假设使用 RSL-RL 后端，球作为 RigidObject)
ball_pos_w = env.scene["ball"].data.root_pos_w[:, :3]  # 同 mjlab
ball_vel_w = env.scene["ball"].data.root_lin_vel_w[:, :3]  # 同 mjlab
# PhysX 和 MuJoCo 的状态接口在 Isaac Lab 和 mjlab 中已经统一

EKF 代码本身与物理引擎无关——它只接收 observation 和 timestamp，不关心 observation 来自 MuJoCo 还是 PhysX。这是分层设计的好处——感知层和物理层解耦。

工程要点：批量处理和 GPU 加速

上述实现使用 torch.bmm（batch matrix multiply）对所有环境并行处理——这是在 GPU 上运行 EKF 的关键。传统 EKF 实现（NumPy 单环境循环）在 4096 环境下不可接受。

性能关键路径：torch.linalg.inv(S) 是最贵的操作（$O(n^3)$ per env），但 $S$ 只有 $3 \times 3$，批量求逆在 GPU 上非常高效。如果状态维度扩展到 9D 或 11D，这个操作会变慢——但仍然比 CPU 循环快几个数量级。

⚠️ 常见陷阱

⚠️ 编程陷阱：$Q$ 和 $R$ 的单位不一致 - 注意 $Q$ 是状态协方差，各块单位不同：位置-位置块 m²、位置-速度块 m²/s、速度-速度块 m²/s²；不能笼统说"$Q$ 用 m²/s²" - 若 $R$ 用 cm² 而状态/Q 用米制 → Kalman 增益无物理意义 - 正确做法：所有量统一 SI 单位（米、秒），$R$ 的位置观测协方差用 m²

⚠️ 概念误区："EKF 太平滑"就是延迟 - 平滑来自模型-观测融合，不一定是延迟。但 timestamp 处理错误确实会产生延迟 - 区分方法：看 innovation $\mathbf{y}_k$ 是否有系统性偏移——如果 innovation 始终偏正或偏负，说明有系统性问题

⚠️ 编程陷阱：协方差更新使用 $P = (I - KH)P$ 而非 Joseph form - 简化形式在数值上不对称，可能导致 $P$ 逐步变成非正定矩阵 → EKF 崩溃（Kalman gain 出 NaN） - 正确做法：使用 Joseph form $P = (I-KH)P(I-KH)^T + KRK^T$（上面代码已实现），它保证 $P$ 始终对称正定。如果仍有数值问题，可在每步后强制对称化 $P = (P + P^T)/2$

⚠️ 思维陷阱：EKF 参数调好就不用改 - 高速球的模型误差比低速球大，$Q$ 应更大；远距离观测噪声更大，$R$ 应动态调整 - 正确做法：至少按球速分档调整 $Q$

练习

[推导题] 为过程模型加入二次阻力 $\mathbf{a}_{\text{drag}} = -\frac{\rho C_d A}{2m} |\mathbf{v}| \mathbf{v}$，推导 Jacobian $F$ 的右下 $3\times3$ 块。提示：$\frac{\partial(|\mathbf{v}|v_i)}{\partial v_j} = v_i v_j / |\mathbf{v}| + |\mathbf{v}| \delta_{ij}$。
[编程题] 使用上述 BallEKF 类，生成 ground truth 抛体轨迹（Ch26 公式），加 $\sigma = 2$ cm 高斯噪声作为观测，运行 EKF 并绘制估计 vs 真实 vs 纯观测三条曲线。
[分析题] 在练习 2 中分别把 $R$ 增大 10 倍、把 $Q$ 增大 10 倍，观察 EKF 输出变化。解释 $Q/R$ 比值如何影响滤波器——什么情况下滤波器"太信模型"？什么情况下"太信观测"？

纯重力模型的 EKF 忽略了空气阻力——Ch26 已经证明阻力让球减速 30-50%。下一节将空气阻力加入过程模型，让 EKF 变成真正的"非线性"滤波器。

27.5 含空气阻力的非线性 EKF ⭐⭐⭐

这一节解决什么问题：将 Ch26 的空气动力学模型集成到 EKF 过程模型中，真正实现"非线性"滤波。

动机：纯重力 EKF 的误差

回顾 Ch26：在 30 m/s 下，空气阻力约 1.04 N——比重力（0.56 N）还大。纯重力 EKF 会系统性地高估球的飞行距离和速度。这个误差会被 $Q$ 矩阵部分吸收（$Q$ 越大，EKF 越不信模型），但更好的做法是在过程模型中直接包含阻力。

含阻力的过程模型

\[\dot{\mathbf{v}} = \mathbf{g} - \frac{\rho C_d A}{2m} |\mathbf{v}| \mathbf{v}\]

令 $\alpha = \frac{\rho C_d A}{2m}$（对标准网球 $\alpha \approx 0.0202$ m⁻¹），离散化：

\[\mathbf{v}_{k+1} = \mathbf{v}_k + (\mathbf{g} - \alpha |\mathbf{v}_k| \mathbf{v}_k) \Delta t$$ $$\mathbf{p}_{k+1} = \mathbf{p}_k + \mathbf{v}_k \Delta t + \frac{1}{2}(\mathbf{g} - \alpha |\mathbf{v}_k| \mathbf{v}_k) \Delta t^2\]

Jacobian 的非线性部分

关键在于对 $\alpha |\mathbf{v}| v_i$ 求偏导。使用链式法则：

\[\frac{\partial(|\mathbf{v}| v_i)}{\partial v_j} = \frac{v_i v_j}{|\mathbf{v}|} + |\mathbf{v}| \delta_{ij}\]

因此：

\[\frac{\partial \dot{v}_i}{\partial v_j} = -\alpha \left(\frac{v_i v_j}{|\mathbf{v}|} + |\mathbf{v}| \delta_{ij}\right)\]

写成矩阵形式：

\[\frac{\partial \dot{\mathbf{v}}}{\partial \mathbf{v}} = -\alpha \left(\frac{\mathbf{v}\mathbf{v}^T}{|\mathbf{v}|} + |\mathbf{v}| I_3\right)\]

离散化后的 Jacobian：

\[F = \begin{pmatrix} I_3 & \Delta t \cdot I_3 + \frac{\Delta t^2}{2} \frac{\partial \dot{\mathbf{v}}}{\partial \mathbf{v}} \\ 0_3 & I_3 + \Delta t \cdot \frac{\partial \dot{\mathbf{v}}}{\partial \mathbf{v}} \end{pmatrix}\]

Python 实现

class BallEKFWithDrag(BallEKF):
    """含空气阻力的非线性 EKF。"""

    def __init__(self, num_envs, device, alpha=0.0202, **kwargs):
        super().__init__(num_envs, device, **kwargs)
        self.alpha = alpha  # ρ·Cd·A / (2m)

    def predict(self, dt):
        """含阻力的非线性预测步。"""
        vel = self.x[:, 3:6]  # [N, 3]
        speed = vel.norm(dim=-1, keepdim=True).clamp(min=1e-6)  # 防除零

        # 加速度 = 重力 + 阻力
        a_drag = -self.alpha * speed * vel  # [N, 3]
        a_total = self.g.unsqueeze(0) + a_drag

        # 状态转移
        x_new = self.x.clone()
        x_new[:, :3] += vel * dt + 0.5 * a_total * dt**2
        x_new[:, 3:6] += a_total * dt
        self.x = x_new

        # Jacobian
        F = torch.eye(self.state_dim, device=self.device).unsqueeze(0).repeat(self.num_envs, 1, 1)

        # ∂ẋ/∂v 的非线性部分
        vvT = torch.bmm(vel.unsqueeze(-1), vel.unsqueeze(-2))  # [N, 3, 3]
        dv_dv = -self.alpha * (vvT / speed.unsqueeze(-1) + speed.unsqueeze(-1) * torch.eye(3, device=self.device))

        F[:, :3, 3:6] = torch.eye(3, device=self.device) * dt + 0.5 * dv_dv * dt**2
        F[:, 3:6, 3:6] = torch.eye(3, device=self.device).unsqueeze(0) + dv_dv * dt

        # 协方差传播
        Q = self._compute_Q(dt)
        self.P = torch.bmm(torch.bmm(F, self.P), F.transpose(1, 2)) + Q

含 drag 的 EKF 模型误差更小（因为模型更接近真实物理），所以 $Q$ 中的 $\sigma_a$ 可以从 5 m/s² 降到 2 m/s²（只需要覆盖 Magnus 力等遗漏项）。

含 Magnus 力的进一步扩展

如果已知球的角速度 $\boldsymbol{\omega}$（从 ground truth 或从发球参数推算），Magnus 力可以作为已知外力加入过程模型，而不需要扩展状态向量：

def predict_with_magnus(self, dt, omega_estimate):
    """含 drag + Magnus 的预测步。"""
    vel = self.x[:, 3:6]
    speed = vel.norm(dim=-1, keepdim=True).clamp(min=1e-6)

    # 阻力
    a_drag = -self.alpha * speed * vel

    # Magnus 力（omega 作为已知输入，不是状态变量）
    CL = 1.0  # lift coefficient
    r = 0.033
    rho = 1.225
    volume = (4.0/3.0) * math.pi * r**3
    a_magnus = CL * volume * rho / 0.057 * torch.cross(omega_estimate, vel, dim=-1)

    a_total = self.g.unsqueeze(0) + a_drag + a_magnus
    # ... 其余与 drag-only 版本相同

这种"已知输入"的方式避免了扩展状态向量——状态仍然是 6D，但过程模型更精确。代价是需要一个合理的 $\boldsymbol{\omega}$ 估计。LATENT 的做法是对球的物理参数做 domain randomization——这意味着策略不需要精确知道 $\boldsymbol{\omega}$，只需要在一个范围内鲁棒工作。

自适应 Q 调整

含 drag 的 EKF 模型误差更小（因为模型更接近真实物理），所以 $Q$ 中的 $\sigma_a$ 可以从 5 m/s² 降到 2 m/s²（只需要覆盖 Magnus 力等遗漏项）。但更好的做法是根据当前球速动态调整 $Q$：

def adaptive_sigma_a(self, vel):
    """根据球速动态计算过程噪声标准差。

    物理依据：
    - 低速球（< 10 m/s）：drag 小，Magnus 小，模型很准 → sigma_a 小
    - 中速球（10-30 m/s）：drag 显著，模型较准（已含 drag）→ sigma_a 中等
    - 高速球（> 30 m/s）：drag 主导但 Magnus 也大，且 Cd 随 Reynolds 数变化 → sigma_a 大
    """
    speed = vel.norm(dim=-1)  # [N]
    # 分段线性
    sigma_a = torch.where(speed < 10.0, 1.0,
              torch.where(speed < 30.0, 1.0 + 0.1 * (speed - 10.0),
                          3.0 + 0.05 * (speed - 30.0)))
    return sigma_a  # [N]

Jacobian 的数值验证

非线性 Jacobian 是 EKF 最容易出错的部分。推荐在开发阶段用有限差分法验证解析 Jacobian：

def verify_jacobian(ekf, eps=1e-5):
    """用有限差分法验证解析 Jacobian。"""
    x0 = ekf.x[0].clone()  # 取第一个环境
    dt = 0.01

    # 解析 Jacobian
    F_analytical = ekf._compute_F_with_drag(x0.unsqueeze(0), dt)[0]  # [6, 6]

    # 数值 Jacobian
    F_numerical = torch.zeros(6, 6, device=x0.device)
    for j in range(6):
        x_plus = x0.clone()
        x_plus[j] += eps
        x_minus = x0.clone()
        x_minus[j] -= eps

        f_plus = ekf._predict_state(x_plus.unsqueeze(0), dt)[0]
        f_minus = ekf._predict_state(x_minus.unsqueeze(0), dt)[0]
        F_numerical[:, j] = (f_plus - f_minus) / (2 * eps)

    # 比较
    max_diff = (F_analytical - F_numerical).abs().max().item()
    print(f"Max Jacobian difference: {max_diff:.2e}")
    assert max_diff < 1e-4, f"Jacobian verification failed: max_diff = {max_diff}"
    print("✅ Jacobian verification PASSED")

工程建议：在每次修改过程模型后都运行此验证。Jacobian 中的一个符号错误可能让 EKF 的协方差传播完全错误——$P$ 可能变成非正定矩阵，导致 Kalman gain 计算出 NaN。

含 drag EKF 的预期改进

在标准网球发球条件下（speed 20-45 m/s），含 drag 的 EKF 相比纯重力 EKF 应该有显著改进：

指标	纯重力 EKF	含 drag EKF	改进
位置 RMSE（0.5s 预测）	2-5 m	0.3-0.8 m	60-85%
速度 RMSE	5-15 m/s	1-3 m/s	70-80%
落地点误差	3-8 m	0.5-1.5 m	70-85%

这些数字说明：含 drag 的 EKF 不是锦上添花，而是必需的——纯重力模型在高速球下的预测误差大到无法用于击球控制。

⚠️ 常见陷阱

⚠️ 编程陷阱：speed.clamp(min=1e-6) 遗漏导致除零 - 当球速接近零时（落地后减速），$|\mathbf{v}| \to 0$，阻力项中的 $v_i v_j / |\mathbf{v}|$ 产生 NaN - 正确做法：对 speed 加 clamp 或 epsilon

⚠️ 概念误区：含阻力的 EKF 一定比纯重力好 - 如果 $\alpha$（阻力系数）设得不准，含阻力的模型可能比纯重力+大 $Q$ 更差 - 正确做法：用 ground truth 轨迹标定 $\alpha$，或把 $\alpha$ 作为可调参数

⚠️ 编程陷阱：Jacobian 验证只在初始化时做一次 - 修改过程模型后如果忘记重新验证 Jacobian，可能引入隐蔽的数值错误 - 正确做法：在单元测试中加入 verify_jacobian()，每次改模型后自动运行

⚠️ 思维陷阱：直接把 drag 系数从 Ch26 硬编码到 EKF - Ch26 的 $\alpha$ 是仿真环境的参数，真实球的 $\alpha$ 可能不同 - 正确做法：EKF 的 $\alpha$ 应该可配置，并在训练中通过 DR 覆盖一个范围

Q/R 参数调优速查表

场景	推荐 $\sigma_a$ (m/s²)	推荐 $\sigma_{obs}$ (m)	备注
纯重力 EKF + 动捕	5-10	0.001	$Q$ 大因为模型遗漏 drag
含 drag EKF + 动捕	1-3	0.001	$Q$ 小因为模型更准
含 drag EKF + RGB 视觉	2-5	0.03-0.05	$R$ 大因为视觉噪声大
含 drag+Magnus EKF + 动捕	0.5-2	0.001	$Q$ 最小因为模型最准
弹跳后前几帧	10+	同上	弹跳后模型不确定性大
漏检帧（只预测不更新）	同上	N/A	$P$ 自动增大

经验法则：$Q/R$ 比值决定滤波器的"性格"——比值大则更信观测（反应快但嘈杂），比值小则更信模型（平滑但可能滞后）。球速越快，$Q/R$ 应越大（因为模型误差随速度增大）。

练习

[推导题] 推导含二次阻力的 Jacobian $F$ 的完整 $6 \times 6$ 矩阵。验证：当 $\alpha = 0$ 时退化为纯重力版本。
[编程题] 实现 BallEKFWithDrag，在同一条轨迹上对比纯重力 EKF 和含 drag EKF 的位置估计误差。使用 Ch26 的 drag 参数。
[分析题] 如果 $\alpha$ 设为真实值的 2 倍（高估了阻力），EKF 的估计误差会怎样变化？用实验验证。

EKF 处理的是"连续飞行"阶段。但网球会弹跳——弹跳时速度方向突变，连续模型完全失效。下一节专门处理弹跳检测和模式切换。

27.6 弹跳检测与模式切换 ⭐⭐⭐

这一节解决什么问题：处理 EKF 在弹跳时刻的失效问题。

动机：弹跳让 EKF 崩溃

弹跳是模式切换（mode change）——飞行中的球突然变成弹跳后的球，速度方向反转。标准 EKF 的连续过程模型假设状态平滑变化，无法处理这种突变。如果不做特殊处理，弹跳后 EKF 会继续按弹跳前的速度外推——预测球向下穿过地面，而实际球在向上飞。

类比理解：这就像导航软件在高速公路上突然遇到 U 形弯——如果 GPS 暂时丢失，导航会按直线外推认为你在继续直行，而你实际上已经转弯了。弹跳检测就是"发现 U 形弯"并切换模型。

三种弹跳检测方法

方法	原理	优点	缺点	推荐阶段
Ground truth 检测	直接读 $z < \epsilon$ 且 $v_z < 0$	完美	仅限仿真	第一阶段
Innovation 检测	$\\|\mathbf{y}_k\\| > \tau$ 时判断	无需额外传感器	阈值敏感	所有阶段
接触力检测	MuJoCo `contact` 数组中有球-地面接触	物理精确	需要 API 支持	仿真阶段

推荐方案：仿真中用接触力检测（最可靠），部署时用 innovation 检测（不需要 ground truth）。

def detect_bounce(env, ekf, z_threshold=0.05, innovation_threshold=0.3):
    """弹跳检测：结合地面高度和 EKF innovation。

    Returns:
        bounced: [N] bool, 哪些环境刚刚发生了弹跳
    """
    ball_z = ekf.estimated_pos[:, 2]
    ball_vz = ekf.estimated_vel[:, 2]

    # 方法 1：地面高度 + 速度方向
    approaching_ground = (ball_z < z_threshold) & (ball_vz < 0)

    # 方法 2：innovation 突变（弹跳后 EKF 预测和观测偏差很大）
    innovation_norm = ekf.last_innovation.norm(dim=-1) if hasattr(ekf, 'last_innovation') else torch.zeros(env.num_envs, device=env.device)
    large_innovation = innovation_norm > innovation_threshold

    # 任一方法触发即判为弹跳（OR）；注意这样误报风险更高，
    # 实际应配合 cooldown/状态机，并优先用接触事件检测（见上表）
    bounced = approaching_ground | large_innovation
    return bounced

弹跳后的 EKF 重置

检测到弹跳后，需要更新 EKF 状态：

def handle_bounce(ekf, bounced_ids, cor=0.75):
    """弹跳后重置 EKF 速度和协方差。

    Args:
        ekf: BallEKF instance
        bounced_ids: [M] 发生弹跳的环境 indices
        cor: 恢复系数（Ch26 标定值）
    """
    if len(bounced_ids) == 0:
        return

    # 速度法向分量反转并衰减
    ekf.x[bounced_ids, 5] = -cor * ekf.x[bounced_ids, 5]  # vz 反转

    # 切向分量因摩擦减速（近似）
    ekf.x[bounced_ids, 3] *= 0.85  # vx 减速 15%
    ekf.x[bounced_ids, 4] *= 0.85  # vy 减速 15%

    # 位置修正：确保 z > 0
    ekf.x[bounced_ids, 2] = torch.clamp(ekf.x[bounced_ids, 2], min=0.01)

    # 重置速度相关的协方差块（弹跳后速度估计不确定性增大）
    ekf.P[bounced_ids, 3:6, 3:6] = torch.eye(3, device=ekf.device) * 5.0
    # 位置协方差保持（弹跳不改变位置估计的不确定性）

为什么重置速度协方差？ 弹跳后的速度由接触力决定，而接触力受 COR、摩擦系数、入射角等多个参数影响。因此弹跳后的速度估计比弹跳前不确定得多，需要增大协方差让 EKF 在后续帧中更信任观测。

弹跳冷却机制

一个常见 bug 是弹跳检测在所有帧都触发——球在地面附近时每帧都被判断为"弹跳"，速度被反复反转。解决方案是加入冷却（cooldown）机制：

class BounceDetectorWithCooldown:
    """带冷却的弹跳检测器。"""

    def __init__(self, num_envs, cooldown_steps=10, device='cuda'):
        self.cooldown_counter = torch.zeros(num_envs, dtype=torch.int, device=device)
        self.cooldown_steps = cooldown_steps
        self.prev_vz = torch.zeros(num_envs, device=device)

    def detect(self, ekf):
        """检测弹跳，返回刚发生弹跳的环境 ids。"""
        ball_z = ekf.estimated_pos[:, 2]
        ball_vz = ekf.estimated_vel[:, 2]

        # 核心条件：vz 从负变正（球从下落变为上升）
        vz_sign_change = (self.prev_vz < -0.5) & (ball_vz > 0.1)

        # 高度条件：球在地面附近
        near_ground = ball_z < 0.1

        # 冷却条件：距上次弹跳已过足够步数
        cooldown_expired = self.cooldown_counter <= 0

        # 综合判断
        bounced = vz_sign_change & near_ground & cooldown_expired

        # 更新冷却计数器
        self.cooldown_counter[bounced] = self.cooldown_steps
        self.cooldown_counter = (self.cooldown_counter - 1).clamp(min=0)

        # 记录当前 vz
        self.prev_vz = ball_vz.clone()

        return bounced

    def reset(self, env_ids):
        """episode reset 时重置检测器状态。"""
        self.cooldown_counter[env_ids] = 0
        self.prev_vz[env_ids] = 0.0

Topspin 弹跳的速度修正

Ch26 验证了 topspin 球弹跳后水平速度会增大（旋转-摩擦耦合效应）。如果 EKF 的弹跳处理只做简单的"vz 反转 + 切向减速"，会错误地让 topspin 球弹跳后减速。更精确的处理需要考虑旋转：

def handle_bounce_with_spin(ekf, bounced_ids, omega_estimate, cor=0.75, mu=0.6):
    """考虑旋转效应的弹跳处理。

    Args:
        omega_estimate: [N, 3] 球的角速度估计（从 launch_cmd 推算或从观测估计）
        mu: 球-地面摩擦系数
    """
    if len(bounced_ids) == 0:
        return

    vx = ekf.x[bounced_ids, 3]
    vy = ekf.x[bounced_ids, 4]
    vz = ekf.x[bounced_ids, 5]
    wy = omega_estimate[bounced_ids, 1]  # topspin 绕 y 轴

    # 法向速度反转
    vz_new = -cor * vz

    # 切向速度：考虑旋转摩擦耦合（仅针对 x 方向切向速度的简化模型）
    # topspin (wy < 0) → 球底部向后运动 → 摩擦力向前 → vx 增大
    # 简化模型：dv_tangential ≈ -mu * COR * |vz| * sign(surface_vel_x - vx)
    # 其中 surface_vel_x = -wy * r（球底部表面在 x 方向的速度）
    r = 0.033
    surface_vel_x = -wy * r  # topspin (wy<0) → surface_vel_x > 0 → 向前
    friction_impulse = mu * cor * vz.abs() * 0.3  # 简化系数

    # 如果 surface_vel > tangential_vel，摩擦加速球
    vx_new = vx + friction_impulse * torch.sign(surface_vel_x - vx)
    vy_new = vy * 0.9  # 侧向简单衰减

    ekf.x[bounced_ids, 3] = vx_new
    ekf.x[bounced_ids, 4] = vy_new
    ekf.x[bounced_ids, 5] = vz_new
    ekf.x[bounced_ids, 2] = torch.clamp(ekf.x[bounced_ids, 2], min=0.01)
    ekf.P[bounced_ids, 3:6, 3:6] = torch.eye(3, device=ekf.device) * 5.0

Phybot 的 prediction-free 变体

Phybot（arXiv:2511.11218）提出了一个有趣的替代方案：不做轨迹预测，让策略直接从当前球状态（不含未来预测）学习击球。他们的实验显示，prediction-free 变体的性能与使用 EKF 预测的版本相当。

这暗示了一个重要的工程洞察：如果 RL 策略的 observation 包含足够的历史信息（多帧球位置），策略本身可以隐式地学习"预测"球的未来位置——不需要显式的 EKF 或预测网络。但这种隐式预测的可解释性和 debug 能力远不如显式预测管线。

工程权衡：

维度	显式预测（EKF + GRU）	隐式预测（策略自己学）
可解释性	高（可以检查每一步的 innovation、gain）	低（预测隐含在网络权重中）
Debug 能力	高（逐层替换 ground truth）	低（端到端黑箱）
工程复杂度	高（多个模块协同）	低（一个策略网络）
灵活性	高（更换预测器不影响策略）	低（更换感知方案需重新训练）
泛化性	取决于物理模型	取决于训练数据分布

对教学项目推荐显式预测（本章的方法）——它更容易教学、更容易 debug、更容易逐步改进。对研究项目，prediction-free 是一个值得探索的方向。

本质洞察：弹跳检测不是"信号处理技巧"，而是 EKF 过程模型的必要补充。一个不处理弹跳的 EKF 在弹跳后等于"瞎了"—— 它会按弹跳前的速度继续外推，与真实轨迹越来越远。

⚠️ 常见陷阱

⚠️ 编程陷阱：弹跳检测在所有帧都触发 - 如果阈值太宽松，球在地面附近每帧都被判断为"弹跳" → 速度被反复反转 - 正确做法：加 cooldown（弹跳后 N 帧内不再检测）或只在 $v_z$ 从负变正时触发

⚠️ 概念误区：COR 是常数 - 实际 COR 随入射角和球速变化 - 对 EKF 来说，不需要精确 COR——$\pm 20\%$ 的误差可以被后续更新步修正

练习

[编程题] 实现 detect_bounce 和 handle_bounce，在一条含弹跳的轨迹上测试。对比有弹跳处理和无弹跳处理的 EKF 估计误差。
[设计题] 设计一个 multiple-model filter：同时维护 airborne 和 bounced 两个 EKF，按 innovation likelihood 加权。比较与 event-based reset 的性能和复杂度。

EKF 提供了当前球状态的最佳估计。但控制器需要的不是"球现在在哪"，而是"球将来在哪"。下一节用 GRU 残差网络在 EKF 估计的基础上做轨迹预测。

27.7 GRU 残差预测器：物理先验 + 数据驱动 ⭐⭐⭐

这一节解决什么问题：纯物理模型无法覆盖所有空气动力学细节，GRU 残差网络用数据驱动方式补偿建模误差。

为什么不直接用 GRU 替代物理模型

抛体方程编码了重力恒定加速度，drag 公式编码了阻力与 $v^2$ 成正比——这些是人类花几百年从实验中总结的物理定律。让神经网络重新"发现"这些定律，不仅浪费已有知识，还需要大量数据。更严重的是泛化性：纯 GRU 可能学到"第 15 个 timestep 时球大概在 x=-3"这种分布统计量（而非物理规律），发球参数一变就崩溃。

物理模型天然具有泛化能力——不管球速多少，$g$ 都是 9.81。这就像明知答案的选择题还靠猜。

反事实推理：如果 GRU 直接输出完整预测（不加物理 baseline），在训练分布内可能表现好，但 speed/elevation 变化时崩溃。Gray-box 乒乓球预测（arXiv:2305.15189）的实验直接对比了 black-box（纯神经网络）和 gray-box（物理+残差）方法——gray-box 在分布外场景的误差比 black-box 低 40-60%。

残差架构的完整实现

import torch
import torch.nn as nn

class PhysicsResidualPredictor(nn.Module):
    """物理先验 + GRU 残差的轨迹预测器。"""

    def __init__(self, history_len=10, pred_horizon=20, hidden_dim=64,
                 cmd_dim=8, alpha=0.0202):
        super().__init__()
        self.pred_horizon = pred_horizon
        self.alpha = alpha
        self.g = torch.tensor([0.0, 0.0, -9.81])

        # GRU 残差网络
        # 输入：每帧 (pos3 + dt1 + confidence1) = 5D × history_len + cmd8
        self.input_dim = 5 * history_len + cmd_dim
        self.gru = nn.GRU(
            input_size=self.input_dim,
            hidden_size=hidden_dim,
            num_layers=2,
            batch_first=True,
        )
        self.residual_head = nn.Sequential(
            nn.Linear(hidden_dim, hidden_dim),
            nn.ReLU(),
            nn.Linear(hidden_dim, 3 * pred_horizon),  # 输出每步 3D 残差
        )

        # 残差幅度限制
        self.residual_clamp = 0.5  # 最大 ± 50 cm

    def physics_baseline(self, state_now, dt_steps):
        """物理 baseline：drag + gravity 前向传播。

        Args:
            state_now: [N, 6] 当前 (pos3, vel3)
            dt_steps: [pred_horizon] 每步时间间隔

        Returns:
            positions: [N, pred_horizon, 3] 预测位置
        """
        positions = []
        pos = state_now[:, :3].clone()
        vel = state_now[:, 3:6].clone()
        g = self.g.to(pos.device)

        for dt in dt_steps:
            speed = vel.norm(dim=-1, keepdim=True).clamp(min=1e-6)
            a_drag = -self.alpha * speed * vel
            a = g + a_drag
            pos = pos + vel * dt + 0.5 * a * dt**2
            vel = vel + a * dt
            positions.append(pos.clone())

        return torch.stack(positions, dim=1)  # [N, H, 3]

    def forward(self, state_now, obs_history, launch_cmd, dt_steps):
        """完整预测 = 物理 baseline + GRU 残差。

        Args:
            state_now: [N, 6] 当前 EKF 估计的 (pos, vel)
            obs_history: [N, history_len, 5] 历史观测 (pos3, dt1, conf1)
            launch_cmd: [N, 8] 发球命令
            dt_steps: [pred_horizon] 每步时间间隔

        Returns:
            predicted_positions: [N, pred_horizon, 3]
        """
        # 物理 baseline
        physics_pred = self.physics_baseline(state_now, dt_steps)

        # GRU 残差
        # 展平历史 + 拼接 launch command
        history_flat = obs_history.reshape(obs_history.shape[0], -1)  # [N, 50]
        gru_input = torch.cat([history_flat, launch_cmd], dim=-1)  # [N, 58]
        gru_input = gru_input.unsqueeze(1)  # [N, 1, 58]（单步 GRU）

        gru_out, _ = self.gru(gru_input)
        residual_flat = self.residual_head(gru_out[:, -1])  # [N, 60]
        residual = residual_flat.reshape(-1, self.pred_horizon, 3)  # [N, 20, 3]

        # Clamp 残差防止发散
        residual = torch.clamp(residual, -self.residual_clamp, self.residual_clamp)

        return physics_pred + residual

    def compute_impact_point(self, predicted_positions, strike_height=0.6):
        """从预测轨迹中提取击球点（球下降穿过击球高度处）。

        击球点 = 球在下降过程中第一次低于 strike_height 的位置——球拍应在
        该高度迎球。注意这**不是落地点**（z≈0）：网球在落地前被击中，把
        球拍引向落地点会让策略学到错误的接近目标（球拍被拉向地面）。
        strike_height 为目标击球高度，可按机器人/打法调节。

        Returns:
            impact_pos: [N, 3] 预测击球位置
            impact_step: [N] 预测击球时间步
        """
        # 找到第一个 z < strike_height 的时间步（球下降穿过击球高度）
        below_strike = predicted_positions[:, :, 2] < strike_height
        # 如果预测窗口内球未降到击球高度，用最后一步兜底
        has_impact = below_strike.any(dim=1)
        impact_step = below_strike.float().argmax(dim=1)  # 第一个 True 的索引
        impact_step[~has_impact] = self.pred_horizon - 1

        # 提取对应的位置
        batch_idx = torch.arange(predicted_positions.shape[0], device=predicted_positions.device)
        impact_pos = predicted_positions[batch_idx, impact_step]

        return impact_pos, impact_step

训练流程

def train_residual_predictor(predictor, train_data, val_data, epochs=100, lr=1e-3):
    """训练 GRU 残差预测器。

    train_data: 从仿真收集的 ground truth 轨迹
    每条轨迹包含：obs_history, launch_cmd, state_now, future_positions (ground truth)
    """
    optimizer = torch.optim.Adam(predictor.parameters(), lr=lr)
    scheduler = torch.optim.lr_scheduler.CosineAnnealingLR(optimizer, epochs)

    for epoch in range(epochs):
        predictor.train()
        total_loss = 0.0

        for batch in train_data:
            state_now = batch["state_now"]
            obs_history = batch["obs_history"]
            launch_cmd = batch["launch_cmd"]
            gt_positions = batch["future_positions"]
            dt_steps = batch["dt_steps"]

            pred_positions = predictor(state_now, obs_history, launch_cmd, dt_steps)

            # L2 loss，对时间步加权（近期比远期更重要）
            time_weights = torch.exp(-0.1 * torch.arange(predictor.pred_horizon, device=pred_positions.device))
            loss = ((pred_positions - gt_positions)**2 * time_weights.unsqueeze(0).unsqueeze(-1)).mean()

            optimizer.zero_grad()
            loss.backward()
            torch.nn.utils.clip_grad_norm_(predictor.parameters(), 1.0)
            optimizer.step()
            total_loss += loss.item()

        # 验证：对 held-out speed/elevation 范围
        predictor.eval()
        with torch.no_grad():
            val_loss = 0.0
            for batch in val_data:
                pred = predictor(batch["state_now"], batch["obs_history"],
                                batch["launch_cmd"], batch["dt_steps"])
                val_loss += ((pred - batch["future_positions"])**2).mean().item()

        scheduler.step()
        if epoch % 10 == 0:
            print(f"Epoch {epoch}: train_loss={total_loss/len(train_data):.6f}, "
                  f"val_loss={val_loss/len(val_data):.6f}")

训练数据收集

从 Ch26 的仿真环境中收集训练数据：

def collect_trajectory_data(env, num_episodes=10000, history_len=10, pred_horizon=20):
    """从仿真中收集 ground truth 轨迹数据。"""
    dataset = []

    for ep in range(num_episodes):
        env.reset()
        trajectory = []

        for step in range(500):  # 5 秒 × 100 Hz
            obs, reward, done, info = env.step(torch.zeros(1, 0, device=env.device))
            label = collect_ground_truth_label(env)
            trajectory.append(label)

            if done[0]:
                break

        # 从轨迹中采样训练样本
        for t in range(history_len, len(trajectory) - pred_horizon):
            sample = {
                "obs_history": torch.stack([trajectory[t-i]["ball_pos_c"]
                                           for i in range(history_len, 0, -1)]),
                "state_now": torch.cat([trajectory[t]["ball_pos_c"],
                                       trajectory[t]["ball_vel_w"]]),
                "launch_cmd": trajectory[0]["launch_cmd"],
                "future_positions": torch.stack([trajectory[t+j]["ball_pos_c"]
                                                for j in range(1, pred_horizon+1)]),
            }
            dataset.append(sample)

    return dataset

数据集分割策略

关键工程决策：如何分割训练和验证数据？

如果只按 episode 随机分割，训练和验证的速度/角度分布一样——无法检测泛化性。正确做法是按物理条件分割：

def split_by_physics(dataset, speed_threshold=35.0):
    """按发球速度分割数据集。

    训练用 speed < threshold 的数据
    验证用 speed >= threshold 的数据
    → 验证集测试的是"对未见过的高速球的泛化能力"
    """
    train_data = [s for s in dataset if s["launch_cmd"][0] < speed_threshold]
    val_data = [s for s in dataset if s["launch_cmd"][0] >= speed_threshold]
    return train_data, val_data

也可以按 elevation 分割（训练用负角度，验证用正角度）或按 topspin 分割（训练用无旋，验证用有旋）。每种分割方式测试不同维度的泛化性。

预测器评估指标

指标	定义	目标值	用途
Position RMSE @ impact	落地点预测误差	< 30 cm	球拍定位
Time-to-impact MAE	落地时间预测误差	< 20 ms	挥拍时机
Trajectory ADE	平均位移误差	< 10 cm	整体轨迹质量
Physics-only baseline	不含 GRU 的纯物理预测误差	参考基线	衡量 GRU 增益

def evaluate_predictor(predictor, test_data):
    """评估预测器性能。"""
    predictor.eval()
    impact_errors = []
    time_errors = []
    ade_errors = []

    with torch.no_grad():
        for batch in test_data:
            # 完整预测
            pred = predictor(batch["state_now"], batch["obs_history"],
                            batch["launch_cmd"], batch["dt_steps"])
            gt = batch["future_positions"]

            # Position RMSE @ impact（最后一步的误差）
            impact_err = (pred[:, -1] - gt[:, -1]).norm(dim=-1)
            impact_errors.append(impact_err)

            # 落地时间步评估（找 z < 0 的第一步）——这里用"落地"作为轨迹质量的
            # 稳定参考事件，衡量轨迹预测精度；与 reward 用的击球 time_to_impact 不同
            pred_impact_step = (pred[:, :, 2] < 0).float().argmax(dim=1)
            gt_impact_step = (gt[:, :, 2] < 0).float().argmax(dim=1)
            time_err = (pred_impact_step - gt_impact_step).abs().float() * 0.01  # steps → seconds
            time_errors.append(time_err)

            # Average Displacement Error
            ade = (pred - gt).norm(dim=-1).mean(dim=1)
            ade_errors.append(ade)

    impact_rmse = torch.cat(impact_errors).mean().item()
    time_mae = torch.cat(time_errors).mean().item()
    ade_mean = torch.cat(ade_errors).mean().item()

    print(f"Impact Position RMSE: {impact_rmse:.4f} m ({impact_rmse*100:.1f} cm)")
    print(f"Time-to-Impact MAE: {time_mae*1000:.1f} ms")
    print(f"Trajectory ADE: {ade_mean:.4f} m ({ade_mean*100:.1f} cm)")

    return {"impact_rmse": impact_rmse, "time_mae": time_mae, "ade": ade_mean}

消融实验：Physics-Only vs Physics+GRU

这是本节最重要的实验——它量化了 GRU 残差的增量贡献：

# 消融实验
# 1. Physics-only baseline
class PhysicsOnlyPredictor(PhysicsResidualPredictor):
    def forward(self, state_now, obs_history, launch_cmd, dt_steps):
        return self.physics_baseline(state_now, dt_steps)  # 不加残差

physics_only = PhysicsOnlyPredictor()
physics_results = evaluate_predictor(physics_only, test_data)

# 2. Physics + GRU
full_predictor = PhysicsResidualPredictor()
# ... 训练 full_predictor ...
full_results = evaluate_predictor(full_predictor, test_data)

# 3. Pure GRU (no physics baseline)
class PureGRUPredictor(nn.Module):
    def forward(self, state_now, obs_history, launch_cmd, dt_steps):
        # 直接从历史预测未来，不用物理 baseline
        # ... GRU → 完整位置预测 ...
        pass

pure_gru = PureGRUPredictor()
# ... 训练 pure_gru ...
gru_results = evaluate_predictor(pure_gru, test_data)

# 对比
print("\n=== Ablation Results ===")
print(f"Physics-only: Impact RMSE = {physics_results['impact_rmse']:.4f} m")
print(f"Physics+GRU:  Impact RMSE = {full_results['impact_rmse']:.4f} m")
print(f"Pure GRU:     Impact RMSE = {gru_results['impact_rmse']:.4f} m")
print(f"\nGRU residual improvement: "
      f"{(1 - full_results['impact_rmse']/physics_results['impact_rmse'])*100:.1f}%")

预期结果： - Physics-only：对有旋转的球预测较差（Magnus 未建模），impact RMSE 约 20-40 cm - Physics+GRU：GRU 补偿了 Magnus 残差，impact RMSE 约 5-15 cm - Pure GRU：在训练分布内可能比 Physics+GRU 稍好，但在 held-out 速度范围上明显更差

PACE 的双通道设计

PACE（arXiv:2509.21690）使用了一个特别优雅的双通道设计，值得在教学中详细分析：

通道 1：Learned Predictor（运行时用）。一个轻量预测器，输入近期球位置历史，输出预测的未来目标击球点 / 弹后 apex（论文中是 $\mathbb{R}^3$ 量级的目标点，而非一整条多步状态轨迹）。这个 predictor 在训练和部署时都使用——它的预测作为 policy observation 的一部分（PACE 论文已 zero-shot 部署到真实 Booster T1 人形）。

通道 2：Physics Predictor（训练时用）。使用精确物理模型（包括弹跳、Magnus），输出精确的未来球状态。它在训练中身兼三职：监督 learned predictor（提供预测标签）、构造 dense reward（"策略应让球拍移动到预测的目标击球点"）、以及作为 critic 的 privileged 信息。部署时 actor 只用 learned predictor 的输出。

注：下文示例代码中的"10 帧输入 / 20 步输出 / input_dim=30"等具体维度是本书教学项目的设定，并非 PACE 论文的原始超参；PACE 官方实现的 predictor 更轻量（近期若干帧 → 目标击球点）。

为什么不用 learned predictor 构造 reward？ 因为 learned predictor 在训练初期不准确（刚开始训练，没见过足够数据），用不准确的预测构造 reward 会导致 reward 噪声大、策略训练不稳定。physics predictor 从第一天起就相当准确（物理定律不需要"学习"），提供了稳定的 reward 信号。

# PACE 的双通道架构（伪代码）
class PACEPredictor:
    def __init__(self):
        self.learned = LearnedMLP(input_dim=30, output_dim=60)  # 10帧×3D → 20帧×3D
        self.physics = PhysicsPredictor()  # Ch26 的弹道模型

    def get_policy_obs(self, ball_history):
        """运行时：learned predictor 的输出作为 policy obs 的一部分。"""
        return self.learned(ball_history)

    def get_reward_target(self, ball_state_gt):
        """训练时：physics predictor 的输出用于构造 reward。"""
        return self.physics.propagate(ball_state_gt)

这种双通道设计在 Ch28 的 reward 工程中非常有用——它让 reward 从第一天起就是准确的（基于物理预测），同时让 policy 逐步学会使用 learned predictor 的（可能不那么准确的）输出。

⚠️ 常见陷阱

⚠️ 概念误区：GRU 残差大说明物理模型差 - 大残差也可能是 GRU 过拟合训练分布的结果 - 正确做法：分别记录 physics-only error 和 physics+GRU error

⚠️ 编程陷阱：GRU 输入不含 $\Delta t$ - 如果帧率变化（从 100 Hz 变为 50 Hz），GRU 不知道时间间隔变了 - 正确做法：每帧附带时间间隔 $\Delta t$

⚠️ 编程陷阱：训练和评估的 held-out split 不合理 - 如果只按 episode 随机分，训练和验证的速度/角度分布一样——无法检测泛化性 - 正确做法：按 speed/elevation 做 held-out split（例如训练用 20-35 m/s，验证用 35-45 m/s）

⚠️ 编程陷阱：残差不做 clamp - 异常输入时 GRU 可能输出极大残差 → 预测点在球场之外 - 正确做法：torch.clamp(residual, -0.5, 0.5)

练习

[设计题] 设计 GRU predictor 的输入输出维度。输入 10 帧 (pos3, confidence1, dt1) + cmd8，输出 20 步残差 (pos3)，总维度各是多少？
[编程题] 收集 1000 条仿真轨迹，训练 PhysicsResidualPredictor，对比 physics-only 和 physics+GRU 的预测误差（position RMSE at impact time）。
[思考题] PACE 的双通道设计中，如果去掉 physics predictor 只用 learned predictor 构造 reward，训练初期会发生什么？提示：考虑 learned predictor 在训练初期的准确度。

预测器给出了未来球位置。但这个预测是基于"过去的观测"做出的——如果观测有延迟，预测的起点就已经偏了。下一节处理延迟补偿。

27.8 延迟补偿的数学与工程 ⭐⭐⭐

这一节解决什么问题：延迟是网球感知中最隐蔽但影响最大的问题。

动机：延迟的定量分析

延迟类型	典型值	来源
相机曝光	1-16 ms	快门速度
图像传输	1-5 ms	USB/网络
检测推理	5-20 ms	CNN forward pass
滤波更新	< 1 ms	EKF 矩阵运算
控制通信	1-5 ms	串口/以太网
执行器响应	5-50 ms	电机惯性

总延迟可达 20-100 ms。30 m/s 球速 + 30 ms 延迟 = 0.9 m 位置误差——整个球拍都够不到。

关键认知：在仿真中 ground truth 没有延迟（ball.data.root_pos_w 是当前帧的精确状态），所以延迟问题在纯仿真阶段不显现。但在 Sim2Real 时这是最大的性能杀手。本章处理延迟的目的是：(1) 建立延迟补偿的数学框架；(2) 在仿真中注入模拟延迟来训练对延迟鲁棒的策略。

延迟补偿的三步法（核心三步 + 可选第四步）

Step 1：给每个观测记录采样 timestamp $t_{\text{obs}}$。

Step 2：EKF 更新到 $t_{\text{obs}}$（而非当前控制时间），得到 $\hat{\mathbf{x}}(t_{\text{obs}})$, $P(t_{\text{obs}})$。

Step 3：用过程模型前推到当前控制时间 $t_{\text{ctrl}}$：

\[\hat{\mathbf{x}}(t_{\text{ctrl}}) = f(\hat{\mathbf{x}}(t_{\text{obs}}), t_{\text{ctrl}} - t_{\text{obs}})$$ $$P(t_{\text{ctrl}}) = F P(t_{\text{obs}}) F^T + Q \cdot \frac{t_{\text{ctrl}} - t_{\text{obs}}}{\Delta t_{\text{nominal}}}\]

Step 4（可选）：继续前推到未来击球时间 $t_{\text{hit}}$。

def compensate_delay(ekf, obs_timestamp, ctrl_timestamp):
    """延迟补偿：从观测时间前推到控制时间。"""
    delay = ctrl_timestamp - obs_timestamp  # 正值，单位秒

    if delay <= 0:
        return ekf.x.clone(), ekf.P.clone()

    # 从当前 EKF 状态前推 delay 秒
    x_compensated = ekf.propagate_to(delay)

    # 协方差也要前推
    F = ekf._compute_F(delay)
    Q = ekf._compute_Q(delay)
    P_compensated = torch.bmm(torch.bmm(F, ekf.P), F.transpose(1, 2)) + Q

    return x_compensated, P_compensated

仿真中注入模拟延迟

为了训练对延迟鲁棒的策略，需要在仿真中模拟延迟。方法是维护一个观测延迟 buffer：

class DelayedObservationBuffer:
    """模拟感知延迟的观测 buffer。"""

    def __init__(self, num_envs, obs_dim, max_delay_steps, device):
        self.max_delay = max_delay_steps
        self.buffer = torch.zeros(num_envs, max_delay_steps + 1, obs_dim, device=device)
        self.timestamps = torch.zeros(num_envs, max_delay_steps + 1, device=device)

    def push(self, obs, timestamp):
        """推入最新观测。"""
        self.buffer = torch.roll(self.buffer, -1, dims=1)
        self.timestamps = torch.roll(self.timestamps, -1, dims=1)
        self.buffer[:, -1] = obs
        self.timestamps[:, -1] = timestamp

    def get_delayed(self, delay_steps):
        """获取延迟后的观测。

        Args:
            delay_steps: [N] int, 每个环境的延迟步数（可以是随机的！）

        Returns:
            delayed_obs: [N, obs_dim]
        """
        batch_idx = torch.arange(self.buffer.shape[0], device=self.buffer.device)
        idx = self.max_delay - delay_steps  # 往回取
        return self.buffer[batch_idx, idx]

    def reset(self, env_ids):
        """重置指定环境的 buffer。"""
        self.buffer[env_ids] = 0.0
        self.timestamps[env_ids] = 0.0

LATENT 的观测噪声与延迟 DR 配置

LATENT 的消融实验是球类运动 RL 中最有说服力的工程证据。以下是其 observation noise 和延迟 DR 的详细配置（根据论文描述重构）：

class LATENTObsNoiseCfg:
    """LATENT 的球状态观测噪声配置。"""

    # 位置噪声
    ball_pos_noise_std: float = 0.02   # 2 cm（外部动捕精度约 1-3 mm，
                                        # 但考虑了球检测和标定误差后放大到 2 cm）

    # 速度噪声
    ball_vel_noise_std: float = 0.5    # 0.5 m/s（从位置差分+噪声放大）

    # 观测延迟（随机化）
    obs_delay_range: tuple = (0, 3)    # 0-3 帧延迟（在 50 Hz 控制频率下
                                        # 对应 0-60 ms）

    # 漏检概率
    miss_rate: float = 0.05            # 5% 的帧检测不到球

消融结果总结：

配置	真机正手成功率	真机反手成功率
完整（noise + delay + DR）	90.9%	77.8%
去掉 dynamics DR	14-29%	14-29%
去掉 observation noise	50%	0%
去掉所有噪声和 DR	~10%	~5%

关键观察：反手成功率从 78% 降到 0%——这意味着 observation noise randomization 对反手击球是生死攸关的。为什么反手比正手更敏感？因为反手动作的机械约束更多（手臂的活动范围更小），需要更精确的时机控制，而精确时机依赖于准确的延迟补偿——如果训练时没见过延迟，部署时延迟就会导致系统性的时机偏差。

延迟补偿在训练中的三种使用方式

方式	描述	优点	缺点
训练时不补偿，部署时补偿	训练用 ground truth，部署时加补偿模块	训练简单	策略没学过处理补偿后的"预测状态"
训练时注入延迟，不做补偿	策略直接学习处理"旧观测"	策略自己学会"提前量"	策略可能学到次优的隐式补偿
训练时注入延迟 + 做补偿	策略看到的是补偿后的预测状态	最接近部署场景	训练实现更复杂

LATENT 使用方式 2（注入延迟但不显式补偿），让策略隐式学习处理延迟。ETH 羽毛球使用方式 3（训练时注入 perception-aware 噪声并做 EKF 补偿）。对教学项目，推荐先用方式 1（最简单），验证后切换到方式 3（最接近部署）。

延迟对 impact time 预测的影响

延迟不仅影响位置估计，还影响时间预测。如果观测延迟 30 ms，你看到的球位置是 30 ms 前的——但你不知道它"已经在这个位置待了 30 ms 了"。如果不做延迟补偿，你对 time-to-impact 的估计会系统性地偏大 30 ms（因为球实际上比你以为的更近了 30 ms 的路程）。

# 延迟对 time-to-impact 的影响
delay = 0.03  # 30 ms
ball_speed = 30.0  # m/s

# 无补偿：以为球在 30ms 前的位置
distance_error = ball_speed * delay  # 0.9 m
# time-to-impact 偏大：多算了 0.9/30 = 0.03 s = 30 ms
# → 策略以为还有时间，实际上球已经快到了

# 有补偿：把球状态前推 30 ms
# → 正确估计当前位置，time-to-impact 准确

这就是为什么 time_to_impact 比 position 更关键——30 ms 的时间误差可能导致挥拍完全打空。

⚠️ 常见陷阱

⚠️ 编程陷阱：timestamp 用帧计数器而非真实时间 - 帧率不稳定时帧计数器不反映真实时间间隔 - 正确做法：用仿真时钟绝对时间

⚠️ 思维陷阱：认为"EKF 平滑"就是延迟 - 平滑可能掩盖了系统性时间滞后 - 区分方法：同时记录 obs time、filter time、ctrl time、predicted impact time

练习

[估算题] 总延迟 40 ms，球速 35 m/s，延迟位置误差多少？是击球面半径 15 cm 的多少倍？
[编程题] 实现 DelayedObservationBuffer，在训练中注入 0-3 帧随机延迟。对比有延迟和无延迟时的 predictor 误差。

所有技术组件就绪后，最后一步是将它们组装成一个可被 Ch28 控制器直接调用的接口。

27.9 预测器输出接口与 Ch28 衔接 ⭐⭐⭐

这一节解决什么问题：定义给控制器的预测输出接口——控制器不关心 EKF 内部状态，只关心"球将到哪、还有多久、可信吗"。

PredictorOutput 数据结构

from dataclasses import dataclass

@dataclass
class PredictorOutput:
    """感知管线输出给控制器的接口。"""

    # 核心字段
    predicted_state_now: torch.Tensor   # [N, 6] 当前球状态估计 (pos3, vel3)
    impact_pos_c: torch.Tensor          # [N, 3] 预测击球点（court-local）
    impact_vel_c: torch.Tensor          # [N, 3] 击球时球速
    time_to_impact: torch.Tensor        # [N] 距击球还有多少秒
    uncertainty: torch.Tensor           # [N] 预测不确定性（标准差 trace）
    validity: torch.Tensor              # [N] bool, 预测是否可用

    # 辅助字段
    mode: torch.Tensor                  # [N] int, 0=airborne, 1=bounced, 2=lost
    predicted_trajectory: torch.Tensor   # [N, H, 3] 完整预测轨迹（可选）
    bounce_count: torch.Tensor           # [N] int, 已弹跳次数

为什么 `time_to_impact` 比 `impact_pos` 更关键

控制器最紧迫的需求是"我还有多少时间准备"——这决定了挥拍时机。impact_pos 告诉"球在哪"，但 time_to_impact 告诉"我还能等多久"。一个有 20 cm 位置误差但时间准确的预测，比一个位置精确但时间偏 50 ms 的预测更有用——因为时间偏差意味着球拍在错误的时刻到达正确的位置（球还没来或已经过去了）。

HITTER 的实验数据可作参考：ball prediction accuracy 在击球前 0.5 秒达到位置误差 < 7.5 cm（约球拍半径），在击球前 0.3 秒达到 timing 误差 < 20 ms（约一个控制步）。也就是说，系统对击球位置与击球时间这两项预测精度都有量化要求（论文并未提出"先 timing 后 position"的优化顺序）。

PredictorOutput 与 Ch28 Reward 的关联

PredictorOutput 的每个字段对应 Ch28 中不同的 reward 组件：

PredictorOutput 字段	Ch28 Reward 用途	具体示例
`impact_pos_c`	approach reward	`r = -\|\|robot_pos - impact_pos\|\|`
`time_to_impact`	timing reward	`r = exp(-\|swing_time - time_to_impact\|/sigma)`
`impact_vel_c`	racket alignment reward	`r = cos_angle(racket_normal, -impact_vel)`
`uncertainty`	confidence gating	`if uncertainty > threshold: r_swing = 0`
`validity`	action masking	`if not validity: disable swing action`

工程关键点：PACE 的双通道设计（27.7 节）在这里特别有用——训练时用 physics predictor（精确）构造上述 reward，确保 reward 信号从第一天起就是准确的。同时让 policy 的 obs 中包含 learned predictor 的输出，让策略逐步学会使用不那么完美的预测。

# Ch28 中使用 PredictorOutput 构造 reward 的示例
def compute_approach_reward(robot_racket_pos, predictor_output):
    """距离击球点越近，reward 越高。"""
    if not predictor_output.validity.any():
        return torch.zeros_like(predictor_output.time_to_impact)

    distance = (robot_racket_pos - predictor_output.impact_pos_c).norm(dim=-1)
    reward = torch.exp(-distance / 0.3)  # 0.3 m sigma
    # 只对 valid 预测给 reward
    reward = reward * predictor_output.validity.float()
    return reward

def compute_timing_reward(current_time, swing_start_time, predictor_output):
    """挥拍时机越准，reward 越高。"""
    time_error = torch.abs(swing_start_time - predictor_output.time_to_impact)
    reward = torch.exp(-time_error / 0.02)  # 20 ms sigma
    return reward * predictor_output.validity.float()

def compute_confidence_gated_swing_reward(contact_occurred, predictor_output):
    """只在预测可信时给挥拍 reward。"""
    confident = predictor_output.uncertainty < 0.3  # 30 cm 阈值
    reward = contact_occurred.float() * confident.float()
    return reward

完整感知管线的集成

class TennisPerceptionPipeline:
    """完整的感知管线：观测 → EKF → 预测 → 接口输出。"""

    def __init__(self, num_envs, device, config):
        self.ekf = BallEKFWithDrag(num_envs, device,
                                    alpha=config.drag_alpha,
                                    sigma_a=config.sigma_a,
                                    sigma_obs=config.sigma_obs)
        self.predictor = PhysicsResidualPredictor(
            history_len=config.history_len,
            pred_horizon=config.pred_horizon,
        ).to(device)
        self.history = BallObsHistoryBuffer(
            num_envs, config.history_len, obs_dim=5, device=device
        )
        self.delay_buffer = DelayedObservationBuffer(
            num_envs, obs_dim=3, max_delay_steps=config.max_delay, device=device
        )
        self.dt = config.env_dt  # 控制时间步

    def step(self, raw_observation, sim_time, launch_cmd,
             delay_steps=None) -> PredictorOutput:
        """每个控制步调用一次。"""
        # 1. 延迟模拟（如果启用）
        self.delay_buffer.push(raw_observation, sim_time)
        if delay_steps is not None:
            obs = self.delay_buffer.get_delayed(delay_steps)
            obs_time = sim_time - delay_steps * self.dt
        else:
            obs = raw_observation
            obs_time = sim_time

        # 2. EKF 预测步
        self.ekf.predict(self.dt)

        # 3. EKF 更新步（假设都检测到了，漏检时 detected=False）
        detected = torch.ones(obs.shape[0], dtype=torch.bool, device=obs.device)
        self.ekf.update(obs, detected)

        # 4. 弹跳检测和处理
        bounced = detect_bounce(None, self.ekf)
        if bounced.any():
            handle_bounce(self.ekf, bounced.nonzero(as_tuple=True)[0])

        # 5. 延迟补偿
        if delay_steps is not None:
            state_now, P_now = compensate_delay(self.ekf, obs_time, sim_time)
        else:
            state_now = self.ekf.x.clone()

        # 6. 更新历史 buffer
        confidence = torch.ones(obs.shape[0], 1, device=obs.device)
        dt_col = torch.full((obs.shape[0], 1), self.dt, device=obs.device)
        self.history.push(
            torch.cat([obs, dt_col, confidence], dim=-1),
            sim_time
        )

        # 7. 轨迹预测
        dt_steps = torch.full((self.predictor.pred_horizon,), self.dt,
                              device=obs.device)
        predicted_traj = self.predictor(
            state_now, self.history.buffer, launch_cmd, dt_steps
        )

        # 8. 提取 impact point
        impact_pos, impact_step = self.predictor.compute_impact_point(predicted_traj)
        time_to_impact = (impact_step + 1).float() * self.dt
        impact_vel = state_now[:, 3:6]  # 简化：用当前速度近似击球时速度

        # 9. 不确定性估计
        uncertainty = self.ekf.position_uncertainty

        # 10. validity 判断
        validity = (
            (time_to_impact > 0.05) &    # 至少 50 ms
            (time_to_impact < 2.0) &      # 不超过 2 秒
            (uncertainty < 1.0) &          # 不确定性 < 1 m
            ((impact_pos[:, 2] - 0.6).abs() < 0.5) # 击球点接近击球高度(strike_height≈0.6m)
        )

        return PredictorOutput(
            predicted_state_now=state_now,
            impact_pos_c=impact_pos,
            impact_vel_c=impact_vel,
            time_to_impact=time_to_impact,
            uncertainty=uncertainty,
            validity=validity,
            mode=torch.zeros(obs.shape[0], dtype=torch.int, device=obs.device),
            predicted_trajectory=predicted_traj,
            bounce_count=torch.zeros(obs.shape[0], dtype=torch.int, device=obs.device),
        )

    def reset(self, env_ids, init_pos, init_vel=None):
        """episode reset 时重置感知管线。"""
        self.ekf.reset(env_ids, init_pos, init_vel)
        self.history.reset(env_ids)

⚠️ 常见陷阱

⚠️ 编程陷阱：teacher-student 评估时泄漏 privileged 信号 - student 评估 pipeline 中仍把 ground truth 拼进 observation → 离线表现好，部署崩溃 - 正确做法：明确两套 observation schema，评估时断言 privileged key 不存在

⚠️ 概念误区：只输出未来位置数组给控制器 - 控制器需要决策语义——球是否到达可击区域、还剩多少时间、是否可信、是否放弃 - 正确做法：输出 PredictorOutput 数据结构，包含 validity 和 uncertainty

练习

[设计题] 如果控制器需要决定"这个球要不要打"（放弃不可达的球），PredictorOutput 中哪些字段参与这个决策？写出决策逻辑的伪代码。
[跨章综合题] 结合 Ch26 的发球参数和本章的 EKF + predictor，设计一个完整的实验：固定发球参数 → 收集 ground truth 轨迹 → 加噪声模拟观测 → 运行 EKF → GRU 预测 → 计算 impact point error 和 time-to-impact error。

27.10 Teacher-Student 感知蒸馏 ⭐⭐⭐

这一节解决什么问题：如何从 privileged teacher 过渡到 deployable student。

动机：为什么需要蒸馏

回顾 Ch09 的核心思想：在仿真中我们有完美信息（ground truth），但部署时没有。直接用 ground truth 训练的策略在部署时会面对从未见过的噪声和延迟——表现可能灾难性地下降。蒸馏是桥梁：先用 privileged teacher 建立性能上限，再把 teacher 的"知识"迁移到只用 sensor observation 的 student。

在网球感知中，蒸馏有两种应用：

状态估计蒸馏：teacher 直接读 ground truth 球状态，student 从有噪声的观测（EKF 输出）估计球状态。蒸馏目标是让 student 的状态估计接近 ground truth。
策略蒸馏：teacher 用 ground truth 球状态做控制决策，student 用 EKF 估计做控制决策。蒸馏目标是让 student 的动作接近 teacher 的动作。

推荐顺序：先做状态估计蒸馏（更简单、更容易 debug），成功后再考虑策略蒸馏。

信息退化阶梯的工程实现

回顾 27.3 的信息退化阶梯，这里给出每一步的具体实现：

Teacher（阶段 1）：ground truth 球状态作为 observation

class TeacherObsCfg:
    """Teacher 看到完美球状态。"""
    ball_state = ObsTermCfg(
        func=ball_state_privileged,   # [pos3 + vel3 + ang_vel3] = 9D
        enable_corruption=False,       # 无噪声
    )
    robot_state = ObsTermCfg(
        func=mdp.joint_pos_rel,
    )
    # ... 其他机器人 obs

Student（阶段 5-6）：有噪声、有延迟的球状态估计

class StudentObsCfg:
    """Student 看到 perception pipeline 的输出。"""
    ball_state_estimate = ObsTermCfg(
        func=perception_pipeline_output,   # EKF 估计 + 噪声
        enable_corruption=True,            # 可选额外噪声
    )
    predictor_output = ObsTermCfg(
        func=predictor_impact_output,      # impact_pos + time_to_impact
    )
    robot_state = ObsTermCfg(
        func=mdp.joint_pos_rel,
    )

蒸馏 target 的选择

不推荐第一版直接蒸馏策略（action distillation）——先蒸馏状态估计，再蒸馏控制。理由：

状态估计蒸馏：teacher 输出 ground truth 球状态，student 从视觉/noisy obs 预测球状态。Loss = L2(student_state_estimate, ground_truth_state)。这是一个监督学习问题，容易训练和 debug。
策略蒸馏：teacher 的 action 和 student 的 obs 不一致时蒸馏困难。ETH 羽毛球的实践是用 DAgger 而非纯离线蒸馏。

状态估计蒸馏的完整训练管线

class BallStateEstimator(nn.Module):
    """Student 网络：从有噪声的观测历史估计球的真实状态。"""

    def __init__(self, obs_history_dim, output_dim=6):
        super().__init__()
        self.encoder = nn.Sequential(
            nn.Linear(obs_history_dim, 128),
            nn.ReLU(),
            nn.Linear(128, 64),
            nn.ReLU(),
        )
        self.gru = nn.GRU(input_size=64, hidden_size=64, num_layers=1, batch_first=True)
        self.head = nn.Linear(64, output_dim)  # 输出 pos3 + vel3

    def forward(self, obs_history):
        """从 N 帧有噪声的观测估计当前球状态。

        Args:
            obs_history: [B, N, obs_per_frame] 最近 N 帧观测

        Returns:
            estimated_state: [B, 6] (pos3 + vel3)
        """
        B, N, D = obs_history.shape
        # 逐帧编码
        encoded = self.encoder(obs_history.reshape(B * N, D)).reshape(B, N, 64)
        # GRU 时序融合
        gru_out, _ = self.gru(encoded)
        # 取最后一帧的输出
        return self.head(gru_out[:, -1])


def train_state_estimator(estimator, env, num_episodes=5000, lr=1e-3):
    """训练球状态估计器（蒸馏 teacher → student）。

    训练数据：在仿真中收集 (noisy_obs_history, ground_truth_state) 对
    """
    optimizer = torch.optim.Adam(estimator.parameters(), lr=lr)
    history_buffer = BallObsHistoryBuffer(
        env.num_envs, history_length=10, obs_dim=3, device=env.device
    )

    for ep in range(num_episodes):
        env.reset()
        history_buffer.reset(torch.arange(env.num_envs, device=env.device))
        episode_loss = 0.0
        steps = 0

        for step in range(500):
            obs, reward, done, info = env.step(
                torch.zeros(env.num_envs, 0, device=env.device)
            )

            # Ground truth（teacher label）
            gt_pos = env.scene["ball"].data.root_pos_w[:, :3] - env.scene.env_origins
            gt_vel = env.scene["ball"].data.root_lin_vel_w[:, :3]
            gt_state = torch.cat([gt_pos, gt_vel], dim=-1)  # [N, 6]

            # 有噪声的观测（student input）
            noise = torch.randn_like(gt_pos) * 0.02
            noisy_obs = gt_pos + noise
            history_buffer.push(noisy_obs, env.sim_time)

            # 只在历史 buffer 填满后开始训练
            if step >= 10:
                obs_history = history_buffer.buffer.clone()  # [N, 10, 3]
                estimated_state = estimator(obs_history)

                # L2 loss（位置和速度分开加权）
                pos_loss = ((estimated_state[:, :3] - gt_state[:, :3])**2).mean()
                vel_loss = ((estimated_state[:, 3:6] - gt_state[:, 3:6])**2).mean()
                loss = pos_loss + 0.1 * vel_loss  # 速度权重较低（更难估计）

                optimizer.zero_grad()
                loss.backward()
                torch.nn.utils.clip_grad_norm_(estimator.parameters(), 1.0)
                optimizer.step()

                episode_loss += loss.item()
                steps += 1

            if done.any():
                reset_ids = done.nonzero(as_tuple=True)[0]
                history_buffer.reset(reset_ids)

        if ep % 100 == 0 and steps > 0:
            print(f"Episode {ep}: avg_loss = {episode_loss/steps:.6f}")

蒸馏效果评估

def evaluate_state_estimator(estimator, env, num_episodes=100):
    """评估 student 状态估计器 vs ground truth。"""
    pos_errors = []
    vel_errors = []

    estimator.eval()
    history_buffer = BallObsHistoryBuffer(
        env.num_envs, history_length=10, obs_dim=3, device=env.device
    )

    for ep in range(num_episodes):
        env.reset()
        history_buffer.reset(torch.arange(env.num_envs, device=env.device))

        for step in range(500):
            obs, reward, done, info = env.step(
                torch.zeros(env.num_envs, 0, device=env.device)
            )

            gt_pos = env.scene["ball"].data.root_pos_w[:, :3] - env.scene.env_origins
            gt_vel = env.scene["ball"].data.root_lin_vel_w[:, :3]
            noisy_obs = gt_pos + torch.randn_like(gt_pos) * 0.02
            history_buffer.push(noisy_obs, env.sim_time)

            if step >= 10:
                with torch.no_grad():
                    est = estimator(history_buffer.buffer.clone())
                pos_err = (est[:, :3] - gt_pos).norm(dim=-1)
                vel_err = (est[:, 3:6] - gt_vel).norm(dim=-1)
                pos_errors.append(pos_err.mean().item())
                vel_errors.append(vel_err.mean().item())

            if done.any():
                history_buffer.reset(done.nonzero(as_tuple=True)[0])

    print(f"Position estimation RMSE: {torch.tensor(pos_errors).mean():.4f} m "
          f"({torch.tensor(pos_errors).mean()*100:.1f} cm)")
    print(f"Velocity estimation RMSE: {torch.tensor(vel_errors).mean():.4f} m/s")
    print(f"Baseline (raw noisy obs): 2.0 cm position, ~1.7 m/s velocity")

ETH 羽毛球的 Perception-Aware Training

ETH 的创新不是蒸馏——而是在 RL 训练阶段就注入感知误差模型。他们的 observation noise 不是简单的高斯噪声，而是基于真实相机数据标定的误差模型：

位置噪声随距离增大——因为远处球在图像中像素更少
位置噪声随机器人运动速度增大——运动模糊效应
漏检概率随球速和遮挡增大——高速小目标更容易丢失

def perception_aware_noise(ball_pos_true, robot_state, config):
    """ETH 风格的 perception-aware 噪声模型。

    噪声量级不是常数，而是随机器人和球的状态动态变化。
    """
    # 距离依赖：远处噪声大
    robot_head_pos = robot_state[:, :3]
    distance = (ball_pos_true - robot_head_pos).norm(dim=-1, keepdim=True)
    distance_noise = config.base_noise + config.distance_scale * (distance / 10.0)

    # 运动依赖：机器人快速运动时噪声大（运动模糊）
    robot_speed = robot_state[:, 3:6].norm(dim=-1, keepdim=True)
    motion_factor = 1.0 + config.motion_scale * robot_speed

    # 球速依赖：高速球在图像中更模糊
    ball_speed = ball_pos_true.diff(dim=0).norm(dim=-1, keepdim=True)  # 近似
    speed_factor = 1.0 + config.ball_speed_scale * ball_speed / 30.0

    # 总噪声标准差
    total_noise_std = distance_noise * motion_factor * speed_factor

    # 漏检模拟
    miss_prob = config.base_miss_rate + config.distance_miss_scale * (distance / 15.0).squeeze(-1)
    detected = torch.rand_like(miss_prob) > miss_prob

    # 带噪声的观测
    ball_pos_noisy = ball_pos_true + torch.randn_like(ball_pos_true) * total_noise_std

    return ball_pos_noisy, detected

核心区别：ETH 的 perception-aware training 和 LATENT 的 observation noise randomization 看似相同（都在训练时注入噪声），但有关键区别：

维度	LATENT（固定噪声 + DR）	ETH（动态噪声模型）
噪声来源	固定 $\sigma$ + 随机化 $\sigma$ 范围	基于物理的噪声模型
噪声是否依赖状态	否（与球位置/机器人状态无关）	是（随距离和运动变化）
策略能否学到"主动感知"	不容易（噪声不可控）	可以（减速 → 噪声降低 → 策略可能学到"先稳定再出手"）
工程复杂度	低	高（需标定噪声模型参数）

ETH 的方法在概念上更优雅，但 LATENT 的方法在工程上更简单。对教学项目，推荐先用 LATENT 方案（固定噪声 + DR），高级项目再考虑 ETH 方案。

Asymmetric Actor-Critic（HITTER 方案）

HITTER 使用了一种更简洁的方法：asymmetric actor-critic。

Critic：看到 ground truth 球状态（privileged）→ 能准确估计 value → 提供稳定的训练信号
Actor：只看到有噪声的观测 → 学会在噪声下做决策

这在 Ch09 中已经介绍过——critic 的 privileged obs 只在训练时使用，部署时只需要 actor。

class AsymmetricObsCfg:
    class ActorCfg:
        # 只有 sensor-based 观测（部署时使用）
        ball_pos_noisy = ObsTermCfg(func=ball_pos_with_noise)
        ball_history = ObsTermCfg(func=ball_pos_history)  # 最近 10 帧
        predictor_output = ObsTermCfg(func=predictor_impact_output)  # impact_pos + tti
        robot_state = ObsTermCfg(func=mdp.joint_pos_rel)
        last_action = ObsTermCfg(func=mdp.last_action)

    class CriticCfg:
        # 包含 privileged 信息（仅训练时使用）
        ball_pos_perfect = ObsTermCfg(func=ball_state_privileged)
        ball_future_trajectory = ObsTermCfg(func=ball_future_positions)  # 未来 20 步真实位置
        ball_pos_noisy = ObsTermCfg(func=ball_pos_with_noise)
        ball_history = ObsTermCfg(func=ball_pos_history)
        robot_state = ObsTermCfg(func=mdp.joint_pos_rel)
        last_action = ObsTermCfg(func=mdp.last_action)

反事实推理：如果 critic 也只看有噪声的观测（对称 actor-critic）——critic 的 value 估计会因噪声而不稳定，导致 PPO 的 advantage 估计嘈杂，训练收敛慢。privileged critic 提供了"干净"的 value 信号，让 actor 在噪声观测下更高效地学习。

感知策略的选择矩阵

方法	适用场景	训练复杂度	部署复杂度	教学推荐
Ground truth only	纯仿真、验证控制上限	⭐	不适用	第一步
Asymmetric AC	有外部动捕的实验室	⭐⭐	⭐	第二步
Perception-aware	机载视觉部署	⭐⭐⭐	⭐⭐⭐	高级
状态估计蒸馏	通用	⭐⭐	⭐⭐	第三步
策略蒸馏 (DAgger)	已有 teacher policy	⭐⭐⭐⭐	⭐⭐	研究级

⚠️ 常见陷阱

⚠️ 编程陷阱：蒸馏时 student obs 中混入了 teacher 信号 - 常见 bug：评估 student 时 observation schema 仍包含 ground truth key - 正确做法：用 assert "privileged" not in obs_keys 硬性防护

⚠️ 概念误区：认为 asymmetric actor-critic 一定比对称版本好 - 如果 privileged 信号和 sensor 信号差异太大，critic 学到的 value 可能对 actor 的 obs 分布不准 - 正确做法：先用对称版本（teacher 的 obs 也给 actor）建立上限，再切换到非对称版本

⚠️ 思维陷阱：跳过 ground truth 阶段直接做蒸馏 - 如果不知道 ground truth 下的性能上限，蒸馏后的性能下降无法归因 - 正确做法：按信息退化阶梯逐步推进，每步记录性能

练习

[设计题] 设计一个完整的 teacher → student 蒸馏训练方案。Teacher 用什么 obs 训练？Student 用什么 obs？蒸馏的 loss 函数是什么？评估时如何确保 privileged 信号不泄漏？
[思考题] ETH 羽毛球的 perception-aware training 和 LATENT 的 observation noise randomization 本质上是同一种方法吗？从"策略能否学到主动感知"的角度分析区别。
[编程题] 实现 BallStateEstimator 并在仿真中训练。对比 student 的位置估计 RMSE 和直接用 noisy observation 的 RMSE。计算 student 相对于 raw observation 的改善百分比。

本章常见误解汇总

误解	正确理解
"detector mAP 高就能打好球"	mAP 不关心延迟和时间连续性
"EKF 是万能滤波器"	EKF 依赖过程模型的正确性
"纯 GRU 比 physics+residual 更灵活"	纯 GRU 泛化性差，physics baseline 是最强先验
"延迟只影响位置估计"	延迟同时影响位置和时间预测
"仿真中没有延迟问题"	Sim2Real 时延迟是最大性能杀手
"弹跳后 EKF 自动适应"	弹跳是 mode change，需要显式处理
"time-to-impact 不如 position 重要"	挥拍时机比到达位置更关键
"端到端视觉控制最简单"	端到端最难 debug，分层验证效率更高

本章建立的心智模型

原始观测（可能有噪声/延迟）
    ↓
Ground Truth Label 设计（frame 统一 + timestamp）
    ↓
观测方案选择（直接状态 → 动捕 → 机载视觉）
    ↓
EKF 状态估计
    ├── 预测步：物理模型（重力 + drag + Magnus）
    ├── 更新步：Kalman 增益自动权衡
    └── 弹跳处理：mode switch + 协方差重置
    ↓
GRU 残差预测（物理 baseline + learned residual）
    ↓
延迟补偿（timestamp → forward propagation）
    ↓
PredictorOutput 接口
    ├── impact_pos_c     → Ch28 球拍目标位置
    ├── time_to_impact   → Ch28 挥拍时机
    ├── uncertainty       → Ch28 是否挥拍
    └── validity          → Ch28 是否放弃本球

本章小结

知识点总表

编号	知识点	核心要点	对应节	难度
1	感知五层分离	真实状态→成像→感知→估计→预测	27.1	⭐⭐
2	六个失败原因	小/快/抛物/弹跳/残差/延迟	27.1	⭐⭐
3	Label 设计	frame 统一 + timestamp + privileged 标注	27.2	⭐⭐
4	观测历史 buffer	滑动窗口 + reset 时清空	27.2	⭐⭐
5	三种观测方案	直接状态 / 外部动捕 / 机载视觉	27.3	⭐⭐
6	信息退化阶梯	逐步移除 privileged signal	27.3	⭐⭐⭐
7	EKF 完整推导	预测步 + 更新步 + Kalman 增益	27.4	⭐⭐⭐
8	Kalman 增益物理含义	R→$\infty$ 信模型，P→$\infty$ 信观测	27.4	⭐⭐⭐
9	批量 GPU EKF 实现	torch.bmm + torch.linalg.inv	27.4	⭐⭐⭐
10	含 drag 的非线性 EKF	Jacobian 的非线性部分推导	27.5	⭐⭐⭐
11	弹跳检测与模式切换	event-based reset + 协方差重置	27.6	⭐⭐⭐
12	GRU 残差预测	物理 baseline + learned residual + clamp	27.7	⭐⭐⭐
13	PACE 双通道设计	learned predictor for obs + physics predictor for reward	27.7	⭐⭐⭐
14	延迟补偿三步法	timestamp → EKF update → forward propagation	27.8	⭐⭐⭐
15	PredictorOutput 接口	impact_pos + time_to_impact + uncertainty + validity	27.9	⭐⭐⭐
16	Asymmetric Actor-Critic	Critic privileged + Actor sensor-only	27.10	⭐⭐⭐
17	Perception-aware training	噪声模型随距离和运动变化	27.10	⭐⭐⭐

累积项目：本章新增模块

项目进度更新：

阶段	能力	新增于
环境构建	EntityCfg → SceneCfg → ManagerBasedRlEnvCfg → Registry	Ch04, Ch15
Obs/Action 设计	五条原则 + 双框架配置	Ch05
Reward/Curriculum	四类奖励 + 渐进 curriculum	Ch06
训练管线	PPO 超参 + 多后端适配	Ch07
Domain Randomization	EventManager + 分阶段 DR	Ch08
Teacher-Student	特权学习 + 蒸馏	Ch09
大规模训练	多 GPU + NaN 排查 + 性能优化	Ch24
训练诊断	九种模式 + 症状索引 + 验证流程	Ch25
球类环境底座	球场坐标 + 球物理 + 发球命令 + 弹道模型	Ch26
感知管线	Label设计 + EKF + GRU残差 + 延迟补偿 + PredictorOutput	Ch27

本章代码产出清单

模块	类/函数名	功能	节
Label 收集	`collect_ground_truth_label()`	ground truth 数据收集	27.2
Frame 转换	`body_to_world_velocity()`	body→world 四元数旋转	27.2
历史 Buffer	`BallObsHistoryBuffer`	N 帧滑动窗口	27.2
基础 EKF	`BallEKF`	6D 纯重力 EKF（批量 GPU）	27.4
含 Drag EKF	`BallEKFWithDrag`	6D 含空气阻力 EKF	27.5
弹跳检测	`BounceDetectorWithCooldown`	带冷却的弹跳检测	27.6
弹跳处理	`handle_bounce_with_spin()`	考虑旋转的弹跳速度修正	27.6
GRU 残差	`PhysicsResidualPredictor`	物理baseline + learned residual	27.7
训练器	`train_residual_predictor()`	带时间权重的 L2 训练	27.7
评估器	`evaluate_predictor()`	Impact RMSE + Time MAE + ADE	27.7
延迟 Buffer	`DelayedObservationBuffer`	延迟模拟	27.8
延迟补偿	`compensate_delay()`	forward propagation	27.8
感知管线	`TennisPerceptionPipeline`	完整管线集成	27.9
输出接口	`PredictorOutput`	Ch28 消费的数据结构	27.9
状态估计器	`BallStateEstimator`	student 蒸馏网络	27.10
蒸馏训练	`train_state_estimator()`	Teacher→Student 训练	27.10

本章实验清单

实验	目的	预期产出
EKF vs raw observation	验证 EKF 的噪声平滑效果	位置 RMSE 降低 50%+
纯重力 vs 含 drag EKF	验证 drag 模型的必要性	预测误差降低 60-85%
Jacobian 数值验证	确保解析 Jacobian 正确	max_diff < 1e-4
弹跳 vs 无弹跳处理	验证弹跳检测的必要性	弹跳后误差降低 80%+
Physics-only vs Physics+GRU	量化 GRU 残差的贡献	Impact RMSE 降低 30-50%
消融 held-out speed	验证 GRU 泛化性	physics+GRU > pure GRU 在 OOD
延迟 0 vs 延迟 30ms	量化延迟影响	位置误差增大 0.9 m @ 30 m/s
信息退化阶梯	逐步移除 privileged	每步记录 impact error

延伸阅读

资料	地址	难度	与本章的关系
Kalman 1960 原始论文	doi:10.1115/1.3662552	⭐⭐⭐	EKF 理论基础
Cho et al. 2014 GRU	arXiv:1406.1078	⭐⭐	GRU 残差网络基础
PACE 代码库	arXiv:2509.21690	⭐⭐⭐	双通道 predictor 设计
Gray-box 乒乓球预测	arXiv:2305.15189	⭐⭐⭐	EKF + 神经旋转估计
HITTER 论文	arXiv:2508.21043	⭐⭐⭐	解析弹道预测 + asymmetric AC
ETH 羽毛球	Science Robotics 2025	⭐⭐⭐	perception-aware training
Phybot 人形羽毛球	arXiv:2511.11218	⭐⭐	EKF + prediction-free 变体
LATENT 代码库	`github.com/GalaxyGeneralRobotics/LATENT`	⭐⭐⭐	dynamics randomization + obs noise
Bar-Shalom et al.	Estimation with Applications to Tracking	⭐⭐⭐⭐	状态估计经典教科书
事件相机乒乓球预测	arXiv:2506.07860	⭐⭐	低延迟感知的前沿方向

🔧 故障排查手册

症状	可能原因	排查步骤	相关节
轨迹预测"慢半拍"	延迟未补偿	1. 记录 obs time 和 ctrl time 2. 检查 compensate_delay 3. 看 innovation 偏移	27.8
EKF 太抖	$R$ 太小	1. 打印 innovation 方差 2. 增大 $R$ 3. 检查观测噪声是否匹配	27.4
EKF 跟不上变化	$Q$ 太小	1. 检查 state error lag 2. 增大 $Q$ 3. 检查是否遗漏 drag	27.4/27.5
GRU 分布外崩溃	过拟合	1. 检查是否 physics+residual 2. held-out split 3. 限制残差幅度	27.7
弹跳后预测失效	无 mode switch	1. 检查 bounce detection 2. 加 event-based reset 3. 重置协方差	27.6
impact_pos 误差大	空气动力学模型不准	1. 对比 physics-only 和 physics+GRU 误差 2. 标定 alpha 3. 增加 GRU 训练数据	27.5/27.7
time_to_impact 总是偏大	忽略了 drag 导致球飞得比预测近	1. 切换到含 drag 的 EKF 2. 降低 sigma_a	27.5
validity 总是 False	不确定性阈值太紧	1. 打印 uncertainty 分布 2. 放宽阈值 3. 检查 P 是否正常减小	27.9
Teacher 性能好 Student 差	蒸馏 loss 不收敛或信号泄漏	1. 检查 student obs 无 privileged key 2. 检查蒸馏 loss 曲线 3. 增加训练数据	27.10

给下一章的桥

本章建立了从原始观测到可消费预测接口的完整感知管线。核心产出是 PredictorOutput 数据结构——它包含了 Ch28 控制器需要的所有信息：击球点在哪（impact_pos_c）、还有多久（time_to_impact）、预测有多可信（uncertainty）、是否应该尝试击球（validity）。

本章向 Ch28 传递的接口规格：

接口	类型	格式	Ch28 如何使用
`predicted_state_now`	状态估计	`[N, 6]`	实时球位置跟踪
`impact_pos_c`	预测击球点	`[N, 3]` court-local	球拍目标位置
`impact_vel_c`	击球时球速	`[N, 3]`	球拍速度和面法向计算
`time_to_impact`	剩余时间	`[N]` 秒	挥拍时机决策
`uncertainty`	不确定性	`[N]` 米	是否激进挥拍
`validity`	可用性	`[N]` bool	是否尝试击球
`predicted_trajectory`	完整轨迹	`[N, H, 3]`	可视化和 debug

Ch28 将在此基础上设计击球控制策略——接收 PredictorOutput，输出机器人的关节动作。本章定义的接口是 Ch28 的"输入规格书"——如果 time_to_impact 不准确，Ch28 的挥拍时机就会错；如果 validity 判断不可靠，Ch28 可能在不该挥拍时挥拍（或该挥拍时不挥）。这就是为什么本章的 EKF 标定和预测器验证如此重要——它们直接决定了 Ch28 控制策略的训练效率和最终性能。

从更宏观的角度看：Ch26 建立了"物理世界"（球场 + 球），Ch27 建立了"感知世界"（状态估计 + 预测），Ch28 将建立"行动世界"（控制策略 + 执行）。三者通过明确的接口连接——这种分层设计让每个模块可以独立开发、独立测试、独立改进。

情况	物理含义	\(K\) 的行为	滤波器表现
\(R \to \infty\)	观测完全不可信	\(K \to 0\)	纯靠模型预测
\(P \to \infty\)	模型完全不可信	位置子空间增益趋于单位映射（\(H\) 为 \(3\times6\) 非方阵，无普通逆），未观测速度由 \(P\) 的位置-速度交叉协方差修正	完全相信观测
\(R \approx 0\)	观测非常精确	\(K\) 较大	每次更新大幅修正
\(Q \approx 0\)	模型非常精确	\(P\) 缓慢增长，\(K\) 较小	观测影响有限

方法	原理	优点	缺点	推荐阶段
Ground truth 检测	直接读 \(z < \epsilon\) 且 \(v_z < 0\)	完美	仅限仿真	第一阶段
Innovation 检测	\(\\|\mathbf{y}_k\\| > \tau\) 时判断	无需额外传感器	阈值敏感	所有阶段
接触力检测	MuJoCo `contact` 数组中有球-地面接触	物理精确	需要 API 支持	仿真阶段

维度	LATENT（固定噪声 + DR）	ETH（动态噪声模型）
噪声来源	固定 \(\sigma\) + 随机化 \(\sigma\) 范围	基于物理的噪声模型
噪声是否依赖状态	否（与球位置/机器人状态无关）	是（随距离和运动变化）
策略能否学到"主动感知"	不容易（噪声不可控）	可以（减速 → 噪声降低 → 策略可能学到"先稳定再出手"）
工程复杂度	低	高（需标定噪声模型参数）

第 27 章 网球感知与轨迹预测——从 Ground Truth 到 EKF 再到残差学习

前置自测

本章目标

27.1 感知问题的本质：为什么"看见球"远远不够 ⭐⭐

动机：六个让天真方案失败的原因

感知链条的五层分离

前沿系统的感知方案对比

⚠️ 常见陷阱

练习

27.2 Ground Truth Label 设计与 Frame 统一 ⭐⭐

动机：仿真最宝贵的资源是 ground truth

Label 数据规格

Frame 统一

观测历史 buffer 的设计

⚠️ 常见陷阱

练习

Isaac Lab 中的 Label 收集对照

27.3 球状态观测方案对比 ⭐⭐

三种观测方案

方案 A：直接状态（仿真阶段推荐）

方案 B：外部动捕（实验室阶段）

方案 C：机载视觉（自主部署阶段）

从 2D 图像到 3D 球心的四条路线

信息退化阶梯

⚠️ 常见陷阱

练习

27.4 扩展卡尔曼滤波器（EKF）：从贝叶斯到工程实现 ⭐⭐⭐

为什么需要滤波而非直接观测

状态空间定义

过程模型推导

EKF 预测步

观测模型与 EKF 更新步

Kalman 增益的物理意义

完整 Python EKF 实现

\(Q\) 和 \(R\) 参数调优指南

Isaac Lab 中的 EKF 对照

工程要点：批量处理和 GPU 加速

⚠️ 常见陷阱

练习

27.5 含空气阻力的非线性 EKF ⭐⭐⭐

动机：纯重力 EKF 的误差

含阻力的过程模型

Jacobian 的非线性部分

Python 实现

含 Magnus 力的进一步扩展

自适应 Q 调整

Jacobian 的数值验证

含 drag EKF 的预期改进

⚠️ 常见陷阱

Q/R 参数调优速查表

练习

27.6 弹跳检测与模式切换 ⭐⭐⭐

动机：弹跳让 EKF 崩溃

三种弹跳检测方法

弹跳后的 EKF 重置

弹跳冷却机制

Topspin 弹跳的速度修正

Phybot 的 prediction-free 变体

⚠️ 常见陷阱

练习

27.7 GRU 残差预测器：物理先验 + 数据驱动 ⭐⭐⭐

为什么不直接用 GRU 替代物理模型

残差架构的完整实现

训练流程

训练数据收集

数据集分割策略

预测器评估指标

消融实验：Physics-Only vs Physics+GRU

PACE 的双通道设计

⚠️ 常见陷阱

练习

27.8 延迟补偿的数学与工程 ⭐⭐⭐

动机：延迟的定量分析

延迟补偿的三步法（核心三步 + 可选第四步）

仿真中注入模拟延迟

LATENT 的观测噪声与延迟 DR 配置

延迟补偿在训练中的三种使用方式

延迟对 impact time 预测的影响

⚠️ 常见陷阱

练习

第 27 章网球感知与轨迹预测——从 Ground Truth 到 EKF 再到残差学习

为什么 `time_to_impact` 比 `impact_pos` 更关键