'''
俯仰角 -- 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))