Pose estimation and localization software » pom-genom3

I'm not absolutely sure how your absolute value trick in the
computation of delta should be reflected on the Jacobian though. We
might want to discuss this again.

You mean using the opposite quaternion above a delta > π?
In practice, there is no sigma points with such a big angle. This can
happen only if the uncertainty is that big, so only if there is no
orientation sensor. So I think this is not a real issue anyway.

My point was that taking the absolute value of qw (line 179) is wrong IMO.
Even if the delta is always <π (except maybe at initialization), you might end up with a negative qw in the resulting state quaternion, in which case taking the absolute value here means wrongly taking (-qw,qx,qy,qz) to compute the Jacobian, hence pom publishes a wrong covariance
I do believe that whatever the delta angle (below or above π), you should take q() as it is to compute that Jacobian, although I'm struggling hard to prove it.

I just check my computation, there was indeed a missing minus before v*v^T. Fixing this, I reach the same formula as in the paper.

It seems to me that the Jacobian of this should be:
J = de/dq(q = identity) * q(t)

This equation does not seem sensible (dimensions do not match).

The point of this Jacobian is to obtain the 4x4 covariance of the quaternion elements, from the 3x3 state covariance (that is, the one propagated through the ukf equations). It concerns the absolute (=world frame) orientation, not the error.

I assume this 3D to be the generalized Rodriguez parameter representation since it's the one you chose to propagate the error, hence the Jacobian to use should be the one we wrote (ie Jacobian of transformation from grp to quaternion.

On Wednesday 6 Jul 2022, at 12:02, Martin Jacquet wrote:

It seems to me that the Jacobian of this should be:
J = de/dq(q = identity) * q(t)

This equation does not seem sensible (dimensions do not match).

Sorry for the approximate writing. I meant * q(t) as a frame
rotation, i.e. multiplication by the rotation 3×3 matrix representing
the rotation defined by the current state, in which case dimensions do
match.

But I did not mean to write a precise equation, I was rather
highlighting the fact that the covariance and the "virtual state"
maintained in the UKF is not the absolute orientation, but rather the
deviation from the last known orientation (the "local error state").

The point of this Jacobian is to obtain the 4x4 covariance of the
quaternion elements, from the 3x3 state covariance (that is, the one
propagated through the ukf equations). It concerns the absolute
(=world frame) orientation, not the error.

This 3×3 state covariance in pom is about the error quaternion, not
the absolute orientation.

Take a look at "Joan Solà. Quaternion kinematics for the error-state
KF. 2015.". (section 3.1.1, eq 186, page 36).
Available here :
https://hal.archives-ouvertes.fr/hal-01122406v3/document

To me it's pretty clear that I'm doing something wrong. This is
confirmed by the fact that the variance of yaw, pitch, roll angles
output of pom-genom3 actually depend on the current orientation (this
sound really wrong) and sometimes even go to 0 (this is obviously
wrong).

[There is also something really wrong in the covariance matrix
published by optitrack-genom3, but this is a different topic.]

I assume this 3D to be the generalized Rodriguez parameter
representation since it's the one you chose to propagate the error,
hence the Jacobian to use should be the one we wrote (ie Jacobian of
transformation from grp to quaternion.

But this transformation is only used for the "delta" quaternion, i.e
close to the origin. Nowhere in the filter the transformation is used
far from this point.

The "real state" is q(t+1) = delta_q_ukf * q(t), where delta_q_ukf
is the actual output of the kalman filter.

So, after fixing small side issues (typo in the optitrack covariance
and mistake in the quaternion -> pitch jacobian), it seems that the
covariance matrix on the orientation is much better.

Variances on roll, pitch and yaw are now constant (using optitrack)
and not related to current orientation. When using gps, the yaw
variance grows as sqrt(time) and pitch, roll variances are still
constant, which is also correct.

I also checked that the analytic expression of the covariance matrix
of optitrack orientation mapped back to the MRP of the innovation
still gives a diagonal matrix with the original stddev assumed by
optitrack.

So this looks all good to me now. Many thanks for you help!