diff --git a/.gitignore b/.gitignore
index 73cb12494a98358d6706d14652b82f0bc4d76025..5b1eba01c9bdefe79c1ce1dea4c82eb893e53ff8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -24,6 +24,9 @@ src/examples/map_polyline_example_write.yaml
 *.graffle
 /Default/
 
+*.csv
+*.csv#
+
 src/CMakeCache.txt
 
 src/CMakeFiles/cmake.check_cache
diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index 275bf6561a88265b51ea547abc1b231de770ec0f..21600e87a6547f8261632d50e6ae704029d8ad24 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -132,15 +132,18 @@ Matrix6d str_to_cov(std::string cov)
 
 int main()
 {
+    /////////////////////////
+    // read the input data //
+    /////////////////////////
 
     std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
-    std::string filename = wolf_root + "/demos/test.csv";
+    std::string filename = wolf_root + "/demos/input_demo2.csv";
     std::vector<std::pair<std::string, std::vector<std::string>>> csv_values;
     csv_values = read_csv(filename);
 
-    /////////////////////////////
-    // setup the wolf problem
-    /////////////////////////////
+    ////////////////////////////
+    // setup the wolf problem //
+    ////////////////////////////
 
     ProblemPtr problem = Problem::create("PO", 3);
     SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
@@ -159,31 +162,51 @@ int main()
 
     // Parsing variables
     int len = csv_values.at(0).second.size();
-    int idx_cov = 9;
+    int idx_cov = 10;
     int idx_pose = 4;
     int idx_obj = 3;
-    int idx_t = 8;
+    int idx_t = 9;
 
+    ///////////////
+    // SLAM loop //
+    ///////////////
+
+
+    double t_cur = 999;
     for (int i = 0; i < len; i++)
     {
         std::cout << i << std::endl;
 
         // Measurement data
-        Isometry3d c_M_o;
-        c_M_o = str_to_isometry(csv_values.at(idx_pose).second.at(i));
+        Isometry3d c_M_o = str_to_isometry(csv_values.at(idx_pose).second.at(i));
         Matrix6d cov;
         cov = str_to_cov(csv_values.at(idx_cov).second.at(i));
         std::string object_name;
         object_name = csv_values.at(idx_obj).second.at(i);
         double t;
         t = std::stod(csv_values.at(idx_t).second.at(i));
-
-        // Capture processing
-        dets.clear();
-        ObjectDetection det = {.measurement = isometry_to_posevec(c_M_o), .meas_cov = cov, .object_type = object_name};
-        dets.push_back(det);
-        CaptureObjectPtr cap = std::make_shared<CaptureObject>(t, sen, dets);
-        cap->process();
+        ObjectDetection det = {.measurement = isometry_to_posevec(*c_M_o), .meas_cov = cov, .object_type = object_name};
+
+        if (t == t_cur){
+            // Capture on the current frame
+            dets.push_back(det);
+        } else if (t_cur == 999){
+            // Capture on the first frame
+            dets.push_back(det);
+        } else if (i == len-1){
+            // Capture on the last frame
+            dets.push_back(det);
+            CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets);
+            cap->process();
+        } else {
+            // Capture on a new frame
+            CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets);
+            cap->process();
+
+            dets.clear();
+            dets.push_back(det);
+        }
+        t_cur = t;
     }
 
     std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
@@ -200,9 +223,9 @@ int main()
     file_map.open(map_filename, std::fstream::out);
 
     // Write headers for csv files
-    std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw\n";
+    std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw,\n";
     file_res << header_res;
-    std::string header_map = "id,px,py,pz,qx,qy,qz,qw\n";
+    std::string header_map = "id,px,py,pz,qx,qy,qz,qw,\n";
     file_map << header_map;
 
     VectorComposite state_est;
