Updating the M formation code
Moving M section forward
Merge branch 'feature/updateMassMatrixMath' into develop