diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index a8a136094..fda75ccd5 100644 --- a/gtsam/navigation/LIEKF.h +++ b/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&) + * + * @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>>> + 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; + return X_.compose(U); + } + /** * Predict step with state-dependent dynamics: * xi = f(X, F) @@ -114,13 +142,10 @@ class LIEKF { !std::is_convertible_v // …and is callable as a true functor && std::is_invocable_r_v&> > > + const G&, OptionalJacobian>>> 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& // H - > > > + OptionalJacobian // 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 F) { + return f(X, u, F); + }; + predict(g, dt, Q); } /** diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 65325c63e..9790833f8 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/gtsam/navigation/tests/testLIEKF.cpp @@ -17,12 +17,14 @@ #include #include #include +#include +#include #include 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(f, R, 1e-6); + auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(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 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 iekf(R, P0); + return iekf.predictMean(exampleSO3::dynamics, dt, Q); + }; + + // numeric Jacobian of g at R0 + Matrix3 expectedH = numericalDerivative11(g, R0); + + EXPECT(assert_equal(expectedH, actualH)); } int main() {