From 87c4445f53ee53f39b2aba7a226fdae32fd97bfd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 21:01:12 -0400 Subject: [PATCH] predictMean for state=control f --- gtsam/navigation/LIEKF.h | 83 ++++++++++++++++------------ gtsam/navigation/tests/testLIEKF.cpp | 30 ++++++++++ 2 files changed, 78 insertions(+), 35 deletions(-) diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index fda75ccd5..9b4b6d2b5 100644 --- a/gtsam/navigation/LIEKF.h +++ b/gtsam/navigation/LIEKF.h @@ -96,53 +96,51 @@ class LIEKF { predict(G::Expmap(u * dt), Q); } + /** + * SFINAE check for correctly typed state-dependent dynamics function. + * xi = f(X, F) + * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) + */ + template + using enable_if_dynamics = std::enable_if_t< + !std::is_convertible_v && + std::is_invocable_r_v&>>; /** * Predict mean only with state-dependent dynamics: * xi = f(X, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * * @param f dynamics functor depending on state and control * @param dt time step * @param Q process noise covariance */ - template - // …and is callable as a true functor - && std::is_invocable_r_v>>> + template > G predictMean(Dynamics&& f, double dt, const Matrix& Q, OptionalJacobian 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; + typename G::Jacobian Df, Dexp; + typename G::TangentVector xi = f(X_, Df); + G U = G::Expmap(xi * dt, Dexp); + // derivative of mean = d[X.compose(U)]/dLog(X) + // = Ad_{U⁻¹} (the “left‐invariant” part) + // plus the effect of X→U via Expmap: + // Dexp * Df * dt (how U moves when X moves through f) + if (A) *A = U.inverse().AdjointMap() + Dexp * Df * dt; return X_.compose(U); } /** * Predict step with state-dependent dynamics: * xi = f(X, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Dynamics signature: G f(const G&, OptionalJacobian&) - * * @param f dynamics functor depending on state and control * @param dt time step * @param Q process noise covariance */ - template - // …and is callable as a true functor - && std::is_invocable_r_v>>> + template > void predict(Dynamics&& f, double dt, const Matrix& Q) { typename G::Jacobian A; X_ = predictMean(f, dt, Q, &A); @@ -150,10 +148,9 @@ class LIEKF { } /** - * Predict step with state and control input dynamics: + * Predict mean only with state and control input dynamics: * xi = f(X, u, F) - * U = Expmap(xi * dt) - * A = Ad_{U^{-1}} * F + * X_.expmap(xi * dt) * * @tparam Control control input type * @tparam Dynamics signature: G f(const G&, const Control&, @@ -164,14 +161,30 @@ class LIEKF { * @param dt time step * @param Q process noise covariance */ - template // H - >>> + template + G predictMean(Dynamics&& f, const Control& u, double dt, const Matrix& Q, + OptionalJacobian A = {}) { + auto g = [f, u](const G& X, OptionalJacobian F) { + return f(X, u, F); + }; + return predictMean(g, dt, Q, A); + } + + /** + * Predict step with state and control input dynamics: + * xi = f(X, u, F) + * X_.expmap(xi * dt) + * + * @tparam Control control input type + * @tparam Dynamics signature: G f(const G&, const Control&, + * OptionalJacobian&) + * + * @param f dynamics functor depending on state and control + * @param u control input + * @param dt time step + * @param Q process noise covariance + */ + template void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { auto g = [f, u](const G& X, OptionalJacobian F) { return f(X, u, F); diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 9790833f8..5577a4357 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -79,6 +79,36 @@ TEST(IEKF, PredictNumericState) { EXPECT(assert_equal(expectedH, actualH)); } +TEST(IEKF, StateAndControl) { + auto f = [](const Rot3& X, const Vector2& dummy_u, + OptionalJacobian<3, 3> H = {}) { + return exampleSO3::dynamics(X, H); + }; + + // GIVEN + Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3); + Matrix3 P0 = Matrix3::Identity() * 0.2; + Vector2 dummy_u(1, 2); + double dt = 0.1; + Matrix3 Q = Matrix3::Zero(); + + // Analytic Jacobian + Matrix3 actualH; + LIEKF iekf0(R0, P0); + iekf0.predictMean(f, dummy_u, dt, Q, actualH); + + // wrap predict into a state->state functor (mapping on SO(3)) + auto g = [&](const Rot3& R) -> Rot3 { + LIEKF iekf(R, P0); + return iekf.predictMean(f, dummy_u, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);