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

Sanity check sampler, and compare 9*9 covariance on NavState

release/4.3a0
Frank Dellaert 2015-12-29 15:38:30 -08:00
parent e52f7ec705
commit 8a31243761
3 changed files with 96 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/navigation/ScenarioRunner.cpp
View File

@ -238,19 +238,40 @@ NavState ScenarioRunner::predict(
return pim.predict(state_i, estimatedBias); return pim.predict(state_i, estimatedBias);
} }
Matrix6 ScenarioRunner::estimatePoseCovariance( Matrix9 ScenarioRunner::estimateCovariance(
double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { double T, size_t N, const imuBias::ConstantBias& estimatedBias) const {
// Get predict prediction from ground truth measurements // Get predict prediction from ground truth measurements
Pose3 prediction = predict(integrate(T)).pose(); NavState prediction = predict(integrate(T));
// Sample ! // Sample !
Matrix samples(9, N); Matrix samples(9, N);
Vector9 sum = Vector9::Zero();
for (size_t i = 0; i < N; i++) {
NavState sampled = predict(integrate(T, estimatedBias, true));
samples.col(i) = sampled.localCoordinates(prediction);
sum += samples.col(i);
}
// Compute MC covariance
Vector9 sampleMean = sum / N;
Matrix9 Q;
Q.setZero();
for (size_t i = 0; i < N; i++) {
Vector9 xi = samples.col(i);
xi -= sampleMean;
Q += xi * xi.transpose();
}
return Q / (N - 1);
}
Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
Matrix samples(6, N);
Vector6 sum = Vector6::Zero(); Vector6 sum = Vector6::Zero();
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose(); samples.col(i) << accSampler_.sample() / sqrt_dt_,
Vector6 xi = sampled.localCoordinates(prediction); gyroSampler_.sample() / sqrt_dt_;
samples.col(i) = xi; sum += samples.col(i);
sum += xi;
} }
// Compute MC covariance // Compute MC covariance

29
gtsam/navigation/ScenarioRunner.h
View File

