三星Galaxy FE21静置时IMU欧拉角漂移问题求助

问题现象 #

使用三星Galaxy FE21的加速度计（420Hz）、陀螺仪（420Hz）、磁力计（100Hz），通过Madgwick滤波计算四元数、欧拉角及旋转矩阵校准加速度计轴时，手机静置状态下出现明显漂移：初始校准后加速度y轴接近重力值-9.81，最终z轴接近该值。

提供的四元数输出片段：

array([[ 0.69195007,  0.02116878,  0.01067919,  0.72155592],
       [ 0.69207007,  0.02128627,  0.01067362,  0.72143744],
       [ 0.69219179,  0.02140226,  0.0106742 ,  0.72131722],
       ...,
       [ 0.99924922,  0.02342693, -0.00675266,  0.03010937],
       [ 0.99926311,  0.02316672, -0.0062859 ,  0.02995027],
       [ 0.99926311,  0.02316672, -0.0062859 ,  0.02995027]])

核心原因分析 #

1. 传感器采样率不匹配与重采样逻辑缺陷 #

• 陀螺仪和加速度计是420Hz，磁力计仅100Hz，直接对高频率传感器做均值重采样到100Hz，会丢失大量高频姿态细节，且容易引入时序对齐误差。
• 代码仅展示了加速度计的重采样流程，若陀螺仪、磁力计的重采样逻辑不一致，会进一步加剧传感器数据的时序偏差，导致Madgwick滤波输入不同步，引发姿态漂移。

2. Madgwick滤波参数与初始化错误 #

• 默认的beta增益参数可能不适合静置场景：静置时应提升加速度计、磁力计的权重，降低陀螺仪权重，否则陀螺仪的零偏误差会持续累积，导致姿态漂移。
• frequency参数设置为100Hz，但需确认重采样后的实际数据频率是否严格等于100Hz，若步长计算错误，滤波积分过程会出现偏差。
• 四元数初始化依赖固定值[0.0, 0.0, 0.707, 0.707]，未根据当前手机实际姿态计算初始值，可能导致滤波收敛缓慢或方向偏移。

3. 坐标系与姿态转换逻辑不统一 #

• 四元数顺序不匹配：ahrs库的Madgwick滤波输出四元数通常为[w, x, y, z]格式，但你的euler_from_quaternion函数将q[3]作为w分量，若实际输出是[x, y, z, w]，会直接导致姿态转换错误，表现为轴的漂移。
• 旋转矩阵顺序错误：手动计算旋转矩阵时采用yaw_mat * pitch_mat * roll_mat的乘法顺序，若与Madgwick滤波的姿态定义（RPY旋转顺序）不一致，会导致加速度计轴映射完全错误。
• 传感器坐标系差异：手机内置传感器的坐标系（x轴向右、y轴向上、z轴垂直屏幕）若与姿态估计所用的ENU/NED坐标系未对齐，也会引发轴的漂移。

4. 传感器零偏未校准 #

• 陀螺仪存在固有零偏，静置时仍会输出微小角速度，积分后会导致姿态偏移；若磁力计受周围磁场干扰，或加速度计零偏未修正，Madgwick滤波的姿态修正能力会大幅下降。

解决方案 #

1. 精确同步传感器数据 #

• 优先将磁力计插值到420Hz（陀螺仪/加速度计的原始频率），保留更多高频姿态数据，再输入Madgwick滤波；若必须用100Hz，需用线性插值而非均值重采样对齐所有传感器数据。
• 确保三个传感器的时间戳基于同一系统基准，避免时序偏差。

2. 优化Madgwick滤波配置 #

• 调整beta参数：静置场景可设置为0.1-0.5，增强加速度计和磁力计的修正权重；动态场景再降低该值。
• 严格匹配frequency参数与实际采样率，比如用420Hz就设为420，100Hz就设为100。
• 用加速度计和磁力计计算初始四元数，替代固定值：

  from ahrs.common.orientation import orientation
  initial_q = orientation(acc_array[0], mag_array[0])
  madgwick = Madgwick(gyr=gyro_array, acc=acc_array, mag=mag_array, frequency=hz, q0=initial_q)
  

