Fixed issue with Coriolis acceleration

[tsetterf]

The corrections for dP and dV are in the navigation frame, whereas xi is in the local frame. The easiest fix is just to rotate it into the local frame within the coriolis function. I have updated the Jacobians as well.

[dellaert]

So (following up from our earlier email conversation) now that I see the change more clearly, I am not convinced that this is right. In particular, I am not convinced dP and dV are in nav coordinates. I'd suggest you read the doc/ImuFactor.pdf where -I skimmed through it again- the tangent space quantities seem to be in body coordinates. Admittedly, the document is a bit of a slug to get through.

Regardless what the case might be, the fact that you can make this change without any unit test failing means this piece of code is not actually tested, and a test should be added that helps us decide on the truth.

I'm hoping you'll stick with me and we'll get to the bottom of it. This is important :-)

[tsetterf]

I am convinced that as written dP and dV are currently in nav coordinates (although they are not supposed to be). omega is planet angular velocity in the planet-centered planet-fixed frame (i.e., the nav frame). n_v and n_t are in the nav frame (as annotated). Therefore their cross products are in the nav frame.

const Vector3 omega_cross_vel = omega.cross(n_v);
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));

These are the only vector terms in dP and dV, so they are also in the nav frame (prior to my fix).

dP(xi) << ((-dt2) * omega_cross_vel); 
dV(xi) << ((-2.0 * dt) * omega_cross_vel);
dP(xi) -= (0.5 * dt2) * omega_cross2_t;
dV(xi) -= dt * omega_cross2_t;

I agree that it seems like this code is not unit tested in a rich enough scenario to reveal the bug. It is tested for self consistency, ensuring that the analytical and numerical Jacobians match.

[dellaert]

Aha, I think I might have had it backwards, indeed ! You are converting from nav to body, to agree with the convention in ImuFactor - that tangent space is in body coordinates. That sounds great, and especially the addition of a fail-first-then-succeed unit test provides a warm feeling :-)

Will review again and merge on CI.

[dellaert]

Nice! One nit left :-)

[dellaert]

Not loving this imperative code here now, so I propose you build n_xi above (you forgot one unrotate, BTW), and then here:

// Convert to body-coordinates:
Vector9 xi;
xi << bRn * dR(n_xi), bRn * dP(n_xi), bRn * dV(n_xi);

Makes it clearer overall as well.

[tsetterf]

I don't think I forgot one unrotate, since dR was already unrotated in your original code. My style was meant to minimize the changes to existing code. But since you asked, I have pushed a commit that I think makes it clearer what is going on, mostly as you suggest.

[dellaert]

OK, this is awesome! Many thanks for this excellent contribution! I will merge now.