Python 从惯性/传感器框架到车身框架的转换

我正在与IMU合作，为我提供通用全球坐标系的方向。我已经从静态姿势创建了一个具有已知方向的线段坐标系。我在大腿上放置了两个传感器，围绕全局Z旋转约90度，在骨盆上放置了一个传感器。我需要将传感器方向转换为CS段方向。当我测量骨盆和CS段之间的角度时，无论哪个大腿传感器控制CS段，结果都应该是相同的

以下是我所知道的：

q_w_Pelvis = []
q_w_R_Upperarm_medial = []
q_w_R_Upperarm_lateral = []
for i in range(len(q_Pelvis_pq)):
    q_w_Pelvis.append(q_Pelvis_pq[i]*q_start_Pelvis.conjugate )
    q_w_R_Upperarm_medial.append(q_R_Upperarm_medial_pq[i]*q_start_R_Upperarm_medial.conjugate)
    q_w_R_Upperarm_lateral.append(q_R_Upperarm_lateral_pq[i]*q_start_R_Upperarm_lateral.conjugate)


q_segment_Pelvis = []
q_segment_R_Upperarm_medial = []
q_segment_R_Upperarm_lateral = []
for i in range(len(q_Pelvis)):
    q_segment_Pelvis.append(q_segment_start_Pelvis*q_w_Pelvis[i]*q_segment_start_Pelvis.conjugate)
    q_segment_R_Upperarm_medial.append(q_segment_start_R_Upperarm_medial*q_w_R_Upperarm_medial[i]*q_segment_start_R_Upperarm_medial.conjugate)
    q_segment_R_Upperarm_lateral.append(q_segment_start_R_Upperarm_lateral*q_w_R_Upperarm_lateral[i]*q_segment_start_R_Upperarm_lateral.conjugate)

接着是欧拉角的计算，我知道这是前一个项目的成果

但结果不正确，因为两个传感器没有为我提供相同的CS段旋转。