3. 统一姿态转换逻辑 #

• 确认Madgwick输出的四元数顺序：查看ahrs库文档，若输出为[w, x, y, z]，则调整euler_from_quaternion函数的索引；或直接用库内置方法转换：

  from ahrs.common.quaternion import Quaternion
  q = Quaternion(qs[i])
  roll_deg, pitch_deg, yaw_deg = q.to_angles()  # 直接得到欧拉角
  rot_matrix = q.to_DCM()  # 直接得到旋转矩阵
  

• 避免手动计算旋转矩阵，减少人为错误。

4. 校准传感器零偏 #

• 陀螺仪零偏校准：静置手机30秒以上，取陀螺仪输出的均值作为零偏，使用前从原始数据中减去该值。
• 加速度计校准：将手机放置在6个不同姿态（正面、反面、上下左右），采集数据后计算零偏和刻度因数，修正加速度计数据。

附用户提供的完整代码片段 #

import pandas as pd
import numpy as np
from ahrs.filters import Madgwick
from ahrs.common.quaternion import Quaternion

freq="0.01S"
hz=100
start=30.0

# begin by adjusting the frequency of the different sensors through resampling. I do this for every sensor.    
acc["Time_s"]=0
acc.Time_s=pd.to_datetime(acc.Time, unit="s")
acc['time_str'] = acc['Time_s'].dt.strftime('%S.%f')
acc.set_index('Time_s', inplace=True)

acc_down=acc.resample(freq).mean()
acc_loc=acc_down.loc[acc_down["Time"]>=start]

min_sample_size=min(len(gyro_loc),len(mag_loc), len(acc_loc))
gyro_loc=gyro_loc.tail(min_sample_size)
acc_loc=acc_loc.tail(min_sample_size)
mag_loc=mag_loc.tail(min_sample_size)

# I then use the outputs for computing the quaternion, euler angles and rotation matrix
madgwick = Madgwick(gyr=gyro_array, acc=acc_array, mag=mag_array, frequency=hz)

#https://automaticaddison.com/how-to-convert-a-quaternion-into-euler-angles-in-python/
q = np.array([0.0, 0.0, 0.707, 0.707])

# Compute the Euler angles
def euler_from_quaternion(q):
    r0=2*(q[3]*q[0]+q[1]*q[2])
    r1=1-2*(q[0]**2+q[1]**2)
    roll = np.arctan2(r0,r1)
    roll_deg = np.degrees(roll)

    p1=2*(q[3] * q[1] - q[2]*q[0])
    pitch=np.arcsin(p1)
    pitch_deg=np.degrees(pitch)

    y1=2*(q[3] * q[2]+q[0]*q[1])
    y2=1-2*(q[1]**2+q[2]**2)
    yaw=np.arctan2(y1,y2)
    yaw_deg=np.degrees(yaw)

    return(roll_deg, pitch_deg, yaw_deg)

#compute rotation matrix
def compute_r_matrix(roll, pitch, yaw):
    roll=np.radians(roll)
    pitch=np.radians(pitch)
    yaw=np.radians(yaw)
    roll_mat=[[1,0,0],[0,np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]
    pitch_mat=[[np.cos(pitch), 0, np.sin(pitch)], [0,1,0], [-np.sin(pitch), 0, np.cos(pitch)]]
    yaw_mat=[[np.cos(yaw), -np.sin(yaw), 0], 
             [np.sin(yaw), np.cos(yaw), 0], 
             [0,0,1]]
    
    rot_mat = np.dot(yaw_mat, np.dot(pitch_mat, roll_mat))
    #print(rot_mat)
    return(rot_mat)

qs=madgwick.Q
i=0
adjust_x_list=list()
adjust_y_list=list()
adjust_z_list=list()
for q in qs:
    roll_deg, pitch_deg, yaw_deg=euler_from_quaternion(q)
    rot_matrix=compute_r_matrix(roll_deg, pitch_deg, yaw_deg)
    adjust_a=np.dot(rot_matrix, acc_array[i])
    i = i + 1
    adjust_x_list.append(adjust_a[0])
    adjust_y_list.append(adjust_a[1])
    adjust_z_list.append(adjust_a[2])

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