1. acc_calib in body

2. acc_calib in world

acc_calib.py

@@ -0,0 +1,34 @@
+
+'''
+俯仰角 -- 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))
+