NavState + IMU EKF

.github/workflows/build-python.yml

@@ -135,9 +135,17 @@ jobs:
       - name: Install (macOS)
         if: runner.os == 'macOS'
         run: |
-          brew tap ProfFan/robotics
-          brew install cmake ninja
-          brew install boost
+          brew update
+          # Avoid reinstalling cmake when a pinned/local tap version is preinstalled on the runner.
+          if brew list cmake >/dev/null 2>&1; then
+            echo "cmake already installed"
+            cmake --version
+          else
+            brew install cmake
+          fi
+          # Install ninja and boost only if missing
+          brew list ninja >/dev/null 2>&1 || brew install ninja
+          brew list boost >/dev/null 2>&1 || brew install boost
           sudo xcode-select -switch /Applications/Xcode.app
           echo "CC=clang" >> $GITHUB_ENV
           echo "CXX=clang++" >> $GITHUB_ENV

examples/NavStateImuExample.cpp

@@ -0,0 +1,110 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  NavStateImuExample.cpp
+ * @brief Run the NavStateImuEKF on an orbiting (constant twist) scenario.
+ *        Uses ScenarioRunner to generate corrupted IMU, de-biases with true
+ *        biases, feeds the EKF, and compares to Scenario ground truth.
+ *
+ * @date   August 2025
+ * @authors You
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/navigation/NavStateImuEKF.h>
+#include <gtsam/navigation/PreintegrationParams.h>
+#include <gtsam/navigation/Scenario.h>
+#include <gtsam/navigation/ScenarioRunner.h>
+
+#include <cmath>
+#include <iomanip>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+static constexpr double kGravity = 9.81;
+
+int main(int argc, char* argv[]) {
+  // --- Scenario: camera orbiting a point ---
+  const double radius = 30.0;
+  const double angular_velocity = M_PI;  // rad/sec
+  const Vector3 w_b(0, 0, -angular_velocity);
+  const Vector3 v_n(radius * angular_velocity, 0, 0);
+  ConstantTwistScenario scenario(w_b, v_n);
+
+  // --- Preintegration/IMU parameters ---
+  auto params = PreintegrationParams::MakeSharedU(kGravity);
+  params->setAccelerometerCovariance(I_3x3 * 0.1);
+  params->setGyroscopeCovariance(I_3x3 * 0.1);
+  params->setIntegrationCovariance(I_3x3 * 0.1);
+  params->setUse2ndOrderCoriolis(false);
+  params->setOmegaCoriolis(Vector3::Zero());
+
+  // True biases used to corrupt measurements in ScenarioRunner
+  imuBias::ConstantBias trueBias(Vector3(0.01, -0.005, 0.02),  // gyro bias
+  Vector3(0.05, 0.03, -0.02));  // accel bias
+
+  // --- Measurement generator ---
+  const double dt = 1.0 / 180.0;         // 1 degree per step
+  ScenarioRunner runner(scenario, params, dt, trueBias);
+
+  // --- Initialize EKF with ground truth
+  const double t0 = 0.0;
+  NavState X0 = scenario.navState(t0);
+  Matrix9 P0 = I_9x9 * 1e-2;  // modest initial uncertainty
+  NavStateImuEKF ekf(X0, P0, params);
+
+  // --- Run for N steps and compare predictions ---
+  const size_t N = 90;
+  cout << fixed << setprecision(3);
+  cout << "step,t(s),rot_err_deg,pos_err,vel_err\n";
+  double t = t0;
+  for (size_t i = 0; i < N; ++i) {
+    // Measurements from runner, *not* corrupted by noise
+    const Vector3 measuredOmega = runner.actualAngularVelocity(t);
+    const Vector3 measuredAcc = runner.actualSpecificForce(t);
+
+    // De-bias using the true (known) biases before feeding EKF
+    const Vector3 omega = measuredOmega - trueBias.gyroscope();
+    const Vector3 acc = measuredAcc - trueBias.accelerometer();
+
+    // Predict one step
+    ekf.predict(omega, acc, dt);
+
+    // Ground truth at next time
+    t += dt;
+    const NavState gt = scenario.navState(t);
+
+    // Print ground truth and EKF state
+    // cout << "Ground Truth: " << gt << "\n";
+    // cout << "EKF State: " << ekf.state() << "\n";
+
+    // Errors
+    const Rot3 dR = gt.attitude().between(ekf.state().attitude());
+    const Vector3 rpy = dR.rpy();
+    const double rot_err_deg =
+        (Vector3(rpy.cwiseAbs()) * (180.0 / M_PI)).norm();
+    const double pos_err = (gt.position() - ekf.state().position()).norm();
+    const double vel_err = (gt.velocity() - ekf.state().velocity()).norm();
+
+    cout << i + 1 << ", error: " << t << "," << rot_err_deg << "," << pos_err
+         << "," << vel_err << "\n";
+  }
+
+  return 0;
+}

