From 192b6a26ffd178c8c778a95d8920741a89714b36 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:48:56 -0400 Subject: [PATCH] Deal with Matrix --- gtsam/navigation/InvariantEKF.h | 10 ++- gtsam/navigation/LieGroupEKF.h | 31 +++++-- gtsam/navigation/tests/testInvariantEKF.cpp | 93 +++++++++++++++++++++ 3 files changed, 125 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index 20d74285f..a2eea3719 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -95,7 +95,15 @@ namespace gtsam { * @param Q Process noise covariance matrix. */ void predict(const TangentVector& u, double dt, const Covariance& Q) { - const G U = traits::Expmap(u * dt); + G U; + if constexpr (std::is_same_v) { + const Matrix& X = static_cast(this->X_); + U.resize(X.rows(), X.cols()); + Eigen::Map(static_cast(U).data(), U.size()) = u * dt; + } + else { + U = traits::Expmap(u * dt); + } predict(U, Q); // Call the group composition predict } diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 07efa0b0b..4421ab660 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -92,15 +92,30 @@ namespace gtsam { G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { Jacobian Df, Dexp; // Eigen::Matrix - TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) - G U = traits::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt - G X_next = this->X_.compose(U); - - if (A) { - // State transition Jacobian for left-invariant EKF: - *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + if constexpr (std::is_same_v) { + std::cout << "We are here" << std::endl; + Jacobian Df; // Eigen::Matrix + TangentVector xi = f(this->X_, Df); + const Matrix& X = static_cast(this->X_); + G U = Matrix(X.rows(), X.cols()); + Eigen::Map(static_cast(U).data(), U.size()) = xi * dt; + if (A) { + const Matrix I_n = Matrix::Identity(this->n_, this->n_); + const Matrix& Dexp = I_n; + *A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix + } + return this->X_ + U; // Matrix addition + } + else { + Jacobian Df, Dexp; // Eigen::Matrix + TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX) + G U = traits::Expmap(xi * dt, A ? &Dexp : nullptr); + if (A) { + // State transition Jacobian for left-invariant EKF: + *A = traits::Inverse(U).AdjointMap() + Dexp * Df * dt; + } + return this->X_.compose(U); } - return X_next; } diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 74a70fbbe..2916d6da0 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -119,6 +119,99 @@ TEST(IEKF_Pose2, PredictUpdateSequence) { } +// Define simple dynamics and measurement for a 2x2 Matrix state +namespace exampleDynamicMatrix { + // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + // This is mainly for verification; IEKF predict will use tangent vector directly. + Matrix predictNextStateManually(const Matrix& p, const Vector& vTangent, double dt) { + return traits::Retract(p, vTangent * dt); + } + // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here) + double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { + // p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11] + // trace = p(0,0) + p(1,1) + // H = d(trace)/d(p_flat) = [1, 0, 0, 1] + // The Jacobian H will be 1x4 for a 2x2 matrix. + H->resize(1, p.size()); // p.size() is rows*cols + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2, so 1x4 + } + return p(0, 0) + p(1, 1); + } +} // namespace exampleDynamicMatrix + +// Test fixture for InvariantEKF with a 2x2 Matrix state +struct DynamicMatrixEKFTest { + Matrix p0Matrix; // Initial state (as 2x2 Matrix) + Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4) + Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix) + double dt; + Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4) + Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1) + DynamicMatrixEKFTest() : + p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()), + p0Covariance(I_4x4 * 0.01), + velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec + dt(0.1), + processNoiseCovariance(I_4x4 * 0.001), + measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005) { + } +}; + +TEST(InvariantEKF_DynamicMatrix, Predict) { + DynamicMatrixEKFTest data; + InvariantEKF ekf(data.p0Matrix, data.p0Covariance); + // For a 2x2 Matrix, tangent space dimension is 2*2=4. + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + // --- Perform EKF prediction using InvariantEKF::predict(tangentVector, dt, Q) --- + ekf.predict(data.velocityTangent, data.dt, data.processNoiseCovariance); + // --- Verification --- + // 1. Calculate expected next state + Matrix pNextExpected = exampleDynamicMatrix::predictNextStateManually(data.p0Matrix, data.velocityTangent, data.dt); + EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); + // 2. Calculate expected covariance + // For VectorSpace, AdjointMap is Identity. So P_next = P_prev + Q. + Matrix pCovarianceExpected = data.p0Covariance + data.processNoiseCovariance; + EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); +} + +TEST(InvariantEKF_DynamicMatrix, Update) { + DynamicMatrixEKFTest data; + Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); + Matrix pStartCovariance = I_4x4 * 0.02; + InvariantEKF ekf(pStartMatrix, pStartCovariance); + EXPECT_LONGS_EQUAL(4, ekf.state().size()); + EXPECT_LONGS_EQUAL(4, ekf.dimension()); + // Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0) + double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here + double zObserved = zTrue - 0.03; // Add some "error" + // --- Perform EKF update --- + // The update method is inherited from ManifoldEKF. + ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); + // --- Verification (Manual Kalman Update Steps) --- + // 1. Predict measurement and get Jacobian H + Matrix H_manual(1, 4); // This will be 1x4 for a 2x2 matrix measurement + double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H_manual); + // 2. Innovation and Innovation Covariance + // EKF calculates innovation_tangent = traits::Local(prediction, zObserved) + // For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual. + double innovationY_tangent = zObserved - zPredictionManual; + Matrix innovationCovarianceS = H_manual * pStartCovariance * H_manual.transpose() + data.measurementNoiseCovariance; + // 3. Kalman Gain K + Matrix kalmanGainK = pStartCovariance * H_manual.transpose() * innovationCovarianceS.inverse(); // K is 4x1 + // 4. State Correction (in tangent space of Matrix) + Vector deltaXiTangent = kalmanGainK * innovationY_tangent; // deltaXi is 4x1 Vector + // 5. Expected Updated State and Covariance (using Joseph form) + Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); + Matrix I_KH = I_4x4 - kalmanGainK * H_manual; + Matrix pUpdatedCovarianceExpected = I_KH * pStartCovariance * I_KH.transpose() + kalmanGainK * data.measurementNoiseCovariance * kalmanGainK.transpose(); + // --- Compare EKF result with manual calculation --- + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);