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

Compute ALL covariances instead of particular ones because the latter does not... 

Compute ALL covariances instead of particular ones because the latter does not seem to work properly. Bp covariance block not found though.
parent e5958877
No related branches found
No related tags found
3 merge requests!18Release after RAL,!17After 2nd RAL submission,!5WIP: Resolve "Any contact type/number refactoring"
Hide whitespace changes
Inline Side-by-side
demos/mcapi_povcdl_estimation.cpp
+ 83
− 20
View file @ f3b1310d
// debug
#include <iostream>
#include <random>
// #include <random>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <ctime>
#include "eigenmvn.h"
#include "pinocchio/parsers/urdf.hpp"
......@@ -138,7 +138,8 @@ int main (int argc, char **argv) {
std::vector<VectorXd> ddq_traj_arr; // [spacc_b, qA_ddot]
std::vector<VectorXd> c_traj_arr; // w_p_wc
std::vector<VectorXd> dc_traj_arr; // w_v_wc
std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the Com expressed in CoM (=World) frame
std::vector<VectorXd> L_traj_arr; // w_Lc: angular momentum at the CoM expressed in CoM (=World) frame
std::vector<VectorXd> bp_traj_arr; // bias on b_p_bc: bias on the CoM position expressed in base frame
std::vector<std::vector<VectorXd>> cforce_traj_arr_lst(nbc);
int N = (max_t - min_t)/dt;
int N_inter = N/10;
......@@ -309,6 +310,11 @@ int main (int argc, char **argv) {
Vector3d b_p_bc = data.com[0]; // meas
// Vector3d b_p_bc = oTb.inverse().act(c); // meas
// compute bias ground truth from the difference between real model and disturbed one
// TODO: for the moment, constant bias put in the traj
bp_traj_arr.push_back(bias_pbc);
/////////////////////////
// bIomg, Lcm
MatrixXd b_Mc = crba(model, data, q); // mass matrix at b frame expressed in b frame
......@@ -483,15 +489,15 @@ int main (int argc, char **argv) {
double ts_since_last_kf = 0;
// Add gaussian noises
Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc, 2)*Matrix3d::Identity());
Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr, 2)*Matrix3d::Identity());
Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc, 2)*Matrix3d::Identity());
Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc, 2)*Matrix3d::Identity());
Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f, 2)*Matrix3d::Identity());
Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau, 2)*Matrix3d::Identity());
std::time_t epoch = std::time(nullptr);
int64_t seed = (int64_t)epoch;
Eigen::EigenMultivariateNormal<double> noise_acc(Vector3d::Zero(), pow(std_acc, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_gyr(Vector3d::Zero(), pow(std_gyr, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_pbc(Vector3d::Zero(), pow(std_pbc, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_vbc(Vector3d::Zero(), pow(std_vbc, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_f (Vector3d::Zero(), pow(std_f, 2)*Matrix3d::Identity(), false, seed);
Eigen::EigenMultivariateNormal<double> noise_tau(Vector3d::Zero(), pow(std_tau, 2)*Matrix3d::Identity(), false, seed);
// wolf::setGravityMagnitude(9.81);
// wolf::setGravityMagnitude(9.806);
int traj_size = problem->getTrajectory()->getFrameList().size();
for (unsigned int ii=1; ii < t_arr.size(); ii++){
......@@ -666,16 +672,20 @@ int main (int argc, char **argv) {
std::fstream file_gtr;
std::fstream file_est;
std::fstream file_kfs;
std::fstream file_cov;
std::fstream file_wre;
file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/gtr.csv", std::fstream::out);
file_est.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/est.csv", std::fstream::out);
file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/kfs.csv", std::fstream::out);
file_wre.open("/home/mfourmy/Documents/Phd_LAAS/general_phd_repo/centroidalkin/wre.csv", std::fstream::out);
std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz\n";
file_gtr << header;
file_est << header;
file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/gtr.csv", std::fstream::out);
file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
file_wre.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/wre.csv", std::fstream::out);
std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz\n";
file_est << header_est;
std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n";
file_kfs << header_kfs;
file_gtr << header_kfs; // at the same frequency as "est" but ground truth biases added for comparison with kfs
std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLx,QLy,QLz,Qbpx,Qbpy,Qbpz\n";
file_cov << header_cov;
std::string header_wre = "t,plfx,plfy,plfz,prfx,prfy,prfz,f_lfx,f_lfy,f_lfz,f_rfx,f_rfy,f_rfz,tau_lfx,tau_lfy,tau_lfz,tau_rfx,tau_rfy,tau_rfz\n";
file_wre << header_wre;
......@@ -700,7 +710,10 @@ int main (int argc, char **argv) {
<< dc_traj_arr[i](2) << ","
<< L_traj_arr[i](0) << ","
<< L_traj_arr[i](1) << ","
<< L_traj_arr[i](2)
<< L_traj_arr[i](2) << ","
<< bp_traj_arr[i](0) << ","
<< bp_traj_arr[i](1) << ","
<< bp_traj_arr[i](2)
<< std::endl;
}
......@@ -757,11 +770,14 @@ int main (int argc, char **argv) {
// }
VectorComposite kf_state;
CaptureBasePtr cap_ikin;
VectorComposite bp_bias_est;
solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
for (auto kf: problem->getTrajectory()->getFrameList()){
if (kf->isKey()){
kf_state = kf->getState();
bp_bias_est = kf->getCaptureOf(sen_ikin)->getState();
cap_ikin = kf->getCaptureOf(sen_ikin);
bp_bias_est = cap_ikin->getState();
file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< kf->getTimeStamp().get() << ","
......@@ -788,12 +804,59 @@ int main (int argc, char **argv) {
<< bp_bias_est['I'](1) << ","
<< bp_bias_est['I'](2)
<< "\n";
// compute covariances of KF and capture stateblocks
Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
// solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended...
// solver->computeCovariances(cap_ikin->getStateBlockVec());
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
problem->getCovarianceBlock(kf->getStateBlock('C'), Qc);
problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
// std::cout << "Qbp\n" << Qbp << std::endl;
file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< kf->getTimeStamp().get() << ","
<< Qp(0,0) << ","
<< Qp(1,1) << ","
<< Qp(2,2) << ","
<< Qo(0,0) << ","
<< Qo(1,1) << ","
<< Qo(2,2) << ","
<< Qv(0,0) << ","
<< Qv(1,1) << ","
<< Qv(2,2) << ","
<< Qc(0,0) << ","
<< Qc(1,1) << ","
<< Qc(2,2) << ","
<< Qd(0,0) << ","
<< Qd(1,1) << ","
<< Qd(2,2) << ","
<< QL(0,0) << ","
<< QL(1,1) << ","
<< QL(2,2) << ","
<< Qbp(0,0) << ","
<< Qbp(1,1) << ","
<< Qbp(2,2)
<< "\n";
}
}
file_gtr.close();
file_est.close();
file_kfs.close();
file_cov.close();
file_wre.close();
......
demos/mcapi_povcdl_estimation.yaml
+ 7
− 6
View file @ f3b1310d
# trajectory handling
dt: 0.001
min_t: -1.0 # -1 means from the beginning of the trajectory
max_t: -1.0 # -1 means until the end of the trajectory
max_t: -1 # -1 means until the end of the trajectory
# solve_every_sec: -1 # < 0 strict --> no solve
solve_every_sec: 0.5 # < 0 strict --> no solve
# solve_end: False
solve_every_sec: 1 # < 0 strict --> no solve
solve_end: True
# sensor noises
std_acc: 0.0001
# std_gyr: 0.0001
std_gyr: 0.0001
std_pbc: 0.0001
std_vbc: 0.0001
......@@ -21,17 +23,16 @@ noisy_measurements: True
std_abs_biasimu: 0.0000001 # almost fixed
std_abs_biasp: 10000000 # noprior
# std_abs_biasp: 0.0000001 # almost fixed
std_bp_drift: 0.05 # no drift
# std_bp_drift: 100000 # small drift
std_bp_drift: 1000000000 # small drift
std_odom3d: 0.0001
# std_odom3d: 100000000
std_prior_p: 0.0001
std_prior_o: 0.0001
std_prior_v: 0.0001
bias_pbc: [0.01,0.02,0.03]
# bias_pbc: [0.01,0.02,0.03]
# bias_pbc_prior: [0.01,0.02,0.03]
# bias_pbc: [0.0,0.0,0.0]
bias_pbc: [0.0,0.0,0.0]
bias_pbc_prior: [0,0,0]
# Robot model
......
demos/mcapi_utils.h
+ 1
− 0
View file @ f3b1310d
#include <vector>
#include "Eigen/Dense"
using namespace Eigen;
......
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