Added Rot3 example to show state-dependent control

examples/LIEKF_NavstateExample.cpp

@@ -11,16 +11,15 @@
 
 /**
  * @file LIEKF_NavstateExample.cpp
- * @brief Example of a Left-Invariant Extended Kalman Filter on NavState
+ * @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
- *        using IMU (predict) and GPS (update) measurements.
  * @date April 25, 2025
  * @authors Scott Baker, Matt Kielo, Frank Dellaert
  */
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/OptionalJacobian.h>
-#include <gtsam/navigation/NavState.h>
 #include <gtsam/navigation/LIEKF.h>
+#include <gtsam/navigation/NavState.h>
 
 #include <iostream>
 
@@ -29,19 +28,14 @@ using namespace gtsam;
 
 /**
  * @brief Left-invariant dynamics for NavState.
- * @param X    Current state (unused for left-invariant error dynamics).
+ * @param imu  6×1 vector [a; ω]: linear acceleration and angular velocity.
- * @param imu  6×1 vector [a; ω]: linear accel (first 3) and angular vel (last
- * 3).
- * @param H    Optional 9×9 Jacobian w.r.t. X (always zero here).
  * @return     9×1 tangent: [ω; 0₃; a].
  */
-Vector9 dynamics(const NavState& X, const Vector6& imu,
+Vector9 dynamics(const Vector6& imu, OptionalJacobian<9, 9> H = {}) {
-                 OptionalJacobian<9, 9> H = {}) {
   auto a = imu.head<3>();
   auto w = imu.tail<3>();
   Vector9 xi;
   xi << w, Vector3::Zero(), a;
-  if (H) *H = Matrix9::Zero();
   return xi;
 }
 
@@ -52,60 +46,51 @@ Vector9 dynamics(const NavState& X, const Vector6& imu,
  * @return     3×1 position vector.
  */
 Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
-  if (H) {
+  if (H) *H << Z_3x3, Z_3x3, X.R().matrix();
-    // H = [ 0₃×3, 0₃×3, R ]
-    *H << Z_3x3, Z_3x3, X.R();
-  }
   return X.t();
 }
 
 int main() {
-  // Initial state, covariance, and time step
+  // Initial state & covariances
-  NavState X0;
+  NavState X0;  // R=I, v=0, t=0
   Matrix9 P0 = Matrix9::Identity() * 0.1;
-  double dt = 1.0;
-
-  // Create the filter with the initial state and covariance.
   LIEKF<NavState> ekf(X0, P0);
 
-  // Process & measurement noise
+  // Noise & timestep
+  double dt = 1.0;
   Matrix9 Q = Matrix9::Identity() * 0.01;
   Matrix3 R = Matrix3::Identity() * 0.5;
 
-  // Create the IMU measurements of the form (linear_acceleration,
+  // Two IMU samples [a; ω]
-  // angular_velocity)
+  Vector6 imu1;
-  Vector6 imu1, imu2;
+  imu1 << 0.1, 0, 0, 0, 0.2, 0;
-  imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0;
+  Vector6 imu2;
-  imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0;
+  imu2 << 0, 0.3, 0, 0.4, 0, 0;
 
-  // Create the GPS measurements of the form (px, py, pz)
+  // Two GPS fixes
-  Vector3 z1, z2;
+  Vector3 z1;
-  z1 << 0.3, 0.0, 0.0;
+  z1 << 0.3, 0, 0;
-  z2 << 0.6, 0.0, 0.0;
+  Vector3 z2;
+  z2 << 0.6, 0, 0;
 
-  cout << "=== Initialization ===\n"
+  cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
-       << "X0: " << ekf.state() << "\n"
+       << "\n\n";
-       << "P0: " << ekf.covariance() << "\n\n";
-
-  ekf.predict(dynamics, imu1, dt, Q);
-  cout << "--- After first predict ---\n"
-       << "X: " << ekf.state() << "\n"
-       << "P: " << ekf.covariance() << "\n\n";
 
+  // --- first predict/update ---
+  ekf.predict(dynamics(imu1), dt, Q);
+  cout << "--- After predict 1 ---\nX: " << ekf.state()
+       << "\nP: " << ekf.covariance() << "\n\n";
   ekf.update(h_gps, z1, R);
-  cout << "--- After first update ---\n"
+  cout << "--- After update 1 ---\nX: " << ekf.state()
-       << "X: " << ekf.state() << "\n"
+       << "\nP: " << ekf.covariance() << "\n\n";
-       << "P: " << ekf.covariance() << "\n\n";
-
-  ekf.predict(dynamics, imu2, dt, Q);
-  cout << "--- After second predict ---\n"
-       << "X: " << ekf.state() << "\n"
-       << "P: " << ekf.covariance() << "\n\n";
 
+  // --- second predict/update ---
+  ekf.predict(dynamics(imu2), dt, Q);
+  cout << "--- After predict 2 ---\nX: " << ekf.state()
+       << "\nP: " << ekf.covariance() << "\n\n";
   ekf.update(h_gps, z2, R);
-  cout << "--- After second update ---\n"
+  cout << "--- After update 2 ---\nX: " << ekf.state()
-       << "X: " << ekf.state() << "\n"
+       << "\nP: " << ekf.covariance() << "\n";
-       << "P: " << ekf.covariance() << "\n";
 
   return 0;
 }

examples/LIEKF_Rot3Example.cpp

@@ -0,0 +1,78 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LIEKF_Rot3Example.cpp
+ * @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
+ * and a single magnetometer update.
+ * @date April 25, 2025
+ * @authors Scott Baker, Matt Kielo, Frank Dellaert
+ */
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/navigation/LIEKF.h>
+
+#include <iostream>
+
+
+using namespace std;
+using namespace gtsam;
+
+// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ ---
+static constexpr double k = 0.5;
+Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
+  // φ = Logmap(R), Dφ = ∂φ/∂δR
+  Matrix3 Dφ;
+  Vector3 φ = Rot3::Logmap(X, Dφ);
+  // zero out yaw
+  φ[2] = 0.0;
+  Dφ.row(2).setZero();
+
+  if (H) *H = -k * Dφ;  // ∂(–kφ)/∂δR
+  return -k * φ;        // xi ∈ 𝔰𝔬(3)
+}
+
+// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
+static const Vector3 m_world(0, 0, -1);
+Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
+  Vector3 z = X.inverse().rotate(m_world);
+  if (H) *H = -skewSymmetric(z);
+  return z;
+}
+
+int main() {
+  // Initial estimate (identity) and covariance
+  const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
+  const Matrix3 P0 = Matrix3::Identity() * 0.1;
+  LIEKF<Rot3> ekf(R0, P0);
+
+  // Timestep, process noise, measurement noise
+  double dt = 0.1;
+  Matrix3 Q = Matrix3::Identity() * 0.01;
+  Matrix3 Rm = Matrix3::Identity() * 0.05;
+
+  cout << "=== Init ===\nR:\n"
+       << ekf.state().matrix() << "\nP:\n"
+       << ekf.covariance() << "\n\n";
+
+  // Predict using state‐dependent f
+  ekf.predict(dynamicsSO3, dt, Q);
+  cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
+
+  // Magnetometer measurement = body‐frame reading of m_world
+  Vector3 z = h_mag(R0);
+  ekf.update(h_mag, z, Rm);
+  cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n";
+
+  return 0;
+}

