diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp index 4543cd6c6a15da77b95dc0adcd760243196c6543..127d90dd798e6556354286fa9b0a4ac8d161d21f 100644 --- a/demos/mcapi_povcdl_estimation.cpp +++ b/demos/mcapi_povcdl_estimation.cpp @@ -627,9 +627,9 @@ int main (int argc, char **argv) { std::fstream file_kfs; 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/gereral_phd_repo/centroidalkin/est.csv", std::fstream::out); - file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/gereral_phd_repo/centroidalkin/kfs.csv", std::fstream::out); - file_wre.open("/home/mfourmy/Documents/Phd_LAAS/gereral_phd_repo/centroidalkin/wre.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; @@ -638,7 +638,6 @@ int main (int argc, char **argv) { 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; - VectorComposite state_est; for (unsigned int i=0; i < t_arr.size(); i++) { file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << t_arr[i] << "," @@ -662,7 +661,10 @@ int main (int argc, char **argv) { << L_traj_arr[i](1) << "," << L_traj_arr[i](2) << std::endl; + } + VectorComposite state_est; + for (unsigned int i=0; i < t_arr.size(); i++) { state_est = problem->getState(t_arr[i]); file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << t_arr[i] << "," @@ -686,7 +688,8 @@ int main (int argc, char **argv) { << state_est['L'](1) << "," << state_est['L'](2) << "\n"; - + } + for (unsigned int i=0; i < t_arr.size(); i++) { file_wre << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << t_arr[i] << "," << p_bll_meas_v[i](0) << ","