vectr-ucla / direct_lidar_inertial_odometry

[IEEE ICRA'23] A new lightweight LiDAR-inertial odometry algorithm with a novel coarse-to-fine approach in constructing continuous-time trajectories for precise motion correction.
MIT License
576 stars 115 forks source link

Regarding geometric observer equations #58

Open VRichardJP opened 3 months ago

VRichardJP commented 3 months ago

In the geometric observer paper ("A Contracting Hierarchical Observer for Pose-Inertial Fusion"), the state update is described by the following equations:

image

image

From these derivative equations, how to you get yours?

image

I understand your update comes from a first order derivative approximation, where the correction is applied a posteriori on the latest state, using the total integration time Dt:

s_hat(t + Dt) = s_hat(t) + Dt * s_hat'(t)
              = s_hat(t) + Dt * [s_imu'(t) + s_err'(t)]
              = s_hat(t) + Dt * s_imu'(t) + Dt * s_err'(t)
              = s_imu(t+dt) + Dt * s_err'(t)

where s_imu is the state computed from imu integration, based on the following equations:

image

and s_err' are the "correction" part of the geometric obserser equations:

image

image

The correction factors looks the same on both papers for q, bw, and p. However for v and ba, I think your implementation is missing a few factors:

image

Why is that? are these factors experimentaly negligible?