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

Tests pass with realistic white noise strengths

release/4.3a0
Frank Dellaert 2016-01-17 16:31:24 -08:00
parent dace8e3770
commit e1d810d37a
2 changed files with 26 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,21 +38,37 @@ static boost::shared_ptr<AggregateImuReadings::Params> defaultParams() {
return p;
}
boost::function<Vector9(const Vector9&, const Vector3&, const Vector3&)> f =
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt,
boost::none, boost::none, boost::none);
/* ************************************************************************* */
TEST(AggregateImuReadings, UpdateEstimate) {
TEST(AggregateImuReadings, UpdateEstimate1) {
AggregateImuReadings pim(defaultParams());
Matrix9 aH1;
Matrix93 aH2, aH3;
Vector9 zeta;
zeta.setZero();
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, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
}
/* ************************************************************************* */
TEST(AggregateImuReadings, UpdateEstimate2) {
AggregateImuReadings pim(defaultParams());
Matrix9 aH1;
Matrix93 aH2, aH3;
boost::function<Vector9(const Vector9&, const Vector3&, const Vector3&)> f =
boost::bind(&AggregateImuReadings::UpdateEstimate, _1, _2, _3, kDt,
boost::none, boost::none, boost::none);
Vector9 zeta;
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);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
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));
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7));
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
}
/* ************************************************************************* */

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

@ -25,8 +25,10 @@ using namespace gtsam;
static const double kDegree = M_PI / 180.0;
static const double kDt = 1e-2;
static const double kGyroSigma = 0.02;
static const double kAccelSigma = 0.1;
// realistic white noise strengths are 0.5 deg/sqrt(hr) and 0.1 (m/s)/sqrt(h)
static const double kGyroSigma = 0.5 * kDegree / 60;
static const double kAccelSigma = 0.1 / 60.0;
static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);