Fix in the inertia matrix computation