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