35 lines
759 B
Python
35 lines
759 B
Python
|
|
'''
|
|
俯仰角 -- pitch -- y
|
|
横滚角 -- roll -- x
|
|
航向角 -- yaw --z
|
|
|
|
ground frame definition:
|
|
Z -- upwards
|
|
|
|
ground frame --> pitch --> roll --> current orientation
|
|
'''
|
|
|
|
import math
|
|
|
|
def acc_calib(acc, rpy):
|
|
'''
|
|
|
|
:param acc: list of measured acceleration along axes xyz,unit g
|
|
:param rpy: list of measured roll, pitch and yaw angles along axes xyz,unit degree
|
|
:return: calibrated acceleration, list, unit g
|
|
'''
|
|
|
|
g_x = - math.sin( math.radians( rpy[1]))
|
|
g_y = math.cos(math.radians(rpy[1])) * math.sin(math.radians(rpy[0]) )
|
|
g_z = math.cos( math.radians(rpy[1]) ) * math.cos( math.radians(rpy[0]) )
|
|
return [acc[0]-g_x, acc[1]-g_y, acc[2]-g_z]
|
|
|
|
|
|
|
|
acc = [0.2, 0.73, 0.66]
|
|
rpy = [46,-10,-8]
|
|
|
|
print(acc_calib(acc, rpy))
|
|
|