Updated translate function to correct assembly movement bug. Also implemented and sanity checked the computation of M.