## Bug #353

closed### Missing scalar in Jacobian computation

Added by Martin Jacquet about 2 years ago. Updated about 2 years ago.

**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**

0001-fix-missing-scalar-in-Jacobian-of-f-theta-q.patch (747 Bytes) 0001-fix-missing-scalar-in-Jacobian-of-f-theta-q.patch | Martin Jacquet, 2022-06-27 18:42 |

#### Updated by Anthony Mallet about 2 years 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.

#### Updated by Martin Jacquet about 2 years ago

**Status**changed from*New*to*Closed*

Applied in changeset pom-genom3|a7a83150f7e228cb254a903233d811274ef3fd54.

#### Updated by Martin Jacquet about 2 years 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.

#### Updated by Anthony Mallet about 2 years 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.

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?

#### Updated by Martin Jacquet about 2 years 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.

#### Updated by Martin Jacquet about 2 years ago

**Status**changed from*Feedback*to*Closed*

Applied in changeset pom-genom3|0327669fdf566fb43d81615df5d07e3774926fcc.

#### Updated by Anthony Mallet about 2 years 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`

).

#### Updated by Martin Jacquet about 2 years 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.

#### Updated by Anthony Mallet about 2 years 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.

#### Updated by Anthony Mallet about 2 years ago

**Status**changed from*Feedback*to*Closed*

Applied in changeset pom-genom3|693c60ca66818c360aef899352a54c12f1c8f32d.

#### Updated by Anthony Mallet about 2 years 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!