UPDATED: slight improvement in expm