testing
debugging: jacobian translation ok. need to fix jacobian quaternion submatrix