diff --git a/demos/map.csv b/demos/map.csv
index 9d3e694e3fe61c15222404642ae77f81c9569379..f1e18713b7920802213c69978a746b6162d4dc71 100644
--- a/demos/map.csv
+++ b/demos/map.csv
@@ -1,2 +1,10 @@
-id,px,py,pz,qx,qy,qz,qw
-0,0.0118101,-0.0103998,0.42091,-0.00461907,0.771858,-0.628642,0.0949934,
+id,px,py,pz,qx,qy,qz,qw,
+0,-0.0596441,-0.10401,0.493655,-0.066251,0.429956,0.900086,0.0243665,
+1,-0.0596441,-0.10401,0.493655,-0.066251,0.429956,0.900086,0.0243665,
+2,-0.0596441,-0.10401,0.493655,-0.066251,0.429956,0.900086,0.0243665,
+3,0.138246,-0.0498245,0.450293,0.278378,0.883805,-0.308363,0.215192,
+4,0.138246,-0.0498245,0.450293,0.278378,0.883805,-0.308363,0.215192,
+5,0.138246,-0.0498245,0.450293,0.278378,0.883805,-0.308363,0.215192,
+6,-0.0947857,0.041793,0.310745,-0.0216406,0.915369,-0.39597,0.0695633,
+7,-0.0947857,0.041793,0.310745,-0.0216406,0.915369,-0.39597,0.0695633,
+8,-0.0947857,0.041793,0.310745,-0.0216406,0.915369,-0.39597,0.0695633,
diff --git a/demos/processor_tracker_landmark_object.yaml b/demos/processor_tracker_landmark_object.yaml
index f95f2422744379c4e658feb8d41e734a99da12a4..d80bfcf56611b66c1dee75f6ffceab45070bcee8 100644
--- a/demos/processor_tracker_landmark_object.yaml
+++ b/demos/processor_tracker_landmark_object.yaml
@@ -5,7 +5,7 @@ time_tolerance: 0.1222
 vote:
     voting_active:              true
     min_time_vote:              0 # s
-    max_time_vote:              0.1 # s
+    max_time_vote:              0.5 # s
     min_features_for_keyframe:  1
     nb_vote_for_every_first:    0
 
