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

make bias scenarios work

release/4.3a0
Frank 2016-01-26 12:14:35 -08:00
parent 4d3f04364e
commit 6d5ca7e546
1 changed files with 39 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
gtsam/navigation/tests/testScenarios.cpp
View File

@ -93,14 +93,13 @@ TEST(ScenarioRunner, Forward) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) { TEST(ScenarioRunner, ForwardWithBias) {
// using namespace forward; using namespace forward;
// ScenarioRunner runner(&scenario, defaultParams(), kDt); 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);
// Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias);
// kNonZeroBias); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -169,18 +168,14 @@ TEST(ScenarioRunner, Accelerating) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(frank):Fails ! TEST(ScenarioRunner, AcceleratingWithBias) {
// TEST(ScenarioRunner, AcceleratingWithBias) { using namespace accelerating;
// using namespace accelerating; ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
// auto pim = runner.integrate(T, EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
// kNonZeroBias); }
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating) { TEST(ScenarioRunner, AcceleratingAndRotating) {
@ -232,18 +227,14 @@ TEST(ScenarioRunner, Accelerating2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(frank):Fails ! TEST(ScenarioRunner, AcceleratingWithBias2) {
// TEST(ScenarioRunner, AcceleratingWithBias2) { using namespace accelerating2;
// using namespace accelerating2; ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
// auto pim = runner.integrate(T, EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
// kNonZeroBias); }
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating2) { TEST(ScenarioRunner, AcceleratingAndRotating2) {
@ -296,18 +287,14 @@ TEST(ScenarioRunner, Accelerating3) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(frank):Fails ! TEST(ScenarioRunner, AcceleratingWithBias3) {
// TEST(ScenarioRunner, AcceleratingWithBias3) { using namespace accelerating3;
// using namespace accelerating3; ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
// auto pim = runner.integrate(T, EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
// kNonZeroBias); }
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating3) { TEST(ScenarioRunner, AcceleratingAndRotating3) {
@ -361,18 +348,14 @@ TEST(ScenarioRunner, Accelerating4) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO(frank):Fails ! TEST(ScenarioRunner, AcceleratingWithBias4) {
// TEST(ScenarioRunner, AcceleratingWithBias4) { using namespace accelerating4;
// using namespace accelerating4; ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
// ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma,
// kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias);
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
// auto pim = runner.integrate(T, EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
// kNonZeroBias); }
// Matrix9 estimatedCov = runner.estimateCovariance(T, 10000,
// kNonZeroBias);
// EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating4) { TEST(ScenarioRunner, AcceleratingAndRotating4) {