Added tests for C66x math, kinematics model and integrator functionality