ekf2: symforce - use builtin Rot3 and Quaternion operations

[bresch]

Solution

Symforce has rotation (https://symforce.org/api/symforce.symbolic.html#symforce.symbolic.Rot3) and quaternion (https://symforce.org/api/symforce.symbolic.html#symforce.symbolic.Quaternion) support so we should use it instead of re-writing all the functions (mult, conversions, ...).

Test coverage

• interestingly some of the jacobians are different from before. This is because of how the quat to rot mat is implemented (assuming unit quaternion, like we do for the covariance prediction).

[dagar]

Time to drop (or relax) the comparisons with old generated?

[bresch]

Time to drop (or relax) the comparisons with old generated?

Yes, I't trying to find a new metric to compare them or just make new unit tests to actually make sure the result is correct instead of just comparing with the old derivation.

[bresch]

The true correct thing would actually be to define the quaternion variance on the tangent-space and not as an uncertainty on each component of the quaternion (the quaternion is not "just a vector"). see A micro Lie theory for state estimation in robotics, p.8-II.H and how to do it in SymForce.

This would reduce the P matrix to 23x23 elements, so it would make sense to also estimate the delta quaternion (3D) in an error-state EKF to also reduce the state vector. Something to keep in mind for later.

[bresch]

to merge after 1.14 release