4. 姿态与位置估计：EKF2状态估计器日志、GPS融合状态、视觉/惯导融合(VIO)日志、位置估计的协方差分析
状态估计是飞控的“大脑”。说白了，它得回答三个问题：我现在在哪儿？我朝向哪？我运动得快不快？PX4 里干这活的主力是 EKF2（扩展卡尔曼滤波器）。
我刚开始接触 PX4 时，总觉得 EKF2 是个黑盒子。直到有一次飞机在空中突然“抽风”，日志里 EKF 的 innovation 飙得老高，我才意识到——不懂 EKF2 日志，你根本没法排查这类疑难杂症。
这一章，我们就来拆解 EKF2 日志里的关键信息。包括 GPS 怎么融合进去的，视觉/惯导（VIO）又是怎么工作的，以及协方差到底在告诉我们什么。
4.1 EKF2 状态估计器日志结构
EKF2 的日志主要分布在几个 uORB 主题里。你打开 .ulog 文件后，最常看的是这几个：
| 主题名 | 频率 | 核心内容 |
|---|---|---|
estimator_status |
~25 Hz | 滤波器状态、故障标志、GPS/视觉健康度 |
estimator_innovations |
~25 Hz | 各传感器的 innovation（观测残差） |
estimator_optical_flow |
~10 Hz | 光流融合状态（如果用了） |
vehicle_attitude |
~100 Hz | 最终输出的姿态四元数 |
vehicle_local_position |
~50 Hz | 最终输出的位置、速度 |
我个人习惯，拿到日志后第一件事就是看 estimator_status 里的 filter_fault_flags。如果这个值不为 0，说明 EKF 内部已经检测到异常了。嗯，这里要注意——不是所有故障都会导致飞机炸机，但它是重要的排查线索。
关键字段速查：
gps_check_fail_flags：GPS 自检失败的原因（卫星数、HDOP、速度精度等）gps_fusion_active：GPS 是否正在被融合进滤波器vision_fusion_active：视觉位置/速度是否正在融合vel_innov/pos_innov：速度/位置的 innovation 值vel_innov_var/pos_innov_var：对应的 innovation 方差
4.2 GPS 融合状态分析
GPS 是室外飞行的主力传感器。但 GPS 信号不是一直可靠的。EKF2 内部有一套 GPS 检查机制，只有通过检查的数据才会被融合。
我记得有一次在楼群间飞行，日志里 GPS 的 HDOP 只有 0.8，看起来很好。但飞机位置却在缓慢漂移。后来发现是 gps_check_fail_flags 里的 GPS_CHECK_FAIL_VEL_ACC 亮了——速度精度超标了。说白了，GPS 位置虽然准，但速度噪声太大，EKF 选择不信任它。
怎么看 GPS 融合是否正常？我一般看这三个指标：
- gps_fusion_active：必须为 1。如果为 0，检查
gps_check_fail_flags。 - gps_pos_innov：位置 innovation，单位米。正常值在 ±0.5 以内。超过 ±1.0 就要警惕。
- gps_vel_innov：速度 innovation，单位 m/s。正常值在 ±0.3 以内。
避坑指南：我曾经遇到一个案例，GPS innovation 一直很小，但飞机位置却偏了 10 米。后来发现是 GPS 天线相位中心与机体坐标系原点不重合，导致位置偏移被 EKF 当成了正常误差。解决办法是检查 EKF2_GPS_POS_X/Y/Z 参数是否设置正确。
4.3 视觉/惯导融合（VIO）日志解读
VIO 在室内或 GPS 信号差的地方是救命稻草。PX4 支持通过 vehicle_visual_odometry 主题接收 VIO 数据（比如来自 Intel T265 或 ZED 相机）。
VIO 融合的状态，同样在 estimator_status 里看：
vision_fusion_active：视觉位置/速度是否在融合vision_pos_innov：视觉位置 innovationvision_vel_innov：视觉速度 innovation
这里有个坑——VIO 数据有 时间戳对齐 的问题。如果 VIO 的时间戳和飞控的 IMU 时间戳不同步，innovation 会周期性跳动。我见过一个案例，T265 的时钟漂移导致每 10 秒出现一次 innovation 尖峰，飞机跟着晃一下。
我的排查技巧：在日志里同时拉出 vehicle_visual_odometry.timestamp 和 sensor_combined.timestamp，计算时间差。如果差值超过 10ms 且不稳定，说明时间同步有问题。可以尝试调整 EKF2_EV_DELAY 参数来补偿。
4.4 位置估计的协方差分析
协方差，说白了就是 EKF 对自己的估计有多“自信”。协方差小，说明滤波器认为当前估计很准；协方差大，说明它心里没底。
在 estimator_status 里，协方差相关的字段有：
| 字段 | 含义 | 正常范围 |
|---|---|---|
covariances[0] |
位置 X 方向协方差 | < 0.1 m² |
covariances[1] |
位置 Y 方向协方差 | < 0.1 m² |
covariances[2] |
位置 Z 方向协方差 | < 0.2 m² |
covariances[3] |
速度 X 方向协方差 | < 0.05 (m/s)² |
covariances[4] |
速度 Y 方向协方差 | < 0.05 (m/s)² |
covariances[5] |
速度 Z 方向协方差 | < 0.1 (m/s)² |
你想想看，如果位置协方差突然从 0.01 跳到 1.0，说明 EKF 对当前位置完全没信心了。这时候如果还在用位置模式飞行，飞机很可能会乱飘。
为什么会这样？常见原因有：
- GPS 信号丢失或质量下降
- IMU 饱和（剧烈运动导致加速度计超出量程）
- 磁力计受到干扰（导致航向发散，进而影响位置）
实战经验：我习惯在日志分析时，把 covariances[0] 和 gps_fusion_active 画在同一张图上。如果协方差上升的同时 GPS 融合标志变为 0，那问题基本就定位了——GPS 丢了，EKF 在靠 IMU 积分，协方差自然发散。
4.5 EKF2 状态估计核心流程
为了让你更直观地理解 EKF2 怎么处理这些传感器数据，我画了一张流程图。它展示了从传感器输入到最终状态输出的关键路径。
这张图里，你重点关注“融合决策”这个节点。EKF2 不是一股脑把所有传感器数据都吃进去，它会根据每个传感器的健康状态、innovation 大小、协方差变化，动态决定信任谁、不信任谁。这也是为什么日志分析时，estimator_status 里的标志位比原始传感器数据更重要。
4.6 实战：一次完整的 EKF 日志分析
假设你拿到一段日志，飞机在起飞后 30 秒开始剧烈晃动。我们按步骤来：
- 看故障标志：打开
estimator_status.filter_fault_flags，发现值为 8（对应FILTER_FAULT_FLAG_BAD_ACC）。说明加速度计数据有问题。 - 看协方差：
covariances[0]在 30 秒时从 0.02 跳升到 0.8。位置估计开始发散。 - 看 innovation：
gps_pos_innov在 30 秒时达到 2.5 米。GPS 和 IMU 的估计出现严重分歧。 - 看原始 IMU：检查
sensor_accel，发现 Z 轴加速度在 30 秒时出现异常尖峰。原来是飞控减震棉脱落，导致加速度计受到高频振动干扰。
我的习惯：在分析这类问题时，我会把 filter_fault_flags、covariances[0]、gps_pos_innov 三个曲线叠在一张图上。故障标志触发的时间点，往往就是协方差和 innovation 开始恶化的起点。这样一眼就能看出因果关系。
好了，这一章的内容就到这里。EKF2 日志分析是个熟能生巧的活，多翻几次日志，你就能建立起“看到某个字段异常 → 联想到可能的原因”的条件反射。