Method for better-conditioned quaternion-based cost errors

[ebianchi]

Starting from the source code from the dairlib repo, this PR brings over the method for better-conditioned quaternion-based cost errors. This relies on computing the 4-by-4 Hessian of the squared angle error between two quaternions, representing current and desired orientations, with respect to the current quaternion components.

Still to do before review:

• Include regularization process necessary for preventing negative definite matrices (required for using it as a cost weight).
• Include unit tests.
• Provide guidance on how to include in an example.

---

This change is