From 062bdf64ea680e3f13d53427a21df6e174ee4ead Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Apr 2025 10:40:28 -0400 Subject: [PATCH] Added Rot3 example to show state-dependent control --- examples/LIEKF_NavstateExample.cpp | 81 ++++++++++++---------------- examples/LIEKF_Rot3Example.cpp | 78 +++++++++++++++++++++++++++ gtsam/navigation/LIEKF.h | 22 ++++++++ gtsam/navigation/tests/testLIEKF.cpp | 36 ++++++------- 4 files changed, 150 insertions(+), 67 deletions(-) create mode 100644 examples/LIEKF_Rot3Example.cpp diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 67ae23fd1..250d5cda8 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/examples/LIEKF_NavstateExample.cpp @@ -11,16 +11,15 @@ /** * @file LIEKF_NavstateExample.cpp - * @brief Example of a Left-Invariant Extended Kalman Filter on NavState - * using IMU (predict) and GPS (update) measurements. + * @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) * @date April 25, 2025 * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #include #include -#include #include +#include #include @@ -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 accel (first 3) and angular vel (last - * 3). - * @param H Optional 9×9 Jacobian w.r.t. X (always zero here). + * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity. * @return 9×1 tangent: [ω; 0₃; a]. */ -Vector9 dynamics(const NavState& X, const Vector6& imu, - OptionalJacobian<9, 9> H = {}) { +Vector9 dynamics(const Vector6& imu, 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) { - // H = [ 0₃×3, 0₃×3, R ] - *H << Z_3x3, Z_3x3, X.R(); - } + if (H) *H << Z_3x3, Z_3x3, X.R().matrix(); return X.t(); } int main() { - // Initial state, covariance, and time step - NavState X0; + // Initial state & covariances + 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 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, - // angular_velocity) - Vector6 imu1, imu2; - imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0; - imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0; + // Two IMU samples [a; ω] + Vector6 imu1; + imu1 << 0.1, 0, 0, 0, 0.2, 0; + Vector6 imu2; + imu2 << 0, 0.3, 0, 0.4, 0, 0; - // Create the GPS measurements of the form (px, py, pz) - Vector3 z1, z2; - z1 << 0.3, 0.0, 0.0; - z2 << 0.6, 0.0, 0.0; + // Two GPS fixes + Vector3 z1; + z1 << 0.3, 0, 0; + Vector3 z2; + z2 << 0.6, 0, 0; - cout << "=== Initialization ===\n" - << "X0: " << ekf.state() << "\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"; + cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << 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" - << "X: " << ekf.state() << "\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"; + cout << "--- After update 1 ---\nX: " << ekf.state() + << "\nP: " << 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" - << "X: " << ekf.state() << "\n" - << "P: " << ekf.covariance() << "\n"; + cout << "--- After update 2 ---\nX: " << ekf.state() + << "\nP: " << ekf.covariance() << "\n"; return 0; } diff --git a/examples/LIEKF_Rot3Example.cpp b/examples/LIEKF_Rot3Example.cpp new file mode 100644 index 000000000..331c01bb1 --- /dev/null +++ b/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 +#include +#include +#include + +#include + + +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 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; +} diff --git a/gtsam/navigation/LIEKF.h b/gtsam/navigation/LIEKF.h index 840d004a7..843365c6d 100644 --- a/gtsam/navigation/LIEKF.h +++ b/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&) + * + * @param f dynamics functor depending on state and control + * @param dt time step + * @param Q process noise covariance + */ + template + 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 diff --git a/gtsam/navigation/tests/testLIEKF.cpp b/gtsam/navigation/tests/testLIEKF.cpp index 81e80a69d..23f812274 100644 --- a/gtsam/navigation/tests/testLIEKF.cpp +++ b/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, - 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; +static constexpr double k = 0.5; +Vector3 dynamics(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) } } // 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), - 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 + Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3); // Analytic Jacobian (always zero for left-invariant dynamics) - OptionalJacobian<9, 9> H_analytic; - example::dynamics(X, imu, H_analytic); - Matrix actualH = *H_analytic; + Matrix3 actualH; + example::dynamics(R, actualH); // Numeric Jacobian w.r.t. the state X - auto f = [&](const NavState& X_) { return example::dynamics(X_, imu); }; - Matrix expectedH = numericalDerivative11(f, X, 1e-6); + auto f = [&](const Rot3& X_) { return example::dynamics(X_); }; + Matrix3 expectedH = numericalDerivative11(f, R, 1e-6); // Compare EXPECT(assert_equal(expectedH, actualH, 1e-8));