Skip to content
Snippets Groups Projects
Commit 72b5ea22 authored by Dinesh Atchuthan's avatar Dinesh Atchuthan
Browse files

[WIP] changed some of the gtest_constraint_imu + added notes

parent c93d423f
No related branches found
No related tags found
1 merge request!147Imu gtests
Hide whitespace changes
Inline Side-by-side
src/test/gtest_constraint_imu.cpp
+ 108
− 129
View file @ 72b5ea22
...@@ -519,7 +519,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test ...@@ -519,7 +519,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
t.set(0); t.set(0);
Eigen::Vector6s data_imu; Eigen::Vector6s data_imu;
origin_bias = Eigen::Vector6s::Random() * 0.001; origin_bias = Eigen::Vector6s::Random() * 0.01; //makes bias of order < 1e-2
data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
data_imu = data_imu + origin_bias; data_imu = data_imu + origin_bias;
expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10 expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
...@@ -695,8 +695,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test ...@@ -695,8 +695,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
t.set(0); t.set(0);
Eigen::Vector6s data_imu, data_imu_initial; Eigen::Vector6s data_imu, data_imu_initial;
origin_bias = Eigen::Vector6s::Random(); origin_bias = Eigen::Vector6s::Random()*0.01;
origin_bias << 0,0,0, 0,0,0;
wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
data_imu_initial = data_imu; data_imu_initial = data_imu;
...@@ -915,7 +914,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test ...@@ -915,7 +914,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
t.set(0); t.set(0);
//set origin of the problem //set origin of the problem
origin_KF = processor_imu->setOrigin(x_origin, t); origin_KF = processor_imu->setOrigin(x_origin, t);
processor_odo->setOrigin(origin_KF); processor_odo->setOrigin(origin_KF);
...@@ -950,7 +948,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test ...@@ -950,7 +948,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
// Time and data variables // Time and data variables
t_imu += dt_imu; t_imu += dt_imu;
// rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s //rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
rateOfTurn << 5, 10, 15; // deg/s rateOfTurn << 5, 10, 15; // deg/s
data_imu.tail<3>() = rateOfTurn * M_PI/180.0; data_imu.tail<3>() = rateOfTurn * M_PI/180.0;
data_imu.head<3>() = quat_imu.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement data_imu.head<3>() = quat_imu.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
...@@ -1767,6 +1765,9 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi ...@@ -1767,6 +1765,9 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
} }
} }
/* States in KF1 and KF2 are fixed. There should not be any observability problem here.
* biases are observable.
*/
TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{ {
//prepare problem for solving //prepare problem for solving
...@@ -1783,8 +1784,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) ...@@ -1783,8 +1784,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
} }
...@@ -1814,8 +1815,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) ...@@ -1814,8 +1815,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-4") //WOLF_INFO("Starting error bias 1e-4")
...@@ -1829,8 +1830,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) ...@@ -1829,8 +1830,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-2") //WOLF_INFO("Starting error bias 1e-2")
...@@ -1844,8 +1845,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) ...@@ -1844,8 +1845,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-1") //WOLF_INFO("Starting error bias 1e-1")
...@@ -1861,12 +1862,17 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) ...@@ -1861,12 +1862,17 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-8)
} }
} }
TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) /* Note : There is no observability issue here. however, the estimation cannot give the exact value of biases we expect to get.
* Reason : we are integrating with initial Zero biases. This test should a least get the right order of magnitude of biases.
* Meaning that if the real biases are in the order of 1e-3, then the estimation error should be lower than 1e-3 (1e-4 used in asserts here)
* In other words : there are local minima
*/
TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->getPPtr()->fix();
...@@ -1878,47 +1884,53 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) ...@@ -1878,47 +1884,53 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
last_KF->getPPtr()->fix(); last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix(); last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix(); last_KF->getVPtr()->fix();
wolf_problem_ptr_->print(4,1,1,1);
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
} }
TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) //something is wrong with this test
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix();
origin_KF->getVPtr()->fix(); last_KF->setState(expected_final_state);
last_KF->getPPtr()->fix(); last_KF->fix();
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix(); wolf_problem_ptr_->print(4,1,1,1);
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
wolf::Scalar epsilon_bias = 0.0000001; wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias); Eigen::VectorXs perturbed_bias(origin_bias);
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-6") WOLF_INFO("Starting error bias 1e-4")
epsilon_bias = 0.000001; epsilon_bias = 0.0001;
Eigen::Vector6s err; Eigen::Vector6s err;
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err; perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state); origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-4") WOLF_INFO("Starting error bias 1e-3")
epsilon_bias = 0.0001; epsilon_bias = 0.001;
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err; perturbed_bias = origin_bias + err;
...@@ -1928,11 +1940,11 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias ...@@ -1928,11 +1940,11 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-2") WOLF_INFO("Starting error bias 1e-2")
epsilon_bias = 0.01; epsilon_bias = 0.01;
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
...@@ -1943,8 +1955,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias ...@@ -1943,8 +1955,8 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-1") //WOLF_INFO("Starting error bias 1e-1")
...@@ -1960,24 +1972,18 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias ...@@ -1960,24 +1972,18 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-4)
} }
} }
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_init0)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix();
origin_KF->getVPtr()->fix();
last_KF->setState(expected_final_state); last_KF->setState(expected_final_state);
last_KF->fix();
last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
...@@ -1990,25 +1996,22 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_i ...@@ -1990,25 +1996,22 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix(); last_KF->fix();
origin_KF->getVPtr()->fix();
last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
wolf::Scalar epsilon_bias = 0.0000001; wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias); Eigen::VectorXs perturbed_bias(origin_bias);
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-6") //WOLF_INFO("Starting error bias 1e-5")
epsilon_bias = 0.000001; epsilon_bias = 0.00001;
Eigen::Vector6s err; Eigen::Vector6s err;
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err; perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state); origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
...@@ -2022,8 +2025,9 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E ...@@ -2022,8 +2025,9 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err; perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state); origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
...@@ -2037,72 +2041,44 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E ...@@ -2037,72 +2041,44 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err; perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state); origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
//==============================================================
//WOLF_INFO("Starting error bias 1e-1")
epsilon_bias = 0.1;
for(int i = 1; i<10; i++)
{
err = Eigen::Vector6s::Random() * epsilon_bias*10;
perturbed_bias = origin_bias + err;
origin_KF->setState(x_origin);
last_KF->setState(expected_final_state);
report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
}
} }
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_init0)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix();
origin_KF->getVPtr()->fix();
last_KF->setState(expected_final_state); last_KF->setState(expected_final_state);
last_KF->fix();
last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
} }
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix(); last_KF->fix();
origin_KF->getVPtr()->fix();
last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
wolf::Scalar epsilon_bias = 0.0000001; wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias); Eigen::VectorXs perturbed_bias(origin_bias);
//============================================================== //==============================================================
//WOLF_INFO("Starting error bias 1e-6") //WOLF_INFO("Starting error bias 1e-5")
epsilon_bias = 0.000001; epsilon_bias = 0.00001;
Eigen::Vector6s err; Eigen::Vector6s err;
err = Eigen::Vector6s::Random() * epsilon_bias*10; err = Eigen::Vector6s::Random() * epsilon_bias*10;
...@@ -2166,29 +2142,39 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 ...@@ -2166,29 +2142,39 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
} }
} }
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK) /* The bias introduced in the data was < 1e-2 (random * 0.01), therefore we check that the estimation is OK at least with precision 1e-3
* because of the existence of local minima due to the Acc/Gyro biases compensating part of the other ones.
* The initial bias for estimation purposes is Zero.
* We would like the precision of the estimation for P2 to be precise at most with 0.1 mm error. Since no other motion constraint is imposed
* we can hardly get better. A small error in the bias estimation will directly have an impact on P2.
*/
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2P2_InvarP1Q1V1Q2V2_initO)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->getPPtr()->fix();
origin_KF->getOPtr()->fix(); origin_KF->getOPtr()->fix();
origin_KF->getVPtr()->unfix(); origin_KF->getVPtr()->fix();
last_KF->setState(expected_final_state);
last_KF->getPPtr()->unfix(); last_KF->getPPtr()->unfix();
last_KF->getOPtr()->fix(); last_KF->getOPtr()->fix();
last_KF->getVPtr()->unfix(); last_KF->getVPtr()->fix();
last_KF->setState(expected_final_state);
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
//Only biases are unfixed ASSERT_MATRIX_APPROX(last_KF ->getState(), expected_final_state, 1e-4)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
} }
// working because we begin with a good prior on P1 /* working because we begin with a good prior on P1
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2P1P2_InvarV1Q1V2Q2_initOK) * The integration bias is set to Zero.
* Due to acceleration bias estimation affecting the state estimation and so on the gyroscope bias (in both sens),
* we can't expect a very precise (error < 1e-4) bias estimation. If the initial bias is correctly set to its real value then the estimation
* can be more precise since we begin the estimation with the optimal estimates.
*/
TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2P1P2_InvarV1Q1V2Q2_init0)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->unfix(); origin_KF->getPPtr()->unfix();
...@@ -2211,15 +2197,10 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2P1P2_InvarV1Q1V2Q2_ini ...@@ -2211,15 +2197,10 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2P1P2_InvarV1Q1V2Q2_ini
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK) TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix();
origin_KF->getVPtr()->fix();
last_KF->setState(expected_final_state); last_KF->setState(expected_final_state);
last_KF->fix();
last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix();
last_KF->getVPtr()->fix();
//perturbation of origin bias //perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001); Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
...@@ -2256,12 +2237,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2 ...@@ -2256,12 +2237,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2
TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK) TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{ {
//prepare problem for solving //prepare problem for solving
origin_KF->getPPtr()->fix(); origin_KF->fix();
origin_KF->getOPtr()->fix();
origin_KF->getVPtr()->fix();
last_KF->setState(expected_final_state); last_KF->setState(expected_final_state);
last_KF->getPPtr()->fix(); last_KF->getPPtr()->fix();
last_KF->getOPtr()->fix(); last_KF->getOPtr()->fix();
last_KF->getVPtr()->unfix(); last_KF->getVPtr()->unfix();
...@@ -2277,7 +2255,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2 ...@@ -2277,7 +2255,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100) ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3),1e-6)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState()); Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10); Eigen::MatrixXs covX(10,10);
...@@ -2591,8 +2569,8 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V ...@@ -2591,8 +2569,8 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V
last_KF->setState(expected_final_state); last_KF->setState(expected_final_state);
last_KF->getPPtr()->fix(); last_KF->getPPtr()->unfix();
last_KF->getOPtr()->fix(); last_KF->getOPtr()->unfix();
last_KF->getVPtr()->fix(); last_KF->getVPtr()->fix();
//perturbation of origin bias //perturbation of origin bias
...@@ -2610,6 +2588,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V ...@@ -2610,6 +2588,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V
ceres_manager->computeCovariances(ALL); ceres_manager->computeCovariances(ALL);
//Only biases are unfixed //Only biases are unfixed
ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-4)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
...@@ -2846,8 +2825,8 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V ...@@ -2846,8 +2825,8 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*:ConstraintIMU_biasTest_Static_NonNullBias.*"; // ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*:ConstraintIMU_biasTest_Static_NonNullBias.*:ConstraintIMU_biasTest_Move_NullBias.*:ConstraintIMU_biasTest_Move_NonNullBiasRotCst.*";
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK"; ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY.*";
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK"; // ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment