Joint offset calibration

Copied @mposa's joint offset calibration script to Drake and wrote a proper unit test for it. I tried to preserve the history of Michael's work, but couldn't figure out how to do it properly due to the fact that the file was moved to a different location after Michael made all his changes. The test generates simulated data and verifies that estimated joint offsets, marker positions, and floating states match their 'actual' values.