4. 姿态与位置估计：EKF2状态估计日志解读、姿态估计精度评估、位置估计误差分析

状态估计是飞控系统的“大脑”。说白了，它要回答三个问题：我现在朝哪？我在哪？我动得有多快？PX4 里干这活的主力是 EKF2（扩展卡尔曼滤波器）。这一章，我们就钻进日志里，看看 EKF2 到底在干什么，它的输出靠不靠谱。

4.1 EKF2 状态估计日志结构

EKF2 的日志主要分布在几个 topic 里：ekf2_innovations、ekf2_innovations_3、estimator_status、estimator_status_flags。我个人习惯先看 estimator_status，因为它包含了滤波器最核心的健康状态。

下面这张图是我自己总结的 EKF2 日志分析框架，你可以把它当作一个检查清单：

4.2 姿态估计精度评估

姿态估计准不准，不能只看飞得稳不稳。你得看日志里的“创新”（innovation）。什么是创新？就是“传感器实际测量值”和“滤波器预测值”之间的差值。如果这个差值一直很大，说明滤波器在“硬拽”状态量，姿态很可能已经偏了。

4.2.1 关键日志字段

字段名	含义	正常范围
ekf2_innovations.ang_innov[0]	横滚角创新 (roll innovation)	±0.1 rad 以内
ekf2_innovations.ang_innov[1]	俯仰角创新 (pitch innovation)	±0.1 rad 以内
ekf2_innovations.ang_innov[2]	偏航角创新 (yaw innovation)	±0.2 rad 以内
estimator_status.gyro_bias[0-2]	陀螺仪零偏估计值	±0.05 rad/s 以内
estimator_status.accel_bias[0-2]	加速度计零偏估计值	±0.5 m/s² 以内

嗯，这里要注意：偏航角的创新范围比横滚俯仰要大一些，因为偏航主要靠磁力计或 GPS 航向约束，本身精度就差一些。我在项目中遇到过一架飞机，偏航创新一直飙到 0.5 rad，结果发现是磁力计没校准好。

4.2.2 如何判断姿态是否发散

我一般看三个指标：

1. 创新序列是否零均值：如果 ang_innov 长时间偏向正方向，说明滤波器有系统性偏差。
2. 方差是否收敛：看 estimator_status.covariances[0-2]，姿态方差应该随着飞行逐渐减小并稳定。
3. 健康标志：estimator_status_flags.cs_attitude 必须为 true，否则姿态估计不可信。

4.3 位置估计误差分析

位置估计比姿态估计更复杂，因为它融合了 GPS、气压计、空速、视觉（如果有）等多个传感器。每个传感器都有它的“脾气”。

4.3.1 位置与速度创新

位置和速度的创新字段在 ekf2_innovations 和 ekf2_innovations_3 里：

字段名	对应传感器	典型误差来源
pos_innov[0-2]	GPS / 视觉位置	GPS 多径、卫星数不足、视觉漂移
vel_innov[0-2]	GPS 速度 / 光流	GPS 速度跳变、光流纹理不足
hagl_innov	激光测距 / 超声波	地面不平、测距仪噪声
baro_innov	气压计	气流扰动、温度变化

举个例子，baro_innov 如果突然变大，通常是因为飞机在低空悬停时，螺旋桨下洗气流吹到了气压计。我曾经调试一架四旋翼，悬停时高度波动 ±0.5 米，一看日志，baro_innov 在油门变化时同步跳变。后来把气压计用海绵隔离了一下，问题就解决了。

4.3.2 位置误差的典型模式

我总结了三种常见的位置估计异常模式：

• 模式一：GPS 跳变——pos_innov 出现尖峰，同时 vel_innov 也异常。这通常是 GPS 信号被遮挡或干扰。解决办法：检查卫星数和 HDOP。
• 模式二：高度漂移——baro_innov 缓慢增大，posD（高度）跟着漂。这多半是气压计温漂。你看 estimator_status.baro_bias 是不是在缓慢变化。
• 模式三：视觉里程计失效——如果用了视觉定位，pos_innov 突然变大且不收敛，很可能是视觉特征丢失。这时候 EKF2 会退回到 GPS+IMU 模式。

4.4 实战：从日志中提取 EKF2 性能指标

下面是我常用的一个分析脚本片段，用 Python 和 pyulog 读取日志，提取关键指标：

import numpy as np
import matplotlib.pyplot as plt
from pyulog import ULog

# 读取日志
ulog = ULog('flight_log.ulg')
data = ulog.get_dataset('ekf2_innovations')

# 提取偏航创新
yaw_innov = data.data['ang_innov'][:, 2]
time_us = data.data['timestamp']

# 提取姿态方差
status = ulog.get_dataset('estimator_status')
yaw_var = status.data['covariances'][:, 2]

# 画图
plt.figure(figsize=(12, 6))
plt.subplot(2, 1, 1)
plt.plot(time_us * 1e-6, yaw_innov, label='Yaw Innovation')
plt.axhline(y=0.2, color='r', linestyle='--', label='Warning Threshold')
plt.axhline(y=-0.2, color='r', linestyle='--')
plt.ylabel('Innovation (rad)')
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(time_us * 1e-6, yaw_var, label='Yaw Variance', color='orange')
plt.ylabel('Variance')
plt.xlabel('Time (s)')
plt.legend()
plt.tight_layout()
plt.show()

这段代码会画出偏航创新和方差的时间序列。如果创新超过 ±0.2 rad 的阈值，或者方差没有收敛趋势，你就该警惕了。

4.5 避坑指南

最后，分享几个我踩过的坑：

• 不要只看均值——创新均值很小，但方差很大，说明滤波器在来回震荡。这时候要检查传感器数据频率是否稳定。
• 注意时间同步——EKF2 对传感器时间戳很敏感。如果 GPS 和 IMU 的时间不同步，你会看到位置创新出现周期性波动。
• 磁力计是“软柿子”——偏航创新异常，十有八九是磁力计问题。先检查磁力计校准，再检查周围有没有强磁场干扰。