NavState + IMU EKF

[dellaert]

Create a new class that specializes LieGroupEKF to NavState driven by an IMU, and gravity.

This is based on Derek's draft PR #2241

Note I derived not from InvariantEKF but LieGroupEKF, as it seems the error dynamics are still dependent on the state, through v_body and g_body.

I think this means, maybe, that the left-invariant version here is not group-affine. But, we're pretty new to this, so let's discuss :-)

I want to also add wrapper and a notebook that mimics the new cpp example, which has an IMU "flying" in a circle.

[dellaert]

Repeating comment from #2241:
I closed #2241, because I think this PR based on code by @drk944 is more in line with the current structure. Also:

• I would love to have a cvs file with your data so we can reproduce the graphs above.
• I think the debate (which we can continue in new PR) is still whether the left-invariant version of IMU dynamics is in fact group invariant. At least, the Jacobians are not independent of state, and so it seems to me that this makes the error dynamics also state-dependent.
• The current "InvariantEKF" class in GTSAM demands the jacobian is just the adjoint, but perhaps that is too restrictive. If these PR's clear up that further, all the more power to us
• PS maybe @contagon could take a look at both PRs as well :-)

[dellaert]

OK, the mean part of the code below now matches @drk944 code exactly (and the paper, I'd think), and it did prompt me to add a new predictIncrement in LieGroupEKF. Maybe should be predictWithCompose. but the (left-invariant) Jacobian is still very state-dependent, so still not InvariantEKF.

NavState navStateImuDynamics(const NavState& X, const Vector3& gyro,
                             const Vector3& accel, double dt,
                             const Vector3& n_gravity,
                             OptionalJacobian<9, 9> H) {
  // Rotation and velocity
  const Rot3& R = X.attitude();
  Matrix3 D_vb_R, D_vb_v, D_gb_R;
  const Vector3& v_n = X.velocity();  // Has D_v_v = R !
  const Vector3 v_body =
      R.unrotate(v_n, H ? &D_vb_R : nullptr, H ? &D_vb_v : nullptr);
  const Vector3 g_body = R.unrotate(n_gravity, H ? &D_gb_R : nullptr);
  const Vector3 a_b_total = accel + g_body;

  // Construct increment directly as group element with body-frame p/v
  // increments
  const Rot3 dR = Rot3::Expmap(gyro * dt);
  const double dt2 = 0.5 * dt * dt;
  const Vector3 dp_body = v_body * dt + a_b_total * dt2;
  const Vector3 dv_body = a_b_total * dt;
  NavState U(dR, dp_body, dv_body);

  if (H) {
    Matrix3 dRt = dR.transpose();  // Jacobian of NavState::Create
    H->setZero();
    // position:
    H->template block<3, 3>(3, 0) = dRt * (D_vb_R * dt + D_gb_R * dt2);
    const Matrix3 D_v_v = R.matrix();  // Jacobian of velocity()
    H->template block<3, 3>(3, 6) = dRt * D_vb_v * D_v_v * dt;
    // velocity:
    H->template block<3, 3>(6, 0) = dRt * D_gb_R * dt;
  }

  return U;
}

[dellaert]

Done with this for a bit :-) I added wrapping and uploaded a notebook. Prediction is perfect, but now there is an issue with update. Could be covariance that is to blame, or calculation of innovation, but not seeing it now.

[dellaert]

OK, I had an error in my measurement Jacobian :-) But, silver lining, many more tests confirm correctness of update. The Notebook now works very well. e.g., position vs. ground truth:

[dellaert]

Note, I'm still working on this. I think the NavStateImuEKF is better done using techniques described here:

Barrau, Axel, and Silvere Bonnabel. "Linear observed systems on groups." Systems & Control Letters 129 (2019): 36-42. Link: https://www.sciencedirect.com/science/article/pii/S0167691119300805

[Copilot]

Pull Request Overview

This PR introduces a new extended Kalman filter (EKF) system specialized for IMU-driven navigation state estimation. The implementation provides a left-invariant Lie group EKF for NavState that integrates IMU measurements with gravity and supports generic measurements.

Key changes:

• Implements a new NavStateImuEKF class that specializes LieGroupEKF for NavState driven by IMU and gravity
• Adds new base classes LeftLinearEKF and enhanced ManifoldEKF for general left-linear prediction models
• Creates comprehensive documentation and example notebooks demonstrating usage

Reviewed Changes

Copilot reviewed 19 out of 20 changed files in this pull request and generated 1 comment.

Show a summary per file

File	Description
gtsam/navigation/NavStateImuEKF.h	Header defining the specialized IMU EKF class with predict and dynamics methods
gtsam/navigation/NavStateImuEKF.cpp	Implementation of IMU EKF with dynamics calculation and predict functionality
gtsam/navigation/LeftLinearEKF.h	New base class for left-linear EKF on Lie groups with general automorphism support
gtsam/navigation/ManifoldEKF.h	Enhanced base class with vector measurement bridge for wrapper compatibility
gtsam/navigation/LieGroupEKF.h	Extended with compose-based prediction API for precomputed increments
gtsam/navigation/NavState.h	Added AutonomousFlow struct for velocity-acting-on-position dynamics
gtsam/navigation/navigation.i	Python wrapper definitions for all new EKF classes
examples/NavStateImuExample.cpp	Complete C++ example demonstrating orbiting scenario with IMU integration
gtsam/navigation/doc/NavStateImuEKF.ipynb	User guide notebook with API overview and examples
gtsam/navigation/doc/InvariantEKF.md	Updated documentation including new NavStateImuEKF section

