Floating base

Adds support for quaternion floating base parameterization and having dq/dt and v be different. 

Not ready to merge. Still need to do mex implementation. I believe that at this point, all the tests that I can get to pass without mex do pass.

Comments very welcome.