Jacobian of unit quaternion

[bresch]

Hi, first of all, thanks for sharing that great piece of work, deriving equations and generating auto-code has never been so easy!

After almost completely migrating the derivation of the PX4's state estimator to SymForce, I still want do do an extra step and use the Quaternion and Rot3 objects provided by your package instead of using custom equivalents implemented in a separate helper file (PX4/PX4-Autopilot#21359).

However, until now, in order to compute the observation Jacobian (H) used in the update step of the EKF, we were rotating vectors using a rotation matrix produced as follows:

Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
                  [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])

as opposed to:

symforce/symforce/geo/rot3.py

Lines 157 to 179 in 8ad6bab

	def to_rotation_matrix(self) -> Matrix33:
	"""
	Converts to a rotation matrix
	"""
	return Matrix33(
	[
	[
	1 - 2 * self.q.y ** 2 - 2 * self.q.z ** 2,
	2 * self.q.x * self.q.y - 2 * self.q.z * self.q.w,
	2 * self.q.x * self.q.z + 2 * self.q.y * self.q.w,
	],
	[
	2 * self.q.x * self.q.y + 2 * self.q.z * self.q.w,
	1 - 2 * self.q.x ** 2 - 2 * self.q.z ** 2,
	2 * self.q.y * self.q.z - 2 * self.q.x * self.q.w,
	],
	[
	2 * self.q.x * self.q.z - 2 * self.q.y * self.q.w,
	2 * self.q.y * self.q.z + 2 * self.q.x * self.q.w,
	1 - 2 * self.q.x ** 2 - 2 * self.q.y ** 2,
	],
	]
	)

which is the "simplified" version of the previous one, assuming a unit quaternion (the rest of the difference between the two equation is just because we're using [w, x, y, z] but this isn't an issue here).

When computing a Jacobian using one or the other "quat -> rot_matrix" methods, it makes sens that both results are numerically different but I haven't yet found a satisfying answer to decide if one or the other is "better". We found in the past that using the "simplified" matrix leads to a more numerically robust covariance prediction (found by experiments) but I'm hoping that you could bring a bit more light to this.

