Added inertia matrix and its inverse