Sanity check sampler, and compare 9*9 covariance on NavState
Frank Dellaert
parent
e52f7ec705
commit
8a31243761
|
|
@ -238,19 +238,40 @@ NavState ScenarioRunner::predict(
|
|||
return pim.predict(state_i, estimatedBias);
|
||||
}
|
||||
|
||||
Matrix6 ScenarioRunner::estimatePoseCovariance(
|
||||
Matrix9 ScenarioRunner::estimateCovariance(
|
||||
double T, size_t N, const imuBias::ConstantBias& estimatedBias) const {
|
||||
// Get predict prediction from ground truth measurements
|
||||
Pose3 prediction = predict(integrate(T)).pose();
|
||||
NavState prediction = predict(integrate(T));
|
||||
|
||||
// Sample !
|
||||
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();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose();
|
||||
Vector6 xi = sampled.localCoordinates(prediction);
|
||||
samples.col(i) = xi;
|
||||
sum += xi;
|
||||
samples.col(i) << accSampler_.sample() / sqrt_dt_,
|
||||
gyroSampler_.sample() / sqrt_dt_;
|
||||
sum += samples.col(i);
|
||||
}
|
||||
|
||||
// Compute MC covariance
|
||||
|
|
|
|||
|
|
@ -66,6 +66,9 @@ class PreintegratedMeasurements2 {
|
|||
k_(0),
|
||||
deltaTij_(0.0) {}
|
||||
|
||||
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
|
||||
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
|
||||
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame)
|
||||
|
|
@ -97,9 +100,6 @@ class PreintegratedMeasurements2 {
|
|||
// We obtain discrete-time noise models by dividing the continuous-time
|
||||
// covariances by dt:
|
||||
|
||||
SharedDiagonal discreteAccelerometerNoiseModel(double dt) const;
|
||||
SharedDiagonal discreteGyroscopeNoiseModel(double dt) const;
|
||||
|
||||
// initialize posterior with first (corrected) IMU measurement
|
||||
SharedBayesNet initPosterior(const Vector3& correctedAcc,
|
||||
const Vector3& correctedOmega, double dt) const;
|
||||
|
|
@ -121,7 +121,7 @@ class ScenarioRunner {
|
|||
private:
|
||||
const Scenario* scenario_;
|
||||
const SharedParams p_;
|
||||
const double imuSampleTime_;
|
||||
const double imuSampleTime_, sqrt_dt_;
|
||||
const imuBias::ConstantBias bias_;
|
||||
|
||||
// Create two samplers for acceleration and omega noise
|
||||
|
|
@ -134,6 +134,7 @@ class ScenarioRunner {
|
|||
: scenario_(scenario),
|
||||
p_(p),
|
||||
imuSampleTime_(imuSampleTime),
|
||||
sqrt_dt_(std::sqrt(imuSampleTime)),
|
||||
bias_(bias),
|
||||
// NOTE(duy): random seeds that work well:
|
||||
gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
|
||||
|
|
@ -155,11 +156,11 @@ class ScenarioRunner {
|
|||
// versions corrupted by bias and noise
|
||||
Vector3 measured_omega_b(double t) const {
|
||||
return actual_omega_b(t) + bias_.gyroscope() +
|
||||
gyroSampler_.sample() / std::sqrt(imuSampleTime_);
|
||||
gyroSampler_.sample() / sqrt_dt_;
|
||||
}
|
||||
Vector3 measured_specific_force_b(double t) const {
|
||||
return actual_specific_force_b(t) + bias_.accelerometer() +
|
||||
accSampler_.sample() / std::sqrt(imuSampleTime_);
|
||||
accSampler_.sample() / sqrt_dt_;
|
||||
}
|
||||
|
||||
const double& imuSampleTime() const { return imuSampleTime_; }
|
||||
|
|
@ -175,19 +176,13 @@ class ScenarioRunner {
|
|||
const imuBias::ConstantBias& estimatedBias =
|
||||
imuBias::ConstantBias()) const;
|
||||
|
||||
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
|
||||
Matrix6 poseCovariance(const PreintegratedMeasurements2& pim) const {
|
||||
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 predict covariance using N samples
|
||||
Matrix9 estimateCovariance(double T, size_t N = 1000,
|
||||
const imuBias::ConstantBias& estimatedBias =
|
||||
imuBias::ConstantBias()) const;
|
||||
|
||||
/// 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 =
|
||||
imuBias::ConstantBias()) const;
|
||||
/// Estimate covariance of sampled noise for sanity-check
|
||||
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
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 kAccelSigma = 0.1;
|
||||
|
||||
|
|
@ -46,14 +46,29 @@ TEST(ScenarioRunner, Spin) {
|
|||
const Vector3 W(0, 0, w), V(0, 0, 0);
|
||||
const ExpmapScenario scenario(W, V);
|
||||
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
const double T = 0.5; // seconds
|
||||
auto p = defaultParams();
|
||||
ScenarioRunner runner(&scenario, p, kDt);
|
||||
const double T = kDt; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
// Check noise model agreement
|
||||
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) {
|
||||
using namespace forward;
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||
const double T = 0.1; // seconds
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, ForwardWithBias) {
|
||||
// using namespace forward;
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), kDeltaT);
|
||||
// ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||
// const double T = 0.1; // seconds
|
||||
//
|
||||
// auto pim = runner.integrate(T, kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000,
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T, 1000,
|
||||
// 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;
|
||||
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
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -109,14 +124,14 @@ TEST(ScenarioRunner, Loop) {
|
|||
const double v = 2, w = 6 * kDegree;
|
||||
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
|
||||
|
||||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -144,8 +159,8 @@ TEST(ScenarioRunner, Accelerating) {
|
|||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -157,9 +172,9 @@ TEST(ScenarioRunner, Accelerating) {
|
|||
//
|
||||
// auto pim = runner.integrate(T,
|
||||
// kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
|
||||
// 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);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -205,8 +220,8 @@ TEST(ScenarioRunner, Accelerating2) {
|
|||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -218,9 +233,9 @@ TEST(ScenarioRunner, Accelerating2) {
|
|||
//
|
||||
// auto pim = runner.integrate(T,
|
||||
// kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
|
||||
// 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);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -267,8 +282,8 @@ TEST(ScenarioRunner, Accelerating3) {
|
|||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -280,9 +295,9 @@ TEST(ScenarioRunner, Accelerating3) {
|
|||
//
|
||||
// auto pim = runner.integrate(T,
|
||||
// kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
|
||||
// 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);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -330,8 +345,8 @@ TEST(ScenarioRunner, Accelerating4) {
|
|||
auto pim = runner.integrate(T);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -343,9 +358,9 @@ TEST(ScenarioRunner, Accelerating4) {
|
|||
//
|
||||
// auto pim = runner.integrate(T,
|
||||
// kNonZeroBias);
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 10000,
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
|
||||
// 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);
|
||||
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||
|
||||
// Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
|
||||
// Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue