ImuFactor with earth rotation

[chrisflesher]

I'm interested in estimating gyro biases for a high performance IMU but can't figure out how to account for earth rotation rate properly in GTSAM.

For example I am seeing 7.5deg/hr heading drift at 30deg latitude.

It seems like the IMU pre-integration code assumes IMU measurements are in the body frame instead of the intertial frame like they are normally given. Any ideas / workarounds would be appreciated!

[chrisflesher]

For now a workaround is to call PreintegratedImuMeasurements::predict for each IMU measurement to remove earth angular velocity from gyro measurements. This seems computationally expensive and not super accurate though. It would be nice to include this functionality in pre-integration eventually...

[dellaert]

You’re probably right that it should be included in the pre-integration math, although I’m not convinced it should be added to the existing factor. Perhaps a derived class for a high quality IMUs is the best approach. Unless the fix is very easy.

Do you have a good reference on the math involved?

[chrisflesher]

The math is:

Frame names:

i: inertial frame (fixed vs stars)
e: earth frame
n: NED frame (north, east, down)
b: body frame (IMU)

Variable names:

w_n_ie: Angular velocity from inertial -> earth frame, roughly 360deg / 24hr
w_n_en: Angular velocity from earth -> NED frame, can assume this is negligible unless you are modeling a rocket
r_b_n: Rotation from NED -> body
w_b_nb: Angular velocity from NED -> body, this is what GTSAM assumes we are providing as input to pre-integration
w_b_ib: Angular velocity from inertial -> body, this is what the IMU is actually measuring

The variables are summed like this:

w_b_ib = r_b_n * (w_n_ie + w_n_en) + w_b_nb

The w_n_ie term depends on latitude, like this:

EARTH_ROTATION_RATE = radians(360) / (23*3600 + 56*60 + 4.1)  # [rad/s]
w_n_ie = [cos(latitude), 0., -sin(latitude)] * EARTH_ROTATION_RATE

[dellaert]

One way to proceed is to prototype this in a derived class that overwrites particular functions and has associated unit test for the derivatives, etc. Then we can see how much new code is involved, and whether It’s worth integrating in the base class or keep it as a derived class. Happy to review a PR to this effect.

[chrisflesher]

Ok, maybe I will take a shot at this!

[stkrauss]

I'm also interested in this. For what it's worth, there is a paper and forked version of GTSAM both linked here that has an IMU factor that is supposed to compensate for rotating earth, but also uses an extended pose so the changes are more extensive.

[dellaert]

Thanks for the link. Note GTSAM uses NavState already, which is just the manifold associated with the $SE_2(3)$ Lie Group - so the state space is the same (9DOF). I should really read that paper/code to see what I’m saying here makes sense :-)

[stefangachter]

May I add a comment? @stkrauss , I had a look at Brossard's implementation and created a pull-request: #1916 (This branch is not yet tested because I worked originally on release 4.2. branch.) @dellaert, yes, NavState is the same state space as SE2(3) but is it also using the same retraction? Currently, the factors are based on <SE(3), R3> or <SO(3)xT3xT3> representation and not on SE2(3). If SE2(3) is used consistently, then only a single variable of SE2(3) will be present and not one for pose and one for velocity as it is currently the case. That is my limited understanding, though. Varun is working on the integration of SE2(3) (#1613), no?

[dellaert]

@chrisflesher , seems that #1916 already adds earth rotation, although in a review just now I suggested it should be in a separate PR, along with the correct Coriolis. If y’all are game I suggest:

• @chrisflesher makes a PR copying the good bits from SE2(3) representation and Lie group pre-integration of Brossard et al. #1916
• @stefangachter -if he has time - finishes Add Lie group capabilities to NavState #1613
• @varunagrawal and myself review both
• @stefangachter adapts SE2(3) representation and Lie group pre-integration of Brossard et al. #1916 to work with NavState according to my comments

of course, this is a bunch of work, Stefan :-) I think @varunagrawal and I are happy to help but can’t run point at this time.