Furthermore, if I generate the Jacobians using the quaternion conjugation directly (v' = q * v * q^-1, without passing by a rotation matrix), I'm getting the same results as when using the "non-simplified quat -> rot_matrix"; which make me unsure if the second method is actually safe to use when computing Jacobians.

Thanks in advance for your help!

[aaron-skydio]

Awesome to hear you all are using SymForce for EKF2!

When computing a Jacobian using one or the other "quat -> rot_matrix" methods, it makes sens that both results are numerically different but I haven't yet found a satisfying answer to decide if one or the other is "better". We found in the past that using the "simplified" matrix leads to a more numerically robust covariance prediction (found by experiments) but I'm hoping that you could bring a bit more light to this.

This is interesting - I don't think we have any theory to add here though

Furthermore, if I generate the Jacobians using the quaternion conjugation directly (v' = q * v * q^-1, without passing by a rotation matrix), I'm getting the same results as when using the "non-simplified quat -> rot_matrix"

I'm maybe a little confused by this, assuming the "non-simplified quat -> rot_matrix" is this matrix:

Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
                  [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])

which when multiplied by a vector should give you the exact equivalent of q * v * q.conj(), without the divisions you should get from q * v * q.inverse() - some notes in colab here.

I guess the other question (which I could maybe figure out by poking around the EKF2 code) is how exactly you're updating the quaternion from the measurement jacobian, I see the jacobians you're computing are with respect to the 4 quaternion elements (what we'd call the quaternion storage in SymForce) - I think at that point it's getting to more of a question of how EKF2 is doing its update and whether it's important for the projection of the measurement jacobian along the quaternion vector direction (orthogonal to the unit norm constraint) to be correct, if that makes sense? In particular I'm definitely not sure whether the rotate_conj or rotate_mat methods as I called them in that colab notebook are meaningfully different from each other, given that neither of them is rotate_inv.

Side note: It might also make sense to add a flag to Rot3.to_rotation_matrix() to use the "correct when the quaternion is not unit norm" version? Although generally most of the methods on Rot3 are intended to work for quaternions at unit norm, and things are primarily geared towards tangent-space jacobians more than 4-dof storage-space/ambient-space jacobians, so I'm not sure

[bresch]

Thanks a lot for your detailed answer @aaron-skydio .

I'm maybe a little confused by this, assuming the "non-simplified quat -> rot_matrix" is this matrix:

Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
                  [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])

which when multiplied by a vector should give you the exact equivalent of q * v * q.conj(), without the divisions you should get from q * v * q.inverse() - some notes in colab `here.

Yes, this is what I called the "non-simplified quat -> rot_matrix". When used to rotate a vector, I totally agree that the result is exactly equivalent, assuming a unit quaternion (which is a valid assumption as we're always using unit quaternions too). My concern was when taking the partial derivatives of this rotated vector w.r.t. our state vector.
And I think you made a fair point here: we're computing the 4-dof storage-space jacobian and not the tangent-space one. I mads a copy of your colab to add an example showing exactly what I meant.

how EKF2 is doing its update

We're simply adding K * innovation to the state vector (EKF2 is a total-state EKF) using a vector addition as done in a standard EKF (not by using an exponential map as you would do to an element of the tangent space to a quaternion) followed be a re-normalization of the quaternion.

whether it's important for the projection of the measurement jacobian along the quaternion vector direction (orthogonal to the unit norm constraint) to be correct

I guess this is exactly my question, but how to answer it?

Side note: It might also make sense to add a flag to Rot3.to_rotation_matrix() to use the "correct when the quaternion is not unit norm" version?

Given that Rot3 is a rotation element, it has to be a unit quaternion, so I don't think it really makes sense to add this.

Although generally most of the methods on Rot3 are intended to work for quaternions at unit norm, and things are primarily geared towards tangent-space jacobians more than 4-dof storage-space/ambient-space jacobians, so I'm not sure

I'm also convinced that using the tangent-space jacobians is the most correct way to do it, and you already have the option to force the computation using the storage elements (tangent_space=False).

Just to see if there is a difference when deriving a tangent-space jacobian I also added another piece of code in colab and it seems to fail to compute it for everything else than "rotate_mat" (i.e.: Rot3 * V3), is that expected?

[aaron-skydio]

Although generally most of the methods on Rot3 are intended to work for quaternions at unit norm, and things are primarily geared towards tangent-space jacobians more than 4-dof storage-space/ambient-space jacobians, so I'm not sure

I'm also convinced that using the tangent-space jacobians is the most correct way to do it, and you already have the option to force the computation using the storage elements (tangent_space=False).

Just to see if there is a difference when deriving a tangent-space jacobian I also added another piece of code in colab and it seems to fail to compute it for everything else than "rotate_mat" (i.e.: Rot3 * V3), is that expected?

Not sure if you fixed this since commenting or if I'm looking in the wrong spot, but looking at the notebook now it seems like it's working and the jacobians computed in that last block (the tangent space jacobians for the three methods) look right to me. Added a line here, but they should all be 0 (even if sympy doesn't make that simplification).

I think basically what's happening here is that if you rotate the vector using any of rotate_conj, rotate_mat, or rotate_full_mat, then the 4dof jacobian will indicate that you can change the measurement by changing the quaternion norm. If you either use rotate_inv, or use the 3dof tangent-space jacobian, you correctly get jacobians of 0 for that example (added a block here showing this). rotate_full_mat and rotate_conj give the same nonzero/"wrong" answer, and rotate_mat seems to give a different nonzero/"wrong" answer, but it's definitely not clear to me if one is better, don't think we can really help you with that

[bresch]

Not sure if you fixed this since commenting or if I'm looking in the wrong spot, but looking at the notebook now it seems like it's working and the jacobians computed in that last block (the tangent space jacobians for the three methods) look right to me. Added a line here, but they should all be 0 (even if sympy doesn't make that simplification).

Thanks, this was in fact a really bad example since the jacobian is obviously 0 for that measurement model...

rotate_full_mat and rotate_conj give the same nonzero/"wrong" answer, and rotate_mat seems to give a different nonzero/"wrong" answer, but it's definitely not clear to me if one is better, don't think we can really help you with that

I think this is actually the answer I was looking for. The quaternion covariance shouldn't be defined on its parameters but rather on the tangent space, I added another example here (with a non-trivial jacobian) that shows myself that the this is the correct way by using the jacobians to propagate a covariance matrix into the measurement space.

I'll keep that in mind and we'll use the correct quaternion uncertainty representation and propagation when changing from total-state to an error-state EKF, where the delta quaternion is also defined in the tangent-space.

edit: we changed the navigation filter of PX4 to use an error-state formulation with a "global error" quaternion: https://github.com/PX4/PX4-Autopilot/blob/741c7ab610a6365abe6aced7d1f0ae192d660bcf/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py#L114

Many thanks again for your help.