diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index ad38bcd8aa9e84d380fa141c024b480a474cbb16..b2cddf4787b5c201d96f18b732b396a7dd69ed9b 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -1,8 +1,8 @@
-// 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();
diff --git a/demos/mcapi_povcdl_estimation.yaml b/demos/mcapi_povcdl_estimation.yaml
index fe9a1a33aa5f256498b374a1944fbcae5bdfd0a9..f2032360b777afbcdf2432e9b803611909843483 100644
--- a/demos/mcapi_povcdl_estimation.yaml
+++ b/demos/mcapi_povcdl_estimation.yaml
@@ -1,13 +1,15 @@
# 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
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
index 3d1bcf64c0e65c430f3898cccd15fb6e78083114..2fda5867a1f35f090e59f8ca492fb163bfb1682b 100644
--- a/demos/mcapi_utils.h
+++ b/demos/mcapi_utils.h
@@ -1,3 +1,4 @@
+#include <vector>
#include "Eigen/Dense"
using namespace Eigen;