convert a set of roll rates from earth frame to body frame

def convert_body_frame(self, phi, theta, phiDot, thetaDot, psiDot):
        '''convert a set of roll rates from earth frame to body frame'''
        p = phiDot - psiDot*math.sin(theta)
        q = math.cos(phi)*thetaDot + math.sin(phi)*psiDot*math.cos(theta)
        r = math.cos(phi)*psiDot*math.cos(theta) - math.sin(phi)*thetaDot
        return (p, q, r)