EKF pose estimation never matches up

Question

Using the EKF filter from the python AHRS library I'm trying to estimate the pose of the STEVAL FCU001 board (which has has the LSM6DSL IMU sensor for acceleration + gyro and LIS2MDL for magneto). However the attitude simply never matches up even though the IMU is feeding seemingly perfect good data. In the image below you can see the sensor readout, EKF results both from python (local) and the embedded board EKF version (remote), the latter can be ignored.

The movements I am performing with the board (starting close to the 2000 mark) is 90deg pitch down, neutral, 90deg pitch up, neutral, 90deg roll left, neutral, 90deg roll right, neutral. The euler angles are already wrong at the start (should be x and y close to 0 deg) and never match up.

I'm using the EKF library as follows:

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)

I've tried every sort of sensor value mapping I could think off (flip x-y axis, flip the sign for one, two or all axis), use the ENU frame instead of NED, using the EKF library with and without the magnetic sensor values ... Sometimes it is correct at the start, but never for the full test sequence.

Additionally I've tried to find a dataset with sample IMU sensor data that would produce the correct results when run through the EKF filter with which I can compare my inputs, but haven't found anything.

Answer 1

The troubleshooting approach is correct. Here are some suggestions.

1. Fix the Frame: Looking at your dataset, the z-axis accelerometer is pointing up. That means you should use ahrs.filters.EKF(frame='ENU'). The snippet you shared assumed NED.
2. Sample Data: You can download sample datasets from https://imuengine.io/resources/ . The same site lets you process imu data into Euler or inclination angles. That could be useful for comparing results.
   • The sample datasets use NED (i.e. z-axis accelerometer nominally points down)
3. Sample Code Here is an updated version of your code snippet using the fixed-wing UAV dataset from the mentioned site.

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()

Plot from running sample code