EKF 姿态估计永远不会匹配

Question

使用 python AHRS 库中的 EKF 滤波器，我尝试估计 STEVAL FCU001 板的位姿（该板具有用于加速的 LSM6DSL IMU 传感器 + 陀螺仪和用于磁电机的 LIS2MDL）。然而，即使 IMU 提供看似完美的良好数据，态度也永远不会匹配。在下图中，您可以看到传感器读数、来自 python（本地）和嵌入式板 EKF 版本（远程）的 EKF 结果，后者可以忽略。

我用冲浪板执行的动作（从接近 2000 标记开始）是 90 度俯仰、中立、90 度向上俯仰、中立、向左滚动 90 度、中立、向右滚动 90 度、中立。欧拉角一开始就已经是错误的（x 和 y 应该接近 0 度）并且永远不会匹配。

我使用 EKF 库如下：

if qpv is None: # qpv is previous quaternion, if any
    qpv = [1., 0., 0., 0.]
orientation = ahrs.filters.EKF(frame='NED')
samples = len(acc)
Q = np.tile(qpv, (samples, 1))
for i in range(1, samples):
    orientation.Dt = tsd[i][0] / 1000.0 # uses monotonic clock values to calculate time delta between measurements
    qp = Q[i - 1]
    Q[i] = orientation.update(qp, acc=acc[i], gyr=gyro[i], mag=None)

我尝试过我能想到的各种传感器值映射（翻转 x-y 轴，翻转一个、两个或所有轴的符号），使用 ENU 框架而不是 NED，使用带或不带磁传感器的 EKF 库值...有时它在开始时是正确的，但对于完整的测试序列却永远不会正确。

此外，我还尝试找到一个包含示例 IMU 传感器数据的数据集，该数据集在通过 EKF 过滤器运行时会产生正确的结果，我可以将其与我的输入进行比较，但没有找到任何内容。

Answer 1

故障排除方法正确。这里有一些建议。

1. 修复框架：查看您的数据集，z 轴加速度计指向上方。这意味着您应该使用

   ahrs.filters.EKF(frame='ENU')

   。您分享的片段假设为

   NED

   。
2. 样本数据：您可以从https://imuengine.io/resources/下载样本数据集。同一站点可让您将 imu 数据处理为欧拉角或倾角。这对于比较结果可能很有用。
   • 示例数据集使用

     NED

     （即 z 轴加速度计名义上指向下方）
3. 示例代码这是使用上述站点的固定翼无人机数据集的代码片段的更新版本。

import ahrs
import pandas as pd
import numpy as np

# Load dataset downloaded from https://imuengine.io
filename = 'motion-engine-2012-umn-uav-thor79.csv'
df = pd.read_csv(filename)
dt_sec = df['TimeFromStart (s)'].diff()[1]
acc = df[['AccelX (m/s^2)', 'AccelY (m/s^2)', 'AccelZ (m/s^2)']].values.tolist()
gyro = df[['AngleRateX (rad/s)', 'AngleRateY (rad/s)', 'AngleRateZ (rad/s)']].values.tolist()
mag = df[['MagFieldX (G)', 'MagFieldY (G)', 'MagFieldZ (G)']].values.tolist()

# Setup Estimator
qpv = None
if qpv is None: # qpv is previous quaternion, if any
    qpv = [1., 0., 0., 0.]
    orientation = ahrs.filters.EKF(frame='NED')
    samples = len(acc)
    Q = np.tile(qpv, (samples, 1))

# Run Estimator
for i in range(1, samples):
    orientation.Dt = dt_sec
    qp = Q[i - 1]
    Q[i] = orientation.update(qp, acc=acc[i], gyr=gyro[i], mag=mag[i])

# Convert to Euler angles
angles = [ahrs.Quaternion(qk).to_angles() for qk in Q]
angles = np.array(angles)

# Plot
from matplotlib import pyplot as plt
plt.plot(np.rad2deg(angles))
plt.legend(['AngleRoll (deg)','AnglePitch (deg)', 'AngleYaw (deg)'])
plt.title('Sample Data from imuengine.io\n %s' % filename)
plt.xlabel('TimeFromStart (s)')
plt.show()

运行示例代码的绘图

Answer 2

感谢您的回答！我也遇到了这个问题，但我通过将参考系从“NED”设置为“ENU”来解决它。首先我使用 ahrs.filter.EFK() 来计算方向。原始 IMU 数据是从 Matlab imuSensor 模拟的，它可以在“NED”帧中生成原始 IMU 数据。但是，如果我在“NED”帧中使用 ahrs.filter.EFK()，它永远不会匹配。我设置为“ENU”框架后结果匹配。我猜 ahrs.filter.EFK() 中的参考系有问题。