Gravity in IMU Preintegration

[raf...@dotproduct3d.com]

Hello,

I would like to use GTSAM's IMU preintegration in my factor graph and I am currently trying to understand it in depth s.t. I am able to debug in case things don't go as planned.

I think I have a pretty good grasp on most things now except for one:
I don't understand how gravity is not a connected/estimated variable in any of the ImuFactor classes even though it clearly influences the preintegrated measurements (PIM).

For instance in CombinedImuFactor::evaluateError() through a number of indirections we get to PreintegrationBase::predict() and in there we find the code:

// Correct for initial velocity and gravity
Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, H2 ? &D_delta_biasCorrected : nullptr);

as we can see p().n_gravity is part of the prediction, however it is just part of the PreintegrationParams (like the IMU noise covariances), so it's a static parameter if I understand correctly.

Is the assumption that, to use IMU Preintegration through GTSAM, a high-accuracy gravity vector must already be given and/or that the coordinate system w.r.t. which my poses / NavStates are defined is already aligned with gravity?

Otherwise, how would this work in a setting where initial orientation of the sensor (and thus gravity direction) is completely unknown?

And even assuming we have an initialization procedure that gives us approx. initial gravity in that case, anything except a perfect initial gravity estimate (or perfect alignment of NavState origin with gravity) would mean our IMU factors could be off badly, no?

Please let me know what I'm missing.

Thanks & Regards.

Rafael

[Dellaert, Frank]

Hi Rafael,

 

Indeed, this might be non-obvious, but the entire point of the factor is to “defer” the influence of gravity to the factor optimization iterations. The PIM is oblivious to the orientation, and the effect of gravity is only added back in by the IMU factor. You could read the code – although it is probably better to start with the papers. See here: https://gtsam.org/notes/IMU-Factor.html

 

Best

FD

 

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/e932f3f6-7126-4629-acac-ddccce0c4f9fn%40googlegroups.com.

[Dellaert, Frank]

That is correct.

Frank

---

From: Rafael Spring <raf...@dotproduct3d.com>
Sent: Sunday, May 19, 2024 2:37:32 PM
To: Dellaert, Frank <frank.d...@cc.gatech.edu>
Cc: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Gravity in IMU Preintegration

 

Hi Frank, thank you for your input, this helps.

Shortly after posting my message to the group, I had the following thought and I'd be great to know if I'm getting closer to the truth or if I'm totally off base:

Maybe the preintegrated IMU factors aren't just constraining the relative position and orientation between two keyframes (as I originally thought) but also their absolute state w.r.t. gravity.

In other words: if you take two keyframes and an IMU factor connecting them and optimize, the end result is always the same regardless of which initial coordinate system the keyframes' poses are expressed in -- except for an unknown rotation around gravity.

Does that make any sense?

[raf...@dotproduct3d.com]

Hi Frank, thank you for clarifying.

There's one "extra screw" however that I don't know where to put in the factor graph.

If it is true that for a pair of keyframes connected with an IMU factor their initial coordinate system doesn't influence their state after optimization (up to rotation around gravity) AND it is true that the PIM is oblivious to the orientation: why do all the IMU initializers in the literature estimate the initial gravity vector?

In other words: once I know my initial gravity vector, where does it go in an optimization involving IMU factors between pairs of frames?

It looks like the PIM doesn't want it and the IMU factor doesn't need it.

Hope you (or someone in the community) can guide me on the right track here.

Thanks & Regards

Rafael

[Dellaert, Frank]

Hi Rafael,

This is a great question, and the answer is non-obvious: the code hides it in a subtle way. 

Note that one of your assumptions is wrong: while the PIM is gravity oblivious, the IMU factor is not. In particular, if the estimated attitude is wrong, there will be a strong corrective gradient in the observable directions (i.e., not yaw). The Jacobian responsible for this is computed in NavState::correctPIM: https://github.com/borglab/gtsam/blob/4cbf67354071a32c1db71cec7baf699a8ffe1049/gtsam/navigation/NavState.cpp#L417

In particular:

  D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;

CorrectPIM just corrects a tangent vector on the NavState, and there will be an *additional* Jacobian from applying that update in PIM::predict

So, in an admittedly opaque way: if the attitude is wrong, both position and velocity errors will be quite high, and the IMU factor will now how to change the attitude to fix it. In steady state, the accelerometer readings will constantly work to keep the attitude at the correct pitch-roll. And, BTW, because the gravity is so large, this will be a rather strong correction.

Now, about initialization: you don't initialize the gravity vector, but the initial attitude. I think that's what you mean, anyway. If the attitude is close to the truth, modulo yaw, the gradients on pitch and roll will tend to zero.

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of raf...@dotproduct3d.com <raf...@dotproduct3d.com>

Sent: Wednesday, January 15, 2025 7:21 AM
To: gtsam users <gtsam...@googlegroups.com>

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/c3179015-c05e-4d0a-bff9-cc2fae022d13n%40googlegroups.com.