IMU Lie Group pre-integration factor

[Affie]

Implements https://hal.science/hal-02183498/document

[mateuszbaran]

Great work! Here are a few quick comments, I will be looking into it more deeply.

[mateuszbaran]

This looks like a matrix form of differential_exp_argument from ManifoldDiff.jl. More specifically, differential_exp_argument_lie_approx!. Having it in matrix form makes sense though for covariance propagation, which can be done using matrix multiplication instead of repeated calls to differential_exp_argument_lie_approx!.

[mateuszbaran]

A slightly off-topic comment.

What I'd consider natural (but haven't seen that anywhere yet) for state estimation:

1. We start with a position space, which is a manifold $M$. For example $M=SO(3)\times \mathbb R^3$ for orientation and position or $M=SO(3)\times \mathbb R^3 \times \mathbb R^3$ when we also want linear speed.
2. We equip $M$ with a connection such that the process follows a geodesic in the absence of observations. Any system is described as a point on the tangent bundle $TM$ -- phase space.
3. Now, we add uncertainty for all components. This may be described as a fiber bundle $FTM$ where each point additionally has the covariance matrix. The bundle has a connection form based on differential/Jacobian of the exponential map on $M$, or in another interpretation sensitivity analysis of equations of motion.
4. All observations also correspond to points on $FTM$. Instead of insisting on going through the Kalman gain linearization, we can compute the new state based on more general optimality criteria.

There is still some room for arbitrary choices here and there (left vs right-invariant errors and much more) but it would be a decent framework to compare and develop different Kalman filter variants and to a degree also more general particle filters. It looks like most people don't care too much about (2) and then choices they make in (3) and (4) feel more arbitrary then they really are, or are incorrect.

Slightly more on-topic, inclusion of $\Delta t$ in the IMU group looks a bit weird?

[dehann]

Hi Mateusz,

but haven't seen that anywhere yet (1. & 2.)

$M=SO(3)\times \mathbb R^3 \times \mathbb R^3$

I like this a lot, and think this is the way to go. I have not seen geodesic IMU integration models either...

Kalman gain

Our stuff explicitly does not follow the standard Kalman assumptions (linear, time-invariant). We are working to write down IMU integration models purely as continuous domain equations only.

Slightly more on-topic, inclusion of in the IMU group looks a bit weird?

Yep, I agree $\Delta t$ should not show up in continuous time equations. I previously derived fully continuous models for imu preintegration.

That said, the assumptions in much of the IMU preintegration literature is that multiplying IMU bias by $\Delta t$ is "safe" since the biases are "stable" over "short" time scales. My preference is to just use the continuous models.

3. [reading back to confirm]

integrating one set of imu measurements for all possible noises will produce one fiber.

Taking all possible imu measurements and noise combinations would produce the full fiber bundle (i.e. range space).

[mateuszbaran]

Hi Dehann,
thank you for confirming that I'm on a reasonable track 🙂 .

integrating one set of imu measurements for all possible noises will produce one fiber.

Taking all possible imu measurements and noise combinations would produce the full fiber bundle (i.e. range space).

Yes, that's correct. One of my current projects is extending Manifolds.jl to support such fiber bundles.

[Affie]

This is starting to work but needs more testing on covariance propagation and the bias correction Jacobian. Will come back to that in separate PRs when time permits.