Project

General

Profile

Actions

Bug #353

closed

Missing scalar in Jacobian computation

Added by Martin Jacquet about 2 months ago. Updated about 1 month ago.

Status:
Closed
Priority:
Normal

Description

One line bugfix for a missing 0.25 scalar in Jacobian computation, I attach the patch.

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.


Files

Actions #1

Updated by Anthony Mallet about 2 months ago

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.

Actions #2

Updated by Martin Jacquet about 2 months ago

  • Status changed from New to Closed
Actions #3

Updated by Martin Jacquet about 2 months ago

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.

Actions #4

Updated by Anthony Mallet about 2 months ago

  • Status changed from Closed to Feedback

My point was that taking the absolute value of qw (line 179) is
wrong IMO.

OK, that makes sense.

Also, there seems to be a sign typo in the Jacobian formula according
to this paper:

Terzakis, George & Lourakis, Manolis & Ait-Boudaoud,
Djamel. (2018). Modified Rodrigues Parameters: An Efficient
Representation of Orientation in 3D Vision and Graphics. Journal of
Mathematical Imaging and Vision. 60. 10.1007/s10851-017-0765-x.

https://www.researchgate.net/publication/320360256_Modified_Rodrigues_Parameters_An_Efficient_Representation_of_Orientation_in_3D_Vision_and_Graphics

Check section 6.1. They use a different scaling of the MRP, but with our scaling the
formula still has an extra minus.

What do you think?

Actions #5

Updated by Martin Jacquet about 2 months ago

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.

Actions #6

Updated by Martin Jacquet about 2 months ago

  • Status changed from Feedback to Closed
Actions #7

Updated by Anthony Mallet about 2 months ago

  • Status changed from Closed to Feedback

I'm reopening this again ...

I though about this, and I think this is still wrong.
The covariance matrix in the state corresponds to the "error quaternion", i.e. the small rotation since the previous timestep. This is obvious when looking at the state update :
q(t+1) = e(t+1) * q(t) (with e the error quaternion represented by the MRP).

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

So something like:

    | 1/2  0   0 |
    | 0  1/2   0 |
J = | 0    0 1/2 | * q(t)
    | 0    0   0 |

The same reasonning also applies for the additive measurement noise (in measure_s::noise).

Actions #8

Updated by Martin Jacquet about 1 month ago

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.

Actions #9

Updated by Anthony Mallet about 1 month ago

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.

Actions #10

Updated by Anthony Mallet about 1 month ago

  • Status changed from Feedback to Closed
Actions #11

Updated by Anthony Mallet about 1 month ago

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!

Actions

Also available in: Atom PDF