gtsam/navigation/LeftLinearEKF.h

@@ -0,0 +1,93 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    LeftLinearEKF.h
+ * @brief   EKF on a Lie group with a general left–linear prediction model.
+ *
+ * See 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
+ *
+ * @date    August, 2025
+ * @authors Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>  // For traits (needed for AdjointMap, Expmap)
+#include <gtsam/navigation/LieGroupEKF.h>  // Include the base class
+
+#include <type_traits>  // For std::conjunction, std::is_invocable_r, std::is_same
+
+namespace gtsam {
+
+/**
+ * @class LeftLinearEKF
+ * @brief EKF on a Lie group with a general left–linear prediction model.
+ *
+ * Discrete step: x⁺ = W · ψ(x) · U, with W,U ∈ G and ψ ∈ Aut(G).
+ * For left-invariant error, the state-independent linearization is
+ * A = Ad_{U^{-1}} · Φ where Φ := dψ|_e (right-trivialized). The left factor
+ * W cancels in A and does not appear there.
+ */
+template <typename G>
+class LeftLinearEKF : public LieGroupEKF<G> {
+ public:
+  using Base = LieGroupEKF<G>;
+  static constexpr int Dim = Base::Dim;  ///< Compile-time dimension of G.
+  using TangentVector = typename Base::TangentVector;
+  using Jacobian = typename Base::Jacobian;
+  using Covariance = typename Base::Covariance;
+
+  LeftLinearEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {}
+
+  /**
+   * @brief SFINAE template to check if a type satisfies the automorphism
+   * concept. Specifically, it checks for the existence of:
+   * - G operator()(const G&) const
+   * - Jacobian dIdentity() const
+   */
+  template <typename Phi>
+  struct is_automorphism
+      : std::conjunction<
+            std::is_invocable_r<G, Phi, const G&>,
+            std::is_same<decltype(std::declval<Phi>().dIdentity()), Jacobian>> {
+  };
+
+  /**
+   * General left–linear dynamics using a φ functor and its differential at e.
+   * Returns W · φ(X) · U, and optional Jacobian A = Ad_{U^{-1}} Φ,  Φ := dφ|_e.
+   */
+  template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
+  static G Dynamics(const G& W, const Phi& phi, const G& X, const G& U,
+                    OptionalJacobian<Dim, Dim> A = {}) {
+    if (A) {
+      const G U_inv = traits<G>::Inverse(U);
+      *A = traits<G>::AdjointMap(U_inv) * phi.dIdentity();
+    }
+    return W * phi(X) * U;
+  }
+
+  /**
+   * General left–linear predict using a φ functor and its differential at e.
+   *   Update: X⁺ = W · φ(X) · U
+   *   Covariance: P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ,  Φ := dφ|_e.
+   */
+  template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
+  void predict(const G& W, const Phi& phi, const G& U, const Covariance& Q) {
+    Jacobian A;
+    this->X_ = this->Dynamics(W, phi, this->X_, U, A);
+    this->P_ = A * this->P_ * A.transpose() + Q;
+  }
+};
+
+}  // namespace gtsam

gtsam/navigation/LieGroupEKF.h

@@ -24,12 +24,13 @@
 
 #pragma once
 
-#include <gtsam/navigation/ManifoldEKF.h> // Include the base class
-#include <gtsam/base/Lie.h> // Include for Lie group traits and operations
+#include <gtsam/base/Lie.h>  // Include for Lie group traits and operations
+#include <gtsam/base/VectorSpace.h>        // Matrix traits
+#include <gtsam/navigation/ManifoldEKF.h>  // Include the base class
 
 #include <Eigen/Dense>
+#include <functional>  // For std::function
 #include <type_traits>