diff --git a/demos/result.csv b/demos/result.csv
index aacb66784956486e690447764e07455294ca58c9..b08d731ad443e69e5eff0c60a3936a0284b2ed92 100644
--- a/demos/result.csv
+++ b/demos/result.csv
@@ -1,164 +1,33 @@
-id,t,px,py,pz,qx,qy,qz,qw
-0,0.000000000,1.84381e-13,-2.798e-11,-1.70113e-12,6.32843e-10,3.91335e-11,7.99363e-11,1,
-1,0.099518000,-0.0104137,0.0077851,0.00181263,0.012509,0.00531949,0.00100668,0.999907,
-2,0.199035000,-0.0150023,0.00500016,0.00324575,0.0109784,0.0036419,-0.00238622,0.99993,
-3,0.298553000,-0.022212,0.000700687,0.00358512,0.00702993,0.00392236,0.00168232,0.999966,
-4,0.398070000,-0.043199,-0.00203051,0.00757717,0.00688483,0.0165713,0.00568339,0.999823,
-5,0.497588000,-0.0460949,-0.00298447,0.0056032,0.00580757,0.00856018,0.00211528,0.999944,
-6,0.597106000,-0.0498825,-0.00230925,0.0059157,0.0083041,0.00695186,0.0006663,0.999941,
-7,0.696623000,-0.056499,-0.00266882,0.00454185,0.009491,0.00548888,0.0055494,0.999924,
-8,0.796141000,-0.0875173,-0.021327,0.0180811,-0.0102778,0.0388762,0.0188157,0.999014,
-9,0.895658000,-0.0784256,-0.0213351,0.0145278,-0.0111415,0.030505,0.0135028,0.999381,
-10,0.995176000,-0.0933745,-0.0336061,0.0249545,-0.0261619,0.0606001,0.0267275,0.997461,
-11,1.094694000,-0.1056,-0.033298,0.0261718,-0.0239909,0.0825309,0.0372965,0.995601,
-12,1.194211000,-0.114915,-0.0265249,0.034208,-0.0172216,0.0956748,0.0446871,0.99426,
-13,1.293729000,-0.120425,-0.0362641,0.042266,-0.0238592,0.100829,0.0534247,0.993182,
-14,1.393246000,-0.139274,-0.0487197,0.0550595,-0.0270022,0.121781,0.0730903,0.989494,
-15,1.492764000,-0.148259,-0.0450491,0.0579103,-0.0184896,0.128445,0.0712859,0.988978,
-16,1.592281000,-0.143472,-0.0414073,0.0573522,-0.0171456,0.122076,0.0652742,0.990224,
-17,1.691799000,-0.137973,-0.0432384,0.0556763,-0.0227657,0.115468,0.0665541,0.990818,
-18,1.791317000,-0.121846,-0.0433565,0.0480688,-0.0251673,0.094104,0.059017,0.993493,
-19,1.890834000,-0.120699,-0.0420157,0.0497147,-0.0242278,0.0955087,0.0511068,0.993821,
-20,1.990352000,-0.11225,-0.0407377,0.0476949,-0.0237189,0.0901472,0.047366,0.994519,
-21,2.089869000,-0.0896885,-0.0431687,0.0457076,-0.0332902,0.0737242,0.0410016,0.995879,
-22,2.189387000,-0.0916551,-0.0425754,0.0545092,-0.0343731,0.0951581,0.0446645,0.993865,
-23,2.288905000,-0.0473993,-0.0139747,0.0420146,-0.0011778,0.0459912,0.0279583,0.99855,
-24,2.388422000,-0.0266703,-0.0259846,0.0467664,-0.0246655,0.0347434,0.0307871,0.998617,
-25,2.454767000,-0.0158515,-0.0183473,0.0454522,-0.0187268,0.029791,0.0266378,0.999026,
-26,2.554285000,0.0168385,-0.00714708,0.0500795,-0.00987754,-0.00299172,0.0229145,0.999684,
-27,2.653802000,0.0323923,-0.00174456,0.0462748,-0.00618191,-0.0126582,0.0187626,0.999725,
-28,2.753320000,0.0587395,-0.00156211,0.0519053,-0.0110108,-0.0357037,0.0100864,0.999251,
-29,2.852837000,0.0736532,0.00130476,0.051088,-0.0275864,-0.0449859,0.00827483,0.998572,
-30,2.952355000,0.103807,0.0131061,0.0595308,-0.0228591,-0.0827264,0.00852907,0.996274,
-31,3.051873000,0.123449,0.019038,0.0606726,-0.0147712,-0.107694,0.000944441,0.994074,
-32,3.151390000,0.14723,0.030637,0.0689698,-0.00947022,-0.144728,-0.00303976,0.989422,
-33,3.250908000,0.171337,0.0251339,0.0783293,-0.0232564,-0.176322,-0.00718808,0.984032,
-34,3.350425000,0.182818,0.0106731,0.0887363,-0.0390438,-0.191863,-0.00832991,0.980609,
-35,3.449943000,0.195935,0.0117309,0.096873,-0.0330681,-0.200225,-0.00886433,0.979152,
-36,3.549461000,0.224604,0.00575342,0.113806,-0.036768,-0.218022,-0.0211271,0.975022,
-37,3.648978000,0.238399,0.0162188,0.121392,-0.0255938,-0.21069,-0.0144261,0.977111,
-38,3.748496000,0.248664,0.0159724,0.12947,-0.0324751,-0.206609,-0.0127105,0.977802,
-39,3.848013000,0.26324,0.0166943,0.137962,-0.0306332,-0.221492,-0.0177966,0.974518,
-40,3.947531000,0.258683,0.0158601,0.141042,-0.0376487,-0.212692,-0.0146157,0.976284,
-41,4.047048000,0.251891,0.0153758,0.138987,-0.0379931,-0.201472,-0.0117588,0.978686,
-42,4.146566000,0.251266,0.00827269,0.147562,-0.0434442,-0.199653,-0.0218317,0.97866,
-43,4.246083999,0.240591,0.0179148,0.146538,-0.0253465,-0.18316,-0.0089846,0.982715,
-44,4.345601000,0.217858,0.0172101,0.126079,-0.0233371,-0.152513,0.00630314,0.988006,
-45,4.445119000,0.211473,0.0181028,0.127235,-0.0173805,-0.154801,0.00703134,0.987768,
-46,4.544636000,0.194919,0.0120887,0.123016,-0.0238574,-0.136024,0.00767788,0.990388,
-47,4.644154000,0.17808,0.00930125,0.118699,-0.0350668,-0.112572,0.0168418,0.992882,
-48,4.743672000,0.164653,0.000632572,0.114512,-0.0441494,-0.0993138,0.0134491,0.993985,
-49,4.843189000,0.149283,0.0124682,0.105175,-0.0258574,-0.0887002,0.0249288,0.995411,
-50,4.942707000,0.120361,0.0113026,0.0938186,-0.0238952,-0.0649532,0.0316011,0.997102,
-51,5.042224000,0.091644,0.0020601,0.0857252,-0.0326401,-0.0446763,0.0373065,0.997771,
-52,5.141742000,0.0565977,-0.0104995,0.0827468,-0.0392545,-0.0180505,0.0400642,0.998263,
-53,5.241259000,0.0375933,-0.0261576,0.0810137,-0.0460884,-0.00750037,0.0391917,0.99814,
-54,5.340777000,0.0303904,-0.0354298,0.0674925,-0.0439438,-0.00208782,0.0453312,0.998003,
-55,5.440295000,0.0163903,-0.0400611,0.0535455,-0.0465724,0.0192031,0.0500442,0.997476,
-56,5.539812000,0.0112069,-0.0443687,0.035413,-0.0499705,0.0266689,0.0487605,0.997203,
-57,5.639330000,0.0125292,-0.0419698,0.0161972,-0.0490445,0.0323669,0.0402546,0.99746,
-58,5.738847000,0.0134146,-0.0450088,-0.00116133,-0.0526253,0.0396234,0.038302,0.997093,
-59,5.838365000,0.0149326,-0.0483298,-0.0216591,-0.0526733,0.0444989,0.0324132,0.997093,
-60,5.937882000,0.0152858,-0.053366,-0.0406393,-0.0529505,0.046936,0.0347396,0.996888,
-61,6.037400000,0.0162815,-0.062939,-0.065322,-0.0569219,0.0447831,0.0329997,0.996828,
-62,6.136918000,0.00414485,-0.0690722,-0.0866392,-0.055911,0.0554775,0.033223,0.99634,
-63,6.236435000,0.00472297,-0.0686868,-0.110617,-0.0541613,0.0504789,0.0306583,0.996784,
-64,6.335953000,0.00398202,-0.0752438,-0.131449,-0.0600321,0.0459694,0.0270537,0.99677,
-65,6.435470000,0.0172541,-0.0724002,-0.155408,-0.0551584,0.0327342,0.0210449,0.997719,
-66,6.534988000,0.0171284,-0.0823973,-0.185068,-0.061622,0.0245121,0.0211813,0.997574,
-67,6.634506000,0.0152082,-0.0775584,-0.209378,-0.0563504,0.00859359,0.0140341,0.998275,
-68,6.734023000,0.0150379,-0.0703369,-0.248667,-0.0513236,-0.0102172,0.00688141,0.998606,
-69,6.833541000,0.0162264,-0.0740766,-0.27287,-0.0575273,-0.0296271,0.00960728,0.997858,
-70,6.899886000,0.0178112,-0.0752163,-0.282423,-0.0715167,-0.0408183,0.0020104,0.996602,
-71,7.032576000,0.0346229,-0.0621397,-0.290854,-0.067904,-0.0653595,-0.00767216,0.995519,
-72,7.132093000,0.0386856,-0.0683244,-0.294024,-0.0763505,-0.063426,-0.0145615,0.994955,
-73,7.231611000,0.0425411,-0.0479403,-0.289069,-0.0630441,-0.0632099,-0.0163591,0.995873,
-74,7.331129000,0.0525976,-0.00951206,-0.288911,-0.0490016,-0.0703121,-0.0135914,0.996228,
-75,7.430646000,0.0723648,0.0273674,-0.278411,-0.0366176,-0.0876624,-0.0159621,0.995349,
-76,7.530164000,0.0705168,0.0427728,-0.270447,-0.0402731,-0.0879527,-0.0193786,0.995122,
-77,7.629681000,0.0925511,0.04559,-0.249387,-0.0498127,-0.102857,-0.0256313,0.993117,
-78,7.729199000,0.089142,0.0537832,-0.230176,-0.0564244,-0.0925481,-0.0275528,0.993726,
-79,7.828717000,0.0821574,0.0693259,-0.206654,-0.0511938,-0.0792254,-0.0183612,0.995372,
-80,7.928234000,0.0845874,0.0797422,-0.184352,-0.0399376,-0.0712882,-0.015193,0.99654,
-81,8.027752000,0.0815232,0.0538593,-0.162747,-0.056871,-0.0603787,-0.0130122,0.996469,
-82,8.127269000,0.0791333,0.0578464,-0.13617,-0.0481003,-0.0535571,-0.00277099,0.997402,
-83,8.226787000,0.0785472,0.0431487,-0.11077,-0.0520426,-0.0549184,-0.000718456,0.997133,
-84,8.326304000,0.0724949,0.0525543,-0.0889835,-0.0334881,-0.0567562,-0.00390391,0.997819,
-85,8.425822000,0.0706863,0.0645771,-0.0584816,-0.0228621,-0.0561139,-0.00659697,0.998141,
-86,8.525340000,0.072803,0.046609,-0.0332548,-0.0241582,-0.0583373,-0.00348289,0.997999,
-87,8.624857000,0.069845,0.029253,-0.00308634,-0.0348911,-0.0498759,0.00177635,0.998144,
-88,8.724375000,0.0692812,0.0193106,0.0171899,-0.0355564,-0.0466686,0.00576222,0.998261,
-89,8.823892000,0.0697556,0.0192878,0.0346468,-0.0220945,-0.0443491,0.0107119,0.998714,
-90,8.923410000,0.0769873,0.00953147,0.0571878,-0.0197131,-0.0494594,0.0119115,0.998511,
-91,9.022927000,0.0697975,0.000874978,0.0746585,-0.0122585,-0.0318222,0.0160514,0.999289,
-92,9.122445000,0.0573281,-0.0206904,0.0874244,-0.0273264,-0.00317054,0.023698,0.999341,
-93,9.221963000,0.0603567,-0.0392483,0.0869072,-0.0449545,0.00192528,0.0225709,0.998732,
-94,9.321480000,0.056046,-0.0497229,0.0857417,-0.0675955,0.0106935,0.0198066,0.997459,
-95,9.420998000,0.0561238,-0.059022,0.0757563,-0.101091,0.00633783,0.00813291,0.994824,
-96,9.520515000,0.058809,-0.0559871,0.0657321,-0.108285,-0.000745311,0.000813119,0.994119,
-97,9.620033000,0.0419056,-0.0707626,0.0517127,-0.148834,0.0207255,0.0134448,0.988554,
-98,9.719551000,0.0272557,-0.0902503,0.0489875,-0.198014,0.0349692,0.0208193,0.979354,
-99,9.819068000,0.0309949,-0.101266,0.0402726,-0.209646,0.0183186,0.0148517,0.977493,
-100,9.918586000,0.0394371,-0.110608,0.0288683,-0.225181,-0.000799872,0.0125256,0.974236,
-101,10.018103000,0.040246,-0.123661,0.0251319,-0.241906,-0.00314017,0.0143593,0.970188,
-102,10.117621000,0.0540018,-0.127667,0.0226253,-0.25238,-0.0132895,0.00485339,0.967525,
-103,10.217138000,0.0631226,-0.130795,0.0252264,-0.261089,-0.0139497,0.00357404,0.965207,
-104,10.316656000,0.0703676,-0.155488,0.0362531,-0.281187,-0.0138255,0.0103031,0.959498,
-105,10.416174000,0.0752088,-0.162294,0.0426866,-0.293234,-0.0158885,0.0170967,0.955756,
-106,10.515691000,0.0889143,-0.177128,0.0442056,-0.297123,-0.0303522,0.0145315,0.954246,
-107,10.615209000,0.0843939,-0.182431,0.0418268,-0.293906,-0.0248731,0.0239809,0.95521,
-108,10.714726000,0.0881223,-0.181943,0.0378129,-0.289595,-0.0337823,0.0165374,0.95641,
-109,10.814244000,0.068785,-0.17998,0.0321479,-0.286489,-0.0111194,0.0238578,0.957722,
-110,10.913762000,0.0769703,-0.169494,0.0296451,-0.268371,-0.0219404,0.0137906,0.962967,
-111,11.013279000,0.069634,-0.148636,0.0225558,-0.233637,-0.00744512,0.0190131,0.97211,
-112,11.112797000,0.0632018,-0.14079,0.0226302,-0.212582,0.00352945,0.0238063,0.976847,
-113,11.212314000,0.0531655,-0.125949,0.0216365,-0.184704,0.0174035,0.023767,0.982353,
-114,11.311832000,0.0399421,-0.110837,0.0297894,-0.165153,0.0408712,0.0292283,0.984987,
-115,11.411349000,0.0381459,-0.0954268,0.0427067,-0.13699,0.0530509,0.0291966,0.98872,
-116,11.510867000,0.0396894,-0.0826852,0.0521846,-0.109137,0.0619422,0.0305627,0.991624,
-117,11.610385000,0.0497736,-0.0611696,0.0646781,-0.0730433,0.0497281,0.0233725,0.995814,
-118,11.709902000,0.0533534,-0.0569946,0.0731508,-0.0583674,0.0493631,0.0132993,0.996985,
-119,11.809420000,0.0491284,-0.0531697,0.0820769,-0.0556825,0.0547627,0.00542945,0.996931,
-120,11.908937000,0.0448327,-0.0548328,0.0820293,-0.0444708,0.0542148,-0.0142108,0.997437,
-121,12.008455000,0.0458415,-0.0639888,0.0767052,-0.0491098,0.0473662,-0.0457252,0.996621,
-122,12.107973000,0.050727,-0.070487,0.0784987,-0.0666401,0.0294514,-0.0782115,0.994271,
-123,12.207490000,0.0572982,-0.0671847,0.0764452,-0.0667752,0.0117672,-0.106679,0.991979,
-124,12.307008000,0.0619866,-0.0567163,0.0702281,-0.0475529,0.000751524,-0.136666,0.989475,
-125,12.406525000,0.0755938,-0.0410174,0.066231,-0.0214464,-0.0205741,-0.169173,0.985138,
-126,12.506043000,0.081834,-0.0376735,0.0629567,-0.0132537,-0.0304331,-0.205027,0.978193,
-127,12.605560000,0.0654946,-0.030638,0.0581242,-0.0061305,-0.00770281,-0.229276,0.973312,
-128,12.705078000,0.0576659,-0.0299603,0.0556941,-0.00502055,0.00559095,-0.249172,0.96843,
-129,12.804596000,0.0543494,-0.033653,0.0553541,-0.0155178,0.00772726,-0.266656,0.963636,
-130,12.904113000,0.0713449,-0.043482,0.057064,-0.0301959,-0.0202825,-0.252863,0.966818,
-131,13.003631000,0.0680846,-0.040269,0.052959,-0.0252205,-0.0106395,-0.221969,0.974669,
-132,13.103148000,0.0718916,-0.0409655,0.0552942,-0.0292326,-0.0168455,-0.190047,0.981195,
-133,13.202666000,0.0648447,-0.0534973,0.0590963,-0.0542405,-0.0033763,-0.148762,0.987379,
-134,13.302183000,0.0539831,-0.0727274,0.0587357,-0.0899558,0.0121011,-0.106478,0.990164,
-135,13.401701000,0.0431021,-0.0733317,0.053925,-0.0920229,0.0275593,-0.0484777,0.994194,
-136,13.501219000,0.0321343,-0.0693872,0.0549765,-0.0955572,0.0323155,-0.0150932,0.994785,
-137,13.600736000,0.0320299,-0.0763657,0.0563521,-0.100079,0.0285241,0.0149607,0.994458,
-138,13.700254000,0.0297575,-0.0939197,0.0524466,-0.103565,0.029246,0.0382136,0.993458,
-139,13.799771000,0.0320771,-0.0937671,0.0497685,-0.0863712,0.0155977,0.0391974,0.995369,
-140,13.899289000,0.0316049,-0.103048,0.0462776,-0.0856385,0.010246,0.0402937,0.995458,
-141,13.998807000,0.0299156,-0.101265,0.0419338,-0.0895835,0.0153219,0.0404324,0.99504,
-142,14.098324000,0.032881,-0.0949276,0.0422738,-0.0916909,0.0118868,0.0379333,0.994994,
-143,14.197842000,0.0330255,-0.0918447,0.0412266,-0.0894177,0.0122519,0.0399016,0.995119,
-144,14.297359000,0.0338196,-0.0932731,0.0424361,-0.0917032,0.00997358,0.0334223,0.995175,
-145,14.396877000,0.0306872,-0.0954926,0.0437848,-0.0958261,0.0150253,0.0387382,0.994531,
-146,14.496394000,0.0313544,-0.0918321,0.0469432,-0.0943184,0.0135618,0.0411557,0.994599,
-147,14.595912000,0.0342644,-0.0906378,0.048435,-0.0933246,0.00878221,0.034582,0.994996,
-148,14.695430000,0.0285105,-0.0891108,0.0481628,-0.0907282,0.0160101,0.0344015,0.995153,
-149,14.794947000,0.0304786,-0.0898752,0.0498181,-0.0918774,0.0142336,0.0309056,0.995189,
-150,14.894465000,0.0314291,-0.0904777,0.0523856,-0.091754,0.0120071,0.0309784,0.995227,
-151,14.993982000,0.030663,-0.0927115,0.0534406,-0.0948109,0.0122253,0.033287,0.994864,
-152,15.093500000,0.0312085,-0.0939924,0.0539139,-0.096421,0.00975606,0.0308862,0.994813,
-153,15.193018000,0.0239234,-0.0929377,0.0537589,-0.0972926,0.018603,0.0304691,0.994615,
-154,15.292535000,0.0235872,-0.0931819,0.0559615,-0.101282,0.0188743,0.0287564,0.994263,
-155,15.392053000,0.0248763,-0.091923,0.0552244,-0.098166,0.0148479,0.0277217,0.994673,
-156,15.491570000,0.0266008,-0.091795,0.0562652,-0.0985591,0.0114943,0.0239646,0.994776,
-157,15.591088000,0.0268559,-0.0906483,0.0566647,-0.0985346,0.0123805,0.0207945,0.994839,
-158,15.690605000,0.0247102,-0.0886304,0.055933,-0.0970209,0.0165746,0.0198804,0.994946,
-159,15.790123000,0.0231996,-0.0890532,0.0576433,-0.0992022,0.0189015,0.0200766,0.994685,
-160,15.889641000,0.0255563,-0.0849885,0.0578528,-0.094851,0.0168522,0.0194006,0.99516,
-161,15.989158000,0.0277467,-0.0848212,0.0593826,-0.0953652,0.0136328,0.0173066,0.995199,
-162,16.088676000,0.0277667,-0.0833906,0.0598822,-0.0931765,0.0134077,0.0174467,0.995406,
+id,t,px,py,pz,qx,qy,qz,qw,
+0,0.000000000,2.51663e-11,-8.71181e-09,-4.19875e-09,2.05988e-09,-1.58398e-10,2.92981e-10,1,
+1,0.497588000,-0.0264847,-8.46493e-05,-0.00399219,-0.0117422,0.00778784,0.0182128,0.999735,
+2,0.995176000,-0.0499237,-0.00899706,-0.00119966,-0.0221522,0.0230906,0.0293954,0.999056,
+3,1.492764000,-0.00546354,-0.00219766,0.00125746,-0.000762948,0.0011901,-0.000294146,0.999999,
+4,1.990352000,1.67905e-05,0.00358173,-0.00627669,0.000321343,-0.00010852,-0.000128037,1,
+5,2.487940000,-0.374216,-0.148556,0.151291,0.0157886,0.647296,0.397281,0.650328,
+6,2.985528000,-0.266949,-0.113775,0.126278,-0.0640574,0.276114,0.209147,0.935903,
+7,3.483116000,-0.299931,-0.174855,0.177958,-0.0802549,0.350739,0.315318,0.878132,
+8,3.980703000,-0.309088,-0.14069,0.159908,-0.0569284,0.426073,0.279001,0.858708,
+9,4.478291000,-0.316165,-0.196705,0.20063,-0.0472104,0.452595,0.342629,0.821909,
+10,4.975879000,0.195235,0.0574257,0.729687,0.827567,0.415299,-0.37767,0.00496301,
+11,5.473467000,0.153329,0.0181056,0.785504,-0.88573,-0.350887,0.301308,-0.0396724,
+12,5.971055000,0.0903288,-0.0429045,0.833297,0.952284,0.245414,-0.179256,0.0281784,
+13,6.468643000,-0.0148607,-0.078459,0.864282,0.978617,0.189864,-0.0784434,0.0103701,
+14,6.966231000,-0.102861,-0.0923376,0.846282,-0.994335,-0.102617,-0.0166198,0.0221761,
+15,7.463819000,-0.180509,-0.0665181,0.825259,-0.991245,-0.0445852,-0.121824,-0.0245684,
+16,7.961407000,-0.267635,-0.0598841,0.787735,0.981775,-0.0100056,0.189779,0.00133286,
+17,8.458995000,0.575689,-0.133409,0.355518,-0.45691,-0.381146,0.334623,0.730745,
+18,8.956582000,0.148425,0.0273143,0.107987,0.075892,0.130381,0.185701,0.970956,
+19,9.454170000,0.0749393,-0.0205812,0.133996,0.0693465,0.205411,0.227354,0.949372,
+20,9.926683000,-0.469961,0.0547173,0.549326,0.804822,-0.242192,0.541524,0.0188678,
+21,10.407293000,-0.46782,0.0899827,0.503899,0.737044,-0.303739,0.603665,0.00988252,
+22,10.879805000,0.413893,-0.215398,0.369712,0.22761,-0.594826,-0.533465,0.556589,
+23,11.377393000,0.421733,-0.0537895,0.280106,0.338641,-0.441323,-0.496279,0.666531,
+24,11.874981000,0.325699,0.0936254,0.220561,0.373795,-0.261175,-0.414722,0.787445,
+25,12.372569000,-0.512935,-0.15546,0.116831,-0.0978231,0.597053,0.478813,0.636158,
+26,12.870157000,0.373667,-0.0455879,0.130669,0.13608,-0.377683,-0.40757,0.820198,
+27,13.367745000,0.202168,0.0942178,0.0352605,0.121426,-0.229017,-0.190458,0.946854,
+28,13.865333000,0.108157,0.0604562,-0.0121661,0.0839108,-0.138218,-0.15068,0.975269,
+29,14.362921000,0.0124425,0.0516145,-0.0236974,0.0974592,-0.0172597,-0.0820719,0.9917,
+30,14.860509000,0.120984,0.0598476,-0.00225833,0.127159,-0.135539,-0.128828,0.974096,
+31,15.358097000,-0.0483837,0.0282599,-0.0341755,0.0736267,0.00758949,-0.0435516,0.996306,