Conceptually done, but not quite working.

In the process of bug fixing.
Second round of main loop get NaN as angular velocities.