Wheel odometry + IMU Preintegration

[Giacomo Piatelli]

Hello everyone!

I'm setting up a state estimation pipeline for my robot, which is equipped with an IMU, wheel odometry, and a 2D lidar. To start, I focused on building a working setup using only the IMU and wheel odometry.

Here’s the approach I took:

1. I accumulate IMU data and integrate it using PreintegratedImuMeasurements until I receive a wheel odometry message.
2. Upon receiving wheel odometry, I create an IMU factor and add it to the factor graph.
3. I add a between factor for the IMU bias, assuming a constant bias model.
4. Next, I transform the wheel odometry data to the IMU frame and compute the relative transformation between the current odometry pose and the previous one (from the last optimization result), then I add a BetweenFactor for this relative transformation.
5. For the state at X(k+1), I use the predicted state from PreintegratedImuMeasurements after the optimization.

As an optimizer, I'm using iSAM2. However, I keep running into an issue: after a few iterations, the system crashes with an error message: "Indeterminate linear system detected while working near variable (Symbol: b424)". This always seems to involve the IMU bias.

Any insights into what might be causing this issue with the bias would be greatly appreciated!

[Brice Rebsamen]

Hello,

This error happens when there is not enough information in your
system. I'm guessing some ambiguity along roll/pitch/z. Is your
odometry between factor 6DOF? What about the roll and pitch
constraints, or z? Perhaps you set a zero delta with a large
covariance? As odometry is typically 2.5D only... Then the solver is
not able to disambiguate gyro bias vs roll/pitch rotation, because
both will result in the same gyro measurements. If you're operating on
a flat surface, try adding some unary constraints that your roll,
pitch, and z are zero. Or a constraint that drives the biases towards
0. Otherwise, you will need to somehow add information about
roll/pitch/z.

Best of luck

> --
> 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 visit https://groups.google.com/d/msgid/gtsam-users/951a28d4-d3d1-43d8-941f-c4d68f6163b3n%40googlegroups.com.

[Giacomo Piatelli]

Thank you, Brice, for your reply.

As you correctly pointed out, my wheel odometry data is 2.5D, providing only x, y, and yaw. From the documentation I reviewed, I noticed the attitude factor, which can be used to constrain pitch and roll. Would adding this factor be sufficient?

When you mention a unary constraint, do you mean a prior factor that enforces pitch, roll, and z to be zero? If so, my understanding is that simply adding a prior factor from the wheel odometry would already enforce zero pitch, roll, and z, but it seems there might be more to it.

Additionally, I observed in my plots that the yaw angle occasionally jumps from −π to π, and the accelerometer biases remain at zero. This occurs with the setup

 where I add an in-between factor for the delta and a prior for the wheel odometry as described earlier.