-#include <functional> // For std::function
 
 namespace gtsam {
 
@@ -180,6 +181,39 @@ namespace gtsam {
       return predict([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, Q);
     }
 
-  }; // LieGroupEKF
+    /**
+     * Predict using a precomputed group increment U and its Jacobian J_UX.
+     *
+     * Contract:
+     * - Input state X_k on G with covariance P_k in local coordinates
+     * - Precomputed increment U = U(X_k) in G
+     * - Jacobian J_UX = d(u_left)/d(local(X)) at X_k, where u_left = Log(U)
+     * - Process noise Q expressed in the same coordinates as u_left
+     *
+     * Update performed:
+     * - X_{k+1} = X_k ∘ U
+     * - A = Ad_{U^{-1}} + J_UX
+     * - P_{k+1} = A P_k A^T + Q
+     *
+     * Notes:
+     * This API is intended for custom integrators that construct U(X) directly
+     * (e.g., second-order kinematics), which may not be expressible as an Euler
+     * step on the tangent used by predict/predictMean.
+     */
+    void predictWithCompose(const G& U, const Jacobian& J_UX,
+                            const Covariance& Q) {
+      Jacobian A_local;
+      if constexpr (std::is_same_v<G, Matrix>) {
+        const Matrix I_n = Matrix::Identity(this->n_, this->n_);
+        A_local = I_n + J_UX;
+        this->X_ = traits<Matrix>::Retract(this->X_, U);
+      } else {
+        A_local = traits<G>::Inverse(U).AdjointMap() + J_UX;
+        this->X_ = this->X_.compose(U);
+      }
+      this->P_ = A_local * this->P_ * A_local.transpose() + Q;
+    }
+
+  };  // LieGroupEKF
 
 }  // namespace gtsam

gtsam/navigation/ManifoldEKF.h

@@ -247,6 +247,34 @@ namespace gtsam {
       update(prediction, H, z, R);
     }
 
+    /// Convenience bridge for wrappers: vector measurement update calling
+    /// update<Vector>. This overload exists to avoid templates in wrappers. It
+    /// validates sizes and forwards to the templated update with Measurement =
+    /// gtsam::Vector (dynamic size).
+    void updateWithVector(const gtsam::Vector& prediction, const Matrix& H,
+                          const gtsam::Vector& z, const Matrix& R) {
+      // Basic dimension checks for dynamic-sized measurement
+      const int m = static_cast<int>(prediction.size());
+      if (static_cast<int>(z.size()) != m) {
+        throw std::invalid_argument(
+            "ManifoldEKF::updateWithVector: prediction and z must have same "
+            "length.");
+      }
+      if (H.rows() != m || H.cols() != n_) {
+        throw std::invalid_argument(
+            "ManifoldEKF::updateWithVector: H must be m x n where m = "
+            "measurement size and n = state dimension.");
+      }
+      if (R.rows() != m || R.cols() != m) {
+        throw std::invalid_argument(
+            "ManifoldEKF::updateWithVector: R must be m x m where m = "
+            "measurement size.");
+      }
+
+      // Forward to templated update with Measurement = Vector
+      this->template update<Vector>(prediction, H, z, R);
+    }
+
   protected:
     M X_;              ///< Manifold state estimate.
     Covariance P_;     ///< Covariance (Eigen::Matrix<double, Dim, Dim>).

gtsam/navigation/NavState.h

@@ -266,7 +266,25 @@ class GTSAM_EXPORT NavState : public MatrixLieGroup<NavState, 9, 5> {
   /// @name Dynamics
   /// @{
 
-  /// Integrate forward in time given angular velocity and acceleration in body frame
+  // Ψ: autonomous flow where velocity acts on position for
+  //   dt (R, p, v) -> p += v·dt.
+  struct AutonomousFlow {
+    double dt;
+
+    // Differential at identity (right-trivialized): Φ = I with ∂p/∂v = dt·I.
+    Jacobian dIdentity() const {
+      Jacobian Phi = I_9x9;
+      Phi.template block<3, 3>(3, 6) = I_3x3 * dt;
+      return Phi;
+    }
+
+    // Apply ψ(x) by p += v·dt
+    NavState operator()(const NavState& X) const {
+      return {X.attitude(), X.position() + X.velocity() * dt, X.velocity()};
+    }
+  };
+
+/// Integrate forward in time given angular velocity and acceleration in body frame
   /// Uses second order integration for position, returns derivatives except dt.
   NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
                   const double dt, OptionalJacobian<9, 9> F = {},