12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

naming

release/4.3a0
Frank Dellaert 2025-05-09 17:57:21 -04:00
parent 8efce78419
commit da09c4a2c3
1 changed files with 47 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
gtsam/navigation/tests/testManifoldEKF.cpp
View File

@ -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<Unit3, Unit3>(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<Unit3, Unit3>(predict_wrapper_zero, data.p0);
EXPECT(assert_equal<Matrix2>(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<Matrix>::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<Matrix> ekf(p_initial, P_initial);
ManifoldEKF<Matrix> 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<Matrix>::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<Matrix>::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);