基于Python AHRS库EKF的STEVAL FCU001板位姿估计结果始终不符

1. 修正时间差（Dt）的计算逻辑 #

你的代码中直接使用tsd[i][0]/1000.0作为Dt是错误的——Dt应该是相邻两个采样点的时间间隔，而非当前时间戳的绝对值。时间差错误会导致陀螺仪积分严重漂移，姿态完全失真。

修正代码：

for i in range(1, samples):
    # 计算相邻采样的时间差（假设tsd存储的是毫秒级绝对时间戳）
    dt = (tsd[i][0] - tsd[i-1][0]) / 1000.0
    orientation.Dt = dt
    qp = Q[i - 1]
    Q[i] = orientation.update(qp, acc=acc[i], gyr=gyro[i], mag=None)

2. 确保传感器数据单位与库要求匹配 #

AHRS库的EKF滤波器默认要求：

• 陀螺仪数据单位为弧度/秒：如果你的陀螺仪输出是度/秒，必须转换：

  gyro = gyro * np.pi / 180.0  # 度转弧度
  

• 加速度计数据需归一化到g单位：如果输出是mg（毫g），需除以1000；如果是原始ADC值，需先通过传感器量程转换为实际加速度值。

3. 正确初始化初始姿态 #

直接使用单位四元数[1.,0.,0.,0.]会导致初始欧拉角与实际静止状态偏差，必须用静态加速度计数据计算初始姿态：

# 取前100个静态采样的平均值（确保板子水平静止）
acc_static = np.mean(acc[:100], axis=0)
# 用加速度计计算初始滚转和俯仰角
roll = np.arctan2(acc_static[1], acc_static[2])
pitch = np.arctan2(-acc_static[0], np.sqrt(acc_static[1]**2 + acc_static[2]**2))
# 转换为四元数作为初始值
q_initial = ahrs.Quaternion.from_euler(roll, pitch, 0.0)
Q = np.tile(q_initial.to_array(), (samples, 1))

4. 严格对齐坐标系与传感器轴映射 #

NED坐标系的定义是：X北、Y东、Z下，传感器轴必须完全匹配该定义：

• 静止时，加速度计Z轴应指向地心（输出为+1g左右），如果实际输出为-1g，需反转Z轴：acc[:,2] = -acc[:,2]
• 陀螺仪旋转方向需符合右手定则：绕X轴向前俯，陀螺仪X输出应为正，否则反转对应轴符号。

可以通过静态测试验证：将板子水平静止，归一化后的加速度计输出应接近[0,0,1]（NED）或[0,0,-1]（ENU），不符合则调整轴的符号或顺序。

5. 执行陀螺仪零偏校准 #

陀螺仪静止时的零偏会导致严重的姿态漂移，必须校准：

# 采集静止状态下1000个陀螺仪采样
gyro_bias = np.mean(gyro[:1000], axis=0)
# 后续所有采样减去零偏
gyro = gyro - gyro_bias

6. 优化EKF使用逻辑 #

AHRS库的EKF类可自行维护状态，无需手动传递前序四元数，简化代码同时避免错误：

# 初始化EKF，先设置默认Dt（后续循环中更新）
orientation = ahrs.filters.EKF(frame='NED', Dt=0.01)
# 用静态加速度计初始化姿态
acc_static = np.mean(acc[:100], axis=0)
q_current = ahrs.Quaternion.from_acceleration(acc_static)
Q = np.zeros((samples, 4))
Q[0] = q_current.to_array()

for i in range(1, samples):
    dt = (tsd[i][0] - tsd[i-1][0]) / 1000.0
    orientation.Dt = dt
    q_current = orientation.update(q_current, acc=acc[i], gyr=gyro[i], mag=None)
    Q[i] = q_current.to_array()

测试步骤建议 #

1. 先单独验证传感器数据：静止时加速度计稳定、陀螺仪输出接近0；动作时数据变化符合预期（比如俯仰时加速度计X/Y波动，陀螺仪X轴有输出）。
2. 先测试静态初始化：水平静止时，初始欧拉角应接近0°，否则调整轴映射。
3. 用固定采样率测试（比如100Hz，Dt=0.01），排除时间差计算错误的影响。

内容的提问来源于stack exchange，提问作者Wannes