Fix calculation of mass matrix