Skip to content
Snippets Groups Projects
Commit ef38ba88 authored by Amanda Sanjuan Sánchez's avatar Amanda Sanjuan Sánchez
Browse files

gtest simulation working

parent 52f3d8f1
No related branches found
No related tags found
2 merge requests!31devel->main,!30Complete UAV identification setup
Hide whitespace changes
Inline Side-by-side
Showing
with 84 additions and 78 deletions
include/bodydynamics/sensor/sensor_force_torque_inertial.h
+ 36
− 27
View file @ ef38ba88
...@@ -37,6 +37,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase ...@@ -37,6 +37,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
// noise std dev // noise std dev
double acc_noise_std; double acc_noise_std;
double gyro_noise_std; double gyro_noise_std;
double accb_initial_std;
double gyrob_initial_std;
double acc_drift_std; double acc_drift_std;
double gyro_drift_std; double gyro_drift_std;
double force_noise_std; double force_noise_std;
...@@ -49,29 +51,33 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase ...@@ -49,29 +51,33 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server) ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server) : ParamsSensorBase(_unique_name, _server)
{ {
acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std"); acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std"); gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std"); accb_initial_std = _server.getParam<double>(prefix + _unique_name + "/accb_initial_std");
gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std"); gyrob_initial_std = _server.getParam<double>(prefix + _unique_name + "/gyrob_initial_std");
force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std"); acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std"); gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
com = _server.getParam<Vector3d>(prefix + _unique_name + "/com"); force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia"); torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
mass = _server.getParam<double>(prefix + _unique_name + "/mass"); com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
mass = _server.getParam<double>(prefix + _unique_name + "/mass");
} }
~ParamsSensorForceTorqueInertial() override = default; ~ParamsSensorForceTorqueInertial() override = default;
std::string print() const override std::string print() const override
{ {
return ParamsSensorBase::print() + "\n" // return ParamsSensorBase::print() + "\n" //
+ "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" // + "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
+ "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" // + "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
+ "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" // + "accb_initial_std: " + std::to_string(accb_initial_std) + "\n" //
+ "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" // + "gyrob_initial_std: " + std::to_string(gyrob_initial_std) + "\n" //
+ "force_noise_std: " + std::to_string(force_noise_std) + "\n" // + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
+ "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" // + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
+ "com: print not implemented." + "\n" // + "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
+ "inertia: print not implemented. \n" // + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
+ "mass: " + std::to_string(mass) + "\n"; // + "com: print not implemented." + "\n" //
+ "inertia: print not implemented. \n" //
+ "mass: " + std::to_string(mass) + "\n"; //
} }
}; };
...@@ -90,14 +96,17 @@ class SensorForceTorqueInertial : public SensorBase ...@@ -90,14 +96,17 @@ class SensorForceTorqueInertial : public SensorBase
WOLF_SENSOR_CREATE(SensorForceTorqueInertial, ParamsSensorForceTorqueInertial, 7); WOLF_SENSOR_CREATE(SensorForceTorqueInertial, ParamsSensorForceTorqueInertial, 7);
// getters return by copy // getters return by copy
Vector6d getImuBias() const; // Imu bias [acc, gyro] Vector6d getImuBias() const; // Imu bias [acc, gyro]
Vector3d getAccBias() const; // Acc bias Vector3d getAccBias() const; // Acc bias
Vector3d getGyroBias() const; // Gyro bias Vector3d getGyroBias() const; // Gyro bias
double getMass() const; // Total mass double getMass() const; // Total mass
Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix) Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
Vector3d getCom() const; // centre of mass Vector3d getCom() const; // centre of mass
Vector7d getModel() const; // dynamic model [com, inertia, mass] Vector7d getModel() const; // dynamic model [com, inertia, mass]
ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial(){return params_fti_;} ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial()
{
return params_fti_;
}
}; };
inline double SensorForceTorqueInertial::getMass() const inline double SensorForceTorqueInertial::getMass() const
......
test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+ 37
− 48
View file @ ef38ba88
...@@ -195,10 +195,23 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T ...@@ -195,10 +195,23 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
// if (counter != 0) // if (counter != 0)
// { // {
// dt = time_stamp[counter] - time_stamp[counter - 1]; // dt = time_stamp[counter] - time_stamp[counter - 1];
// a_meas_i = force_i/mass_true;
// //a_meas_i = (vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity(); // //a_meas_i = (vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity();
// a_meas.push_back(a_meas_i); // a_meas.push_back(a_meas_i);
// // We have to be careful with the index // // We have to be careful with the index
// } // }
// if (counter != 0)
// {
// dt = time_stamp[counter] - time_stamp[counter - 1];
// WOLF_INFO("Massa : ", mass_true);
// WOLF_INFO("Forces : ", (force_i).transpose());
// WOLF_INFO("Acc mesurada amb forces: ", (force_i/mass_true).transpose());
// WOLF_INFO("Acc mesurada amb la vel: ", ((vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity()).transpose())
// //a_meas_i = (vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity();
// // a_meas.push_back(a_meas_i);
// // We have to be careful with the index
// }
counter++; counter++;
} }
...@@ -206,6 +219,11 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T ...@@ -206,6 +219,11 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
void SetUp() override void SetUp() override
{ {
bias_true = Vector6d::Zero();
cdm_true = {0, 0, 0};
inertia_true = {0.0134943, 0.0141622, 0.0237319}; // rounded {0.017598, 0.017957, 0.029599}
mass_true = 1.9828;
std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR; std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
extractAndCompleteData(wolf_bodydynamics_root + "/test/dades_simulacio_pep.csv"); extractAndCompleteData(wolf_bodydynamics_root + "/test/dades_simulacio_pep.csv");
...@@ -217,7 +235,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T ...@@ -217,7 +235,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
P = Problem::autoSetup(server); P = Problem::autoSetup(server);
solver = std::make_shared<SolverCeres>(P); solver = std::make_shared<SolverCeres>(P);
auto options = solver->getSolverOptions(); // auto options = solver->getSolverOptions();
// solver->getSolverOptions().max_num_iterations = 1e4; // solver->getSolverOptions().max_num_iterations = 1e4;
// solver->getSolverOptions().function_tolerance = 1e-15; // solver->getSolverOptions().function_tolerance = 1e-15;
// solver->getSolverOptions().gradient_tolerance = 1e-15; // solver->getSolverOptions().gradient_tolerance = 1e-15;
...@@ -226,11 +244,6 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T ...@@ -226,11 +244,6 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front()); p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
// need to change with the real values, waiting for Pep's answer
bias_true = Vector6d::Zero();
cdm_true = {0, 0, 0};
inertia_true = {0.0134943, 0.0141622, 0.0237319}; // rounded {0.017598, 0.017957, 0.029599}
mass_true = 1.9828;
} }
}; };
...@@ -334,6 +347,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an ...@@ -334,6 +347,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
// results printed in the csv in the order established before // results printed in the csv in the order established before
Vector3d position_est(p->getLast()->getFrame()->getStateBlock('P')->getState()); Vector3d position_est(p->getLast()->getFrame()->getStateBlock('P')->getState());
Vector4d quaternion_coeffs_est(p->getLast()->getFrame()->getStateBlock('O')->getState()); Vector4d quaternion_coeffs_est(p->getLast()->getFrame()->getStateBlock('O')->getState());
fout << time_stamp[i] << "," << position[i](0) << "," << position[i](1) << "," << position[i](2) << "," fout << time_stamp[i] << "," << position[i](0) << "," << position[i](1) << "," << position[i](2) << ","
<< quaternion[i].coeffs()(0) << "," << quaternion[i].coeffs()(1) << "," << quaternion[i].coeffs()(2) << quaternion[i].coeffs()(0) << "," << quaternion[i].coeffs()(1) << "," << quaternion[i].coeffs()(2)
<< "," << quaternion[i].coeffs()(3) << "," << position_est(0) << "," << position_est(1) << "," << "," << quaternion[i].coeffs()(3) << "," << position_est(0) << "," << position_est(1) << ","
...@@ -347,39 +361,31 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an ...@@ -347,39 +361,31 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation) TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
{ {
// Calibration params // Calibration params
Vector6d bias_guess = bias_true; Vector6d bias_guess = S->getStateBlock('I')->getState();
// Vector3d cdm_guess(0.005, 0.005, 0.01); Vector3d cdm_guess = S->getStateBlock('C')->getState();
Vector3d cdm_guess = cdm_true; Vector3d inertia_guess = S->getStateBlock('i')->getState();
// Vector3d inertia_guess(0.013, 0.013, 0.024); double mass_guess = S->getStateBlock('m')->getState()(0);
Vector3d inertia_guess = inertia_true;
// double mass_gues_s = 2.00;
double mass_guess = mass_true;
S->getStateBlock('I')->setState(bias_guess);
S->getStateBlock('C')->setState(cdm_guess);
S->getStateBlock('i')->setState(inertia_guess);
S->getStateBlock('m')->setState(Vector1d(mass_guess));
S->getStateBlock('P')->fix(); S->getStateBlock('P')->fix();
S->getStateBlock('O')->fix(); S->getStateBlock('O')->fix();
S->getStateBlock('I')->fix(); S->getStateBlock('I')->unfix();
S->getStateBlock('C')->unfix(); S->getStateBlock('C')->unfix();
S->getStateBlock('i')->fix(); S->getStateBlock('i')->unfix();
S->getStateBlock('m')->fix(); S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false); S->setStateBlockDynamic('I', false);
// // add regularization, uncomment if the parameter is not fixed // // add regularization, uncomment if the parameter is not fixed
// S->addPriorParameter('C', // cdm // S->addPriorParameter('C', // cdm
// cdm_guess, // cdm // cdm_guess, // cdm
// Matrix3d::Identity(), // cov // 0.01*0.01*Matrix3d::Identity(), // cov
// 0, // start: X coordinate // 0, // start: X coordinate
// 3); // size // 3); // size
// S->addPriorParameter('i', // inertia // S->addPriorParameter('i', // inertia
// inertia_guess, // inertia // inertia_guess, // inertia
// Matrix3d::Identity(), // cov // 0.01*0.01*Matrix3d::Identity(), // cov
// 0, // start: X coordinate // 0, // start: X coordinate
// 3); // 3);
...@@ -428,7 +434,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation) ...@@ -428,7 +434,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr); CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process(); C->process();
P->print(4, 1, 1, 1); P->print(4, 1, 1, 1);
// check if new KF // check if new KF
...@@ -451,34 +457,11 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation) ...@@ -451,34 +457,11 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
P->print(4, 1, 1, 1); P->print(4, 1, 1, 1);
// results of this iteration // results of this iteration
WOLF_INFO("Torque : ", torque[i].transpose(), " N m .");
WOLF_INFO("Angular velocity : ", vang[i].transpose(), " rad/s .");
WOLF_INFO("Angular momentum : ",
p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
" kg m²/s .");
WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg."); WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg."); WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------"); WOLF_INFO("-----------------------------");
// for (auto ftr : p->getOrigin()->getFeatureList())
// {
// if (std::dynamic_pointer_cast<FeatureMotion>(ftr) != nullptr)
// {
// auto fac =
// std::static_pointer_cast<FactorForceTorqueInertialDynamics>(ftr->getFactorList().front());
// WOLF_INFO(
// "Residual FTI: ",
// (ftr->getMeasurementSquareRootInformationUpper().inverse() * fac->residual()).transpose());
// }
// else
// {
// auto fac = std::static_pointer_cast<FactorAngularMomentum>(ftr->getFactorList().front());
// WOLF_INFO("Residual L: ", fac->residual().transpose());
// }
// }
// if (i>100)break;
} }
} }
...@@ -486,6 +469,9 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation) ...@@ -486,6 +469,9 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
// FINAL RESULTS // FINAL RESULTS
WOLF_INFO("True bias : ", bias_true.transpose());
WOLF_INFO("Guess bias : ", bias_guess.transpose());
WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose());
WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m."); WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m."); WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
...@@ -501,7 +487,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation) ...@@ -501,7 +487,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
// ::testing::GTEST_FLAG(filter) =
// "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
::testing::GTEST_FLAG(filter) = ::testing::GTEST_FLAG(filter) =
"Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation"; "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+ 2
− 0
View file @ ef38ba88
...@@ -31,6 +31,8 @@ config: ...@@ -31,6 +31,8 @@ config:
torque_noise_std: 0.1 # std dev of torque noise in N/m torque_noise_std: 0.1 # std dev of torque noise in N/m
acc_noise_std: 0.01 # std dev of acc noise in m/s2 acc_noise_std: 0.01 # std dev of acc noise in m/s2
gyro_noise_std: 0.01 # std dev of gyro noise in rad/s gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
accb_initial_std: 0.01 # m/s2 - initial bias
gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s) acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s) gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
......
test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
+ 5
− 3
View file @ ef38ba88
...@@ -35,6 +35,8 @@ config: ...@@ -35,6 +35,8 @@ config:
# IMU # IMU
acc_noise_std: 0.001 # std dev of acc noise in m/s2 acc_noise_std: 0.001 # std dev of acc noise in m/s2
gyro_noise_std: 0.001 # std dev of gyro noise in rad/s gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
accb_initial_std: 0.01 # m/s2 - initial bias
gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s) acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s) gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
...@@ -43,9 +45,9 @@ config: ...@@ -43,9 +45,9 @@ config:
torque_noise_std: 0.0001 # std dev of torque noise in N/m torque_noise_std: 0.0001 # std dev of torque noise in N/m
# Dynamics # Dynamics
com: [0,0,0.0341] # center of mass [m] com: [0.005,0.005,0.01] # center of mass [m]
inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2] inertia: [0.013,0.013,0.024] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 1.952 # mass [kg] mass: 2.00 # mass [kg]
processors: processors:
- -
......
test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+ 2
− 0
View file @ ef38ba88
...@@ -35,6 +35,8 @@ config: ...@@ -35,6 +35,8 @@ config:
# IMU # IMU
acc_noise_std: 0.001 # std dev of acc noise in m/s2 acc_noise_std: 0.001 # std dev of acc noise in m/s2
gyro_noise_std: 0.001 # std dev of gyro noise in rad/s gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
accb_initial_std: 0.01 # m/s2 - initial bias
gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s) acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s) gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
......
test/yaml/sensor_force_torque_inertial.yaml
+ 2
− 0
View file @ ef38ba88
...@@ -2,6 +2,8 @@ force_noise_std: 0.1 # std dev of force noise in N ...@@ -2,6 +2,8 @@ force_noise_std: 0.1 # std dev of force noise in N
torque_noise_std: 0.1 # std dev of torque noise in N/m torque_noise_std: 0.1 # std dev of torque noise in N/m
acc_noise_std: 0.01 # std dev of acc noise in m/s2 acc_noise_std: 0.01 # std dev of acc noise in m/s2
gyro_noise_std: 0.01 # std dev of gyro noise in rad/s gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
accb_initial_std: 0 # m/s2 - initial bias
gyrob_initial_std: 0 # rad/sec - initial bias
acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s) acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s) gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
......
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