Comments suppressed due to low confidence (5)

gtsam/navigation/tests/testNavStateImuEKF.cpp:1

• The date 'April 26, 2025' appears to be in the future and inconsistent with the current development timeline. Consider using the actual file creation date or removing the specific date.

/* ----------------------------------------------------------------------------

gtsam/navigation/tests/testLeftLinearEKF.cpp:1

• The date 'August, 2025' appears to be in the future and inconsistent with the current development timeline. Consider using the actual file creation date or removing the specific date.

/* ----------------------------------------------------------------------------

gtsam/navigation/NavStateImuEKF.h:1

• The date 'August 2025' appears to be in the future and inconsistent with the current development timeline. Consider using the actual file creation date or removing the specific date.

/* ----------------------------------------------------------------------------

gtsam/navigation/NavStateImuEKF.cpp:1

• The date 'August 2025' appears to be in the future and inconsistent with the current development timeline. Consider using the actual file creation date or removing the specific date.

/* ----------------------------------------------------------------------------

gtsam/navigation/LeftLinearEKF.h:1

• The date 'August, 2025' appears to be in the future and inconsistent with the current development timeline. Consider using the actual file creation date or removing the specific date.

/* ----------------------------------------------------------------------------

[dellaert]

This line proves a hunch I had, that the old LieGroupEKF version, which is tested here, canceled the gravity pieces of the Jacobian A. PS D_X_X0 is Adjoint(inv(U)) and D_X_U = identity.

[dellaert]

More words: in the LeftLinearEKF, A is independent of the gravity update W, and the above proves:

A = Ad(inv(U)) + D_U_X0

where D_U_X0 is the Jacobian of the old dynamics. In fact, that is exactly what was implemented in the LieGroupEKF::predictWithIncrement.

[scottiyio]

Would this not be divides by dt?

I would think normal way is our std deviation is divided by sqrt(dt) so then covariance is divided by dt

[dellaert]

I don’t think so - but we can chat. It would be good to sanity-check this and the calculations of the Jacobian A using NEES in the ImuFactors repo.

[dellaert]

A note on the LeftLinearEKF vs InvariantEKF. Note currently the InvariantEKF class does not allow giving an arbitrary Jacobian/Transition matrix, but the LeftLinearEKF should cover cases where we don't just multiply with a $U_k$:

[varunagrawal]

I randomly came across this. Is there a reason we are not prioritizing landing #1613 and giving NavState the proper $SE_2(3)$ definitions? IIRC NavState is not a proper Lie Group at the moment and my aim with #1613 was to chunk the development of invariant filtering and smoothing in the following steps:

1. Make NavState $\in SE_2(3)$
2. Add the necessary machinery for the group affine property so we can generate the appropriate $A$ matrix (I believe this has been accomplished in the InEKF class).
3. Make some examples using both simulation and real data so we can confirm the working of it.

I won't be able to get to these until after September 25th, but just wanted to point it out so it can be considered while planning for this.

[dellaert]

That was done a while ago. The only things outstanding in #1613 were some transpose operators that were not tested yet. Please check the review comments. And, there is an undergrad - Warren - who volunteered to do that. Invariant filtering is indeed done, and now this new class, which goes beyond it. And Jennifer is working on an equivariant template. there are three c++ IEKF examples and one notebook, in this PR, with Left-linear. Lets chat on Thursday.

…

On September 3, 2025, GitHub ***@***.***> wrote: varunagrawal left a comment (borglab/gtsam#2242) <#2242 (comment)> I randomly came across this. Is there a reason we are not prioritizing landing #1613 <#1613> and giving NavState the proper $SE_2(3)$ definitions? IIRC NavState is not a proper Lie Group at the moment and my aim with #1613 <#1613> was to chunk the development of invariant filtering and smoothing in the following steps: 1. Make NavState $\in SE_2(3)$  2. Add the necessary machinery for the group affine property so we can generate the appropriate $A$ matrix (I believe this has been accomplished in the InEKF class). 3. Make some examples using both simulation and real data so we can confirm the working of it. I won't be able to get to these until after September 25th, but just wanted to point it out so it can be considered while planning for this. — Reply to this email directly, view it on GitHub <#2242 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe- auth/ACQHGSKSITV2KPEJT7ZK2BT3Q5PANAVCNFSM6AAAAACFGWOVOCVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZTENJQHEZDIOBZHE>. You are receiving this because you authored the thread.Message ID: ***@***.***>

[dellaert]

Please note that this still uses a "body update" for the GPS measurement. Proposition 25 in B+B SCL paper seems to suggest we should think about how to incorporate world-frame measurements: