well-tested predictMean

gtsam/navigation/LIEKF.h

@@ -96,6 +96,34 @@ class LIEKF {
     predict(G::Expmap(u * dt), Q);
   }
 
+  /**
+   * Predict mean only 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,
+            typename = std::enable_if_t<
+                // not a plain vector…
+                !std::is_convertible_v<Dynamics, typename G::TangentVector>
+                // …and is callable as a true functor
+                && std::is_invocable_r_v<typename G::TangentVector, Dynamics,
+                                         const G&, OptionalJacobian<n, n>>>>
+  G predictMean(Dynamics&& f, double dt, const Matrix& Q,
+                OptionalJacobian<n, n> A = {}) const {
+    typename G::Jacobian F1, F2;
+    typename G::TangentVector xi = f(X_, F1);
+    G U = G::Expmap(xi * dt, F2);
+    if (A) *A = U.inverse().AdjointMap() + F2 * F1 * dt;
+    return X_.compose(U);
+  }
+
   /**
    * Predict step with state-dependent dynamics:
    *   xi = f(X, F)
@@ -114,13 +142,10 @@ class LIEKF {
                 !std::is_convertible_v<Dynamics, typename G::TangentVector>
                 // …and is callable as a true functor
                 && std::is_invocable_r_v<typename G::TangentVector, Dynamics,
-                                         const G&, OptionalJacobian<n, n>&> > >
+                                         const G&, OptionalJacobian<n, n>>>>
   void predict(Dynamics&& f, double dt, const Matrix& Q) {
-    typename G::Jacobian F;
-    typename G::TangentVector xi = f(X_, F);
-    G U = G::Expmap(xi * dt);
-    typename G::Jacobian A = U.inverse().AdjointMap() * F;
-    X_ = X_.compose(U);
+    typename G::Jacobian A;
+    X_ = predictMean(f, dt, Q, &A);
     P_ = A * P_ * A.transpose() + Q;
   }
 
@@ -145,15 +170,13 @@ class LIEKF {
                                       Dynamics,                   // functor
                                       const G&,                   // X
                                       const Control&,             // u
-                                      OptionalJacobian<n, n>&     // H
+                                      OptionalJacobian<n, n>      // H
                                       >>>
   void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
-    typename G::Jacobian F;
-    typename G::TangentVector xi = f(X_, u, F);
-    G U = G::Expmap(xi * dt);
-    typename G::Jacobian A = U.inverse().AdjointMap() * F;
-    X_ = X_.compose(U);
-    P_ = A * P_ * A.transpose() + Q;
+    auto g = [f, u](const G& X, OptionalJacobian<n, n> F) {
+      return f(X, u, F);
+    };
+    predict(g, dt, Q);
   }
 
   /**

gtsam/navigation/tests/testLIEKF.cpp

@@ -17,12 +17,14 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/navigation/LIEKF.h>
 #include <gtsam/navigation/NavState.h>
 
 using namespace gtsam;
 
 // Duplicate the dynamics function in LIEKF_Rot3Example
-namespace example {
+namespace exampleSO3 {
 static constexpr double k = 0.5;
 Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
   // φ = Logmap(R), Dφ = ∂φ/∂δR
@@ -35,22 +37,46 @@ Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
   if (H) *H = -k * D_phi;  // ∂(–kφ)/∂δR
   return -k * phi;         // xi ∈ 𝔰𝔬(3)
 }
-}  // namespace example
+}  // namespace exampleSO3
 
-TEST(LIEKFNavState, dynamicsJacobian) {
+TEST(IEKF, dynamicsJacobian) {
   // Construct a nontrivial state and IMU input
   Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3);
 
-  // Analytic Jacobian (always zero for left-invariant dynamics)
+  // Analytic Jacobian
   Matrix3 actualH;
-  example::dynamics(R, actualH);
+  exampleSO3::dynamics(R, actualH);
 
   // Numeric Jacobian w.r.t. the state X
-  auto f = [&](const Rot3& X_) { return example::dynamics(X_); };
-  Matrix3 expectedH = numericalDerivative11<Vector3, Rot3>(f, R, 1e-6);
+  auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); };
+  Matrix3 expectedH = numericalDerivative11<Vector3, Rot3>(f, R);
 
   // Compare
-  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+TEST(IEKF, PredictNumericState) {
+  // GIVEN
+  Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3);
+  Matrix3 P0 = Matrix3::Identity() * 0.2;
+  double dt = 0.1;
+  Matrix3 Q = Matrix3::Zero();
+
+  // Analytic Jacobian
+  Matrix3 actualH;
+  LIEKF<Rot3> iekf0(R0, P0);
+  iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH);
+
+  // wrap predict into a state->state functor (mapping on SO(3))
+  auto g = [&](const Rot3& R) -> Rot3 {
+    LIEKF<Rot3> iekf(R, P0);
+    return iekf.predictMean(exampleSO3::dynamics, dt, Q);
+  };
+
+  // numeric Jacobian of g at R0
+  Matrix3 expectedH = numericalDerivative11<Rot3, Rot3>(g, R0);
+
+  EXPECT(assert_equal(expectedH, actualH));
 }
 
 int main() {