@ -66,6 +66,9 @@ class PreintegratedMeasurements2 {
k_(0), k_(0),
deltaTij_(0.0) {} deltaTij_(0.0) {}
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
/** /**
* Add a single IMU measurement to the preintegration. * Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame) * @param measuredAcc Measured acceleration (in body frame)
@ -97,9 +100,6 @@ class PreintegratedMeasurements2 {
// We obtain discrete-time noise models by dividing the continuous-time // We obtain discrete-time noise models by dividing the continuous-time
// covariances by dt: // covariances by dt:
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
// initialize posterior with first (corrected) IMU measurement // initialize posterior with first (corrected) IMU measurement
SharedBayesNet initPosterior(const Vector3& correctedAcc, SharedBayesNet initPosterior(const Vector3& correctedAcc,
const Vector3& correctedOmega, double dt) const; const Vector3& correctedOmega, double dt) const;
@ -121,7 +121,7 @@ class ScenarioRunner {
private: private:
const Scenario* scenario_; const Scenario* scenario_;
const SharedParams p_; const SharedParams p_;
const double imuSampleTime_; const double imuSampleTime_, sqrt_dt_;
const imuBias::ConstantBias bias_; const imuBias::ConstantBias bias_;
// Create two samplers for acceleration and omega noise // Create two samplers for acceleration and omega noise
@ -134,6 +134,7 @@ class ScenarioRunner {
: scenario_(scenario), : scenario_(scenario),
p_(p), p_(p),
imuSampleTime_(imuSampleTime), imuSampleTime_(imuSampleTime),
sqrt_dt_(std::sqrt(imuSampleTime)),
bias_(bias), bias_(bias),
// NOTE(duy): random seeds that work well: // NOTE(duy): random seeds that work well:
gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
@ -155,11 +156,11 @@ class ScenarioRunner {
// versions corrupted by bias and noise // versions corrupted by bias and noise
Vector3 measured_omega_b(double t) const { Vector3 measured_omega_b(double t) const {
return actual_omega_b(t) + bias_.gyroscope() + return actual_omega_b(t) + bias_.gyroscope() +
gyroSampler_.sample() / std::sqrt(imuSampleTime_); gyroSampler_.sample() / sqrt_dt_;
} }
Vector3 measured_specific_force_b(double t) const { Vector3 measured_specific_force_b(double t) const {
return actual_specific_force_b(t) + bias_.accelerometer() + return actual_specific_force_b(t) + bias_.accelerometer() +
accSampler_.sample() / std::sqrt(imuSampleTime_); accSampler_.sample() / sqrt_dt_;
} }
const double& imuSampleTime() const { return imuSampleTime_; } const double& imuSampleTime() const { return imuSampleTime_; }
@ -175,19 +176,13 @@ class ScenarioRunner {
const imuBias::ConstantBias& estimatedBias = const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const; imuBias::ConstantBias()) const;
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately /// Compute a Monte Carlo estimate of the predict covariance using N samples
Matrix6 poseCovariance(const PreintegratedMeasurements2& pim) const { Matrix9 estimateCovariance(double T, size_t N = 1000,
Matrix9 cov = pim.preintMeasCov();
Matrix6 poseCov;
poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), //
cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3);
return poseCov;
}
/// Compute a Monte Carlo estimate of the PIM pose covariance using N samples
Matrix6 estimatePoseCovariance(double T, size_t N = 1000,
const imuBias::ConstantBias& estimatedBias = const imuBias::ConstantBias& estimatedBias =
imuBias::ConstantBias()) const; imuBias::ConstantBias()) const;
/// Estimate covariance of sampled noise for sanity-check
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
}; };
} // namespace gtsam } // namespace gtsam

97
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -23,7 +23,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
static const double kDegree = M_PI / 180.0; static const double kDegree = M_PI / 180.0;
static const double kDeltaT = 1e-2; static const double kDt = 1e-2;
static const double kGyroSigma = 0.02; static const double kGyroSigma = 0.02;
static const double kAccelSigma = 0.1; static const double kAccelSigma = 0.1;
@ -46,14 +46,29 @@ TEST(ScenarioRunner, Spin) {
const Vector3 W(0, 0, w), V(0, 0, 0); const Vector3 W(0, 0, w), V(0, 0, 0);
const ExpmapScenario scenario(W, V); const ExpmapScenario scenario(W, V);
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); auto p = defaultParams();
const double T = 0.5; // seconds ScenarioRunner runner(&scenario, p, kDt);
const double T = kDt; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Check noise model agreement
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); EXPECT(assert_equal(p->accelerometerCovariance / kDt,
pim.discreteAccelerometerNoiseModel(kDt)->covariance()));
EXPECT(assert_equal(p->gyroscopeCovariance / kDt,
pim.discreteGyroscopeNoiseModel(kDt)->covariance()));
// Check sampled noise is kosher
Matrix6 expected;
expected << p->accelerometerCovariance / kDt, Z_3x3, //
Z_3x3, p->gyroscopeCovariance / kDt;
Matrix6 actual = runner.estimateNoiseCovariance(10000);
EXPECT(assert_equal(expected, actual, 1e-2));
// Check calculated covariance against Monte Carlo estimate
Matrix9 estimatedCov = runner.estimateCovariance(T);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -64,26 +79,26 @@ ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Forward) {
using namespace forward; using namespace forward;
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); ScenarioRunner runner(&scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) { TEST(ScenarioRunner, ForwardWithBias) {
// using namespace forward; // using namespace forward;
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); // ScenarioRunner runner(&scenario, defaultParams(), kDt);
// const double T = 0.1; // seconds // const double T = 0.1; // seconds
// //
// auto pim = runner.integrate(T, kNonZeroBias); // auto pim = runner.integrate(T, kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, // Matrix9 estimatedCov = runner.estimateCovariance(T, 1000,
// kNonZeroBias); // kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -92,14 +107,14 @@ TEST(ScenarioRunner, Circle) {
const double v = 2, w = 6 * kDegree; const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); ScenarioRunner runner(&scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -109,14 +124,14 @@ TEST(ScenarioRunner, Loop) {
const double v = 2, w = 6 * kDegree; const double v = 2, w = 6 * kDegree;
ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT); ScenarioRunner runner(&scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -144,8 +159,8 @@ TEST(ScenarioRunner, Accelerating) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -157,9 +172,9 @@ TEST(ScenarioRunner, Accelerating) {
// //
// auto pim = runner.integrate(T, // auto pim = runner.integrate(T,
// kNonZeroBias); // kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, // Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias); // kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -175,8 +190,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -205,8 +220,8 @@ TEST(ScenarioRunner, Accelerating2) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -218,9 +233,9 @@ TEST(ScenarioRunner, Accelerating2) {
// //
// auto pim = runner.integrate(T, // auto pim = runner.integrate(T,
// kNonZeroBias); // kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, // Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias); // kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -236,8 +251,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -267,8 +282,8 @@ TEST(ScenarioRunner, Accelerating3) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -280,9 +295,9 @@ TEST(ScenarioRunner, Accelerating3) {
// //
// auto pim = runner.integrate(T, // auto pim = runner.integrate(T,
// kNonZeroBias); // kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, // Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias); // kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -298,8 +313,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -330,8 +345,8 @@ TEST(ScenarioRunner, Accelerating4) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -343,9 +358,9 @@ TEST(ScenarioRunner, Accelerating4) {
// //
// auto pim = runner.integrate(T, // auto pim = runner.integrate(T,
// kNonZeroBias); // kNonZeroBias);
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000, // Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias); // kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -361,8 +376,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T); // Matrix9 estimatedCov = runner.estimateCovariance(T);
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */