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

Monte Carlo analysis

release/4.3a0
Frank 2015-12-22 14:01:16 -08:00
parent 95745015e0
commit 69fa553495
3 changed files with 97 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/navigation/Scenario.h
View File

@ -16,12 +16,13 @@
*/
#pragma once
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
/**
* Simple class with constant twist 3D trajectory.
* Simple IMU simulator with constant twist 3D trajectory.
* It is also assumed that gravity is magically counteracted and has no effect
* on trajectory. Hence, a simulated IMU yields the actual body angular
* velocity, and negative G acceleration plus the acceleration created by the
@ -31,8 +32,12 @@ class Scenario {
public:
/// Construct scenario with constant twist [w,v]
Scenario(const Vector3& w, const Vector3& v,
double imuSampleTime = 1.0 / 100.0)
: twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {}
double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17,
double accSigma = 0.01)
: twist_((Vector6() << w, v).finished()),
imuSampleTime_(imuSampleTime),
gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)),
accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {}
const double& imuSampleTime() const { return imuSampleTime_; }
@ -40,6 +45,17 @@ class Scenario {
// also, uses g=10 for easy debugging
Vector3 gravity() const { return Vector3(0, 0, -10.0); }
const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const {
return gyroNoiseModel_;
}
const noiseModel::Diagonal::shared_ptr& accNoiseModel() const {
return accNoiseModel_;
}
Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); }
Matrix3 accCovariance() const { return accNoiseModel_->covariance(); }
Vector3 angularVelocityInBody() const { return twist_.head<3>(); }
Vector3 linearVelocityInBody() const { return twist_.tail<3>(); }
@ -76,6 +92,7 @@ class Scenario {
private:
Vector6 twist_;
double imuSampleTime_;
noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_;
};
} // namespace gtsam

92
gtsam/navigation/ScenarioRunner.h
View File

@ -16,59 +16,100 @@
*/
#pragma once
#include <gtsam/linear/Sampler.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/Scenario.h>
#include <iostream>
#include <cmath>
namespace gtsam {
double accNoiseVar = 0.01;
double omegaNoiseVar = 0.03;
double intNoiseVar = 0.0001;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3;
const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
static double intNoiseVar = 0.0001;
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
/// Simple class to test navigation scenarios
class ScenarioRunner {
public:
ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {}
// Integrate measurements for T seconds
ImuFactor::PreintegratedMeasurements integrate(double T) {
/// Integrate measurements for T seconds into a PIM
ImuFactor::PreintegratedMeasurements integrate(
double T, boost::optional<Sampler&> gyroSampler = boost::none,
boost::optional<Sampler&> accSampler = boost::none) {
// TODO(frank): allow non-zero
const imuBias::ConstantBias zeroBias;
const bool use2ndOrderCoriolis = true;
ImuFactor::PreintegratedMeasurements result(
zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
ImuFactor::PreintegratedMeasurements pim(
zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(),
kIntegrationErrorCovariance, use2ndOrderCoriolis);
const Vector3 measuredOmega = scenario_.angularVelocityInBody();
const double deltaT = scenario_.imuSampleTime();
const size_t nrSteps = T / deltaT;
const double dt = scenario_.imuSampleTime();
const double sqrt_dt = std::sqrt(dt);
const size_t nrSteps = T / dt;
double t = 0;
for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
const Vector3 measuredAcc = scenario_.accelerationInBody(t);
result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
for (size_t k = 0; k < nrSteps; k++, t += dt) {
Vector3 measuredOmega = scenario_.angularVelocityInBody();
if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt;
Vector3 measuredAcc = scenario_.accelerationInBody(t);
if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt;
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
}
return result;
return pim;
}
// Predict mean
Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) {
// TODO(frank): allow non-standard
/// Predict predict given a PIM
PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim) {
// TODO(frank): allow non-zero bias, omegaCoriolis
const imuBias::ConstantBias zeroBias;
const Pose3 pose_i = Pose3::identity();
const Vector3 vel_i = scenario_.velocity(0);
const Vector3 omegaCoriolis = Vector3::Zero();
const bool use2ndOrderCoriolis = true;
const PoseVelocityBias prediction =
integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
omegaCoriolis, use2ndOrderCoriolis);
return prediction.pose;
return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(),
omegaCoriolis, use2ndOrderCoriolis);
}
/// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
Matrix6 poseCovariance(const ImuFactor::PreintegratedMeasurements& pim) {
Matrix9 cov = pim.preintMeasCov(); // _ position rotation
Matrix6 poseCov;
poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), //
cov.block<3, 3>(3, 6), 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) {
// Get predict prediction from ground truth measurements
Pose3 prediction = predict(integrate(T)).pose;
// Create two samplers for acceleration and omega noise
Sampler gyroSampler(scenario_.gyroNoiseModel(), 29285);
Sampler accSampler(scenario_.accNoiseModel(), 29284);
// Sample !
Matrix samples(9, N);
Vector6 sum = Vector6::Zero();
for (size_t i = 0; i < N; i++) {
Pose3 sampled = predict(integrate(T, gyroSampler, accSampler)).pose;
Vector6 xi = sampled.localCoordinates(prediction);
samples.col(i) = xi;
sum += xi;
}
// Compute MC covariance
Vector6 sampleMean = sum / N;
Matrix6 Q;
Q.setZero();
for (size_t i = 0; i < N; i++) {
Vector6 xi = samples.col(i);
xi -= sampleMean;
Q += xi * (xi.transpose() / (N - 1));
}
return Q;
}
private:
@ -76,4 +117,3 @@ class ScenarioRunner {
};
} // namespace gtsam

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

@ -27,12 +27,15 @@ static const double degree = M_PI / 180.0;
/* ************************************************************************* */
TEST(ScenarioRunner, Forward) {
const double v = 2; // m/s
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0));
Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.1, 0.00001);
ScenarioRunner runner(forward);
const double T = 1; // seconds
ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9));
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9));
Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9));
}
/* ************************************************************************* */
@ -43,8 +46,9 @@ TEST(ScenarioRunner, Circle) {
ScenarioRunner runner(circle);
const double T = 15; // seconds
ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1));
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1));
}
/* ************************************************************************* */
@ -56,8 +60,8 @@ TEST(ScenarioRunner, Loop) {
ScenarioRunner runner(loop);
const double T = 30; // seconds
ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T);
EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1));
ImuFactor::PreintegratedMeasurements pim = runner.integrate(T);
EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1));
}
/* ************************************************************************* */