Fix for Inertia matrix, matlab functions

Now Inertia matrix is generated for more than 24 links,
Element-wise initialization of result in matlab generated function