fixed rpy calculation