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.
FYI, this class has been moved and modified in my pending pull request. You'll want to rebase once that goes in.