C66x dynamics model optimisation and bug fix

Fixes #11, fixes #12