From da09c4a2c30226c10e05d88ae8d2d118a5c1c689 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 17:57:21 -0400 Subject: [PATCH] naming --- gtsam/navigation/tests/testManifoldEKF.cpp | 104 ++++++++++----------- 1 file changed, 47 insertions(+), 57 deletions(-) diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 8fe96557d..ff7a9a68d 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -30,7 +30,7 @@ using namespace gtsam; namespace exampleUnit3 { // Predicts the next state given current state, tangent velocity, and dt - Unit3 predictNextState(const Unit3& p, const Vector2& v, double dt) { + Unit3 f(const Unit3& p, const Vector2& v, double dt) { return p.retract(v * dt); } @@ -76,13 +76,13 @@ TEST(ManifoldEKF_Unit3, Predict) { // --- Prepare inputs for ManifoldEKF::predict --- // 1. Compute expected next state - Unit3 p_next_expected = exampleUnit3::predictNextState(data.p0, data.velocity, data.dt); + Unit3 p_next_expected = exampleUnit3::f(data.p0, data.velocity, data.dt); // 2. Compute state transition Jacobian F = d(local(p_next)) / d(local(p)) - // We can compute this numerically using the predictNextState function. + // We can compute this numerically using the f function. // GTSAM's numericalDerivative handles derivatives *between* manifolds. auto predict_wrapper = [&](const Unit3& p) -> Unit3 { - return exampleUnit3::predictNextState(p, data.velocity, data.dt); + return exampleUnit3::f(p, data.velocity, data.dt); }; Matrix2 F = numericalDerivative11(predict_wrapper, data.p0); @@ -100,7 +100,7 @@ TEST(ManifoldEKF_Unit3, Predict) { // Check F manually for a simple case (e.g., zero velocity should give Identity) Vector2 zero_velocity = Vector2::Zero(); auto predict_wrapper_zero = [&](const Unit3& p) -> Unit3 { - return exampleUnit3::predictNextState(p, zero_velocity, data.dt); + return exampleUnit3::f(p, zero_velocity, data.dt); }; Matrix2 F_zero = numericalDerivative11(predict_wrapper_zero, data.p0); EXPECT(assert_equal(I_2x2, F_zero, 1e-8)); @@ -152,23 +152,19 @@ 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) { + 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) // H is the Jacobian dh/d(flatten(p)) - double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H_opt = {}) { - if (H_opt) { + double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) { + if (H) { // 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; + H->resize(1, p.size()); + (*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) } if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error double trace = 0.0; @@ -181,70 +177,64 @@ namespace exampleDynamicMatrix { } // namespace exampleDynamicMatrix 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) + Matrix pInitial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); + Matrix pInitialCovariance = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4) + Vector vTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec + double deltaTime = 0.1; + Matrix processNoiseCovariance = I_4x4 * 0.001; // Process noise covariance (4x4) + Matrix measurementNoiseCovariance = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1) - ManifoldEKF ekf(p_initial, P_initial); + ManifoldEKF ekf(pInitial, pInitialCovariance); // For a 2x2 Matrix, tangent space dimension is 2*2=4. EXPECT_LONGS_EQUAL(4, ekf.state().size()); - EXPECT_LONGS_EQUAL(p_initial.rows() * p_initial.cols(), ekf.state().size()); + EXPECT_LONGS_EQUAL(pInitial.rows() * pInitial.cols(), ekf.state().size()); // Predict Step - Matrix p_predicted_mean = exampleDynamicMatrix::predictNextState(p_initial, v_tangent, dt); + Matrix pPredictedMean = exampleDynamicMatrix::f(pInitial, vTangent, deltaTime); - // 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; + // For this linear prediction model (pNext = pCurrent + V*dt in tangent space), + // Derivative w.r.t deltaXi is Identity. + Matrix fJacobian = I_4x4; - ekf.predict(p_predicted_mean, F, Q); + ekf.predict(pPredictedMean, fJacobian, processNoiseCovariance); - 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)); + EXPECT(assert_equal(pPredictedMean, ekf.state(), 1e-9)); + Matrix pPredictedCovarianceExpected = fJacobian * pInitialCovariance * fJacobian.transpose() + processNoiseCovariance; + EXPECT(assert_equal(pPredictedCovarianceExpected, ekf.covariance(), 1e-9)); // Update Step - Matrix p_current_for_update = ekf.state(); - Matrix P_current_for_update = ekf.covariance(); + Matrix pCurrentForUpdate = ekf.state(); + Matrix pCurrentCovarianceForUpdate = 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; + // True trace of pCurrentForUpdate (which is pPredictedMean) + double zTrue = exampleDynamicMatrix::h(pCurrentForUpdate); + EXPECT_DOUBLES_EQUAL(5.0, zTrue, 1e-9); + double zObserved = zTrue - 0.03; - ekf.update(exampleDynamicMatrix::measureTrace, z_observed, R); + ekf.update(exampleDynamicMatrix::h, zObserved, measurementNoiseCovariance); // 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)); + Matrix hJacobian(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement) + double zPredictionManual = exampleDynamicMatrix::h(pCurrentForUpdate, hJacobian); + Matrix hJacobianExpected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished(); + EXPECT(assert_equal(hJacobianExpected, hJacobian, 1e-9)); + // Innovation: y = zObserved - zPredictionManual (since measurement is double) + double yInnovation = zObserved - zPredictionManual; + Matrix innovationCovariance = hJacobian * pCurrentCovarianceForUpdate * hJacobian.transpose() + measurementNoiseCovariance; - // 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 + Matrix kalmanGain = pCurrentCovarianceForUpdate * hJacobian.transpose() * innovationCovariance.inverse(); // K is 4x1 // State Correction (in tangent space of Matrix) - Vector delta_xi_tangent = K_gain * y_innovation; // delta_xi is 4x1 Vector + Vector deltaXiTangent = kalmanGain * yInnovation; // deltaXi 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; + Matrix pUpdatedManualExpected = traits::Retract(pCurrentForUpdate, deltaXiTangent); + Matrix pUpdatedCovarianceManualExpected = (I_4x4 - kalmanGain * hJacobian) * pCurrentCovarianceForUpdate; - EXPECT(assert_equal(p_updated_manual_expected, ekf.state(), 1e-9)); - EXPECT(assert_equal(P_updated_manual_expected, ekf.covariance(), 1e-9)); + EXPECT(assert_equal(pUpdatedManualExpected, ekf.state(), 1e-9)); + EXPECT(assert_equal(pUpdatedCovarianceManualExpected, ekf.covariance(), 1e-9)); } - - int main() { TestResult tr; return TestRegistry::runAllTests(tr);