Add inertia functions.
Finished solve method.

* Change forward and backward prototype.
* Change test accordingly.
Solve acceleration using lu.