Skip to content
Snippets Groups Projects
Commit 358ba67e authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Computing running imu bias covariance

parent 5a629fc4
No related branches found
No related tags found
2 merge requests!18Release after RAL,!17After 2nd RAL submission
...@@ -61,42 +61,25 @@ typedef pinocchio::ForceTpl<double> Force; ...@@ -61,42 +61,25 @@ typedef pinocchio::ForceTpl<double> Force;
int main (int argc, char **argv) { int main (int argc, char **argv) {
// retrieve parameters from yaml file // retrieve parameters from yaml file
YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml"); YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
std::string robot_urdf = config["robot_urdf"].as<std::string>();
int dimc = config["dimc"].as<int>();
int nb_feet = config["nb_feet"].as<int>();
double dt = config["dt"].as<double>(); double dt = config["dt"].as<double>();
double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>(); double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
bool solve_end = config["solve_end"].as<bool>(); bool solve_end = config["solve_end"].as<bool>();
// Ceres setup // Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>(); int max_num_iterations = config["max_num_iterations"].as<int>();
// estimator sensor noises
double std_pbc_est = config["std_pbc_est"].as<double>();
double std_vbc_est = config["std_vbc_est"].as<double>();
double std_f_est = config["std_f_est"].as<double>();
double std_tau_est = config["std_tau_est"].as<double>();
double std_odom3d_est = config["std_odom3d_est"].as<double>();
// priors // priors
double std_prior_p = config["std_prior_p"].as<double>(); double std_prior_p = config["std_prior_p"].as<double>();
double std_prior_o = config["std_prior_o"].as<double>(); double std_prior_o = config["std_prior_o"].as<double>();
double std_prior_v = config["std_prior_v"].as<double>(); double std_prior_v = config["std_prior_v"].as<double>();
double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>(); double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>(); double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
double std_bp_drift = config["std_bp_drift"].as<double>();
std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>(); std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data()); Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>(); std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>(); std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data()); Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data()); Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
double fz_thresh = config["fz_thresh"].as<double>();
// MOCAP // MOCAP
double std_pose_p = config["std_pose_p"].as<double>(); double std_pose_p = config["std_pose_p"].as<double>();
...@@ -201,8 +184,8 @@ int main (int argc, char **argv) { ...@@ -201,8 +184,8 @@ int main (int argc, char **argv) {
// SENSOR + PROCESSOR IMU // SENSOR + PROCESSOR IMU
SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml"); SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base); SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml"); ProcessorBasePtr proc_imu_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base); ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_imu_base);
// SENSOR + PROCESSOR POSE (for mocap) // SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb) // pose sensor and proc (to get extrinsics in the prb)
...@@ -226,10 +209,13 @@ int main (int argc, char **argv) { ...@@ -226,10 +209,13 @@ int main (int argc, char **argv) {
sensor_pose->fixExtrinsics(); sensor_pose->fixExtrinsics();
} }
VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()}); Matrix3d w_R_m_init = q2R(w_qvec_wm);
Vector3d w_p_wi_init = w_p_wm + w_R_m_init*b_p_bi;
VectorComposite x_origin("POV", {w_p_wi_init, w_qvec_wm, Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()}); VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
// FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005); // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005); // if mocap used FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, time_tolerance_mocap); // if mocap used
proc_imu->setOrigin(KF1); proc_imu->setOrigin(KF1);
...@@ -259,6 +245,10 @@ int main (int argc, char **argv) { ...@@ -259,6 +245,10 @@ int main (int argc, char **argv) {
double* imu_bias_fbk_carr = new double[6*N]; double* imu_bias_fbk_carr = new double[6*N];
double* extr_mocap_fbk_carr = new double[7*N]; double* extr_mocap_fbk_carr = new double[7*N];
// covariances computed on the fly
std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
//
std::vector<Vector3d> i_omg_oi_v; std::vector<Vector3d> i_omg_oi_v;
////////////////////////////////////////// //////////////////////////////////////////
...@@ -266,22 +256,12 @@ int main (int argc, char **argv) { ...@@ -266,22 +256,12 @@ int main (int argc, char **argv) {
for (unsigned int i=0; i < N; i++){ for (unsigned int i=0; i < N; i++){
TimeStamp ts(t_arr[i]); TimeStamp ts(t_arr[i]);
////////////////////////////////////
// Retrieve current state
VectorComposite curr_state = problem->getState();
Vector4d o_qvec_i_curr = curr_state['O'];
Quaterniond o_q_i_curr(o_qvec_i_curr);
Vector3d o_v_oi_curr = curr_state['V'];
//////////////////////////////////// ////////////////////////////////////
//////////////////////////////////// ////////////////////////////////////
// Get measurements // Get measurements
// retrieve traj data // retrieve traj data
Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i); Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case) Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i); Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i); Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
w_qvec_wm.normalize(); // not close enough to wolf eps sometimes w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
...@@ -311,11 +291,30 @@ int main (int argc, char **argv) { ...@@ -311,11 +291,30 @@ int main (int argc, char **argv) {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " "; std::cout << ts << " ";
std::cout << report << std::endl; std::cout << report << std::endl;
nb_kf = problem->getTrajectory()->getFrameMap().size();
// recover covariances at this point
auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
Qbi_fbk_v.push_back(Qbi_fbk_diag);
// std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
// std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
nb_kf++;
} }
// Store current state estimation // Store current state estimation
VectorComposite state_fbk = problem->getState(ts); VectorComposite state_fbk = problem->getState();
// VectorComposite state_fbk = problem->getState(ts);
Vector3d o_p_oi = state_fbk['P']; Vector3d o_p_oi = state_fbk['P'];
Vector4d o_qvec_i = state_fbk['O']; Vector4d o_qvec_i = state_fbk['O'];
Vector3d o_v_oi = state_fbk['V']; Vector3d o_v_oi = state_fbk['V'];
...@@ -326,11 +325,12 @@ int main (int argc, char **argv) { ...@@ -326,11 +325,12 @@ int main (int argc, char **argv) {
Matrix3d o_R_b = o_R_i * i_R_b; Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs(); Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib; Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*b_p_bi); Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*i_R_b*b_p_bi);
// Vector3d o_p_ob = o_p_oi; // Vector3d o_p_ob = o_p_oi;
// Vector3d o_v_ob = o_v_oi; // Vector3d o_v_ob = o_v_oi;
imu_bias = sen_imu->getIntrinsic()->getState(); imu_bias = sen_imu->getIntrinsic()->getState();
// imu_bias = sen_imu->getIntrinsic(ts)->getState();
Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState(); Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double)); mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
...@@ -394,6 +394,9 @@ int main (int argc, char **argv) { ...@@ -394,6 +394,9 @@ int main (int argc, char **argv) {
double* Qv_carr = new double[3*Nkf]; double* Qv_carr = new double[3*Nkf];
double* Qbi_carr = new double[6*Nkf]; double* Qbi_carr = new double[6*Nkf];
double* Qm_carr = new double[6*Nkf]; double* Qm_carr = new double[6*Nkf];
// feedback covariances
double* Qbi_fbk_carr = new double[6*Nkf];
// factor errors // factor errors
double* fac_imu_err_carr = new double[9*Nkf]; double* fac_imu_err_carr = new double[9*Nkf];
double* fac_pose_err_carr = new double[6*Nkf]; double* fac_pose_err_carr = new double[6*Nkf];
...@@ -440,6 +443,10 @@ int main (int argc, char **argv) { ...@@ -440,6 +443,10 @@ int main (int argc, char **argv) {
memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double)); memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double)); memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
// Recover feedback covariances
memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
// Factor errors // Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl; // std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
FactorBasePtrList fac_lst; FactorBasePtrList fac_lst;
...@@ -507,6 +514,7 @@ int main (int argc, char **argv) { ...@@ -507,6 +514,7 @@ int main (int argc, char **argv) {
cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a"); cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a"); cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a"); cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a"); cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a"); cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a"); cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment