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

General noise models

release/4.3a0
Frank Dellaert 2016-01-03 09:58:36 -08:00
parent 07693337af
commit fb94e621e0
6 changed files with 53 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/navigation/AggregateImuReadings.cpp
View File

@ -25,8 +25,10 @@ namespace gtsam {
AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
const Bias& estimatedBias)
: p_(p),
accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)),
gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)),
accelerometerNoiseModel_(
noiseModel::Gaussian::Covariance(p->accelerometerCovariance, true)),
gyroscopeNoiseModel_(
noiseModel::Gaussian::Covariance(p->gyroscopeCovariance, true)),
estimatedBias_(estimatedBias),
k_(0),
deltaTij_(0.0) {
@ -34,28 +36,19 @@ AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
cov_.setZero();
}
SharedDiagonal AggregateImuReadings::discreteAccelerometerNoiseModel(
double dt) const {
return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() /
std::sqrt(dt));
}
SharedDiagonal AggregateImuReadings::discreteGyroscopeNoiseModel(
double dt) const {
return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() /
std::sqrt(dt));
}
// Tangent space sugar.
namespace sugar {
typedef const Vector9 constV9;
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); }
static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); }
static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); }
typedef const Vector9 constV9;
static Eigen::Block<constV9, 3, 1> dR(constV9& v) { return v.segment<3>(0); }
static Eigen::Block<constV9, 3, 1> dP(constV9& v) { return v.segment<3>(3); }
static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
}
} // namespace sugar
Vector9 AggregateImuReadings::UpdateEstimate(
const Vector9& zeta, const Vector3& correctedAcc,
@ -63,24 +56,38 @@ Vector9 AggregateImuReadings::UpdateEstimate(
OptionalJacobian<9, 3> Ba, OptionalJacobian<9, 3> Bw) {
using namespace sugar;
const Vector3 a_dt = correctedAcc * dt;
const Vector3 w_dt = correctedOmega * dt;
// Calculate exact mean propagation
Matrix3 dexp;
const Rot3 R = Rot3::Expmap(dR(zeta), dexp);
const Matrix3 F = dexp.inverse() * dt, H = R.matrix() * dt;
Matrix3 D_R_theta;
const Rot3 R = Rot3::Expmap(dR(zeta), D_R_theta).matrix();
const Matrix3 invH = D_R_theta.inverse();
Matrix3 D_Radt_R, D_Radt_adt;
const Vector3 Radt = R.rotate(a_dt, A ? &D_Radt_R : 0, A ? &D_Radt_adt : 0);
Vector9 zeta_plus;
dR(zeta_plus) = dR(zeta) + F * correctedOmega;
dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + H * (0.5 * dt) * correctedAcc;
dV(zeta_plus) = dV(zeta) + H * correctedAcc;
const double dt2 = 0.5 * dt;
dR(zeta_plus) = dR(zeta) + invH * w_dt;
dP(zeta_plus) = dP(zeta) + dV(zeta) * dt + Radt * dt2;
dV(zeta_plus) = dV(zeta) + Radt;
if (A) {
const Matrix3 Avt = skewSymmetric(-correctedAcc * dt);
// Exact derivative of R*a*dt with respect to theta:
const Matrix3 D_Radt_theta = D_Radt_R * D_R_theta;
// First order (small angle) approximation of derivative of invH*w*dt:
const Matrix3 D_invHwdt_theta = skewSymmetric(-0.5 * w_dt);
A->setIdentity();
A->block<3, 3>(6, 0) = Avt;
A->block<3, 3>(0, 0) += D_invHwdt_theta;
A->block<3, 3>(3, 0) = D_Radt_theta * dt2;
A->block<3, 3>(3, 6) = I_3x3 * dt;
A->block<3, 3>(6, 0) = D_Radt_theta;
}
if (Ba) *Ba << Z_3x3, H*(dt * 0.5), H;
if (Bw) *Bw << F, Z_3x3, Z_3x3;
if (Ba) *Ba << Z_3x3, D_Radt_adt* dt* dt2, D_Radt_adt* dt;
if (Bw) *Bw << invH* dt, Z_3x3, Z_3x3;
return zeta_plus;
}

24
gtsam/navigation/AggregateImuReadings.h
View File

@ -22,16 +22,6 @@
namespace gtsam {
// Convert covariance to diagonal noise model, if possible, otherwise throw
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
bool smart = true;
auto model = noiseModel::Gaussian::Covariance(covariance, smart);
auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonal)
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
return diagonal;
}
/**
* Class that integrates state estimate on the manifold.
* We integrate zeta = [theta, position, velocity]
@ -44,7 +34,7 @@ class AggregateImuReadings {
private:
const boost::shared_ptr<Params> p_;
const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
const SharedGaussian accelerometerNoiseModel_, gyroscopeNoiseModel_;
const Bias estimatedBias_;
size_t k_; ///< index/count of measurements integrated
@ -61,12 +51,6 @@ class AggregateImuReadings {
const Vector9& zeta() const { return zeta_; }
const Matrix9& zetaCov() const { return cov_; }
// We obtain discrete-time noise models by dividing the continuous-time
// covariances by dt:
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)
@ -91,9 +75,9 @@ class AggregateImuReadings {
static Vector9 UpdateEstimate(const Vector9& zeta,
const Vector3& correctedAcc,
const Vector3& correctedOmega, double dt,
OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> Ba,
OptionalJacobian<9, 3> Bw);
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> Ba = boost::none,
OptionalJacobian<9, 3> Bw = boost::none);
};
} // namespace gtsam

2
gtsam/navigation/ScenarioRunner.cpp
View File

@ -65,7 +65,7 @@ Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
Vector9 sum = Vector9::Zero();
for (size_t i = 0; i < N; i++) {
auto pim = integrate(T, estimatedBias, true);
#if 1
#if 0
NavState sampled = predict(pim);
Vector9 xi = sampled.localCoordinates(prediction);
#else

10
gtsam/navigation/ScenarioRunner.h
View File

@ -22,6 +22,16 @@
namespace gtsam {
// Convert covariance to diagonal noise model, if possible, otherwise throw
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
bool smart = true;
auto model = noiseModel::Gaussian::Covariance(covariance, smart);
auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonal)
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
return diagonal;
}
/*
* Simple class to test navigation scenarios.
* Takes a trajectory scenario as input, and can generate IMU measurements

10
gtsam/navigation/tests/testAggregateImuReadings.cpp
View File

@ -178,12 +178,12 @@ TEST(AggregateImuReadings, UpdateEstimate) {
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt,
boost::none, boost::none, boost::none);
Vector9 zeta;
zeta << 0.1, 0.2, 0.3, 0.1, 0.2, 0.3, 0.1, 0.2, 0.3;
const Vector3 acc(0.1, 0.2, 0.3), omega(0.1, 0.2, 0.3);
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
pim.UpdateEstimate(zeta, acc, omega, kDt, aH1, aH2, aH3);
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3));
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-5));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-5));
}
/* ************************************************************************* */

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

@ -56,12 +56,6 @@ TEST(ScenarioRunner, Spin) {
auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
// Check noise model agreement
EXPECT(assert_equal(p->accelerometerCovariance / kDt,
pim.discreteAccelerometerNoiseModel(kDt)->covariance()));
EXPECT(assert_equal(p->gyroscopeCovariance / kDt,
pim.discreteGyroscopeNoiseModel(kDt)->covariance()));
#if 0
// Check sampled noise is kosher
Matrix6 expected;