偏移旋转矩阵

如何解决偏移旋转矩阵

我正在使用 2 个 imu。我需要用传感器的第一帧偏移所有帧。我创建了一个虚构的场景，在那里我精确地知道旋转和想要的结果。我需要两个传感器在减去它们的初始（开始）方向时显示相同的结果。

import numpy as np
# Sensor 0,1 and 2 start orientation in degrees
s0_x = 30
s0_y = 0
s0_z = 0

s1_x = 0
s1_y = 40
s1_z = 0

s2_x = 10
s2_y = 40
s2_z= -10

# Change from start frame 1

x1 = 20
y1 = 10
z1 = 0

# Change from start frame 2

x2 = 60
y2 = 30
z2 = 30

GCS= [[1,0],[0,1,1]]
sensor0 = [[s0_x,s0_y,s0_z],[s0_x,s0_z]]
sensor1 = [[s1_x,s1_y,s1_z],[s1_x + x1,s1_y + y1,s1_z + z1],[s1_x + x1 + x2,s1_y + y1+ y2,s1_z + z1+ z2]]
sensor2 = [[s2_x,s2_y,s2_z],[s2_x + x1,s2_y + y1,s2_z + z1],[s2_x + x1+ x2,s2_y + y1+ y2,s2_z + z1+ z2]]


def Rot_Mat_X(theta):
    r = np.array([[1,np.cos(np.deg2rad(theta)),-np.sin(np.deg2rad(theta))],np.sin(np.deg2rad(theta)),np.cos(np.deg2rad(theta))]]) 
    return r

# rotation the rotation matrix around the Y axis (input in deg)
def Rot_Mat_Y(theta):
    r = np.array([[np.cos(np.deg2rad(theta)),np.sin(np.deg2rad(theta))],[-np.sin(np.deg2rad(theta)),np.cos(np.deg2rad(theta))]]) 
    return r


# rotation the rotation matrix around the Z axis (input in deg)
def Rot_Mat_Z(theta):
    r = np.array([[np.cos(np.deg2rad(theta)),-np.sin(np.deg2rad(theta)),[np.sin(np.deg2rad(theta)),1]]) 
    return r



# Creating the rotation matrices
r_sensor0 = []
r_sensor1= []
r_sensor2= []
for i in range(len(sensor1)):
    r_sensor1_z = np.matmul(Rot_Mat_X(sensor1[i][0]),GCS)
    r_sensor1_zy = np.matmul(Rot_Mat_Y(sensor1[i][1]),r_sensor1_z)
    r_R_Upperarm_medial_zyx = np.matmul(Rot_Mat_Z(sensor1[i][2]),r_sensor1_zy )
    r_sensor1.append(r_R_Upperarm_medial_zyx )
    r_sensor2_z = np.matmul(Rot_Mat_X(sensor2[i][0]),GCS)
    r_sensor2_zy = np.matmul(Rot_Mat_Y(sensor2[i][1]),r_sensor2_z )
    r_sensor2_zyx = np.matmul(Rot_Mat_Z(sensor2[i][2]),r_sensor2_zy )
    r_sensor2.append(r_sensor2_zyx )


r_start_sensor1 = r_sensor1[0]
r_start_sensor2 = r_sensor2[0] 

r_offset_sensor1 = []
r_offset_sensor2 = []
for i in range(len(sensor0)):
    r_offset_sensor1.append(np.matmul(np.transpose(r_start_sensor1),r_sensor1[i]))
    r_offset_sensor2.append(np.matmul(np.transpose(r_start_sensor2),r_sensor2[i]))


# result: 
r_offset_sensor1[0] = [[1,1]]
r_offset_sensor1[1] = [[0.984,0.059,0.163],0.939,-0.342],[-0.173,0.336,0.925]]
r_offset_sensor1[2] = [[0.748,0.466,0.471],[0.086,0.635,-0.767],[-0.657,0.615,0.434]]

r_offset_sensor2[0] = [[1,1]]
r_offset_sensor2[1] = [[0.984,0.086,0.150],[-0.03,0.938,-0.344],[-0.171,0.334,0.926]]
r_offset_sensor2[2] = [[0.748,0.541,0.383],[-0.028,0.603,-0.797],[-0.662,0.585,0.466]]

我希望传感器 1 和 2 的结果对于所有帧都相等，但事实并非如此？他们应该是：

frame[0] = [1,1]
frame[1] = [0.984,0.173],[0.059,-0.336],[-0.163,0.342,0.9254]
frame[2] = [0.750,-0.433,0.50],[0.625,0.216,-0.750],[0.216,0.875,0.433]