From 5758c7ef0cc83ab3d738d699947062d65af6b712 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 15:58:15 -0400 Subject: [PATCH] Simplify tests --- gtsam/navigation/tests/testInvariantEKF.cpp | 121 ++++++--------- gtsam/navigation/tests/testManifoldEKF.cpp | 157 +++++++++----------- 2 files changed, 118 insertions(+), 160 deletions(-) diff --git a/gtsam/navigation/tests/testInvariantEKF.cpp b/gtsam/navigation/tests/testInvariantEKF.cpp index 2916d6da0..4797f79a1 100644 --- a/gtsam/navigation/tests/testInvariantEKF.cpp +++ b/gtsam/navigation/tests/testInvariantEKF.cpp @@ -121,97 +121,66 @@ 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) { + Matrix f(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 = {}) { + double h(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 + H->resize(1, p.size()); + (*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2 } - return p(0, 0) + p(1, 1); + return p(0, 0) + p(1, 1); // trace ! } } // 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, PredictAndUpdate) { + // --- Setup --- + Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix p0Covariance = I_4x4 * 0.01; + Vector velocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); + double dt = 0.1; + Matrix Q = I_4x4 * 0.001; + Matrix R = 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); + InvariantEKF ekf(p0Matrix, p0Covariance); 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. + + // --- Predict --- + ekf.predict(velocityTangent, dt, Q); + + // Verification for Predict + Matrix pPredictedExpected = exampleDynamicMatrix::f(p0Matrix, velocityTangent, dt); + Matrix pCovariancePredictedExpected = p0Covariance + Q; + EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9)); + + // --- Update --- + // Use the state after prediction for the update step + Matrix pStateBeforeUpdate = ekf.state(); + Matrix pCovarianceBeforeUpdate = ekf.covariance(); + + double zTrue = exampleDynamicMatrix::h(pStateBeforeUpdate); + double zObserved = zTrue - 0.03; // Simulated measurement with some error + + ekf.update(exampleDynamicMatrix::h, zObserved, R); + + // Verification for Update (Manual Kalman Steps) + Matrix H(1, 4); + double zPredictionManual = exampleDynamicMatrix::h(pStateBeforeUpdate, H); 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 --- + Matrix S = H * pCovarianceBeforeUpdate * H.transpose() + R; + Matrix kalmanGainK = pCovarianceBeforeUpdate * H.transpose() * S.inverse(); + Vector deltaXiTangent = kalmanGainK * innovationY_tangent; + Matrix pUpdatedExpected = traits::Retract(pStateBeforeUpdate, deltaXiTangent); + Matrix I_KH = I_4x4 - kalmanGainK * H; + Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + kalmanGainK * R * kalmanGainK.transpose(); + EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9)); EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9)); } + int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 7516cd1cc..8fe96557d 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -151,110 +151,99 @@ TEST(ManifoldEKF_Unit3, Update) { namespace exampleDynamicMatrix { // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt. + // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v. Matrix predictNextState(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 << 1.0, 0.0, 0.0, 1.0; + // H is the Jacobian dh/d(flatten(p)) + double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H_opt = {}) { + if (H_opt) { + // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size(). + // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1]. + Matrix H = Matrix::Zero(1, p.size()); + if (p.rows() >= 2 && p.cols() >= 2) { // Ensure it's at least 2x2 for trace definition + H(0, 0) = 1.0; // d(trace)/dp00 + H(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major) + // For 2x2: H(0, 2*1 + 1) = H(0,3) + } + *H_opt = H; } - return p(0, 0) + p(1, 1); + if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error + double trace = 0.0; + for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) { + trace += p(i, i); + } + return trace; } } // namespace exampleDynamicMatrix -// Test fixture for ManifoldEKF 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) +TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) { + Matrix p_initial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix P_initial = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) + Vector v_tangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec + double dt = 0.1; + Matrix Q = I_4x4 * 0.001; // Process noise covariance (4x4) + Matrix R = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (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(ManifoldEKF_DynamicMatrix, Predict) { - DynamicMatrixEKFTest data; - - ManifoldEKF ekf(data.p0Matrix, data.p0Covariance); + ManifoldEKF ekf(p_initial, P_initial); // 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(p_initial.rows() * p_initial.cols(), ekf.state().size()); - // --- Prepare inputs for ManifoldEKF::predict --- - Matrix pNextExpected = exampleDynamicMatrix::predictNextState(data.p0Matrix, data.velocityTangent, data.dt); + // Predict Step + Matrix p_predicted_mean = exampleDynamicMatrix::predictNextState(p_initial, v_tangent, dt); - // For this linear prediction model (p_next = p_current + V*dt in tangent space), F is Identity. - Matrix jacobianF = I_4x4; // Jacobian of the state transition function + // For this linear prediction model (p_next = p_current + V*dt in tangent space), + // Derivative w.r.t delta_xi is Identity. + Matrix F = I_4x4; - // --- Perform EKF prediction --- - ekf.predict(pNextExpected, jacobianF, data.processNoiseCovariance); + ekf.predict(p_predicted_mean, F, Q); - // --- Verification --- - EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9)); - Matrix pCovarianceExpected = jacobianF * data.p0Covariance * jacobianF.transpose() + data.processNoiseCovariance; - EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(p_predicted_mean, ekf.state(), 1e-9)); + Matrix P_predicted_expected = F * P_initial * F.transpose() + Q; + EXPECT(assert_equal(P_predicted_expected, ekf.covariance(), 1e-9)); + + // Update Step + Matrix p_current_for_update = ekf.state(); + Matrix P_current_for_update = ekf.covariance(); + + // True trace of p_current_for_update (which is p_predicted_mean) + // p_predicted_mean = p_initial + v_tangent*dt + // = [1.0, 2.0; 3.0, 4.0] + [0.05, -0.01; 0.01, -0.05] (v_tangent reshaped to 2x2 col-major) + // Trace = 1.05 + 3.95 = 5.0 + double z_true = exampleDynamicMatrix::measureTrace(p_current_for_update); + EXPECT_DOUBLES_EQUAL(5.0, z_true, 1e-9); + double z_observed = z_true - 0.03; + + ekf.update(exampleDynamicMatrix::measureTrace, z_observed, R); + + // Manual Kalman Update Steps for Verification + Matrix H(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) + double z_prediction_manual = exampleDynamicMatrix::measureTrace(p_current_for_update, H); + Matrix H_expected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); + EXPECT(assert_equal(H_expected, H, 1e-9)); + + + // Innovation: y = z_observed - z_prediction_manual (since measurement is double) + double y_innovation = z_observed - z_prediction_manual; + Matrix S = H * P_current_for_update * H.transpose() + R; // S is 1x1 + + Matrix K_gain = P_current_for_update * H.transpose() * S.inverse(); // K is 4x1 + + // State Correction (in tangent space of Matrix) + Vector delta_xi_tangent = K_gain * y_innovation; // delta_xi is 4x1 Vector + + Matrix p_updated_manual_expected = traits::Retract(p_current_for_update, delta_xi_tangent); + Matrix P_updated_manual_expected = (I_4x4 - K_gain * H) * P_current_for_update; + + EXPECT(assert_equal(p_updated_manual_expected, ekf.state(), 1e-9)); + EXPECT(assert_equal(P_updated_manual_expected, ekf.covariance(), 1e-9)); } -TEST(ManifoldEKF_DynamicMatrix, Update) { - DynamicMatrixEKFTest data; - Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished(); - Matrix pStartCovariance = I_4x4 * 0.02; - ManifoldEKF ekf(pStartMatrix, pStartCovariance); - EXPECT_LONGS_EQUAL(4, ekf.state().size()); - - // 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 --- - ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance); - - // --- Verification (Manual Kalman Update Steps) --- - // 1. Predict measurement and get Jacobian H - Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement - double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H); - - // 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 = zObserved - zPredictionManual; - Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance; - - // 3. Kalman Gain K - Matrix kalmanGainK = pStartCovariance * H.transpose() * innovationCovarianceS.inverse(); // K is 4x1 - - // 4. State Correction (in tangent space of Matrix) - Vector deltaXiTangent = kalmanGainK * innovationY; // deltaXi is 4x1 Vector - - // 5. Expected Updated State and Covariance - Matrix pUpdatedExpected = traits::Retract(pStartMatrix, deltaXiTangent); - - // Covariance update: P_new = (I - K H) P_old - Matrix pUpdatedCovarianceExpected = (I_4x4 - kalmanGainK * H) * pStartCovariance; - - // --- 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;