结论：locationd 的健康状态是四个独立维度——deviceMotion.valid、inputsOK、sensorsOK、posenetOK，它们不能压缩成一个「正常」布尔值。顶层 valid 只等于 filter_initialized && isfinite(kf.t)，不直接包含另外三项；而 filter_initialized 一旦置真，不会在消息异常时自动变回假。名义频率下，约第 2 个相机坏输入或第 10 个 IMU 坏输入才会让 critical_service_inputs_valid 变假。
调度与启动门：先看名义频率
main() 以 cameraOdometry 为 SubMaster 轮询主题，另开独立 socket 批量读取 IMU 消息。服务表配置 加速度计/陀螺仪名义 104 Hz，相机里程计和 deviceMotion 20 Hz。
| 主题 | 名义频率 | 作用 |
|---|---|---|
| 加速度计 / 陀螺仪 | 104 Hz | IMU 观测源，经独立 socket 批量读取 |
| 相机里程计 | 20 Hz | 轮询主题，同时触发 deviceMotion 发布 |
deviceMotion |
20 Hz | 仅在 sm.updated['cameraOdometry'] 时发布 |
启动门的细节很关键：初始化阶段只在 sm.all_checks() 与 sensor_all_checks() 同时满足时把 filter_initialized 置真；之后此布尔值不会在消息异常时自动变回假。输出侧，deviceMotion 仅在 sm.updated['cameraOdometry'] 时发布——单有 IMU 更新不触发输出。
循环把 IMU 与 SubMaster 更新的消息按 logMonoTime 排序，仅把顶层 valid=True 的消息交给 handle_log()。这是当前批次内排序，不能保证跨批次全局严格有序；历史观测由滤波器最大回绕窗口约束。进程还可读取持久的 LocationFilterInitialState 作为初始状态/协方差来源，但本快照常规运行代码未定位到写入该键的对应路径——不能假定每次启动都有历史姿态。
十八维状态与四类观测
States 把状态分为 NED 姿态、设备速度、角速度、陀螺偏置、设备加速度、加计偏置，共 18 维。generate_code() 定义姿态旋转、速度积分以及 IMU/相机四类观测方程，运行时通过 rednose 生成的 EKF 执行——不是简单对四种速度取平均。四类观测的门各不相同：
| 观测 | 生产条件与变换 | 拒绝 / 降权边界 |
|---|---|---|
| 加速度计 | msg.which() == acceleration，硬件数组重排取负为 [-v[2], -v[1], -v[0]]，用传感器纳秒时间戳 |
时间戳不能为零，与 logMonoTime 差严格不超过 0.1 秒，不能早于滤波状态 0.8 秒以上；bmx055 作为第二 IMU 源被忽略；范数须 <100 m/s² |
| 陀螺仪 | 同样重排取负，先减滤波器陀螺偏置，再与相机偏航率分布交叉比较 | 除时间/来源门外，角速度范数须 <10 rad/s，偏航差须低于相机偏航标准差的 30 倍；只有通过才作为 PHONE_GYRO 观测 |
| 相机旋转 | 以 timestampEof - 0.1 秒作为视觉姿态时刻，用 device_from_calib 旋到设备系 |
受 0.8 秒回绕门；旋转范数 ≤10 rad/s；标准差各分量须 >1e-5，范数不可超对应上限的 10 倍；入滤波前标准差乘 10 |
| 相机平移 | 与旋转同一延迟时间和标定旋转；视觉平移写入 CAMERA_ODO_TRANSLATION |
平移范数 ≤200 m/s；标准差须 >1e-5 且范数不超过 10*200，入滤波前标准差乘 4；方差旋到设备系后给 EKF |
两处时间语义差异必须记住：carState 在此只更新 abs(vEgo) 供 posenetOK 门使用，不是这只姿态滤波器的直接速度观测；extrinsicsCalibration 在 rpyCalib 非空且分量位于 ±0.5 rad 内时更新 device_from_calib。更关键的是：相机观测没有像 IMU 那样检查自身时间戳与 logMonoTime 的 0.1 秒差，而是按 timestampEof-0.1 与滤波器时刻比较——新硬件不能把两种时间语义混为一谈。
坏输入计数：第 2 个相机、第 10 个 IMU
HandleLogResult 区分成功、时间非法、值非法和传感器来源非法。主循环只对关键主题的时间/值非法增加坏输入计数，成功时乘以按频率计算的 calculate_invalid_input_decay()；SENSOR_SOURCE_INVALID 不进入这两个分支。
名义频率下，相机 input_invalid_limit=2、IMU 各为 round(2*104/20)=10，输出门使用比它们各低 0.5 的阈值——所以约第 2 个相机坏输入或第 10 个 IMU 坏输入，才会让 critical_service_inputs_valid=False。源文档特别澄清：实际恢复取决于后续成功样本数量和消息速率，不是固定墙上 10 秒。
四种健康含义各不相同
| 输出字段 | 源码计算 | 不代表什么 |
|---|---|---|
deviceMotion.valid |
filter_initialized && isfinite(kf.t)；同样设置四组 XYZMeasurement.valid |
不直接包含当前 inputsOK/sensorsOK/posenetOK；某传感器掉线时顶层 valid 不一定同步变假 |
inputsOK |
sm.all_valid() 与关键观测坏输入计数门同时成立 |
all_valid() 只检查消息有效位，不等于全部主题当前 alive/频率正常 |
sensorsOK |
对两路 IMU 各保存最近消息的 valid 位；非仿真要求距接收单调时间 <0.1 秒 | 不等于加速度/陀螺数值一定通过 handle_log() 的物理合理性与时间检查 |
posenetOK |
对相机平移标准差的前/后各 20 项均值作比，后均值是前均值 4 倍以上且 >7、车速 >5 m/s 时置假（低速放行） | 它是模型运动估计的波动门，不是整条视觉链的真实性证明 |
复位逻辑同样要分清：_finite_check() 在滤波返回的状态或协方差出现非有限值时调用 reset(t)；但它不自动重置 filter_initialized 或所有输入计数。上游 selfdrived 对 posenetOK/inputsOK 另产生事件，controlsd 消费设备姿态与车辆参数——健康位与事件门是分层契约。
测试与硬件插件边界
test_locationd_scenarios.py 使用路段回放构造 IMU 缺失、数值峰值和 150 ms 时间偏移场景；其测试依赖路段数据下载/缓存，本轮未执行，不据测试文件存在而宣称当前设备已验证。
自制 IMU/相机插件至少要保持轴序、符号、单位、硬件时间戳、logMonoTime 对齐、噪声标准差及顶层 valid 语义；对异步/乱序观测要验证 0.1 秒传感器门、0.8 秒回绕窗口和视觉 0.1 秒模型延迟。算法层允许回退或过滤观测，独立执行器安全限制仍在 panda 固件。
常见问题
顶层 valid 为真，能否说明所有传感器都正常？
不能。deviceMotion.valid 只等于 filter_initialized && isfinite(kf.t)，不直接包含 inputsOK/sensorsOK/posenetOK。由于 filter_initialized 一旦置真不会在消息异常时自动变回假，首次初始化后某传感器掉线，顶层 valid 不一定同步变假。
为什么相机和 IMU 的坏输入阈值差这么多？
因为阈值按名义频率换算：相机 20 Hz 对应 input_invalid_limit=2，IMU 104 Hz 对应 round(2*104/20)=10，输出门再各低 0.5。所以约第 2 个相机坏输入或第 10 个 IMU 坏输入才触发，恢复则取决于后续成功样本数量和消息速率，不是固定时长。
posenetOK 变假是否等于视觉定位失效？
不等于。它是对相机平移标准差前后各 20 项均值作比得到的波动门（后半均值是前半 4 倍以上且 >7、车速 >5 m/s 时置假，低速放行），只反映模型运动估计的波动，不是整条视觉链的真实性证明。
相机观测和 IMU 观测的时间门一样吗？
不一样。IMU 检查自身时间戳不能为零、与 logMonoTime 差严格不超过 0.1 秒、且不早于滤波状态 0.8 秒以上；相机观测没有检查自身时间戳与该 0.1 秒差，而是按 timestampEof-0.1 与滤波器时刻比较。两种时间语义不能混用。
延伸阅读
- CAN 报文解析的「健康位」在说什么：校验、计数器与总线空窗 —— 与本文的多种健康位形成对照：不同层的「健康」含义不同。
- 车载信号为什么会说谎：TTL、源时间与 500 ms 新鲜度阈值 —— 承接 0.1 秒传感器门与数据新鲜度。
- IMU 中断与 I2C 采样契约：一次采样到底在哪一刻发生 —— 与本文 IMU 时间戳与轴序契约直接相关。
- 硬件几何变换：相机内外参怎么进到模型输入 —— 承接
device_from_calib与标定旋转。 - 从使能到车轮：执行链上的八道门，以及它们各自不保证什么 —— 说明算法层允许回退而执行门独立存在。
参考资料
- 项目 02 ——
openpilot-research/LOCATIOND_SENSOR_FUSION_HEALTH_DEEP_DIVE.md（取用了 18 维状态、四类观测门、坏输入计数与四种健康含义）。 - 项目 02 ——
openpilot-research/CAR_PROTOCOL_IMPLEMENTATIONS.md（仅用于确认selfdrived/controlsd与健康位事件的分层关系）。
说明：本文为对固定源码快照的静态结构分析，未运行完整测试套件、未连接硬件、未验证目标车型的真实参数组合。文中所有阈值、行号索引与配置均出自源文档，标注为未验证的结论保留其原有边界。
更新记录
首次发布 2026-10-11 最近核对：2026-10-11
发表回复