gtsam/navigation/LIEKF.h

@@ -98,6 +98,28 @@ class LIEKF {
 
   /**
    * Predict step with state-dependent dynamics:
+   *   xi = f(X, F)
+   *   U  = Expmap(xi * dt)
+   *   A  = Ad_{U^{-1}} * F
+   *
+   * @tparam Dynamics  signature: G f(const G&,  OptionalJacobian<n,n>&)
+   *
+   * @param f   dynamics functor depending on state and control
+   * @param dt  time step
+   * @param Q   process noise covariance
+   */
+  template <typename Dynamics>
+  void predict(Dynamics&& f, double dt, const Matrix& Q) {
+    typename G::Jacobian F;
+    auto xi = f(X_, F);
+    G U = G::Expmap(xi * dt);
+    auto A = U.inverse().AdjointMap() * F;
+    X_ = X_.compose(U);
+    P_ = A * P_ * A.transpose() + Q;
+  }
+
+  /**
+   * Predict step with state and control input dynamics:
    *   xi = f(X, u, F)
    *   U  = Expmap(xi * dt)
    *   A  = Ad_{U^{-1}} * F

gtsam/navigation/tests/testLIEKF.cpp

@@ -21,35 +21,33 @@
 
 using namespace gtsam;
 
-// Duplicate the dynamics function under test:
+// Duplicate the dynamics function in LIEKF_Rot3Example
 namespace example {
-Vector9 dynamics(const NavState& X, const Vector6& imu,
+static constexpr double k = 0.5;
-                 OptionalJacobian<9, 9> H = {}) {
+Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
-  auto a = imu.head<3>();
+  // φ = Logmap(R), Dφ = ∂φ/∂δR
-  auto w = imu.tail<3>();
+  Matrix3 Dφ;
-  Vector9 xi;
+  Vector3 φ = Rot3::Logmap(X, Dφ);
-  xi << w, Vector3::Zero(), a;
+  // zero out yaw
-  if (H) *H = Matrix9::Zero();
+  φ[2] = 0.0;
-  return xi;
+  Dφ.row(2).setZero();
+
+  if (H) *H = -k * Dφ;  // ∂(–kφ)/∂δR
+  return -k * φ;        // xi ∈ 𝔰𝔬(3)
 }
 }  // namespace example
 
 TEST(LIEKFNavState, dynamicsJacobian) {
   // Construct a nontrivial state and IMU input
-  NavState X(Rot3::RzRyRx(0.1, -0.2, 0.3), Point3(1.0, 2.0, 3.0),
+  Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3);
-             Vector3(0.5, -0.5, 0.5));
-  Vector6 imu;
-  imu << 0.1, -0.1, 0.2,  // acceleration
-      0.01, -0.02, 0.03;  // angular velocity
 
   // Analytic Jacobian (always zero for left-invariant dynamics)
-  OptionalJacobian<9, 9> H_analytic;
+  Matrix3 actualH;
-  example::dynamics(X, imu, H_analytic);
+  example::dynamics(R, actualH);
-  Matrix actualH = *H_analytic;
 
   // Numeric Jacobian w.r.t. the state X
-  auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); };
+  auto f = [&](const Rot3& X_) { return example::dynamics(X_); };
-  Matrix expectedH = numericalDerivative11<Vector9, NavState>(f, X, 1e-6);
+  Matrix3 expectedH = numericalDerivative11<Vector3, Rot3>(f, R, 1e-6);
 
   // Compare
   EXPECT(assert_equal(expectedH, actualH, 1e-8));