From 77f69146f65e0ff179e538f0877f1cb62abbe24f Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 2 Mar 2015 15:14:05 -0500 Subject: [PATCH] Unit test that fails! Bias estimated is opposite in direction for test bodyPSensorWithBias! Not equal: expected: .biasAcc [0 0 0] expected: .biasGyro [ 0 0.01 0] actual: .biasAcc [-0.00012 3.6e-16 0.00015] actual: .biasGyro [-3.7e-18 -0.01 -3.6e-18] --- gtsam/navigation/tests/testImuFactor.cpp | 135 +++++++++++++++++++++++ 1 file changed, 135 insertions(+) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9c82a7dfa..ecf019a04 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -922,6 +922,141 @@ TEST(ImuFactor, PredictRotation) { EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } +/* ************************************************************************* */ +TEST(ImuFactor, bodyPSensorNoBias) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Measurements + Vector3 n_gravity; n_gravity << 0, 0, -9.81; // z-up nav frame + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 s_omegaMeas_ns; s_omegaMeas_ns << 0, 0.1, M_PI/10; + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force + // w.r.t gravity + Vector3 s_accMeas; s_accMeas << 0,0,-9.81; + double dt = 0.001; + Pose3 b_P_s(Rot3::ypr(0,0,M_PI), Point3(0,0,0)); // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + + Matrix I6x6(6,6); + I6x6 = Matrix::Identity(6,6); + + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), true); + + for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, b_P_s); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); + + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, n_gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(-M_PI/10, 0, 0), Point3(0, 0, 0)); + Vector3 expectedVelocity; expectedVelocity<<0,0,0; + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); +} + +/* ************************************************************************* */ +#include +#include +#include +#include +#include + +TEST(ImuFactor, bodyPSensorWithBias) { + int numFactors = 80; + imuBias::ConstantBias zeroBias(Vector3(0, 0, 0), Vector3(0.0, 0, 0)); // Biases (acc, rot) + Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = noiseModel::Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements + Vector3 n_gravity; n_gravity << 0, 0, -9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega; measuredOmega << 0, 0.01, 0.0; + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force + // w.r.t gravity + Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + + Pose3 bPs(Rot3::ypr(0,0,M_PI), Point3()); + + Matrix3 accCov = 1e-4 * I_3x3; + Matrix3 gyroCov = 1e-6 * I_3x3; + Matrix3 integrationCov = 1e-8 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + Vector6 priorNoisePoseSigmas((Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); + SharedDiagonal priorNoisePose = noiseModel::Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = noiseModel::Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = noiseModel::Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0.0, 0.0); + + + + Values values; + NonlinearFactorGraph graph; + + PriorFactor priorPose(X(0), Pose3(), priorNoisePose); + graph.add(priorPose); + values.insert(X(0), Pose3()); + + PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); + graph.add(priorVel); + values.insert(V(0), zeroVel); + + PriorFactor priorBias(B(0), zeroBias, priorNoiseBias); + graph.add(priorBias); + values.insert(B(0), zeroBias); + + for (int i = 1; i < numFactors; i++) { + ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0), + Vector3(0.0, 0.0, 0.0)), accCov, gyroCov, integrationCov, true); + for (int j = 0; j<200; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT, bPs); + + // Create factor + ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, n_gravity, omegaCoriolis); + graph.add(factor); + graph.add(BetweenFactor(B(i-1), B(i), zeroBias, biasNoiseModel)); + + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), zeroBias); + } + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + cout.precision(2); + Marginals marginals(graph, results); + imuBias::ConstantBias biasActual = results.at(B(numFactors-1)); + + imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.01, 0.0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); + +// results.print("Results: \n"); +// +// for (int i = 0; i < numFactors; i++) { +// imuBias::ConstantBias currentBias = results.at(B(i)); +// cout << "currentBiasEstimate: " << currentBias.vector().transpose() << endl; +// } +// for (int i = 0; i < numFactors; i++) { +// Matrix biasCov = marginals.marginalCovariance(B(i)); +// cout << "b" << i << " cov: " << biasCov.diagonal().transpose() << endl; +// } +// +// for (int i = 0; i < numFactors; i++) { +// Pose3 currentPose = results.at(X(i)); +// cout << "currentYPREstimate: " << currentPose.rotation().ypr().transpose()*180/M_PI << endl; +// } +// for (int i = 0; i < numFactors; i++) +// cout << "x" << i << " covariance: " << marginals.marginalCovariance(X(i)).diagonal().transpose() << endl; +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */