/SLAM-Hive/slam_hive_results/mapping_results/2834/2042_vins-fusion-mono-imu+MH_02_easy+Image_comb_test-2.yamlERROR: Unable to communicate with master!
[31m[ERROR] [1720469314.703707765]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...[0m
[0m[ INFO] [1720469314.704604376]: init begins[0m
[31m[ERROR] [1720469314.705753133]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...[0m
... logging to /root/.ros/log/db090d34-3d65-11ef-8bd1-0242ac110005/roslaunch-ff87f69422b5-117.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
]2;roscore
[1mstarted roslaunch server http://ff87f69422b5:43987/[0m
ros_comm version 1.12.17


SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.17

NODES

auto-starting new master
[1mprocess[master]: started with pid [154][0m
[1mROS_MASTER_URI=http://ff87f69422b5:11311/[0m
]2;roscore http://ff87f69422b5:11311/
[1msetting /run_id to db090d34-3d65-11ef-8bd1-0242ac110005[0m
[0m[ INFO] [1720469315.859197036]: Connected to master at [localhost:11311][0m
[0m[ INFO] [1720469315.860630756]: Connected to master at [localhost:11311][0m
config_file: /slamhive/mappingtask.yaml
USE_IMU: 1
IMU_TOPIC: /imu0
result path /home/output/vio.csv
[33m[ WARN] [1720469315.867818275]:  fix extrinsic param [0m
camera number 1
[0m[ INFO] [1720469315.868021552]: Synchronized sensors, fix time offset: 0[0m
[0m[ INFO] [1720469315.868053367]: ROW: 288 COL: 451 [0m
 exitrinsic cam 0
 0.0148655  -0.999881  0.0041403
  0.999557  0.0149672  0.0257155
-0.0257744 0.00375619   0.999661
-0.0216401  -0.064677 0.00981073
set g       0       0 9.81007
[0m[ INFO] [1720469315.868273485]: reading paramerter of camera /slamhive/cam0_pinhole.yaml[0m
MULTIPLE_THREAD is 1
[33m[ WARN] [1720469315.868563028]: waiting for image and imu...[0m
config_file: /slamhive/mappingtask.yaml
vocabulary_file/home/catkin_ws/src/VINS-Fusion/loop_fusion/../support_files/brief_k10L6.bin
loop start load vocabulary
BRIEF_PATTERN_FILE/home/catkin_ws/src/VINS-Fusion/loop_fusion/../support_files/brief_pattern.yml
cam calib path: /slamhive/cam0_pinhole.yaml
VIO input, perfrom 4 DoF (x, y, z, yaw) pose graph optimization
no previous pose graph
[1mprocess[rosout-1]: started with pid [229][0m
started core service [/rosout]
rosbag play /slamhive/dataset/MH_02_easy.bag  --clock 
[0m[ INFO] [1720469324.855468111]: Opening /slamhive/dataset/MH_02_easy.bag[0m

Waiting 0.2 seconds after advertising topics... done.

Hit space to toggle paused, or 's' to step.

 [DELAYED]  Bag Time: 1403636856.854208   Duration: 0.000000 / 153.782490   Delay: 316832468.23 
init first imu pose
averge acc 9.439445 -0.460368 -3.138128
init R0 
  0.318508   0.137589   0.937882
         0   -0.98941   0.145148
   0.94792 -0.0462307  -0.315135
wait for imu ... 
[33m[ WARN] [1720469328.171970431]: gyroscope bias initial calibration -0.00772493   0.0378885   0.0830686[0m
[0m[ INFO] [1720469328.201254911]: Initialization finish![0m
time: 1403636859.701667, t: 0.021129 -0.071128 0.238185 q: -0.036831 0.817314 0.045553 0.573208 
time: 1403636859.801666, t: 0.027729 -0.073319 0.289023 q: -0.028037 0.811016 0.045315 0.582592 
wait for imu ... 
time: 1403636859.901666, t: 0.031517 -0.073377 0.332207 q: -0.020521 0.807287 0.046035 0.588003 
wait for imu ... 
time: 1403636860.001667, t: 0.031532 -0.073170 0.360162 q: -0.017869 0.806152 0.044491 0.589763 
time: 1403636860.101666, t: 0.032488 -0.076337 0.366653 q: -0.016142 0.808366 0.040816 0.587042 
wait for imu ... 
time: 1403636860.201667, t: 0.032363 -0.082098 0.349415 q: -0.017511 0.812548 0.038558 0.581354 
wait for imu ... 
time: 1403636860.301666, t: 0.028802 -0.086102 0.304644 q: -0.022923 0.820072 0.036106 0.570659 
time: 1403636860.401666, t: 0.024118 -0.090260 0.247757 q: -0.025085 0.832137 0.032497 0.553049 
time: 1403636860.501667, t: 0.017935 -0.092558 0.190352 q: -0.027809 0.844468 0.028520 0.534123 
wait for imu ... 
time: 1403636860.601666, t: 0.012003 -0.094136 0.143844 q: -0.032518 0.853672 0.026905 0.519098 
wait for imu ... 
time: 1403636860.701667, t: 0.006072 -0.095688 0.112831 q: -0.036891 0.857010 0.029750 0.513116 
time: 1403636860.801666, t: 0.001728 -0.097486 0.104466 q: -0.033485 0.856615 0.036428 0.513578 
time: 1403636860.901666, t: 0.000652 -0.098312 0.123997 q: -0.031210 0.855804 0.042772 0.514583 
wait for imu ... 
wait for imu ... 
time: 1403636861.001667, t: 0.001737 -0.098784 0.168343 q: -0.030936 0.853498 0.047027 0.518046 
wait for imu ... 
time: 1403636861.101666, t: 0.005543 -0.099427 0.226680 q: -0.031328 0.846072 0.047125 0.530057 
wait for imu ... 
time: 1403636861.201667, t: 0.010955 -0.100421 0.289578 q: -0.030984 0.833962 0.046750 0.548965 
wait for imu ... 
time: 1403636861.301666, t: 0.015060 -0.101794 0.351630 q: -0.032154 0.820219 0.045974 0.569293 
wait for imu ... 
time: 1403636861.401666, t: 0.017995 -0.104237 0.401738 q: -0.033490 0.808995 0.042239 0.585339 
time: 1403636861.501667, t: 0.020211 -0.106586 0.425818 q: -0.032908 0.802021 0.039490 0.595079 
time: 1403636861.601666, t: 0.022359 -0.108883 0.416056 q: -0.031037 0.802873 0.038408 0.594102 
wait for imu ... 
time: 1403636861.701667, t: 0.024183 -0.109273 0.377659 q: -0.032516 0.809378 0.039181 0.585077 
time: 1403636861.801666, t: 0.023085 -0.109322 0.325567 q: -0.031835 0.816261 0.041679 0.575297 
time: 1403636861.901666, t: 0.019879 -0.107045 0.268527 q: -0.035234 0.824128 0.043440 0.563635 
time: 1403636862.001667, t: 0.014746 -0.104298 0.210601 q: -0.039275 0.835704 0.042943 0.546088 
wait for imu ... 
time: 1403636862.101666, t: 0.009240 -0.099698 0.158040 q: -0.042632 0.844884 0.040660 0.531696 
wait for imu ... 
time: 1403636862.201667, t: 0.005441 -0.096601 0.119708 q: -0.045540 0.849569 0.041598 0.523859 
wait for imu ... 
time: 1403636862.301666, t: 0.003053 -0.096563 0.107023 q: -0.040026 0.853266 0.047068 0.517803 
time: 1403636862.401666, t: 0.002986 -0.097359 0.127333 q: -0.038503 0.856338 0.050273 0.512519 
wait for imu ... 
time: 1403636862.501667, t: 0.004606 -0.098364 0.175004 q: -0.037297 0.854267 0.050684 0.516011 
wait for imu ... 
time: 1403636862.601666, t: 0.010381 -0.101213 0.237202 q: -0.026203 0.848301 0.048817 0.526608 
time: 1403636862.701667, t: 0.016375 -0.102737 0.305418 q: -0.017990 0.840518 0.046276 0.539504 
wait for imu ... 
wait for imu ... 
time: 1403636862.801666, t: 0.021402 -0.102335 0.374157 q: -0.019558 0.835711 0.039215 0.547418 
wait for imu ... 
time: 1403636862.901666, t: 0.024821 -0.101841 0.438303 q: -0.022453 0.832595 0.030449 0.552589 
wait for imu ... 
time: 1403636863.001667, t: 0.023724 -0.101674 0.482207 q: -0.027038 0.826995 0.026740 0.560922 
time: 1403636863.101666, t: 0.023850 -0.102016 0.490455 q: -0.020802 0.820958 0.031055 0.569764 
time: 1403636863.201667, t: 0.022631 -0.103703 0.464474 q: -0.012710 0.818405 0.036475 0.573342 
time: 1403636863.301666, t: 0.022727 -0.103124 0.412559 q: -0.006838 0.819766 0.039937 0.571263 
time: 1403636863.401666, t: 0.021449 -0.104407 0.349445 q: -0.001006 0.821957 0.039503 0.568178 
wait for imu ... 
time: 1403636863.501667, t: 0.018849 -0.104367 0.281197 q: -0.001273 0.826744 0.034771 0.561501 
wait for imu ... 
time: 1403636863.601666, t: 0.012799 -0.104561 0.213816 q: -0.013287 0.834728 0.031876 0.549578 
time: 1403636863.701667, t: 0.006152 -0.105186 0.155120 q: -0.023155 0.843244 0.029244 0.536235 
time: 1403636863.801666, t: 0.002371 -0.106243 0.116704 q: -0.027191 0.851138 0.026059 0.523589 
wait for imu ... 
time: 1403636863.901666, t: 0.000442 -0.107460 0.111427 q: -0.027688 0.856768 0.027306 0.514234 
time: 1403636864.001667, t: 0.000426 -0.108623 0.143290 q: -0.030159 0.857790 0.030237 0.512223 
wait for imu ... 
wait for imu ... 
time: 1403636864.101666, t: 0.001836 -0.109109 0.202600 q: -0.028378 0.854151 0.035240 0.518054 
time: 1403636864.201667, t: 0.005635 -0.109400 0.275062 q: -0.019970 0.849509 0.037121 0.525888 
wait for imu ... 
wait for imu ... 
time: 1403636864.301666, t: 0.009719 -0.107496 0.344892 q: -0.018618 0.846463 0.036854 0.530844 
wait for imu ... 
wait for imu ... 
time: 1403636864.401666, t: 0.012918 -0.104335 0.406229 q: -0.020392 0.842571 0.034220 0.537111 
time: 1403636864.501667, t: 0.015740 -0.102282 0.457497 q: -0.022185 0.832684 0.026571 0.552665 
wait for imu ... 
time: 1403636864.601666, t: 0.017574 -0.099567 0.486547 q: -0.026525 0.821147 0.020798 0.569721 
wait for imu ... 
time: 1403636864.701667, t: 0.017423 -0.099773 0.483427 q: -0.020948 0.815732 0.023962 0.577554 
time: 1403636864.801666, t: 0.016636 -0.100486 0.449629 q: -0.015231 0.818445 0.029157 0.573642 
wait for imu ... 
time: 1403636864.901666, t: 0.017153 -0.099453 0.394933 q: -0.013545 0.823855 0.029804 0.565854 
wait for imu ... 
time: 1403636865.001667, t: 0.017320 -0.100438 0.334062 q: -0.006938 0.830155 0.029087 0.556731 
time: 1403636865.101666, t: 0.017405 -0.101611 0.272143 q: -0.006405 0.837335 0.028464 0.545912 
wait for imu ... 
time: 1403636865.201667, t: 0.017433 -0.101487 0.216633 q: -0.021634 0.842773 0.019833 0.537468 
wait for imu ... 
time: 1403636865.301666, t: 0.016469 -0.101357 0.174128 q: -0.035195 0.845197 0.014065 0.533109 
wait for imu ... 
time: 1403636865.401666, t: 0.016071 -0.102342 0.147431 q: -0.034378 0.846259 0.013756 0.531483 
wait for imu ... 
time: 1403636865.501667, t: 0.016653 -0.102534 0.141595 q: -0.026845 0.850866 0.019812 0.524322 
wait for imu ... 
wait for imu ... 
time: 1403636865.601666, t: 0.017827 -0.100820 0.164786 q: -0.023551 0.856354 0.025535 0.515220 
wait for imu ... 
time: 1403636865.701667, t: 0.017978 -0.096060 0.211859 q: -0.027534 0.855799 0.028370 0.515795 
wait for imu ... 
wait for imu ... 
time: 1403636865.801666, t: 0.019763 -0.091550 0.273342 q: -0.030210 0.849142 0.027676 0.526573 
wait for imu ... 
time: 1403636865.901666, t: 0.022827 -0.088742 0.341240 q: -0.032769 0.839278 0.024970 0.542139 
wait for imu ... 
time: 1403636866.001667, t: 0.024717 -0.086711 0.409705 q: -0.036139 0.825369 0.021531 0.563024 
wait for imu ... 
wait for imu ... 
time: 1403636866.101666, t: 0.024423 -0.085274 0.470909 q: -0.034636 0.809634 0.017917 0.585638 
wait for imu ... 
time: 1403636866.201667, t: 0.022757 -0.083983 0.512890 q: -0.030673 0.799520 0.018720 0.599563 
time: 1403636866.301666, t: 0.020335 -0.083441 0.524513 q: -0.024424 0.798863 0.020392 0.600671 
time: 1403636866.401666, t: 0.019619 -0.084241 0.505571 q: -0.016427 0.805011 0.021756 0.592633 
wait for imu ... 
time: 1403636866.501667, t: 0.020379 -0.085160 0.462743 q: -0.013183 0.811714 0.023193 0.583445 
wait for imu ... 
time: 1403636866.601666, t: 0.021400 -0.087952 0.407044 q: -0.010412 0.818189 0.023453 0.574376 
wait for imu ... 
time: 1403636866.701667, t: 0.020599 -0.093510 0.354666 q: -0.006066 0.825463 0.023548 0.563932 
wait for imu ... 
time: 1403636866.801666, t: 0.015886 -0.098608 0.312701 q: -0.009552 0.831307 0.027385 0.555057 
time: 1403636866.901666, t: 0.007695 -0.102747 0.278720 q: -0.021495 0.837756 0.029686 0.544814 
wait for imu ... 
time: 1403636867.001667, t: 0.000434 -0.104540 0.251897 q: -0.030649 0.846396 0.029325 0.530862 
wait for imu ... 
time: 1403636867.101666, t: -0.006315 -0.105190 0.235474 q: -0.031093 0.852636 0.029094 0.520768 
wait for imu ... 
time: 1403636867.201667, t: -0.016090 -0.104960 0.228448 q: -0.037900 0.855844 0.031351 0.514890 
time: 1403636867.301666, t: -0.026752 -0.103990 0.227702 q: -0.046824 0.858040 0.037489 0.510068 
wait for imu ... 
time: 1403636867.401666, t: -0.034343 -0.104132 0.231757 q: -0.046833 0.861518 0.039709 0.504001 
wait for imu ... 
time: 1403636867.501667, t: -0.042104 -0.103586 0.237856 q: -0.042467 0.868294 0.040214 0.492589 
time: 1403636867.601666, t: -0.051284 -0.104268 0.239582 q: -0.036549 0.883456 0.043644 0.465043 
wait for imu ... 
time: 1403636867.701667, t: -0.061827 -0.104939 0.235712 q: -0.035523 0.907330 0.043775 0.416622 
wait for imu ... 
time: 1403636867.801666, t: -0.072230 -0.107074 0.229965 q: -0.031493 0.931116 0.041250 0.361011 
optimize pose graph 
wait for imu ... 
wait for imu ... 
time: 1403636867.901666, t: -0.077645 -0.108870 0.227902 q: -0.028441 0.943755 0.035362 0.327517 
time: 1403636868.001667, t: -0.077734 -0.107965 0.231565 q: -0.028680 0.944390 0.030685 0.326134 
time: 1403636868.101666, t: -0.078119 -0.106471 0.237621 q: -0.029482 0.939228 0.029709 0.340734 
wait for imu ... 
time: 1403636868.201667, t: -0.081006 -0.105373 0.241102 q: -0.031924 0.935485 0.029938 0.350646 
wait for imu ... 
time: 1403636868.301666, t: -0.087844 -0.103192 0.242437 q: -0.034048 0.935484 0.028408 0.350577 
wait for imu ... 
time: 1403636868.401666, t: -0.095289 -0.098447 0.242467 q: -0.035727 0.936803 0.024999 0.347129 
time: 1403636868.501667, t: -0.103048 -0.091709 0.244112 q: -0.033712 0.934453 0.022449 0.353775 
time: 1403636868.601666, t: -0.110094 -0.085400 0.249075 q: -0.029902 0.930969 0.017759 0.363438 
time: 1403636868.701667, t: -0.117644 -0.080926 0.253742 q: -0.028160 0.931146 0.015185 0.363240 
time: 1403636868.801666, t: -0.125872 -0.076488 0.254760 q: -0.027897 0.933973 0.020100 0.355686 
time: 1403636868.901666, t: -0.131032 -0.072699 0.255652 q: -0.024050 0.934835 0.024702 0.353405 
time: 1403636869.001667, t: -0.132646 -0.067743 0.257142 q: -0.026256 0.932600 0.026639 0.358969 
time: 1403636869.101666, t: -0.133039 -0.063902 0.258439 q: -0.032199 0.930746 0.028023 0.363166 
wait for imu ... 
time: 1403636869.201667, t: -0.133379 -0.060896 0.257331 q: -0.035388 0.930301 0.030368 0.363821 
wait for imu ... 
time: 1403636869.301666, t: -0.132583 -0.059714 0.255711 q: -0.035549 0.930416 0.032389 0.363337 
time: 1403636869.401666, t: -0.131120 -0.059140 0.254590 q: -0.036981 0.930220 0.033243 0.363619 
time: 1403636869.501667, t: -0.129462 -0.060990 0.253821 q: -0.037329 0.930163 0.033150 0.363737 
time: 1403636869.601666, t: -0.126412 -0.069864 0.252958 q: -0.036939 0.930310 0.032443 0.363464 
time: 1403636869.701667, t: -0.121178 -0.088650 0.251832 q: -0.035216 0.931819 0.031356 0.359847 
wait for imu ... 
time: 1403636869.901666, t: -0.099730 -0.152356 0.252486 q: -0.029384 0.931274 0.020874 0.362531 
wait for imu ... 
time: 1403636870.001667, t: -0.084845 -0.194478 0.253230 q: -0.026302 0.929373 0.013212 0.367968 
wait for imu ... 
time: 1403636870.101666, t: -0.068247 -0.241225 0.252564 q: -0.022408 0.930235 0.004579 0.366250 
time: 1403636870.201667, t: -0.051275 -0.291115 0.252999 q: -0.015397 0.932509 -0.000076 0.360818 
wait for imu ... 
time: 1403636870.301666, t: -0.033596 -0.341085 0.254500 q: -0.012766 0.932143 -0.004139 0.361842 
time: 1403636870.401666, t: -0.015951 -0.390618 0.254805 q: -0.007930 0.929309 -0.008569 0.369119 
time: 1403636870.501667, t: -0.001990 -0.435316 0.251900 q: -0.005306 0.928727 -0.013826 0.370469 
time: 1403636870.601666, t: 0.007685 -0.471377 0.246497 q: -0.002320 0.931452 -0.020621 0.363272 
time: 1403636870.701667, t: 0.016617 -0.500925 0.242778 q: 0.005354 0.932419 -0.024903 0.360480 
time: 1403636870.801666, t: 0.024278 -0.518237 0.239161 q: 0.002887 0.929087 -0.029347 0.368683 
time: 1403636870.901666, t: 0.027745 -0.530104 0.234853 q: -0.000634 0.925111 -0.030736 0.378449 
time: 1403636871.001667, t: 0.028697 -0.536114 0.230258 q: -0.005631 0.923122 -0.032564 0.383085 
time: 1403636871.101666, t: 0.027647 -0.535776 0.224981 q: -0.007151 0.925878 -0.036389 0.375999 
time: 1403636871.201667, t: 0.025975 -0.531804 0.224123 q: -0.000483 0.929639 -0.038940 0.366407 
time: 1403636871.301666, t: 0.023701 -0.520154 0.227633 q: -0.000064 0.929553 -0.040169 0.366494 
time: 1403636871.401666, t: 0.018935 -0.499165 0.231366 q: 0.000667 0.929168 -0.035872 0.367911 
time: 1403636871.501667, t: 0.012728 -0.467964 0.234098 q: -0.001002 0.928888 -0.031746 0.368997 
time: 1403636871.601666, t: 0.004665 -0.427385 0.236629 q: -0.003400 0.929453 -0.029623 0.367733 
time: 1403636871.701667, t: -0.005507 -0.378499 0.240459 q: -0.010910 0.930460 -0.026942 0.365239 
time: 1403636871.801666, t: -0.015523 -0.324775 0.243781 q: -0.018653 0.930772 -0.022474 0.364431 
time: 1403636871.901666, t: -0.024094 -0.268832 0.244854 q: -0.022237 0.930288 -0.015404 0.365831 
wait for imu ... 
time: 1403636872.001667, t: -0.030981 -0.209099 0.244289 q: -0.030903 0.928030 -0.009654 0.371095 
time: 1403636872.101666, t: -0.037214 -0.149932 0.241803 q: -0.040519 0.925439 -0.003013 0.376712 
time: 1403636872.201667, t: -0.044646 -0.094064 0.237868 q: -0.044653 0.924982 0.003381 0.377364 
wait for imu ... 
time: 1403636872.301666, t: -0.052107 -0.045530 0.231839 q: -0.049670 0.927200 0.011117 0.371092 
wait for imu ... 
time: 1403636872.401666, t: -0.059642 -0.006114 0.226248 q: -0.053881 0.929725 0.018669 0.363813 
time: 1403636872.501667, t: -0.064034 0.018365 0.224062 q: -0.056614 0.930405 0.023247 0.361388 
wait for imu ... 
time: 1403636872.601666, t: -0.065579 0.026705 0.224494 q: -0.058343 0.929098 0.027956 0.364132 
time: 1403636872.701667, t: -0.065678 0.022544 0.226749 q: -0.057436 0.927250 0.030873 0.368721 
time: 1403636872.801666, t: -0.063800 0.008799 0.228091 q: -0.053356 0.927328 0.030286 0.369186 
time: 1403636872.901666, t: -0.057895 -0.017651 0.227956 q: -0.051175 0.928469 0.025210 0.367002 
time: 1403636873.001667, t: -0.048728 -0.057715 0.226606 q: -0.046072 0.929197 0.019502 0.366183 
time: 1403636873.101666, t: -0.037284 -0.107122 0.225031 q: -0.041133 0.929289 0.014034 0.366788 
time: 1403636873.201667, t: -0.023999 -0.163267 0.224235 q: -0.037430 0.930145 0.006598 0.365219 
time: 1403636873.301666, t: -0.009725 -0.223411 0.224077 q: -0.033023 0.930853 -0.001874 0.363893 
time: 1403636873.401666, t: 0.002757 -0.284343 0.223518 q: -0.028682 0.931327 -0.008965 0.362941 
time: 1403636873.501667, t: 0.012508 -0.343031 0.221746 q: -0.025437 0.932089 -0.015216 0.361014 
time: 1403636873.601666, t: 0.019147 -0.396107 0.218817 q: -0.018149 0.933680 -0.022246 0.356956 
time: 1403636873.701667, t: 0.023320 -0.440436 0.214711 q: -0.012664 0.934955 -0.028219 0.353416 
time: 1403636873.801666, t: 0.025516 -0.472284 0.209812 q: -0.012152 0.936017 -0.029518 0.350504 
time: 1403636873.901666, t: 0.025758 -0.489781 0.205851 q: -0.012347 0.935921 -0.031816 0.350553 
time: 1403636874.001667, t: 0.026260 -0.496109 0.202591 q: -0.010432 0.935250 -0.034767 0.352121 
time: 1403636874.101666, t: 0.026497 -0.494084 0.200858 q: -0.009807 0.935242 -0.035821 0.352055 
time: 1403636874.201667, t: 0.026296 -0.484111 0.201270 q: -0.010523 0.935918 -0.034742 0.350343 
time: 1403636874.301666, t: 0.024344 -0.463948 0.203227 q: -0.012048 0.936900 -0.030012 0.348099 
time: 1403636874.401666, t: 0.021813 -0.429183 0.204223 q: -0.017693 0.937658 -0.024352 0.346254 
time: 1403636874.501667, t: 0.018390 -0.383436 0.205904 q: -0.023013 0.937059 -0.018953 0.347895 
time: 1403636874.601666, t: 0.012936 -0.330346 0.207991 q: -0.025890 0.935512 -0.013257 0.352095 
wait for imu ... 
time: 1403636874.701667, t: 0.005737 -0.270226 0.207870 q: -0.032236 0.934403 -0.005888 0.354708 
time: 1403636874.801666, t: -0.001225 -0.209751 0.205699 q: -0.038210 0.934048 0.000492 0.355097 
time: 1403636874.901666, t: -0.008716 -0.153061 0.201524 q: -0.047036 0.934484 0.004534 0.352855 
time: 1403636875.001667, t: -0.012294 -0.106873 0.197637 q: -0.052505 0.933868 0.008123 0.353650 
time: 1403636875.101666, t: -0.011418 -0.072067 0.194463 q: -0.050655 0.929138 0.003136 0.366234 
time: 1403636875.201667, t: -0.011920 -0.045737 0.191184 q: -0.042555 0.922136 -0.013295 0.384287 
wait for imu ... 
time: 1403636875.301666, t: -0.018163 -0.023648 0.187352 q: -0.035382 0.917253 -0.037542 0.394949 
wait for imu ... 
time: 1403636875.401666, t: -0.029720 -0.006125 0.183078 q: -0.034694 0.915673 -0.064509 0.395193 
time: 1403636875.501667, t: -0.052568 0.004398 0.180577 q: -0.037992 0.916335 -0.084401 0.389568 
time: 1403636875.601666, t: -0.085412 0.003171 0.178966 q: -0.041406 0.917429 -0.098889 0.383184 
time: 1403636875.701667, t: -0.122588 -0.011399 0.179444 q: -0.037892 0.916784 -0.111574 0.381605 
time: 1403636875.801666, t: -0.160743 -0.035596 0.179504 q: -0.027166 0.914691 -0.126682 0.382824 
time: 1403636875.901666, t: -0.198384 -0.066787 0.179302 q: -0.014864 0.913813 -0.144602 0.379230 
wait for imu ... 
time: 1403636876.001667, t: -0.232308 -0.104503 0.181398 q: -0.000054 0.911048 -0.166883 0.377017 
time: 1403636876.101666, t: -0.257287 -0.150581 0.186341 q: 0.020936 0.904925 -0.189042 0.380704 
time: 1403636876.201667, t: -0.268696 -0.200453 0.191465 q: 0.038264 0.898055 -0.209108 0.385108 
time: 1403636876.301666, t: -0.267680 -0.252637 0.193424 q: 0.049690 0.894418 -0.225364 0.383091 
wait for imu ... 
time: 1403636876.401666, t: -0.246037 -0.305099 0.191748 q: 0.058194 0.893655 -0.237757 0.376119 
wait for imu ... 
time: 1403636876.501667, t: -0.213312 -0.358959 0.191189 q: 0.070074 0.892370 -0.242448 0.374145 
time: 1403636876.601666, t: -0.166204 -0.408100 0.191694 q: 0.079516 0.889647 -0.236468 0.382477 
time: 1403636876.701667, t: -0.109764 -0.446474 0.195182 q: 0.081467 0.888102 -0.223027 0.393569 
time: 1403636876.801666, t: -0.049158 -0.466517 0.198746 q: 0.076926 0.889709 -0.202537 0.401845 
time: 1403636876.901666, t: 0.010130 -0.467371 0.202177 q: 0.068787 0.892428 -0.175634 0.409869 
wait for imu ... 
time: 1403636877.001667, t: 0.066758 -0.444681 0.203629 q: 0.053509 0.894384 -0.144997 0.419750 
wait for imu ... 
time: 1403636877.101666, t: 0.117409 -0.401897 0.201947 q: 0.032550 0.897175 -0.108423 0.426921 
wait for imu ... 
time: 1403636877.201667, t: 0.159415 -0.344773 0.198891 q: 0.008043 0.900226 -0.068477 0.429929 
time: 1403636877.301666, t: 0.182057 -0.273770 0.200467 q: -0.016169 0.901991 -0.031704 0.430286 
time: 1403636877.401666, t: 0.188629 -0.199705 0.200989 q: -0.037585 0.902928 0.004705 0.428119 
time: 1403636877.501667, t: 0.176770 -0.129088 0.199381 q: -0.057985 0.904101 0.035247 0.421897 
wait for imu ... 
time: 1403636877.601666, t: 0.152671 -0.066921 0.194476 q: -0.075692 0.906967 0.054799 0.410706 
time: 1403636877.701667, t: 0.115179 -0.021841 0.191103 q: -0.089735 0.910205 0.065443 0.398988 
time: 1403636877.801666, t: 0.066866 0.010621 0.188595 q: -0.096686 0.913801 0.065796 0.388961 
time: 1403636877.901666, t: 0.015264 0.028071 0.184533 q: -0.095502 0.918308 0.056849 0.379944 
time: 1403636878.001667, t: -0.038172 0.029973 0.180248 q: -0.086698 0.923234 0.042782 0.371877 
time: 1403636878.101666, t: -0.087559 0.017403 0.174660 q: -0.072255 0.926372 0.022817 0.368907 
time: 1403636878.201667, t: -0.130720 -0.009657 0.169120 q: -0.056965 0.926733 -0.002070 0.371371 
wait for imu ... 
time: 1403636878.301666, t: -0.164559 -0.050059 0.163237 q: -0.044876 0.925414 -0.028498 0.375210 
time: 1403636878.401666, t: -0.190550 -0.100768 0.159022 q: -0.034242 0.923179 -0.056464 0.378656 
time: 1403636878.501667, t: -0.205138 -0.159940 0.158389 q: -0.018774 0.921000 -0.082114 0.380348 
time: 1403636878.601666, t: -0.206221 -0.223438 0.162485 q: -0.001055 0.918112 -0.105082 0.382134 
time: 1403636878.701667, t: -0.192031 -0.286144 0.169927 q: 0.017596 0.914430 -0.122631 0.385318 
time: 1403636878.801666, t: -0.169892 -0.346163 0.178752 q: 0.032844 0.909948 -0.133895 0.391136 
time: 1403636878.901666, t: -0.138151 -0.397920 0.186869 q: 0.039401 0.907423 -0.140518 0.394062 
time: 1403636879.001667, t: -0.099094 -0.437283 0.190235 q: 0.040217 0.907698 -0.143009 0.392449 
wait for imu ... 
wait for imu ... 
time: 1403636879.101666, t: -0.056374 -0.465886 0.197084 q: 0.038382 0.906601 -0.143192 0.395091 
time: 1403636879.201667, t: -0.009722 -0.478533 0.205428 q: 0.035416 0.901758 -0.138467 0.407928 
wait for imu ... 
time: 1403636879.301666, t: 0.038155 -0.477147 0.213781 q: 0.032625 0.897195 -0.127384 0.421604 
time: 1403636879.401666, t: 0.077654 -0.457356 0.222682 q: 0.025815 0.895067 -0.107558 0.431995 
time: 1403636879.501667, t: 0.109327 -0.421496 0.229972 q: 0.012119 0.895416 -0.080449 0.437735 
time: 1403636879.601666, t: 0.130930 -0.375636 0.235510 q: -0.004242 0.897543 -0.053298 0.437674 
time: 1403636879.701667, t: 0.139151 -0.319637 0.242439 q: -0.023872 0.898867 -0.032384 0.436371 
wait for imu ... 
time: 1403636879.801666, t: 0.132655 -0.259180 0.247138 q: -0.041626 0.900961 -0.014009 0.431671 
wait for imu ... 
time: 1403636879.901666, t: 0.113692 -0.203857 0.248525 q: -0.053224 0.904483 0.000151 0.423175 
time: 1403636880.001667, t: 0.080354 -0.160030 0.248626 q: -0.060046 0.908264 0.008643 0.413977 
wait for imu ... 
time: 1403636880.101666, t: 0.041069 -0.135712 0.248879 q: -0.062571 0.910957 0.012950 0.407523 
time: 1403636880.201667, t: -0.002713 -0.136029 0.250350 q: -0.058012 0.911639 0.008818 0.406781 
time: 1403636880.301666, t: -0.047478 -0.151829 0.251820 q: -0.044811 0.910229 -0.007271 0.411609 
time: 1403636880.401666, t: -0.092381 -0.175979 0.254324 q: -0.026009 0.907789 -0.028817 0.417626 
time: 1403636880.501667, t: -0.139013 -0.197742 0.261722 q: -0.006297 0.905073 -0.050419 0.422210 
time: 1403636880.601666, t: -0.185133 -0.208236 0.269817 q: 0.009561 0.904326 -0.068351 0.421226 
time: 1403636880.701667, t: -0.229180 -0.202126 0.276300 q: 0.016647 0.906863 -0.079685 0.413489 
time: 1403636880.801666, t: -0.267059 -0.181426 0.279932 q: 0.015419 0.909929 -0.084538 0.405765 
time: 1403636880.901666, t: -0.293447 -0.149864 0.282314 q: 0.006770 0.910978 -0.080151 0.404537 
wait for imu ... 
time: 1403636881.001667, t: -0.305524 -0.113076 0.284052 q: -0.001931 0.908319 -0.069195 0.412511 
time: 1403636881.101666, t: -0.301437 -0.074671 0.285778 q: -0.009885 0.903735 -0.052399 0.424758 
wait for imu ... 
time: 1403636881.201667, t: -0.286782 -0.037420 0.287153 q: -0.025213 0.900169 -0.031512 0.433668 
time: 1403636881.301666, t: -0.260616 -0.007451 0.289552 q: -0.041920 0.897982 -0.008450 0.437949 
time: 1403636881.401666, t: -0.223955 0.009974 0.292395 q: -0.050428 0.896520 0.011485 0.439973 
wait for imu ... 
time: 1403636881.501667, t: -0.177911 0.011509 0.295188 q: -0.050649 0.894085 0.026884 0.444211 
time: 1403636881.601666, t: -0.126869 -0.001304 0.296689 q: -0.047159 0.890668 0.039537 0.450471 
time: 1403636881.701667, t: -0.072699 -0.029131 0.294408 q: -0.043345 0.888699 0.050863 0.453594 
time: 1403636881.801666, t: -0.020200 -0.068166 0.289079 q: -0.037535 0.889546 0.054820 0.451989 
optimize pose graph 
time: 1403636881.901666, t: 0.024748 -0.113444 0.280966 q: -0.032729 0.891397 0.046984 0.449592 
time: 1403636882.001667, t: 0.057565 -0.161737 0.271413 q: -0.029808 0.892958 0.034683 0.447811 
wait for imu ... 
time: 1403636882.101666, t: 0.075779 -0.209334 0.259860 q: -0.026047 0.892745 0.022447 0.449247 
wait for imu ... 
time: 1403636882.201667, t: 0.078450 -0.250297 0.245685 q: -0.019820 0.891407 0.012308 0.452602 
time: 1403636882.301666, t: 0.067151 -0.280425 0.230908 q: -0.013530 0.888774 0.005382 0.458115 
wait for imu ... 
time: 1403636882.401666, t: 0.049013 -0.294049 0.215607 q: -0.015536 0.886520 0.005019 0.462402 
wait for imu ... 
time: 1403636882.501667, t: 0.029140 -0.291143 0.200672 q: -0.025090 0.884803 0.008265 0.465215 
time: 1403636882.601666, t: 0.018894 -0.277021 0.188191 q: -0.036200 0.881265 0.015169 0.470989 
time: 1403636882.701667, t: 0.023524 -0.257118 0.177678 q: -0.048638 0.870945 0.024815 0.488337 
wait for imu ... 
time: 1403636882.801666, t: 0.039191 -0.235135 0.164352 q: -0.066869 0.854258 0.041594 0.513851 
time: 1403636882.901666, t: 0.059987 -0.214361 0.144865 q: -0.084256 0.838558 0.060925 0.534798 
time: 1403636883.001667, t: 0.079168 -0.194698 0.121196 q: -0.102267 0.831233 0.080184 0.540523 
time: 1403636883.101666, t: 0.097303 -0.177025 0.096660 q: -0.114433 0.828599 0.098659 0.539068 
wait for imu ... 
time: 1403636883.201667, t: 0.113065 -0.162266 0.073586 q: -0.117156 0.822835 0.113686 0.544327 
wait for imu ... 
time: 1403636883.301666, t: 0.126563 -0.149302 0.052712 q: -0.120942 0.816382 0.123460 0.551045 
time: 1403636883.401666, t: 0.137122 -0.140318 0.035972 q: -0.126218 0.812669 0.127948 0.554318 
wait for imu ... 
time: 1403636883.501667, t: 0.143992 -0.134820 0.025812 q: -0.131590 0.807362 0.132876 0.559639 
time: 1403636883.601666, t: 0.148699 -0.131222 0.019668 q: -0.137012 0.799915 0.139315 0.567411 
time: 1403636883.701667, t: 0.152764 -0.128729 0.014751 q: -0.140192 0.796227 0.147398 0.569774 
wait for imu ... 
time: 1403636883.801666, t: 0.155813 -0.127057 0.011006 q: -0.142650 0.795292 0.151261 0.569458 
time: 1403636883.901666, t: 0.158313 -0.126109 0.006600 q: -0.150324 0.794052 0.152104 0.568988 
time: 1403636884.001667, t: 0.160025 -0.125801 0.004314 q: -0.151850 0.791247 0.155148 0.571664 
time: 1403636884.101666, t: 0.160808 -0.126433 0.003024 q: -0.157656 0.786877 0.156330 0.575787 
time: 1403636884.201667, t: 0.160683 -0.127478 0.003055 q: -0.155344 0.783203 0.158578 0.580788 
time: 1403636884.301666, t: 0.159363 -0.127208 0.003572 q: -0.154683 0.780839 0.157851 0.584334 
time: 1403636884.401666, t: 0.158929 -0.126902 0.003623 q: -0.155838 0.780458 0.157228 0.584705 
time: 1403636884.501667, t: 0.158951 -0.127170 0.003665 q: -0.154882 0.780388 0.157744 0.584913 
time: 1403636884.601666, t: 0.158869 -0.127072 0.003730 q: -0.155290 0.780353 0.157495 0.584919 
time: 1403636884.701667, t: 0.158969 -0.127133 0.003670 q: -0.155211 0.780632 0.157410 0.584589 
time: 1403636884.801666, t: 0.158998 -0.127187 0.003749 q: -0.154848 0.780738 0.157314 0.584570 
time: 1403636884.901666, t: 0.159044 -0.127226 0.003733 q: -0.154883 0.780770 0.157270 0.584530 
time: 1403636885.001667, t: 0.159041 -0.127222 0.003748 q: -0.154995 0.780874 0.157146 0.584395 
time: 1403636885.101666, t: 0.158993 -0.127209 0.003764 q: -0.155061 0.780905 0.156962 0.584385 
time: 1403636885.201667, t: 0.158993 -0.127211 0.003775 q: -0.155075 0.780991 0.156852 0.584296 
time: 1403636885.301666, t: 0.159015 -0.127242 0.003776 q: -0.154978 0.781020 0.156856 0.584282 
wait for imu ... 
time: 1403636885.401666, t: 0.159014 -0.127252 0.003770 q: -0.154838 0.781033 0.156766 0.584327 
time: 1403636885.501667, t: 0.159036 -0.127261 0.003767 q: -0.154790 0.781055 0.156740 0.584316 
time: 1403636885.601666, t: 0.159015 -0.127284 0.003788 q: -0.154698 0.781087 0.156701 0.584309 
time: 1403636885.701667, t: 0.159084 -0.127258 0.003768 q: -0.154774 0.781208 0.156630 0.584146 
time: 1403636885.801666, t: 0.159097 -0.127266 0.003775 q: -0.154708 0.781209 0.156632 0.584161 
time: 1403636885.901666, t: 0.159081 -0.127245 0.003769 q: -0.154762 0.781288 0.156569 0.584058 
time: 1403636886.001667, t: 0.159072 -0.127236 0.003772 q: -0.154807 0.781277 0.156528 0.584072 
time: 1403636886.101666, t: 0.159063 -0.127267 0.003773 q: -0.154632 0.781297 0.156573 0.584079 
time: 1403636886.201667, t: 0.159071 -0.127284 0.003767 q: -0.154503 0.781318 0.156613 0.584075 
time: 1403636886.301666, t: 0.159065 -0.127291 0.003773 q: -0.154453 0.781309 0.156643 0.584092 
time: 1403636886.401666, t: 0.159062 -0.127272 0.003766 q: -0.154568 0.781324 0.156580 0.584059 
time: 1403636886.501667, t: 0.159053 -0.127269 0.003766 q: -0.154569 0.781321 0.156574 0.584063 
wait for imu ... 
time: 1403636886.601666, t: 0.159054 -0.127271 0.003754 q: -0.154536 0.781335 0.156577 0.584054 
time: 1403636886.701667, t: 0.159074 -0.127281 0.003749 q: -0.154505 0.781351 0.156579 0.584039 
time: 1403636886.801666, t: 0.159063 -0.127288 0.003754 q: -0.154503 0.781342 0.156600 0.584046 
time: 1403636886.901666, t: 0.159074 -0.127282 0.003751 q: -0.154559 0.781382 0.156582 0.583982 
time: 1403636887.001667, t: 0.159072 -0.127278 0.003750 q: -0.154424 0.781373 0.156628 0.584018 
time: 1403636887.101666, t: 0.159082 -0.127298 0.003738 q: -0.154467 0.781384 0.156611 0.583997 
time: 1403636887.201667, t: 0.159083 -0.127297 0.003731 q: -0.154488 0.781415 0.156578 0.583958 
time: 1403636887.301666, t: 0.159099 -0.127311 0.003715 q: -0.154478 0.781431 0.156530 0.583953 
wait for imu ... 
time: 1403636887.401666, t: 0.159109 -0.127302 0.003729 q: -0.154369 0.781453 0.156589 0.583936 
wait for imu ... 
time: 1403636887.501667, t: 0.159099 -0.127310 0.003723 q: -0.154351 0.781472 0.156606 0.583910 
time: 1403636887.601666, t: 0.159087 -0.127290 0.003726 q: -0.154414 0.781454 0.156547 0.583934 
time: 1403636887.701667, t: 0.159071 -0.127277 0.003717 q: -0.154453 0.781468 0.156532 0.583910 
time: 1403636887.801666, t: 0.159080 -0.127277 0.003720 q: -0.154465 0.781502 0.156526 0.583862 
time: 1403636887.901666, t: 0.159087 -0.127273 0.003722 q: -0.154419 0.781534 0.156520 0.583833 
time: 1403636888.001667, t: 0.159101 -0.127282 0.003717 q: -0.154383 0.781583 0.156516 0.583778 
time: 1403636888.101666, t: 0.159103 -0.127291 0.003719 q: -0.154408 0.781567 0.156522 0.583792 
wait for imu ... 
time: 1403636888.201667, t: 0.159100 -0.127311 0.003694 q: -0.154434 0.781567 0.156497 0.583790 
time: 1403636888.301666, t: 0.159109 -0.127319 0.003691 q: -0.154433 0.781557 0.156503 0.583803 
time: 1403636888.401666, t: 0.159121 -0.127336 0.003685 q: -0.154357 0.781597 0.156529 0.583763 
time: 1403636888.501667, t: 0.159128 -0.127341 0.003692 q: -0.154347 0.781624 0.156502 0.583737 
time: 1403636888.601666, t: 0.159074 -0.127328 0.003728 q: -0.154341 0.781585 0.156415 0.583814 
time: 1403636888.701667, t: 0.159032 -0.127249 0.003777 q: -0.154368 0.781433 0.156519 0.583982 
time: 1403636888.801666, t: 0.159078 -0.127167 0.003774 q: -0.154393 0.781399 0.156733 0.583964 
time: 1403636888.901666, t: 0.158978 -0.127116 0.003785 q: -0.154373 0.781600 0.156424 0.583782 
time: 1403636889.001667, t: 0.158979 -0.127074 0.003853 q: -0.154691 0.781465 0.156558 0.583844 
time: 1403636889.101666, t: 0.158992 -0.127143 0.003837 q: -0.154388 0.781564 0.156286 0.583864 
wait for imu ... 
time: 1403636889.201667, t: 0.159009 -0.127159 0.003825 q: -0.154430 0.781619 0.156219 0.583796 
time: 1403636889.301666, t: 0.159079 -0.127193 0.003762 q: -0.154411 0.781628 0.156094 0.583824 
wait for imu ... 
time: 1403636889.401666, t: 0.159119 -0.127195 0.003777 q: -0.154572 0.781542 0.156418 0.583809 
time: 1403636889.501667, t: 0.159097 -0.127214 0.003781 q: -0.154227 0.781587 0.156341 0.583861 
time: 1403636889.601666, t: 0.159122 -0.127228 0.003801 q: -0.153999 0.781567 0.156205 0.583984 
time: 1403636889.701667, t: 0.158998 -0.127334 0.003822 q: -0.154093 0.781416 0.156343 0.584125 
time: 1403636889.801666, t: 0.158975 -0.127305 0.003794 q: -0.154162 0.781402 0.156448 0.584096 
time: 1403636889.901666, t: 0.158981 -0.127316 0.003807 q: -0.154045 0.781532 0.156099 0.584048 
time: 1403636890.001667, t: 0.158994 -0.127318 0.003772 q: -0.154140 0.781574 0.156326 0.583905 
time: 1403636890.101666, t: 0.159042 -0.127430 0.003822 q: -0.153897 0.781631 0.155619 0.584082 
time: 1403636890.201667, t: 0.158971 -0.127332 0.003904 q: -0.154330 0.781234 0.156148 0.584358 
time: 1403636890.301666, t: 0.158944 -0.127232 0.003840 q: -0.154355 0.781565 0.156556 0.583799 
time: 1403636890.401666, t: 0.158990 -0.127301 0.003809 q: -0.153870 0.781785 0.155971 0.583788 
time: 1403636890.501667, t: 0.159013 -0.127329 0.003878 q: -0.153893 0.781337 0.155784 0.584432 
time: 1403636890.601666, t: 0.159000 -0.127192 0.003905 q: -0.154272 0.781211 0.156611 0.584280 
time: 1403636890.701667, t: 0.158974 -0.127216 0.003782 q: -0.154214 0.781661 0.156592 0.583697 
time: 1403636890.801666, t: 0.159028 -0.127351 0.003840 q: -0.153942 0.781652 0.155835 0.583984 
time: 1403636890.901666, t: 0.159037 -0.127345 0.003929 q: -0.154081 0.781445 0.155946 0.584195 
time: 1403636891.001667, t: 0.159013 -0.127324 0.003854 q: -0.154241 0.781495 0.155993 0.584074 
time: 1403636891.101666, t: 0.159072 -0.127280 0.003842 q: -0.154079 0.781527 0.156032 0.584062 
time: 1403636891.201667, t: 0.159075 -0.127268 0.003866 q: -0.154045 0.781490 0.156108 0.584101 
time: 1403636891.301666, t: 0.159053 -0.127307 0.003824 q: -0.154069 0.781505 0.156160 0.584061 
time: 1403636891.401666, t: 0.159083 -0.127265 0.003830 q: -0.154022 0.781465 0.156135 0.584134 
time: 1403636891.501667, t: 0.159107 -0.127256 0.003846 q: -0.154082 0.781484 0.156165 0.584084 
time: 1403636891.601666, t: 0.159055 -0.127309 0.003843 q: -0.154065 0.781533 0.156152 0.584026 
time: 1403636891.701667, t: 0.159073 -0.127287 0.003833 q: -0.154020 0.781522 0.156084 0.584070 
time: 1403636891.801666, t: 0.159043 -0.127279 0.003832 q: -0.154053 0.781517 0.156092 0.584067 
time: 1403636891.901666, t: 0.159034 -0.127304 0.003819 q: -0.154084 0.781508 0.156113 0.584065 
time: 1403636892.001667, t: 0.159074 -0.127280 0.003825 q: -0.153991 0.781517 0.156102 0.584080 
time: 1403636892.101666, t: 0.159095 -0.127289 0.003833 q: -0.154016 0.781517 0.156135 0.584065 
time: 1403636892.201667, t: 0.159052 -0.127328 0.003815 q: -0.153969 0.781519 0.156135 0.584075 
time: 1403636892.301666, t: 0.159127 -0.127389 0.003808 q: -0.153649 0.781540 0.156252 0.584099 
time: 1403636892.401666, t: 0.159105 -0.127364 0.003831 q: -0.153817 0.781544 0.155945 0.584133 
time: 1403636892.501667, t: 0.159079 -0.127327 0.003847 q: -0.153841 0.781478 0.156024 0.584194 
time: 1403636892.601666, t: 0.159024 -0.127352 0.003841 q: -0.153867 0.781477 0.156055 0.584180 
time: 1403636892.701667, t: 0.159065 -0.127313 0.003838 q: -0.153840 0.781473 0.156095 0.584181 
time: 1403636892.801666, t: 0.159067 -0.127305 0.003863 q: -0.153828 0.781450 0.156210 0.584184 
time: 1403636892.901666, t: 0.159058 -0.127341 0.003822 q: -0.153813 0.781562 0.156146 0.584056 
time: 1403636893.001667, t: 0.159095 -0.127339 0.003832 q: -0.153756 0.781482 0.156184 0.584168 
time: 1403636893.101666, t: 0.159107 -0.127315 0.003827 q: -0.153763 0.781467 0.156166 0.584191 
wait for imu ... 
time: 1403636893.201667, t: 0.159042 -0.127345 0.003801 q: -0.153801 0.781482 0.156116 0.584174 
time: 1403636893.301666, t: 0.159063 -0.127327 0.003798 q: -0.153825 0.781487 0.156103 0.584163 
time: 1403636893.401666, t: 0.159069 -0.127327 0.003811 q: -0.153806 0.781524 0.156116 0.584116 
time: 1403636893.501667, t: 0.159077 -0.127346 0.003802 q: -0.153781 0.781518 0.156108 0.584133 
time: 1403636893.601666, t: 0.159093 -0.127334 0.003810 q: -0.153720 0.781524 0.156099 0.584143 
time: 1403636893.701667, t: 0.159090 -0.127341 0.003808 q: -0.153732 0.781508 0.156182 0.584140 
wait for imu ... 
time: 1403636893.801666, t: 0.159081 -0.127321 0.003814 q: -0.153751 0.781557 0.156130 0.584082 
time: 1403636893.901666, t: 0.159067 -0.127329 0.003808 q: -0.153792 0.781566 0.156092 0.584070 
wait for imu ... 
time: 1403636894.001667, t: 0.159068 -0.127322 0.003812 q: -0.153811 0.781583 0.156045 0.584055 
time: 1403636894.101666, t: 0.159045 -0.127335 0.003811 q: -0.153761 0.781604 0.156029 0.584045 
wait for imu ... 
time: 1403636894.201667, t: 0.159064 -0.127349 0.003785 q: -0.153786 0.781566 0.156041 0.584085 
time: 1403636894.301666, t: 0.159049 -0.127335 0.003785 q: -0.153773 0.781533 0.156014 0.584140 
time: 1403636894.401666, t: 0.159052 -0.127354 0.003787 q: -0.153751 0.781588 0.156038 0.584066 
time: 1403636894.501667, t: 0.159077 -0.127332 0.003793 q: -0.153807 0.781568 0.156061 0.584072 
time: 1403636894.601666, t: 0.159062 -0.127324 0.003791 q: -0.153743 0.781589 0.155990 0.584080 
time: 1403636894.701667, t: 0.159038 -0.127371 0.003757 q: -0.153836 0.781526 0.155997 0.584138 
time: 1403636894.801666, t: 0.158731 -0.127031 0.003720 q: -0.154788 0.780322 0.157149 0.585186 
time: 1403636894.901666, t: 0.158778 -0.127026 0.003751 q: -0.154763 0.780251 0.157204 0.585272 
wait for imu ... 
time: 1403636895.001667, t: 0.158779 -0.127024 0.003732 q: -0.154916 0.780237 0.157312 0.585222 
wait for imu ... 
time: 1403636895.101666, t: 0.158800 -0.126993 0.003739 q: -0.154987 0.780246 0.157321 0.585188 
time: 1403636895.201667, t: 0.158757 -0.127015 0.003732 q: -0.154988 0.780199 0.157238 0.585272 
time: 1403636895.301666, t: 0.158771 -0.126967 0.003754 q: -0.155032 0.780187 0.157251 0.585273 
time: 1403636895.401666, t: 0.158743 -0.126977 0.003759 q: -0.154969 0.780217 0.157298 0.585237 
time: 1403636895.501667, t: 0.158756 -0.126972 0.003761 q: -0.154998 0.780169 0.157341 0.585282 
wait for imu ... 
time: 1403636895.601666, t: 0.158754 -0.126989 0.003776 q: -0.154998 0.780230 0.157317 0.585208 
time: 1403636895.701667, t: 0.158770 -0.126990 0.003768 q: -0.154978 0.780165 0.157363 0.585288 
wait for imu ... 
time: 1403636895.801666, t: 0.158762 -0.127001 0.003793 q: -0.154884 0.780107 0.157404 0.585378 
time: 1403636895.901666, t: 0.158806 -0.126991 0.003798 q: -0.154936 0.780132 0.157509 0.585304 
time: 1403636896.001667, t: 0.158761 -0.126984 0.003821 q: -0.154901 0.780162 0.157560 0.585259 
time: 1403636896.101666, t: 0.158956 -0.127031 0.003750 q: -0.155003 0.780599 0.157600 0.584638 
time: 1403636896.201667, t: 0.159201 -0.127100 0.003581 q: -0.155098 0.781267 0.157577 0.583726 
time: 1403636896.301666, t: 0.159171 -0.127131 0.003627 q: -0.155018 0.781148 0.157766 0.583856 
time: 1403636896.401666, t: 0.159028 -0.127088 0.003705 q: -0.154974 0.780770 0.157814 0.584359 
time: 1403636896.501667, t: 0.158988 -0.127061 0.003772 q: -0.154973 0.780581 0.157832 0.584607 
time: 1403636896.601666, t: 0.158962 -0.127099 0.003775 q: -0.154890 0.780597 0.157851 0.584603 
wait for imu ... 
time: 1403636896.701667, t: 0.159109 -0.127151 0.003744 q: -0.154960 0.780873 0.157970 0.584183 
time: 1403636896.801666, t: 0.159182 -0.127159 0.003687 q: -0.155096 0.781105 0.158124 0.583795 
time: 1403636896.901666, t: 0.159204 -0.127216 0.003768 q: -0.154914 0.781110 0.158377 0.583768 
time: 1403636897.001667, t: 0.158992 -0.127142 0.003916 q: -0.155183 0.780568 0.158392 0.584417 
wait for imu ... 
time: 1403636897.101666, t: 0.158432 -0.127308 0.004245 q: -0.156023 0.779947 0.160518 0.584443 
time: 1403636897.201667, t: 0.157225 -0.127691 0.004035 q: -0.157706 0.779767 0.163308 0.583458 
time: 1403636897.301666, t: 0.155166 -0.130808 0.003506 q: -0.156898 0.780752 0.162243 0.582656 
time: 1403636897.401666, t: 0.150256 -0.138810 0.003071 q: -0.151701 0.784411 0.159603 0.579839 
wait for imu ... 
wait for imu ... 
time: 1403636897.501667, t: 0.142545 -0.150485 0.002505 q: -0.143192 0.789834 0.157746 0.575130 
time: 1403636897.601666, t: 0.132560 -0.164589 0.002500 q: -0.133719 0.797907 0.157206 0.566347 
wait for imu ... 
time: 1403636897.701667, t: 0.120315 -0.180879 0.007490 q: -0.120835 0.807978 0.161010 0.553756 
time: 1403636897.801666, t: 0.108829 -0.197072 0.019608 q: -0.112230 0.815711 0.165986 0.542649 
wait for imu ... 
time: 1403636897.901666, t: 0.100525 -0.211054 0.038364 q: -0.108603 0.820706 0.169493 0.534714 
time: 1403636898.001667, t: 0.098499 -0.220953 0.061043 q: -0.110354 0.815359 0.171098 0.541975 
wait for imu ... 
time: 1403636898.101666, t: 0.099421 -0.227434 0.076269 q: -0.115334 0.809225 0.171370 0.549987 
time: 1403636898.201667, t: 0.101810 -0.232929 0.088566 q: -0.118656 0.806052 0.172503 0.553574 
wait for imu ... 
time: 1403636898.301666, t: 0.107873 -0.235168 0.102405 q: -0.120296 0.805106 0.174800 0.553876 
time: 1403636898.401666, t: 0.114899 -0.235112 0.119389 q: -0.122302 0.804599 0.177517 0.553309 
time: 1403636898.501667, t: 0.122089 -0.234201 0.139488 q: -0.125639 0.804632 0.180075 0.551684 
wait for imu ... 
time: 1403636898.601666, t: 0.130707 -0.232255 0.160480 q: -0.127128 0.804337 0.182763 0.550888 
time: 1403636898.701667, t: 0.140280 -0.229003 0.180201 q: -0.127487 0.804305 0.185333 0.549993 
time: 1403636898.801666, t: 0.151587 -0.223829 0.198088 q: -0.129867 0.804156 0.186082 0.549400 
time: 1403636898.901666, t: 0.164971 -0.217136 0.213581 q: -0.133206 0.805214 0.185244 0.547331 
time: 1403636899.001667, t: 0.180764 -0.210121 0.226617 q: -0.139209 0.806552 0.181207 0.545215 
time: 1403636899.101666, t: 0.200690 -0.204051 0.237593 q: -0.140107 0.806993 0.172746 0.547075 
wait for imu ... 
time: 1403636899.201667, t: 0.223618 -0.201146 0.248808 q: -0.132145 0.805751 0.162165 0.554081 
time: 1403636899.301666, t: 0.247552 -0.201055 0.259486 q: -0.122236 0.802582 0.145096 0.565569 
time: 1403636899.401666, t: 0.270997 -0.204436 0.268756 q: -0.106370 0.801006 0.125165 0.575682 
wait for imu ... 
time: 1403636899.501667, t: 0.291506 -0.210139 0.275597 q: -0.095833 0.804822 0.103502 0.576511 
wait for imu ... 
wait for imu ... 
time: 1403636899.601666, t: 0.310391 -0.217495 0.280104 q: -0.089620 0.811992 0.086750 0.570185 
time: 1403636899.701667, t: 0.329679 -0.227542 0.285889 q: -0.084588 0.816111 0.075238 0.566698 
time: 1403636899.801666, t: 0.350343 -0.240667 0.290992 q: -0.077557 0.819038 0.069052 0.564264 
wait for imu ... 
time: 1403636899.901666, t: 0.373244 -0.256057 0.294181 q: -0.067977 0.819128 0.068734 0.565406 
wait for imu ... 
time: 1403636900.001667, t: 0.397717 -0.272277 0.294246 q: -0.061343 0.815525 0.069287 0.571275 
time: 1403636900.101666, t: 0.422661 -0.289052 0.293668 q: -0.059437 0.810336 0.069241 0.578817 
time: 1403636900.201667, t: 0.444977 -0.306327 0.300615 q: -0.058360 0.806449 0.070348 0.584197 
wait for imu ... 
time: 1403636900.301666, t: 0.463285 -0.324492 0.316636 q: -0.057564 0.807027 0.072637 0.583197 
time: 1403636900.401666, t: 0.479397 -0.342675 0.340760 q: -0.059272 0.811662 0.074809 0.576278 
time: 1403636900.501667, t: 0.497794 -0.359900 0.364676 q: -0.060419 0.815307 0.078488 0.570495 
wait for imu ... 
time: 1403636900.601666, t: 0.513584 -0.376961 0.384983 q: -0.062375 0.817169 0.080538 0.567325 
time: 1403636900.701667, t: 0.530005 -0.394351 0.399430 q: -0.064143 0.821739 0.082092 0.560261 
time: 1403636900.801666, t: 0.548774 -0.411504 0.407325 q: -0.067732 0.825607 0.082538 0.554052 
time: 1403636900.901666, t: 0.570085 -0.429737 0.409604 q: -0.075936 0.824063 0.078700 0.555843 
time: 1403636901.001667, t: 0.595448 -0.449027 0.409877 q: -0.081112 0.821191 0.075604 0.559776 
time: 1403636901.101666, t: 0.624685 -0.471968 0.405825 q: -0.085258 0.819792 0.073154 0.561535 
time: 1403636901.201667, t: 0.656304 -0.500040 0.399757 q: -0.086360 0.818842 0.072272 0.562865 
time: 1403636901.301666, t: 0.690098 -0.528652 0.395037 q: -0.082386 0.818000 0.074827 0.564349 
wait for imu ... 
time: 1403636901.401666, t: 0.726352 -0.564488 0.398421 q: -0.078804 0.817509 0.077240 0.565246 
time: 1403636901.501667, t: 0.763825 -0.602527 0.409565 q: -0.073592 0.816648 0.080643 0.566716 
time: 1403636901.601666, t: 0.802416 -0.641019 0.419045 q: -0.070042 0.815925 0.082827 0.567891 
time: 1403636901.701667, t: 0.840777 -0.679358 0.421920 q: -0.069390 0.814021 0.082863 0.570691 
time: 1403636901.801666, t: 0.878856 -0.716196 0.417991 q: -0.070385 0.813501 0.082061 0.571427 
time: 1403636901.901666, t: 0.916298 -0.755274 0.408307 q: -0.069842 0.813375 0.082862 0.571556 
wait for imu ... 
time: 1403636902.001667, t: 0.953736 -0.797248 0.397390 q: -0.068801 0.812987 0.084382 0.572013 
wait for imu ... 
time: 1403636902.101666, t: 0.991201 -0.836061 0.386077 q: -0.068803 0.812224 0.085601 0.572914 
wait for imu ... 
time: 1403636902.201667, t: 1.032472 -0.879488 0.384802 q: -0.068682 0.812307 0.086977 0.572604 
time: 1403636902.301666, t: 1.069488 -0.916590 0.384563 q: -0.070954 0.812748 0.084564 0.572062 
time: 1403636902.401666, t: 1.106988 -0.957836 0.376552 q: -0.071616 0.813586 0.082663 0.571066 
time: 1403636902.501667, t: 1.143656 -0.996456 0.352735 q: -0.068526 0.814466 0.082298 0.570242 
time: 1403636902.601666, t: 1.179244 -1.034939 0.326741 q: -0.062177 0.814429 0.083483 0.570850 
wait for imu ... 
time: 1403636902.701667, t: 1.211253 -1.069758 0.305813 q: -0.057073 0.813466 0.083768 0.572712 
time: 1403636902.801666, t: 1.242185 -1.103297 0.291965 q: -0.056595 0.813028 0.081947 0.573644 
time: 1403636902.901666, t: 1.272922 -1.136909 0.286383 q: -0.058670 0.810835 0.081667 0.576572 
time: 1403636903.001667, t: 1.305159 -1.170271 0.286594 q: -0.060259 0.810828 0.082570 0.576289 
time: 1403636903.101666, t: 1.335215 -1.203314 0.292461 q: -0.062414 0.811261 0.083602 0.575301 
time: 1403636903.201667, t: 1.366802 -1.235644 0.309577 q: -0.064392 0.811986 0.084885 0.573870 
time: 1403636903.301666, t: 1.396650 -1.270086 0.331221 q: -0.064628 0.813344 0.087525 0.571519 
time: 1403636903.401666, t: 1.427089 -1.305480 0.350407 q: -0.062969 0.814036 0.090637 0.570233 
time: 1403636903.501667, t: 1.452487 -1.335775 0.365151 q: -0.062440 0.814195 0.092273 0.569802 
time: 1403636903.601666, t: 1.477747 -1.362587 0.374178 q: -0.062984 0.814556 0.093049 0.569099 
time: 1403636903.701667, t: 1.502262 -1.389455 0.372264 q: -0.066407 0.817660 0.091449 0.564499 
time: 1403636903.801666, t: 1.526805 -1.415849 0.369421 q: -0.070480 0.820232 0.089266 0.560611 
time: 1403636903.901666, t: 1.557169 -1.441919 0.371557 q: -0.074750 0.820623 0.086464 0.559923 
time: 1403636904.001667, t: 1.592334 -1.468803 0.381815 q: -0.078451 0.818956 0.083989 0.562229 
time: 1403636904.101666, t: 1.625793 -1.497576 0.399125 q: -0.081091 0.817027 0.082073 0.564938 
time: 1403636904.201667, t: 1.660847 -1.528220 0.422571 q: -0.083869 0.816625 0.079842 0.565433 
wait for imu ... 
time: 1403636904.301666, t: 1.694357 -1.562788 0.446609 q: -0.083622 0.815825 0.078987 0.566743 
wait for imu ... 
time: 1403636904.401666, t: 1.730862 -1.597813 0.469677 q: -0.081524 0.814720 0.078883 0.568650 
time: 1403636904.501667, t: 1.764830 -1.635352 0.489004 q: -0.078352 0.814359 0.079820 0.569481 
wait for imu ... 
time: 1403636904.601666, t: 1.801888 -1.674267 0.502714 q: -0.074091 0.814219 0.081102 0.570070 
time: 1403636904.701667, t: 1.841936 -1.714483 0.514373 q: -0.069469 0.814577 0.081980 0.570015 
time: 1403636904.801666, t: 1.880220 -1.756574 0.523802 q: -0.065706 0.814436 0.082524 0.570585 
time: 1403636904.901666, t: 1.921276 -1.798492 0.532103 q: -0.061244 0.814885 0.083504 0.570296 
time: 1403636905.001667, t: 1.960649 -1.840353 0.534478 q: -0.058015 0.814893 0.083400 0.570638 
wait for imu ... 
time: 1403636905.101666, t: 2.001672 -1.878908 0.533855 q: -0.055182 0.814480 0.083034 0.571562 
wait for imu ... 
time: 1403636905.201667, t: 2.038625 -1.916859 0.535596 q: -0.052841 0.814056 0.082252 0.572499 
wait for imu ... 
time: 1403636905.301666, t: 2.080107 -1.952220 0.544292 q: -0.049762 0.815114 0.082095 0.571291 
time: 1403636905.401666, t: 2.115477 -1.982869 0.556492 q: -0.047985 0.817822 0.081634 0.567627 
wait for imu ... 
time: 1403636905.501667, t: 2.152508 -2.011824 0.576256 q: -0.047893 0.818170 0.087014 0.566333 
wait for imu ... 
time: 1403636905.601666, t: 2.189429 -2.035903 0.592902 q: -0.047895 0.817629 0.093641 0.566056 
time: 1403636905.701667, t: 2.226771 -2.057543 0.602256 q: -0.052556 0.817919 0.096194 0.564795 
wait for imu ... 
time: 1403636905.801666, t: 2.263995 -2.079897 0.605167 q: -0.058140 0.817836 0.097125 0.564209 
time: 1403636905.901666, t: 2.300326 -2.097465 0.600902 q: -0.061594 0.818568 0.098347 0.562566 
time: 1403636906.001667, t: 2.340038 -2.111858 0.590900 q: -0.063518 0.820759 0.099970 0.558862 
time: 1403636906.101666, t: 2.383701 -2.125974 0.579330 q: -0.062217 0.822445 0.103312 0.555914 
time: 1403636906.201667, t: 2.426828 -2.134672 0.569858 q: -0.063120 0.821437 0.104522 0.557075 
wait for imu ... 
time: 1403636906.301666, t: 2.472772 -2.139961 0.568627 q: -0.066794 0.816041 0.103592 0.564698 
time: 1403636906.401666, t: 2.520078 -2.145177 0.574468 q: -0.072956 0.812413 0.099875 0.569814 
wait for imu ... 
time: 1403636906.501667, t: 2.565554 -2.150552 0.583487 q: -0.084978 0.812152 0.091322 0.569955 
time: 1403636906.601666, t: 2.609294 -2.157280 0.589775 q: -0.090201 0.815321 0.087433 0.565218 
time: 1403636906.701667, t: 2.652660 -2.167090 0.591534 q: -0.085146 0.815312 0.090740 0.565494 
time: 1403636906.801666, t: 2.697530 -2.179226 0.588281 q: -0.076901 0.814559 0.096423 0.566818 
wait for imu ... 
time: 1403636906.901666, t: 2.742774 -2.191516 0.581156 q: -0.072289 0.814667 0.100868 0.566495 
wait for imu ... 
time: 1403636907.001667, t: 2.790878 -2.201962 0.577663 q: -0.070135 0.814443 0.104382 0.566452 
time: 1403636907.101666, t: 2.836338 -2.211770 0.580652 q: -0.070431 0.814915 0.106087 0.565419 
time: 1403636907.201667, t: 2.886825 -2.217729 0.582974 q: -0.069647 0.817765 0.108055 0.561012 
time: 1403636907.301666, t: 2.933639 -2.224108 0.577526 q: -0.071296 0.820947 0.108580 0.556034 
time: 1403636907.401666, t: 2.983475 -2.228492 0.572012 q: -0.076979 0.822290 0.106037 0.553778 
time: 1403636907.501667, t: 3.039499 -2.229202 0.572803 q: -0.082794 0.822818 0.102955 0.552735 
time: 1403636907.601666, t: 3.093113 -2.233774 0.582494 q: -0.087864 0.823152 0.099921 0.552011 
time: 1403636907.701667, t: 3.156823 -2.238317 0.598656 q: -0.090807 0.823154 0.097699 0.551930 
time: 1403636907.901666, t: 3.290449 -2.259044 0.634922 q: -0.093099 0.821646 0.096350 0.554029 
time: 1403636908.001667, t: 3.362678 -2.271160 0.638832 q: -0.087658 0.817706 0.100994 0.559887 
time: 1403636908.101666, t: 3.440215 -2.285259 0.635709 q: -0.082582 0.813946 0.106743 0.565047 
time: 1403636908.201667, t: 3.514901 -2.299793 0.636567 q: -0.081816 0.811251 0.110113 0.568378 
time: 1403636908.301666, t: 3.588185 -2.314836 0.640356 q: -0.082174 0.810418 0.112833 0.568981 
time: 1403636908.401666, t: 3.656150 -2.329000 0.640338 q: -0.083173 0.810808 0.115044 0.567836 
time: 1403636908.501667, t: 3.724375 -2.343756 0.634719 q: -0.084719 0.811657 0.116960 0.566000 
time: 1403636908.601666, t: 3.795306 -2.356022 0.627337 q: -0.085943 0.812009 0.118866 0.564912 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636908.701667, t: 3.860526 -2.370755 0.623538 q: -0.086866 0.813114 0.120970 0.562732 
wait for imu ... 
wait for imu ... 
time: 1403636908.801666, t: 3.929283 -2.381719 0.628211 q: -0.089418 0.815830 0.121006 0.558376 
wait for imu ... 
time: 1403636908.901666, t: 4.002125 -2.394987 0.644508 q: -0.090805 0.818805 0.121004 0.553779 
time: 1403636909.001667, t: 4.072541 -2.404093 0.664117 q: -0.092736 0.819456 0.120728 0.552555 
time: 1403636909.101666, t: 4.151331 -2.413983 0.687107 q: -0.095182 0.820138 0.118855 0.551532 
time: 1403636909.201667, t: 4.230472 -2.421263 0.703024 q: -0.095578 0.820414 0.117864 0.551265 
wait for imu ... 
time: 1403636909.301666, t: 4.314006 -2.430420 0.712574 q: -0.095400 0.820980 0.116811 0.550678 
time: 1403636909.401666, t: 4.397471 -2.442137 0.718272 q: -0.093987 0.819643 0.116673 0.552937 
wait for imu ... 
time: 1403636909.501667, t: 4.484307 -2.455434 0.721408 q: -0.090887 0.816491 0.117285 0.557967 
wait for imu ... 
time: 1403636909.601666, t: 4.574357 -2.467670 0.717535 q: -0.091404 0.815492 0.115090 0.559797 
wait for imu ... 
time: 1403636909.701667, t: 4.663233 -2.480800 0.706443 q: -0.094769 0.813589 0.111017 0.562821 
time: 1403636909.801666, t: 4.750893 -2.494974 0.691557 q: -0.090271 0.808638 0.112865 0.570277 
time: 1403636909.901666, t: 4.836647 -2.510540 0.670297 q: -0.077122 0.804369 0.120977 0.576548 
time: 1403636910.001667, t: 4.919968 -2.524968 0.645278 q: -0.065635 0.798975 0.128264 0.583849 
time: 1403636910.101666, t: 4.997107 -2.534540 0.618178 q: -0.067009 0.798108 0.127631 0.585016 
time: 1403636910.201667, t: 5.070825 -2.539665 0.594805 q: -0.075307 0.803463 0.122890 0.577645 
time: 1403636910.301666, t: 5.140008 -2.542169 0.581224 q: -0.080459 0.807573 0.120373 0.571719 
wait for imu ... 
time: 1403636910.401666, t: 5.207857 -2.543385 0.573986 q: -0.083073 0.810290 0.119849 0.567596 
time: 1403636910.501667, t: 5.272616 -2.544523 0.575769 q: -0.084199 0.810822 0.121097 0.566404 
time: 1403636910.601666, t: 5.338613 -2.546547 0.575636 q: -0.083420 0.811000 0.123064 0.565841 
time: 1403636910.701667, t: 5.403515 -2.546602 0.570450 q: -0.083742 0.811030 0.123781 0.565593 
wait for imu ... 
time: 1403636910.801666, t: 5.472001 -2.544798 0.557201 q: -0.085139 0.811527 0.123101 0.564820 
time: 1403636910.901666, t: 5.535409 -2.540203 0.542604 q: -0.088018 0.812292 0.121399 0.563646 
wait for imu ... 
time: 1403636911.001667, t: 5.604333 -2.536630 0.525453 q: -0.094048 0.806487 0.117868 0.571700 
time: 1403636911.101666, t: 5.671715 -2.532782 0.513302 q: -0.096757 0.799201 0.116197 0.581734 
time: 1403636911.201667, t: 5.733299 -2.531821 0.504985 q: -0.093912 0.802666 0.118096 0.577028 
time: 1403636911.301666, t: 5.792795 -2.533361 0.503658 q: -0.090441 0.808111 0.120394 0.569458 
time: 1403636911.401666, t: 5.843709 -2.536847 0.511687 q: -0.087569 0.809498 0.122184 0.567552 
time: 1403636911.501667, t: 5.890382 -2.538796 0.527508 q: -0.085678 0.810094 0.122575 0.566905 
time: 1403636911.601666, t: 5.937762 -2.538363 0.540727 q: -0.084959 0.810548 0.122728 0.566331 
wait for imu ... 
time: 1403636911.701667, t: 5.989088 -2.541438 0.549365 q: -0.084850 0.810531 0.122674 0.566385 
time: 1403636911.801666, t: 6.041148 -2.540410 0.552567 q: -0.088761 0.811177 0.118956 0.565652 
wait for imu ... 
time: 1403636911.901666, t: 6.087471 -2.541296 0.549591 q: -0.095979 0.813712 0.113134 0.562016 
time: 1403636912.001667, t: 6.148497 -2.544559 0.541769 q: -0.098512 0.815670 0.110365 0.559282 
time: 1403636912.101666, t: 6.202052 -2.548036 0.530402 q: -0.095885 0.815760 0.110873 0.559508 
time: 1403636912.201667, t: 6.273255 -2.552345 0.523283 q: -0.092905 0.814379 0.112675 0.561658 
time: 1403636912.301666, t: 6.327061 -2.560022 0.522741 q: -0.089048 0.812553 0.114857 0.564478 
wait for imu ... 
time: 1403636912.401666, t: 6.389840 -2.566573 0.527949 q: -0.088037 0.812672 0.114593 0.564519 
time: 1403636912.501667, t: 6.446739 -2.575055 0.531977 q: -0.093274 0.813126 0.110310 0.563877 
time: 1403636912.601666, t: 6.509339 -2.584254 0.529013 q: -0.093959 0.813571 0.108443 0.563484 
time: 1403636912.701667, t: 6.566881 -2.595742 0.520310 q: -0.098599 0.813904 0.104069 0.563035 
time: 1403636912.801666, t: 6.626371 -2.609687 0.505019 q: -0.102995 0.813766 0.100396 0.563114 
time: 1403636912.901666, t: 6.683203 -2.629261 0.489147 q: -0.099753 0.812878 0.102056 0.564679 
time: 1403636913.001667, t: 6.739822 -2.648957 0.466829 q: -0.093874 0.808997 0.105866 0.570530 
time: 1403636913.101666, t: 6.796283 -2.673524 0.444273 q: -0.089828 0.805854 0.108665 0.575084 
time: 1403636913.201667, t: 6.846557 -2.695156 0.420900 q: -0.086905 0.803276 0.110681 0.578744 
time: 1403636913.301666, t: 6.901226 -2.719851 0.409757 q: -0.085726 0.803833 0.111799 0.577932 
time: 1403636913.401666, t: 6.946349 -2.743255 0.402991 q: -0.089448 0.807571 0.109482 0.572575 
wait for imu ... 
time: 1403636913.501667, t: 6.996198 -2.770336 0.405153 q: -0.093452 0.810249 0.106482 0.568705 
wait for imu ... 
time: 1403636913.601666, t: 7.043505 -2.800840 0.402438 q: -0.090184 0.810638 0.108214 0.568351 
time: 1403636913.701667, t: 7.091091 -2.830799 0.395987 q: -0.087134 0.810902 0.109400 0.568222 
wait for imu ... 
time: 1403636913.801666, t: 7.144129 -2.862535 0.383397 q: -0.085851 0.810461 0.110246 0.568884 
time: 1403636913.901666, t: 7.193639 -2.891296 0.375478 q: -0.084506 0.809790 0.111002 0.569892 
time: 1403636914.001667, t: 7.246013 -2.923438 0.372069 q: -0.083817 0.809319 0.112073 0.570454 
time: 1403636914.101666, t: 7.295177 -2.955465 0.378148 q: -0.086836 0.809265 0.110241 0.570436 
time: 1403636914.201667, t: 7.347533 -2.990277 0.380139 q: -0.088864 0.809723 0.108750 0.569759 
time: 1403636914.301666, t: 7.394218 -3.022255 0.380986 q: -0.087096 0.810366 0.109411 0.568990 
time: 1403636914.401666, t: 7.443580 -3.058315 0.376311 q: -0.083503 0.809408 0.111561 0.570474 
time: 1403636914.501667, t: 7.488091 -3.089854 0.376162 q: -0.080718 0.809199 0.113133 0.570861 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636914.601666, t: 7.536071 -3.123344 0.381821 q: -0.079815 0.809038 0.113523 0.571139 
time: 1403636914.701667, t: 7.578671 -3.154503 0.396557 q: -0.079638 0.807932 0.113241 0.572783 
time: 1403636914.801666, t: 7.619370 -3.185979 0.413297 q: -0.078465 0.807337 0.112998 0.573831 
wait for imu ... 
time: 1403636914.901666, t: 7.657994 -3.215344 0.424838 q: -0.077395 0.808366 0.112509 0.572622 
time: 1403636915.001667, t: 7.689008 -3.245880 0.428697 q: -0.076453 0.810482 0.111952 0.569860 
time: 1403636915.101666, t: 7.726154 -3.274023 0.427816 q: -0.075261 0.812116 0.111955 0.567687 
wait for imu ... 
time: 1403636915.201667, t: 7.766445 -3.303983 0.420352 q: -0.074076 0.814542 0.111997 0.564349 
time: 1403636915.301666, t: 7.802331 -3.327642 0.408131 q: -0.072602 0.819365 0.112229 0.557472 
time: 1403636915.401666, t: 7.846981 -3.352127 0.394102 q: -0.071648 0.823008 0.112976 0.552052 
wait for imu ... 
time: 1403636915.501667, t: 7.892579 -3.373932 0.382268 q: -0.075197 0.821291 0.110621 0.554607 
time: 1403636915.601666, t: 7.938122 -3.392463 0.376828 q: -0.084725 0.816046 0.104349 0.562141 
time: 1403636915.701667, t: 7.985345 -3.412390 0.378945 q: -0.093225 0.812457 0.099144 0.566915 
time: 1403636915.801666, t: 8.032028 -3.437134 0.389839 q: -0.095166 0.810797 0.098554 0.569069 
wait for imu ... 
time: 1403636915.901666, t: 8.079076 -3.464290 0.409349 q: -0.095564 0.809831 0.099177 0.570268 
time: 1403636916.001667, t: 8.125029 -3.492493 0.429262 q: -0.089545 0.808893 0.103894 0.571734 
wait for imu ... 
time: 1403636916.101666, t: 8.169517 -3.523987 0.444025 q: -0.081153 0.808147 0.110320 0.572837 
wait for imu ... 
time: 1403636916.201667, t: 8.213954 -3.556862 0.452556 q: -0.077076 0.808233 0.114094 0.572539 
wait for imu ... 
time: 1403636916.301666, t: 8.256510 -3.587759 0.462835 q: -0.078758 0.808434 0.113387 0.572166 
time: 1403636916.401666, t: 8.294826 -3.616846 0.478553 q: -0.077730 0.809094 0.113877 0.571277 
time: 1403636916.501667, t: 8.335461 -3.646010 0.492787 q: -0.070899 0.809259 0.109569 0.572773 
time: 1403636916.601666, t: 8.374635 -3.674426 0.502908 q: -0.060723 0.810384 0.099814 0.574132 
wait for imu ... 
time: 1403636916.701667, t: 8.413340 -3.699199 0.506548 q: -0.052129 0.811605 0.089408 0.574966 
time: 1403636916.801666, t: 8.450026 -3.720861 0.505579 q: -0.046996 0.812085 0.080254 0.576080 
time: 1403636916.901666, t: 8.483428 -3.738824 0.500041 q: -0.041402 0.812632 0.074637 0.576494 
time: 1403636917.001667, t: 8.517992 -3.753385 0.489117 q: -0.038249 0.812641 0.070597 0.577206 
time: 1403636917.101666, t: 8.550505 -3.766526 0.472463 q: -0.038970 0.813197 0.066065 0.576912 
time: 1403636917.201667, t: 8.581033 -3.779526 0.452785 q: -0.040601 0.813619 0.060447 0.576821 
time: 1403636917.301666, t: 8.610570 -3.791956 0.425925 q: -0.035008 0.813580 0.051113 0.578143 
time: 1403636917.401666, t: 8.634824 -3.806409 0.395750 q: -0.022171 0.813663 0.037301 0.579716 
time: 1403636917.501667, t: 8.662966 -3.819288 0.360520 q: -0.009906 0.814130 0.020528 0.580235 
wait for imu ... 
time: 1403636917.601666, t: 8.689041 -3.830726 0.329575 q: -0.002553 0.813900 0.006207 0.580966 
wait for imu ... 
time: 1403636917.701667, t: 8.711852 -3.840359 0.306197 q: 0.003496 0.813124 -0.004160 0.582066 
time: 1403636917.801666, t: 8.731146 -3.849132 0.290494 q: 0.010032 0.812512 -0.012261 0.582729 
time: 1403636917.901666, t: 8.749358 -3.855491 0.284356 q: 0.016025 0.811748 -0.018060 0.583509 
time: 1403636918.001667, t: 8.763024 -3.859247 0.282653 q: 0.020105 0.812325 -0.021909 0.582446 
wait for imu ... 
time: 1403636918.101666, t: 8.774962 -3.861850 0.276040 q: 0.024043 0.813303 -0.023257 0.580878 
time: 1403636918.201667, t: 8.786509 -3.862667 0.265641 q: 0.027572 0.814654 -0.022510 0.578854 
time: 1403636918.301666, t: 8.796221 -3.861601 0.258613 q: 0.029909 0.815487 -0.021081 0.577618 
time: 1403636918.401666, t: 8.805390 -3.857688 0.257535 q: 0.030122 0.821494 -0.020116 0.569066 
time: 1403636918.501667, t: 8.816336 -3.850287 0.263790 q: 0.030127 0.827407 -0.018980 0.560473 
wait for imu ... 
time: 1403636918.601666, t: 8.830689 -3.839847 0.277175 q: 0.029706 0.829188 -0.017337 0.557911 
time: 1403636918.701667, t: 8.850886 -3.826478 0.302184 q: 0.026788 0.827768 -0.016357 0.560191 
time: 1403636918.801666, t: 8.874970 -3.810442 0.332843 q: 0.013845 0.824750 -0.022729 0.564871 
time: 1403636918.901666, t: 8.900547 -3.794665 0.370237 q: 0.001159 0.818872 -0.031521 0.573109 
wait for imu ... 
time: 1403636919.001667, t: 8.925890 -3.782917 0.409320 q: -0.004246 0.814025 -0.043079 0.579214 
time: 1403636919.101666, t: 8.952889 -3.776400 0.451473 q: -0.005469 0.811210 -0.051769 0.582433 
time: 1403636919.201667, t: 8.977252 -3.776582 0.495410 q: -0.002784 0.809500 -0.056945 0.584345 
wait for imu ... 
time: 1403636919.301666, t: 8.998627 -3.783315 0.539367 q: 0.001457 0.808902 -0.059988 0.584873 
wait for imu ... 
time: 1403636919.401666, t: 9.016270 -3.796044 0.578183 q: 0.008349 0.808263 -0.062547 0.585430 
time: 1403636919.501667, t: 9.030322 -3.814271 0.611298 q: 0.020356 0.808099 -0.059814 0.585648 
wait for imu ... 
time: 1403636919.601666, t: 9.040737 -3.834834 0.636783 q: 0.032302 0.808033 -0.055745 0.585604 
time: 1403636919.701667, t: 9.046730 -3.855291 0.654340 q: 0.038802 0.807040 -0.053847 0.586754 
time: 1403636919.801666, t: 9.049278 -3.875371 0.664525 q: 0.038816 0.806130 -0.055212 0.587877 
time: 1403636919.901666, t: 9.050795 -3.893368 0.669958 q: 0.037175 0.805542 -0.056847 0.588632 
time: 1403636920.001667, t: 9.048790 -3.911579 0.667118 q: 0.032624 0.804215 -0.059588 0.590443 
time: 1403636920.101666, t: 9.043827 -3.931074 0.659631 q: 0.026604 0.802835 -0.062748 0.592292 
time: 1403636920.201667, t: 9.034091 -3.952330 0.647505 q: 0.021189 0.802024 -0.065200 0.593344 
time: 1403636920.301666, t: 9.019252 -3.976008 0.632679 q: 0.026522 0.802403 -0.060429 0.593123 
wait for imu ... 
time: 1403636920.401666, t: 9.000284 -4.002958 0.614638 q: 0.035041 0.802802 -0.053627 0.592795 
time: 1403636920.501667, t: 8.977662 -4.028462 0.599600 q: 0.040093 0.805450 -0.049168 0.589258 
wait for imu ... 
time: 1403636920.601666, t: 8.949549 -4.054516 0.592385 q: 0.041166 0.809219 -0.047744 0.584115 
time: 1403636920.701667, t: 8.920292 -4.076288 0.593240 q: 0.041254 0.813697 -0.047984 0.577834 
wait for imu ... 
time: 1403636920.801666, t: 8.892540 -4.098636 0.600668 q: 0.040802 0.817594 -0.048693 0.572281 
time: 1403636920.901666, t: 8.864138 -4.115830 0.616552 q: 0.039860 0.819657 -0.049749 0.569296 
time: 1403636921.001667, t: 8.838437 -4.134191 0.635789 q: 0.040060 0.820044 -0.050145 0.568690 
time: 1403636921.101666, t: 8.814786 -4.148296 0.650950 q: 0.039967 0.819666 -0.051023 0.569163 
time: 1403636921.201667, t: 8.794299 -4.161277 0.661343 q: 0.040763 0.818209 -0.051087 0.571195 
time: 1403636921.301666, t: 8.774695 -4.172549 0.666697 q: 0.041229 0.812338 -0.050960 0.579492 
time: 1403636921.401666, t: 8.753350 -4.182875 0.665240 q: 0.036860 0.808007 -0.054347 0.585502 
time: 1403636921.501667, t: 8.732409 -4.192838 0.658148 q: 0.030077 0.806574 -0.059104 0.587401 
time: 1403636921.601666, t: 8.704838 -4.203179 0.644806 q: 0.024272 0.809617 -0.063803 0.582976 
wait for imu ... 
time: 1403636921.701667, t: 8.678988 -4.215620 0.625452 q: 0.022991 0.818814 -0.065419 0.569856 
time: 1403636921.801666, t: 8.650728 -4.229891 0.604018 q: 0.025975 0.827770 -0.064550 0.556736 
time: 1403636921.901666, t: 8.630512 -4.247395 0.584249 q: 0.030250 0.828353 -0.062900 0.555842 
time: 1403636922.001667, t: 8.613823 -4.267146 0.565600 q: 0.036108 0.822270 -0.061689 0.564590 
time: 1403636922.101666, t: 8.599647 -4.284856 0.545098 q: 0.043052 0.815989 -0.062886 0.573021 
time: 1403636922.201667, t: 8.583391 -4.302259 0.529035 q: 0.048832 0.813048 -0.065674 0.576416 
time: 1403636922.301666, t: 8.566984 -4.319030 0.519829 q: 0.054188 0.816964 -0.068410 0.570047 
time: 1403636922.401666, t: 8.553853 -4.333192 0.514882 q: 0.056946 0.820464 -0.072268 0.564245 
wait for imu ... 
time: 1403636922.501667, t: 8.540542 -4.347460 0.506020 q: 0.061130 0.823624 -0.078139 0.558391 
wait for imu ... 
time: 1403636922.601666, t: 8.530849 -4.361803 0.491469 q: 0.067307 0.824349 -0.087037 0.555286 
time: 1403636922.701667, t: 8.527302 -4.374372 0.474871 q: 0.074987 0.818564 -0.094058 0.561678 
time: 1403636922.801666, t: 8.526046 -4.383896 0.458115 q: 0.076956 0.808860 -0.102263 0.573903 
time: 1403636922.901666, t: 8.522063 -4.390469 0.445209 q: 0.072924 0.801913 -0.111616 0.582374 
wait for imu ... 
time: 1403636923.001667, t: 8.515282 -4.396070 0.438913 q: 0.067266 0.797535 -0.120059 0.587367 
wait for imu ... 
time: 1403636923.101666, t: 8.505138 -4.404012 0.434825 q: 0.068838 0.796756 -0.125334 0.587139 
time: 1403636923.201667, t: 8.491107 -4.414347 0.424787 q: 0.075444 0.797916 -0.133110 0.583026 
time: 1403636923.301666, t: 8.471859 -4.426940 0.412191 q: 0.086974 0.802181 -0.139450 0.574016 
wait for imu ... 
time: 1403636923.401666, t: 8.450581 -4.440318 0.393834 q: 0.096170 0.807911 -0.144484 0.563165 
time: 1403636923.501667, t: 8.430053 -4.454203 0.371178 q: 0.101199 0.814036 -0.149648 0.552006 
time: 1403636923.601666, t: 8.413351 -4.468429 0.350350 q: 0.102754 0.820142 -0.154004 0.541379 
time: 1403636923.701667, t: 8.404544 -4.483793 0.338997 q: 0.104890 0.823033 -0.155888 0.536017 
time: 1403636923.801666, t: 8.404033 -4.500203 0.339620 q: 0.106720 0.822492 -0.156319 0.536360 
time: 1403636923.901666, t: 8.411250 -4.514930 0.342677 q: 0.107548 0.822671 -0.155993 0.536015 
time: 1403636924.001667, t: 8.427982 -4.529459 0.347314 q: 0.108850 0.822928 -0.154303 0.535846 
time: 1403636924.101666, t: 8.450327 -4.544352 0.342316 q: 0.109840 0.822436 -0.153213 0.536713 
wait for imu ... 
time: 1403636924.201667, t: 8.478019 -4.559981 0.332399 q: 0.110653 0.821355 -0.151186 0.538772 
time: 1403636924.301666, t: 8.510997 -4.575740 0.320756 q: 0.109883 0.820119 -0.150360 0.541038 
time: 1403636924.401666, t: 8.550096 -4.590867 0.316278 q: 0.103017 0.818055 -0.153963 0.544489 
wait for imu ... 
time: 1403636924.501667, t: 8.592564 -4.606181 0.321970 q: 0.096092 0.812426 -0.157702 0.553046 
time: 1403636924.601666, t: 8.638555 -4.625794 0.331551 q: 0.088069 0.806182 -0.161657 0.562300 
time: 1403636924.701667, t: 8.683376 -4.647105 0.340725 q: 0.082588 0.801423 -0.164393 0.569100 
time: 1403636924.801666, t: 8.726744 -4.672252 0.343427 q: 0.076751 0.796405 -0.165995 0.576449 
time: 1403636924.901666, t: 8.768031 -4.703357 0.338321 q: 0.075734 0.792305 -0.166876 0.581953 
time: 1403636925.001667, t: 8.803521 -4.740182 0.324517 q: 0.080480 0.790923 -0.172529 0.581547 
time: 1403636925.101666, t: 8.831405 -4.781813 0.302927 q: 0.092671 0.792224 -0.174043 0.577496 
time: 1403636925.201667, t: 8.854649 -4.825085 0.276376 q: 0.108671 0.796319 -0.171034 0.569925 
time: 1403636925.301666, t: 8.875931 -4.869003 0.245830 q: 0.118921 0.802942 -0.170147 0.558742 
time: 1403636925.401666, t: 8.898369 -4.912553 0.211101 q: 0.123899 0.811096 -0.171774 0.545221 
time: 1403636925.501667, t: 8.925715 -4.955802 0.178958 q: 0.125542 0.816748 -0.174423 0.535479 
time: 1403636925.601666, t: 8.957194 -4.997312 0.152790 q: 0.125816 0.821802 -0.176953 0.526782 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636925.701667, t: 8.997563 -5.038501 0.136228 q: 0.125518 0.824827 -0.179138 0.521359 
time: 1403636925.801666, t: 9.048543 -5.080994 0.131168 q: 0.127401 0.825380 -0.178984 0.520078 
time: 1403636925.901666, t: 9.109471 -5.123442 0.133934 q: 0.129643 0.819257 -0.177645 0.529578 
time: 1403636926.001667, t: 9.178472 -5.164930 0.143180 q: 0.134113 0.808656 -0.174197 0.545660 
time: 1403636926.101666, t: 9.251419 -5.204441 0.148322 q: 0.132195 0.796921 -0.174918 0.562890 
time: 1403636926.201667, t: 9.321416 -5.240580 0.144694 q: 0.125547 0.788671 -0.178228 0.574866 
time: 1403636926.301666, t: 9.387994 -5.275627 0.133791 q: 0.119070 0.784378 -0.181351 0.581107 
time: 1403636926.401666, t: 9.448480 -5.309058 0.116087 q: 0.112017 0.781864 -0.184202 0.584987 
time: 1403636926.501667, t: 9.500950 -5.343085 0.093948 q: 0.108220 0.782130 -0.184859 0.585140 
time: 1403636926.601666, t: 9.546537 -5.376950 0.074438 q: 0.110639 0.785723 -0.181185 0.581009 
time: 1403636926.701667, t: 9.585438 -5.411533 0.062576 q: 0.113767 0.788817 -0.177724 0.577269 
time: 1403636926.801666, t: 9.618975 -5.446119 0.058182 q: 0.113480 0.792205 -0.182228 0.571249 
time: 1403636926.901666, t: 9.650513 -5.482163 0.060603 q: 0.113836 0.793263 -0.193367 0.566026 
time: 1403636927.001667, t: 9.680850 -5.523157 0.062488 q: 0.118686 0.790753 -0.208095 0.563311 
time: 1403636927.101666, t: 9.711155 -5.567806 0.057687 q: 0.129332 0.786718 -0.223574 0.560681 
time: 1403636927.201667, t: 9.732717 -5.613705 0.048839 q: 0.142107 0.781208 -0.241704 0.557762 
time: 1403636927.301666, t: 9.752204 -5.662266 0.033626 q: 0.158124 0.775709 -0.259354 0.553180 
time: 1403636927.401666, t: 9.770740 -5.712623 0.013327 q: 0.172826 0.776078 -0.271321 0.542419 
time: 1403636927.501667, t: 9.788926 -5.762962 -0.011519 q: 0.181239 0.781219 -0.280924 0.527192 
wait for imu ... 
time: 1403636927.601666, t: 9.806929 -5.813080 -0.036614 q: 0.185682 0.786159 -0.289047 0.513739 
time: 1403636927.701667, t: 9.832695 -5.867016 -0.054749 q: 0.190396 0.787937 -0.293382 0.506786 
wait for imu ... 
time: 1403636927.801666, t: 9.867315 -5.925700 -0.063226 q: 0.191393 0.787208 -0.297162 0.505339 
time: 1403636927.901666, t: 9.906522 -5.986208 -0.064756 q: 0.191962 0.785734 -0.298629 0.506550 
time: 1403636928.001667, t: 9.952318 -6.050124 -0.069192 q: 0.192779 0.784060 -0.298803 0.508727 
time: 1403636928.101666, t: 10.001607 -6.116120 -0.075770 q: 0.195077 0.780770 -0.296594 0.514174 
time: 1403636928.201667, t: 10.056263 -6.184705 -0.085703 q: 0.196938 0.774106 -0.294157 0.524830 
time: 1403636928.301666, t: 10.114663 -6.253184 -0.099204 q: 0.195801 0.766275 -0.293110 0.537189 
wait for imu ... 
time: 1403636928.401666, t: 10.170763 -6.319718 -0.117875 q: 0.196184 0.760694 -0.290811 0.546154 
time: 1403636928.501667, t: 10.222693 -6.384657 -0.137296 q: 0.198402 0.758476 -0.288035 0.549896 
time: 1403636928.601666, t: 10.272281 -6.447739 -0.153148 q: 0.199261 0.760802 -0.286194 0.547329 
time: 1403636928.701667, t: 10.320094 -6.509960 -0.161597 q: 0.198843 0.766813 -0.285627 0.539330 
wait for imu ... 
time: 1403636928.801666, t: 10.367997 -6.569829 -0.163546 q: 0.198462 0.772399 -0.285273 0.531632 
time: 1403636928.901666, t: 10.416682 -6.629005 -0.156592 q: 0.198551 0.774661 -0.285067 0.528408 
wait for imu ... 
time: 1403636929.001667, t: 10.469473 -6.688701 -0.140134 q: 0.199413 0.768729 -0.284200 0.537140 
time: 1403636929.101666, t: 10.521180 -6.745541 -0.115330 q: 0.194893 0.759680 -0.287110 0.549973 
wait for imu ... 
time: 1403636929.201667, t: 10.572257 -6.801550 -0.086211 q: 0.189544 0.755046 -0.290515 0.556398 
time: 1403636929.301666, t: 10.618411 -6.855390 -0.058225 q: 0.183339 0.754669 -0.294170 0.557070 
time: 1403636929.401666, t: 10.656910 -6.908217 -0.035483 q: 0.182133 0.755405 -0.293822 0.556650 
time: 1403636929.501667, t: 10.691146 -6.964841 -0.019090 q: 0.185022 0.757956 -0.290547 0.553942 
wait for imu ... 
time: 1403636929.601666, t: 10.721375 -7.020998 -0.008873 q: 0.191539 0.760861 -0.284885 0.550676 
wait for imu ... 
time: 1403636929.701667, t: 10.745655 -7.073414 -0.002596 q: 0.197729 0.763019 -0.279688 0.548161 
time: 1403636929.801666, t: 10.769592 -7.123443 -0.003840 q: 0.200901 0.764479 -0.276959 0.546355 
wait for imu ... 
time: 1403636929.901666, t: 10.793070 -7.171431 -0.007708 q: 0.199558 0.763443 -0.277367 0.548086 
time: 1403636930.001667, t: 10.814226 -7.213333 -0.010624 q: 0.195562 0.761685 -0.279983 0.550636 
time: 1403636930.101666, t: 10.837402 -7.259107 -0.004473 q: 0.192745 0.761842 -0.281673 0.550551 
wait for imu ... 
time: 1403636930.301666, t: 10.872802 -7.342235 0.025512 q: 0.186537 0.762644 -0.286607 0.549031 
time: 1403636930.401666, t: 10.888619 -7.387822 0.051147 q: 0.188984 0.762636 -0.285170 0.548953 
time: 1403636930.501667, t: 10.901165 -7.431498 0.072054 q: 0.194770 0.763447 -0.281544 0.547673 
time: 1403636930.601666, t: 10.911410 -7.472246 0.085846 q: 0.197051 0.763351 -0.279739 0.547917 
wait for imu ... 
time: 1403636930.701667, t: 10.919467 -7.509623 0.090367 q: 0.192899 0.761862 -0.282489 0.550051 
time: 1403636930.801666, t: 10.925030 -7.547320 0.091267 q: 0.184960 0.759999 -0.287707 0.552644 
time: 1403636930.901666, t: 10.927040 -7.585363 0.087524 q: 0.180548 0.757637 -0.292204 0.554982 
wait for imu ... 
time: 1403636931.001667, t: 10.926361 -7.627483 0.078335 q: 0.176860 0.753425 -0.302931 0.556151 
time: 1403636931.101666, t: 10.920683 -7.673131 0.068811 q: 0.184206 0.749192 -0.310317 0.555412 
time: 1403636931.201667, t: 10.908504 -7.720231 0.065591 q: 0.200461 0.745615 -0.309565 0.555017 
time: 1403636931.301666, t: 10.890154 -7.766678 0.069950 q: 0.213558 0.740572 -0.308585 0.557423 
time: 1403636931.401666, t: 10.866668 -7.809936 0.080769 q: 0.223655 0.739242 -0.307551 0.555799 
wait for imu ... 
time: 1403636931.501667, t: 10.838363 -7.846705 0.096520 q: 0.234819 0.741618 -0.304009 0.549947 
time: 1403636931.601666, t: 10.806909 -7.873875 0.116230 q: 0.245555 0.744481 -0.299560 0.543797 
time: 1403636931.701667, t: 10.776903 -7.894945 0.132449 q: 0.251239 0.745027 -0.297351 0.541660 
time: 1403636931.801666, t: 10.747039 -7.907986 0.139979 q: 0.241084 0.746279 -0.305662 0.539923 
time: 1403636931.901666, t: 10.715050 -7.916674 0.140457 q: 0.231910 0.748218 -0.312824 0.537149 
wait for imu ... 
time: 1403636932.001667, t: 10.685057 -7.923096 0.136044 q: 0.226911 0.748728 -0.317012 0.536116 
time: 1403636932.101666, t: 10.652297 -7.926526 0.126226 q: 0.226668 0.746060 -0.317670 0.539539 
wait for imu ... 
time: 1403636932.201667, t: 10.621990 -7.930098 0.113519 q: 0.229194 0.743115 -0.316997 0.542921 
time: 1403636932.301666, t: 10.586331 -7.928372 0.106224 q: 0.239796 0.741219 -0.311464 0.544134 
wait for imu ... 
time: 1403636932.401666, t: 10.549992 -7.920321 0.106517 q: 0.249643 0.740824 -0.307108 0.542718 
wait for imu ... 
time: 1403636932.501667, t: 10.517247 -7.907976 0.114092 q: 0.254687 0.739051 -0.309518 0.541421 
wait for imu ... 
time: 1403636932.601666, t: 10.480368 -7.887032 0.127451 q: 0.257470 0.736020 -0.320890 0.537601 
time: 1403636932.701667, t: 10.441904 -7.860766 0.148030 q: 0.258688 0.729855 -0.337185 0.535442 
wait for imu ... 
time: 1403636932.801666, t: 10.404800 -7.828526 0.175607 q: 0.262984 0.724349 -0.349077 0.533202 
time: 1403636932.901666, t: 10.365906 -7.790256 0.205759 q: 0.267026 0.721493 -0.359239 0.528292 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636933.001667, t: 10.326800 -7.748467 0.226293 q: 0.269994 0.720255 -0.367467 0.522785 
wait for imu ... 
time: 1403636933.101666, t: 10.285733 -7.704884 0.232429 q: 0.275061 0.717083 -0.376848 0.517802 
time: 1403636933.201667, t: 10.245212 -7.658249 0.227215 q: 0.275041 0.717947 -0.386675 0.509301 
wait for imu ... 
time: 1403636933.301666, t: 10.205041 -7.608998 0.214278 q: 0.270675 0.726199 -0.396619 0.492000 
time: 1403636933.401666, t: 10.166450 -7.561047 0.198518 q: 0.265677 0.730469 -0.403304 0.482884 
time: 1403636933.501667, t: 10.132493 -7.518717 0.182263 q: 0.267284 0.727629 -0.403458 0.486144 
time: 1403636933.601666, t: 10.101482 -7.479864 0.167586 q: 0.270880 0.723092 -0.400689 0.493164 
time: 1403636933.701667, t: 10.074211 -7.442876 0.158849 q: 0.274365 0.719190 -0.397058 0.499834 
time: 1403636933.801666, t: 10.047827 -7.409137 0.157461 q: 0.277297 0.715930 -0.392913 0.506132 
time: 1403636933.901666, t: 10.023061 -7.373085 0.163538 q: 0.276894 0.712126 -0.392653 0.511888 
time: 1403636934.001667, t: 9.996705 -7.339327 0.173267 q: 0.277658 0.708564 -0.398303 0.512053 
time: 1403636934.101666, t: 9.968503 -7.303565 0.176923 q: 0.283797 0.702524 -0.408988 0.508574 
time: 1403636934.201667, t: 9.938576 -7.271028 0.167196 q: 0.295802 0.693860 -0.422991 0.502133 
wait for imu ... 
time: 1403636934.301666, t: 9.904466 -7.233679 0.147191 q: 0.308988 0.682874 -0.439892 0.494677 
wait for imu ... 
time: 1403636934.401666, t: 9.870398 -7.194043 0.125308 q: 0.318625 0.667936 -0.461610 0.489138 
time: 1403636934.501667, t: 9.835362 -7.153952 0.106978 q: 0.326875 0.652452 -0.484676 0.482233 
time: 1403636934.601666, t: 9.794319 -7.111162 0.097677 q: 0.339268 0.638130 -0.504367 0.472548 
time: 1403636934.701667, t: 9.752140 -7.068536 0.098265 q: 0.349593 0.627233 -0.521277 0.461122 
time: 1403636934.801666, t: 9.708525 -7.025463 0.108665 q: 0.358792 0.624095 -0.531408 0.446518 
time: 1403636934.901666, t: 9.664102 -6.981661 0.126154 q: 0.364665 0.623900 -0.537850 0.434151 
wait for imu ... 
time: 1403636935.001667, t: 9.616262 -6.937489 0.150124 q: 0.367687 0.623633 -0.541104 0.427895 
wait for imu ... 
time: 1403636935.101666, t: 9.572191 -6.895707 0.182771 q: 0.372199 0.620882 -0.539247 0.430333 
wait for imu ... 
time: 1403636935.201667, t: 9.527954 -6.856707 0.218774 q: 0.373068 0.616149 -0.537015 0.439085 
time: 1403636935.301666, t: 9.488460 -6.821967 0.250693 q: 0.370946 0.614393 -0.535738 0.444865 
time: 1403636935.401666, t: 9.445243 -6.788227 0.274142 q: 0.367211 0.614672 -0.534895 0.448578 
time: 1403636935.501667, t: 9.402479 -6.752363 0.291718 q: 0.365187 0.615770 -0.533104 0.450850 
time: 1403636935.601666, t: 9.357199 -6.716505 0.300979 q: 0.363416 0.616964 -0.531602 0.452420 
time: 1403636935.701667, t: 9.308772 -6.687977 0.302659 q: 0.362283 0.618437 -0.529704 0.453541 
time: 1403636935.801666, t: 9.260380 -6.651848 0.298052 q: 0.358988 0.624211 -0.530255 0.447569 
time: 1403636935.901666, t: 9.209908 -6.617973 0.285985 q: 0.354741 0.630207 -0.532112 0.440289 
wait for imu ... 
time: 1403636936.001667, t: 9.158138 -6.597587 0.264471 q: 0.345081 0.637173 -0.537659 0.431106 
time: 1403636936.101666, t: 9.106559 -6.578414 0.241163 q: 0.340306 0.641246 -0.540286 0.425542 
wait for imu ... 
time: 1403636936.201667, t: 9.058325 -6.565962 0.217801 q: 0.343064 0.641732 -0.538715 0.424585 
wait for imu ... 
wait for imu ... 
time: 1403636936.301666, t: 9.009612 -6.557257 0.199822 q: 0.349366 0.638643 -0.535158 0.428584 
time: 1403636936.401666, t: 8.963369 -6.553770 0.188673 q: 0.355429 0.633784 -0.532226 0.434424 
time: 1403636936.501667, t: 8.922109 -6.555611 0.185097 q: 0.361049 0.628467 -0.529570 0.440713 
wait for imu ... 
time: 1403636936.601666, t: 8.885835 -6.562673 0.188078 q: 0.364196 0.624908 -0.529425 0.443350 
wait for imu ... 
time: 1403636936.701667, t: 8.850729 -6.570759 0.186019 q: 0.365221 0.623188 -0.530896 0.443170 
time: 1403636936.801666, t: 8.807319 -6.570644 0.177919 q: 0.367450 0.622562 -0.531095 0.441967 
wait for imu ... 
time: 1403636936.901666, t: 8.774088 -6.579119 0.170631 q: 0.370213 0.623276 -0.530825 0.438969 
wait for imu ... 
time: 1403636937.001667, t: 8.732352 -6.580874 0.168223 q: 0.372254 0.624083 -0.530298 0.436729 
time: 1403636937.101666, t: 8.699177 -6.588416 0.173809 q: 0.372671 0.624018 -0.531125 0.435458 
time: 1403636937.201667, t: 8.661238 -6.590215 0.185417 q: 0.370027 0.621553 -0.533997 0.437720 
wait for imu ... 
time: 1403636937.301666, t: 8.628388 -6.597991 0.194862 q: 0.369431 0.620759 -0.535377 0.437664 
time: 1403636937.401666, t: 8.592222 -6.601237 0.198996 q: 0.369970 0.619787 -0.535917 0.437926 
wait for imu ... 
time: 1403636937.501667, t: 8.557415 -6.606186 0.196511 q: 0.370345 0.619179 -0.536460 0.437805 
time: 1403636937.601666, t: 8.525284 -6.614270 0.187632 q: 0.371887 0.618717 -0.536464 0.437145 
wait for imu ... 
time: 1403636937.701667, t: 8.492023 -6.617693 0.174747 q: 0.377302 0.621018 -0.533819 0.432455 
time: 1403636937.801666, t: 8.463288 -6.622580 0.156041 q: 0.383530 0.623027 -0.530348 0.428337 
wait for imu ... 
time: 1403636937.901666, t: 8.441548 -6.632139 0.132254 q: 0.390984 0.619673 -0.526299 0.431446 
time: 1403636938.001667, t: 8.420340 -6.633880 0.105942 q: 0.399708 0.616036 -0.521518 0.434455 
time: 1403636938.101666, t: 8.401963 -6.632428 0.074882 q: 0.405501 0.610161 -0.520150 0.438995 
time: 1403636938.201667, t: 8.390074 -6.622653 0.041997 q: 0.405280 0.601327 -0.525844 0.444569 
time: 1403636938.301666, t: 8.382995 -6.605398 0.012026 q: 0.400494 0.592634 -0.534253 0.450514 
time: 1403636938.401666, t: 8.374369 -6.581793 -0.011341 q: 0.384364 0.583334 -0.549433 0.458377 
wait for imu ... 
time: 1403636938.501667, t: 8.358981 -6.549615 -0.030805 q: 0.369192 0.578067 -0.563314 0.460666 
wait for imu ... 
time: 1403636938.601666, t: 8.334587 -6.515130 -0.041497 q: 0.362569 0.578513 -0.570761 0.456177 
wait for imu ... 
time: 1403636938.701667, t: 8.299995 -6.480885 -0.043713 q: 0.361298 0.578825 -0.573463 0.453393 
time: 1403636938.801666, t: 8.258976 -6.447147 -0.039227 q: 0.365675 0.581643 -0.572200 0.447839 
time: 1403636938.901666, t: 8.210056 -6.416666 -0.033477 q: 0.373125 0.588208 -0.567980 0.438392 
wait for imu ... 
time: 1403636939.001667, t: 8.159328 -6.380760 -0.027621 q: 0.378671 0.594500 -0.564832 0.429118 
time: 1403636939.101666, t: 8.105254 -6.346461 -0.026892 q: 0.381227 0.597129 -0.562807 0.425853 
time: 1403636939.201667, t: 8.051730 -6.314192 -0.030596 q: 0.383236 0.596560 -0.560318 0.428125 
wait for imu ... 
time: 1403636939.301666, t: 7.997635 -6.287177 -0.040368 q: 0.384006 0.596099 -0.558271 0.430741 
time: 1403636939.401666, t: 7.942555 -6.256623 -0.051824 q: 0.383979 0.597104 -0.555952 0.432370 
time: 1403636939.501667, t: 7.888105 -6.228839 -0.058295 q: 0.385649 0.597174 -0.552100 0.435710 
wait for imu ... 
time: 1403636939.601666, t: 7.833883 -6.199733 -0.056091 q: 0.390210 0.595488 -0.546494 0.440993 
wait for imu ... 
time: 1403636939.801666, t: 7.720643 -6.129898 -0.040818 q: 0.392475 0.592086 -0.542910 0.447935 
wait for imu ... 
time: 1403636939.901666, t: 7.664610 -6.089973 -0.040314 q: 0.389905 0.592372 -0.544955 0.447318 
wait for imu ... 
time: 1403636940.001667, t: 7.603568 -6.044002 -0.047093 q: 0.388481 0.588824 -0.550586 0.446344 
time: 1403636940.101666, t: 7.544441 -5.995546 -0.059666 q: 0.389449 0.585327 -0.557647 0.441307 
time: 1403636940.201667, t: 7.480058 -5.942255 -0.077711 q: 0.387787 0.582139 -0.565237 0.437313 
wait for imu ... 
time: 1403636940.301666, t: 7.410923 -5.887045 -0.097955 q: 0.388668 0.579075 -0.569627 0.434896 
time: 1403636940.401666, t: 7.342068 -5.832970 -0.122455 q: 0.390655 0.575734 -0.572628 0.433608 
wait for imu ... 
time: 1403636940.501667, t: 7.272503 -5.777264 -0.146693 q: -0.391986 -0.573364 0.575302 -0.432005 
time: 1403636940.601666, t: 7.200411 -5.720157 -0.165386 q: -0.389242 -0.574623 0.580154 -0.426287 
time: 1403636940.701667, t: 7.124948 -5.663325 -0.175348 q: -0.388835 -0.578805 0.582640 -0.417520 
wait for imu ... 
wait for imu ... 
time: 1403636940.901666, t: 6.977777 -5.557820 -0.171049 q: 0.388699 0.585468 -0.584348 0.405805 
time: 1403636941.001667, t: 6.901170 -5.509380 -0.156760 q: 0.390650 0.585524 -0.582786 0.406098 
time: 1403636941.101666, t: 6.826115 -5.465888 -0.137008 q: 0.393699 0.584451 -0.580405 0.408104 
time: 1403636941.201667, t: 6.754028 -5.423697 -0.114946 q: 0.395594 0.583482 -0.578275 0.410673 
time: 1403636941.301666, t: 6.682003 -5.383763 -0.092437 q: 0.397816 0.581889 -0.575219 0.415055 
wait for imu ... 
time: 1403636941.401666, t: 6.610616 -5.343104 -0.075746 q: 0.399128 0.582456 -0.571729 0.417814 
time: 1403636941.501667, t: 6.542040 -5.302317 -0.063819 q: 0.399530 0.583467 -0.568557 0.420340 
wait for imu ... 
time: 1403636941.601666, t: 6.474939 -5.261537 -0.058262 q: 0.398555 0.585880 -0.566639 0.420499 
wait for imu ... 
time: 1403636941.801666, t: 6.341829 -5.179378 -0.057861 q: 0.395597 0.589460 -0.564661 0.420948 
time: 1403636941.901666, t: 6.277178 -5.139156 -0.052096 q: 0.392958 0.590935 -0.565061 0.420816 
time: 1403636942.001667, t: 6.213639 -5.096807 -0.038665 q: 0.390041 0.591196 -0.566141 0.421711 
time: 1403636942.101666, t: 6.148920 -5.061321 -0.023640 q: 0.387997 0.589940 -0.567179 0.423954 
wait for imu ... 
time: 1403636942.201667, t: 6.086801 -5.018240 -0.006172 q: 0.387386 0.588041 -0.567852 0.426244 
time: 1403636942.301666, t: 6.020919 -4.982604 0.000926 q: 0.386161 0.585926 -0.569532 0.428023 
time: 1403636942.401666, t: 5.959121 -4.944457 0.003544 q: 0.384269 0.582597 -0.572288 0.430586 
time: 1403636942.501667, t: 5.892866 -4.905542 0.000724 q: 0.381900 0.580583 -0.575683 0.430889 
time: 1403636942.601666, t: 5.827648 -4.867604 -0.008433 q: 0.379605 0.580947 -0.579002 0.427969 
time: 1403636942.701667, t: 5.756226 -4.825347 -0.020157 q: 0.379595 0.583699 -0.580568 0.422072 
wait for imu ... 
time: 1403636942.801666, t: 5.682796 -4.785498 -0.034117 q: 0.381340 0.585232 -0.580734 0.418128 
time: 1403636942.901666, t: 5.609229 -4.748576 -0.040875 q: 0.387836 0.584153 -0.577747 0.417799 
wait for imu ... 
time: 1403636943.001667, t: 5.534132 -4.713354 -0.039430 q: 0.392545 0.583107 -0.575692 0.417700 
time: 1403636943.101666, t: 5.458668 -4.678625 -0.029212 q: 0.397280 0.581392 -0.573624 0.418459 
wait for imu ... 
time: 1403636943.201667, t: 5.382637 -4.642737 -0.017684 q: 0.399009 0.578711 -0.573757 0.420343 
wait for imu ... 
time: 1403636943.301666, t: 5.309249 -4.609235 -0.011145 q: 0.401108 0.577824 -0.573952 0.419298 
time: 1403636943.401666, t: 5.238095 -4.575083 -0.011250 q: 0.403654 0.576141 -0.574287 0.418712 
time: 1403636943.501667, t: 5.168969 -4.540336 -0.017504 q: 0.405521 0.575162 -0.574637 0.417772 
time: 1403636943.601666, t: 5.100587 -4.504233 -0.029246 q: 0.407928 0.574610 -0.574270 0.416691 
time: 1403636943.701667, t: 5.032412 -4.465993 -0.045066 q: -0.409943 -0.573730 0.573749 -0.416645 
time: 1403636943.801666, t: 4.969835 -4.427889 -0.067578 q: 0.411218 0.573971 -0.573795 0.414990 
time: 1403636943.901666, t: 4.905015 -4.385705 -0.095745 q: 0.404905 0.578541 -0.573965 0.414617 
time: 1403636944.001667, t: 4.842876 -4.343161 -0.130500 q: 0.390882 0.590361 -0.575226 0.409635 
time: 1403636944.101666, t: 4.780247 -4.303148 -0.164079 q: 0.377305 0.602774 -0.576839 0.401946 
time: 1403636944.201667, t: 4.721069 -4.271008 -0.189907 q: 0.369601 0.611245 -0.575851 0.397705 
time: 1403636944.301666, t: 4.664448 -4.246349 -0.205419 q: 0.366747 0.614085 -0.573254 0.399721 
time: 1403636944.401666, t: 4.610254 -4.231020 -0.212387 q: 0.369655 0.612131 -0.567996 0.407470 
time: 1403636944.501667, t: 4.558669 -4.217409 -0.212044 q: 0.380129 0.609947 -0.558338 0.414397 
time: 1403636944.601666, t: 4.509368 -4.207759 -0.204741 q: 0.394052 0.609681 -0.546653 0.417351 
time: 1403636944.701667, t: 4.467982 -4.197318 -0.193128 q: 0.401901 0.612781 -0.537060 0.417782 
time: 1403636944.801666, t: 4.431749 -4.185116 -0.178827 q: 0.396660 0.612959 -0.535413 0.424587 
time: 1403636944.901666, t: 4.401598 -4.172402 -0.158072 q: 0.390939 0.612624 -0.535187 0.430619 
time: 1403636945.001667, t: 4.375197 -4.155723 -0.138046 q: 0.387494 0.612422 -0.534496 0.434858 
time: 1403636945.101666, t: 4.352810 -4.136738 -0.129921 q: 0.386533 0.613052 -0.532931 0.436744 
time: 1403636945.301666, t: 4.309780 -4.088967 -0.138922 q: 0.384987 0.614297 -0.531376 0.438250 
time: 1403636945.401666, t: 4.292703 -4.062398 -0.137863 q: 0.385255 0.614910 -0.531212 0.437354 
time: 1403636945.501667, t: 4.278173 -4.033906 -0.130858 q: 0.385614 0.614232 -0.531809 0.437263 
time: 1403636945.601666, t: 4.267689 -4.001340 -0.128891 q: 0.382008 0.615265 -0.536067 0.433765 
time: 1403636945.701667, t: 4.256534 -3.969630 -0.132544 q: 0.375130 0.621073 -0.542771 0.423019 
wait for imu ... 
time: 1403636945.801666, t: 4.247713 -3.940584 -0.142449 q: 0.368564 0.627437 -0.548860 0.411383 
time: 1403636945.901666, t: 4.243488 -3.913759 -0.155946 q: 0.366333 0.627821 -0.552268 0.408217 
time: 1403636946.001667, t: 4.241011 -3.895355 -0.171587 q: 0.374256 0.622560 -0.548168 0.414563 
time: 1403636946.101666, t: 4.242388 -3.878519 -0.191415 q: 0.381013 0.617375 -0.545037 0.420253 
time: 1403636946.201667, t: 4.246042 -3.863559 -0.219363 q: 0.383257 0.614163 -0.545355 0.422501 
time: 1403636946.301666, t: 4.251297 -3.847804 -0.252308 q: 0.388070 0.610954 -0.543610 0.425001 
time: 1403636946.401666, t: 4.260225 -3.832415 -0.291443 q: 0.391241 0.607651 -0.543293 0.427227 
wait for imu ... 
time: 1403636946.501667, t: 4.270031 -3.815003 -0.336946 q: 0.392281 0.605671 -0.544651 0.427358 
time: 1403636946.601666, t: 4.281871 -3.796608 -0.383260 q: 0.393849 0.604192 -0.545421 0.427026 
time: 1403636946.701667, t: 4.294756 -3.774145 -0.424840 q: 0.387690 0.601602 -0.551214 0.428876 
wait for imu ... 
time: 1403636946.801666, t: 4.308367 -3.751217 -0.459767 q: 0.373206 0.593449 -0.562421 0.438427 
time: 1403636946.901666, t: 4.317516 -3.727819 -0.489862 q: 0.363584 0.588669 -0.569754 0.443459 
time: 1403636947.001667, t: 4.321114 -3.702687 -0.522628 q: 0.351706 0.582546 -0.577994 0.450406 
time: 1403636947.101666, t: 4.315830 -3.680539 -0.557450 q: 0.349291 0.582960 -0.579925 0.449267 
time: 1403636947.201667, t: 4.299868 -3.661607 -0.589531 q: 0.356982 0.594290 -0.575566 0.433714 
time: 1403636947.301666, t: 4.277747 -3.645294 -0.616131 q: 0.361769 0.609395 -0.572847 0.411833 
wait for imu ... 
time: 1403636947.401666, t: 4.255625 -3.634134 -0.634403 q: 0.361018 0.618949 -0.573269 0.397404 
time: 1403636947.501667, t: 4.234209 -3.631698 -0.645419 q: 0.361029 0.619039 -0.573872 0.396383 
wait for imu ... 
time: 1403636947.601666, t: 4.215115 -3.644509 -0.644917 q: 0.364835 0.611854 -0.572343 0.406145 
time: 1403636947.701667, t: 4.195895 -3.662554 -0.639397 q: 0.371574 0.609732 -0.567917 0.409426 
wait for imu ... 
time: 1403636947.801666, t: 4.178247 -3.687226 -0.636253 q: 0.378509 0.612206 -0.562723 0.406543 
time: 1403636947.901666, t: 4.165182 -3.715539 -0.637953 q: 0.388390 0.616730 -0.555094 0.400835 
wait for imu ... 
time: 1403636948.001667, t: 4.154697 -3.743578 -0.645462 q: 0.388796 0.618053 -0.553940 0.399998 
wait for imu ... 
time: 1403636948.101666, t: 4.149933 -3.774712 -0.658061 q: 0.381312 0.618865 -0.558218 0.400000 
wait for imu ... 
time: 1403636948.201667, t: 4.147477 -3.808685 -0.674077 q: 0.381984 0.620634 -0.556206 0.399421 
time: 1403636948.301666, t: 4.150495 -3.846182 -0.692456 q: 0.383930 0.621922 -0.553183 0.399749 
time: 1403636948.401666, t: 4.158075 -3.887305 -0.714396 q: 0.386462 0.624007 -0.549377 0.399309 
wait for imu ... 
time: 1403636948.501667, t: 4.171682 -3.929554 -0.737391 q: 0.392932 0.627846 -0.542313 0.396623 
time: 1403636948.601666, t: 4.191674 -3.972857 -0.763176 q: 0.405635 0.632543 -0.530688 0.392070 
wait for imu ... 
time: 1403636948.701667, t: 4.224719 -4.016189 -0.783378 q: 0.412317 0.632965 -0.523713 0.393796 
time: 1403636948.801666, t: 4.270199 -4.058857 -0.796169 q: 0.403536 0.625973 -0.528895 0.406924 
time: 1403636948.901666, t: 4.323499 -4.101331 -0.801888 q: 0.393270 0.617271 -0.534730 0.422349 
wait for imu ... 
time: 1403636949.001667, t: 4.381341 -4.139595 -0.803947 q: 0.383443 0.607463 -0.539889 0.438726 
wait for imu ... 
time: 1403636949.101666, t: 4.436556 -4.177693 -0.811249 q: 0.381070 0.604913 -0.539920 0.444243 
time: 1403636949.201667, t: 4.489597 -4.212396 -0.822893 q: 0.382150 0.606587 -0.537909 0.443473 
time: 1403636949.301666, t: 4.541294 -4.243007 -0.839730 q: 0.382498 0.608269 -0.536875 0.442119 
time: 1403636949.401666, t: 4.591480 -4.271803 -0.860088 q: 0.379323 0.606515 -0.538064 0.445803 
time: 1403636949.501667, t: 4.640366 -4.295112 -0.885006 q: 0.376851 0.604355 -0.539329 0.449291 
time: 1403636949.601666, t: 4.684969 -4.318240 -0.908031 q: 0.373201 0.602641 -0.541133 0.452459 
wait for imu ... 
time: 1403636949.701667, t: 4.727502 -4.339798 -0.925227 q: 0.369785 0.605573 -0.543630 0.448337 
time: 1403636949.801666, t: 4.763084 -4.355681 -0.934167 q: 0.367791 0.611318 -0.544779 0.440722 
time: 1403636949.901666, t: 4.798373 -4.370051 -0.938495 q: 0.362608 0.615729 -0.548521 0.434188 
time: 1403636950.001667, t: 4.829469 -4.386405 -0.936907 q: 0.358257 0.618089 -0.551661 0.430452 
time: 1403636950.101666, t: 4.860241 -4.409335 -0.933688 q: 0.354335 0.619755 -0.554775 0.427289 
wait for imu ... 
time: 1403636950.201667, t: 4.888677 -4.436508 -0.932587 q: 0.356557 0.621901 -0.554004 0.423303 
time: 1403636950.301666, t: 4.913881 -4.464114 -0.935686 q: 0.361163 0.622682 -0.551247 0.421847 
time: 1403636950.401666, t: 4.941793 -4.498962 -0.943407 q: 0.367950 0.621820 -0.546968 0.422822 
wait for imu ... 
time: 1403636950.501667, t: 4.970160 -4.534195 -0.954774 q: 0.377333 0.621036 -0.540696 0.423771 
time: 1403636950.601666, t: 4.998411 -4.567197 -0.964791 q: 0.386815 0.621753 -0.533685 0.423057 
time: 1403636950.701667, t: 5.031827 -4.600488 -0.973122 q: 0.388154 0.620446 -0.532223 0.425584 
wait for imu ... 
time: 1403636950.801666, t: 5.068678 -4.634280 -0.975626 q: 0.385526 0.616426 -0.533325 0.432381 
time: 1403636950.901666, t: 5.105924 -4.664447 -0.969073 q: 0.382300 0.614253 -0.534537 0.436818 
time: 1403636951.001667, t: 5.144740 -4.694327 -0.957341 q: 0.380729 0.613280 -0.534786 0.439246 
time: 1403636951.101666, t: 5.184431 -4.720734 -0.940103 q: 0.374845 0.610399 -0.538421 0.443856 
time: 1403636951.201667, t: 5.224953 -4.749925 -0.928238 q: 0.366397 0.611610 -0.544320 0.442044 
wait for imu ... 
time: 1403636951.301666, t: 5.261588 -4.778762 -0.923523 q: 0.362713 0.617087 -0.547263 0.433758 
time: 1403636951.401666, t: 5.295655 -4.804821 -0.922355 q: 0.359371 0.622736 -0.549708 0.425293 
time: 1403636951.501667, t: 5.331731 -4.843017 -0.929937 q: 0.359231 0.625576 -0.550196 0.420584 
time: 1403636951.601666, t: 5.367680 -4.881923 -0.937779 q: 0.361430 0.626992 -0.549049 0.418083 
wait for imu ... 
time: 1403636951.701667, t: 5.400866 -4.919389 -0.944793 q: 0.366118 0.626247 -0.546244 0.418795 
wait for imu ... 
time: 1403636951.801666, t: 5.436237 -4.965676 -0.947806 q: 0.370907 0.623577 -0.543143 0.422582 
time: 1403636951.901666, t: 5.474358 -5.019253 -0.945006 q: 0.377761 0.623127 -0.538362 0.423292 
time: 1403636952.001667, t: 5.515047 -5.073237 -0.940507 q: 0.383522 0.624144 -0.534434 0.421586 
time: 1403636952.101666, t: 5.562864 -5.128280 -0.939967 q: 0.385764 0.622588 -0.533262 0.423322 
time: 1403636952.201667, t: 5.608626 -5.175442 -0.942448 q: 0.384836 0.618804 -0.533924 0.428844 
time: 1403636952.301666, t: 5.662155 -5.227203 -0.953797 q: 0.381382 0.613230 -0.536181 0.437043 
wait for imu ... 
time: 1403636952.401666, t: 5.712780 -5.274353 -0.970116 q: 0.378646 0.608366 -0.537993 0.443938 
wait for imu ... 
time: 1403636952.501667, t: 5.757217 -5.315883 -0.990044 q: 0.381145 0.608328 -0.535644 0.444692 
time: 1403636952.601666, t: 5.803244 -5.355229 -1.011876 q: 0.384503 0.610260 -0.532833 0.442526 
time: 1403636952.701667, t: 5.850914 -5.391158 -1.028487 q: 0.385972 0.611056 -0.531525 0.441720 
time: 1403636952.801666, t: 5.894402 -5.420023 -1.043106 q: 0.382816 0.614593 -0.533706 0.436904 
time: 1403636952.901666, t: 5.946884 -5.450288 -1.052977 q: 0.377496 0.617103 -0.537928 0.432798 
wait for imu ... 
time: 1403636953.001667, t: 5.992713 -5.479436 -1.069210 q: 0.367877 0.617323 -0.545290 0.431552 
time: 1403636953.101666, t: 6.043747 -5.510791 -1.088677 q: 0.360310 0.614952 -0.551241 0.433754 
time: 1403636953.201667, t: 6.088677 -5.542816 -1.114806 q: 0.357575 0.614387 -0.553511 0.433929 
time: 1403636953.301666, t: 6.127313 -5.577473 -1.144631 q: 0.357955 0.615217 -0.552999 0.433092 
time: 1403636953.401666, t: 6.163982 -5.614144 -1.165628 q: 0.361912 0.616570 -0.549710 0.432064 
time: 1403636953.501667, t: 6.196898 -5.654403 -1.180672 q: 0.369060 0.618415 -0.544002 0.430604 
time: 1403636953.601666, t: 6.228250 -5.694845 -1.188443 q: 0.373262 0.620495 -0.540250 0.428710 
time: 1403636953.701667, t: 6.258819 -5.732316 -1.190451 q: 0.374549 0.626129 -0.537842 0.422376 
time: 1403636953.801666, t: 6.293924 -5.772482 -1.181236 q: 0.378063 0.631221 -0.534050 0.416435 
time: 1403636953.901666, t: 6.334729 -5.814617 -1.165328 q: 0.378485 0.631918 -0.532297 0.417240 
wait for imu ... 
time: 1403636954.001667, t: 6.383657 -5.860426 -1.145954 q: 0.375353 0.630378 -0.533842 0.420412 
time: 1403636954.101666, t: 6.430159 -5.903924 -1.127946 q: 0.374126 0.629148 -0.534432 0.422592 
time: 1403636954.201667, t: 6.479933 -5.950152 -1.116895 q: 0.373276 0.628224 -0.534863 0.424172 
wait for imu ... 
time: 1403636954.301666, t: 6.533071 -6.000220 -1.108456 q: 0.371987 0.629205 -0.535984 0.422430 
time: 1403636954.401666, t: 6.590970 -6.056116 -1.099353 q: 0.369735 0.629941 -0.538529 0.420067 
time: 1403636954.501667, t: 6.650517 -6.107114 -1.079530 q: 0.368334 0.629357 -0.541126 0.418836 
time: 1403636954.601666, t: 6.714196 -6.168985 -1.064858 q: 0.370886 0.629084 -0.541555 0.416430 
time: 1403636954.701667, t: 6.776627 -6.227265 -1.056597 q: 0.374966 0.625902 -0.541650 0.417448 
time: 1403636954.801666, t: 6.844597 -6.292233 -1.068040 q: 0.379629 0.620056 -0.541699 0.421871 
time: 1403636954.901666, t: 6.912964 -6.356741 -1.087164 q: 0.383725 0.614425 -0.542241 0.425690 
time: 1403636955.001667, t: 6.983148 -6.419295 -1.112772 q: 0.387658 0.610143 -0.543141 0.427136 
wait for imu ... 
time: 1403636955.101666, t: 7.053906 -6.477834 -1.136995 q: 0.390438 0.609191 -0.545038 0.423531 
time: 1403636955.201667, t: 7.124042 -6.534029 -1.154921 q: 0.393477 0.608099 -0.546760 0.420053 
wait for imu ... 
time: 1403636955.301666, t: 7.194148 -6.589171 -1.166169 q: 0.395353 0.604718 -0.549520 0.419572 
time: 1403636955.401666, t: 7.267887 -6.642394 -1.169928 q: 0.397996 0.602123 -0.551599 0.418073 
wait for imu ... 
time: 1403636955.501667, t: 7.345227 -6.696334 -1.174906 q: 0.399909 0.600927 -0.553199 0.415850 
time: 1403636955.601666, t: 7.422763 -6.748041 -1.186422 q: 0.401030 0.599397 -0.554854 0.414771 
wait for imu ... 
time: 1403636955.701667, t: 7.503145 -6.798042 -1.200842 q: 0.402142 0.598477 -0.556000 0.413487 
wait for imu ... 
time: 1403636955.801666, t: 7.587002 -6.847226 -1.212672 q: 0.402995 0.598025 -0.556569 0.412544 
wait for imu ... 
time: 1403636956.001667, t: 7.757236 -6.943138 -1.213531 q: 0.392346 0.589612 -0.566515 0.421289 
time: 1403636956.101666, t: 7.842072 -6.991710 -1.202532 q: 0.384543 0.582267 -0.573192 0.429584 
time: 1403636956.201667, t: 7.916030 -7.031144 -1.187470 q: 0.380568 0.578659 -0.577169 0.432663 
time: 1403636956.301666, t: 7.979993 -7.067836 -1.176882 q: 0.382262 0.579285 -0.577265 0.430198 
wait for imu ... 
time: 1403636956.401666, t: 8.041466 -7.104100 -1.176183 q: 0.391988 0.585071 -0.571769 0.420853 
time: 1403636956.501667, t: 8.099693 -7.139887 -1.178000 q: 0.399912 0.589480 -0.565280 0.415985 
time: 1403636956.601666, t: 8.163300 -7.173679 -1.186951 q: 0.399969 0.590200 -0.557745 0.424981 
wait for imu ... 
time: 1403636956.701667, t: 8.220013 -7.203326 -1.198902 q: 0.387956 0.591695 -0.551926 0.441321 
time: 1403636956.801666, t: 8.278681 -7.230713 -1.213245 q: 0.369445 0.600037 -0.544428 0.455043 
wait for imu ... 
time: 1403636956.901666, t: 8.330365 -7.253382 -1.220353 q: 0.351873 0.615816 -0.530550 0.464191 
wait for imu ... 
time: 1403636957.001667, t: 8.380252 -7.276656 -1.219081 q: 0.341806 0.634723 -0.508519 0.470854 
time: 1403636957.101666, t: 8.426414 -7.298429 -1.211217 q: 0.333806 0.654489 -0.482813 0.476560 
time: 1403636957.201667, t: 8.473016 -7.319770 -1.198601 q: 0.324239 0.672700 -0.459388 0.480943 
wait for imu ... 
time: 1403636957.301666, t: 8.515145 -7.338088 -1.183045 q: 0.313688 0.684684 -0.443123 0.486260 
time: 1403636957.401666, t: 8.555972 -7.356447 -1.172655 q: 0.304791 0.692467 -0.431870 0.490999 
wait for imu ... 
time: 1403636957.501667, t: 8.598789 -7.376285 -1.170009 q: 0.298207 0.696644 -0.425390 0.494777 
time: 1403636957.601666, t: 8.641073 -7.394824 -1.175981 q: 0.291412 0.699686 -0.420179 0.498967 
time: 1403636957.701667, t: 8.680342 -7.409601 -1.189054 q: 0.282934 0.704225 -0.412205 0.504085 
time: 1403636957.801666, t: 8.717983 -7.421437 -1.207932 q: 0.268919 0.712135 -0.400274 0.510222 
wait for imu ... 
time: 1403636957.901666, t: 8.753887 -7.431489 -1.229941 q: 0.250904 0.723073 -0.381373 0.518427 
time: 1403636958.001667, t: 8.787451 -7.440137 -1.249998 q: 0.231910 0.735509 -0.357010 0.527056 
time: 1403636958.101666, t: 8.819335 -7.448590 -1.261475 q: 0.213791 0.747825 -0.331660 0.533902 
time: 1403636958.201667, t: 8.849847 -7.458498 -1.264380 q: 0.198541 0.757806 -0.310428 0.538467 
time: 1403636958.301666, t: 8.875170 -7.469300 -1.255938 q: 0.186526 0.764935 -0.290556 0.543746 
wait for imu ... 
time: 1403636958.401666, t: 8.894765 -7.479782 -1.240810 q: 0.178234 0.772632 -0.272327 0.545078 
time: 1403636958.501667, t: 8.910504 -7.490909 -1.220606 q: 0.172637 0.777782 -0.260395 0.545386 
wait for imu ... 
time: 1403636958.601666, t: 8.927313 -7.504361 -1.205297 q: 0.169758 0.780093 -0.253595 0.546193 
time: 1403636958.701667, t: 8.944502 -7.519930 -1.193325 q: 0.169605 0.780984 -0.249907 0.546667 
time: 1403636958.801666, t: 8.962506 -7.535771 -1.185631 q: 0.169948 0.781185 -0.249173 0.546607 
wait for imu ... 
time: 1403636958.901666, t: 8.985445 -7.554533 -1.184066 q: 0.171910 0.781770 -0.250244 0.544665 
time: 1403636959.001667, t: 9.005080 -7.571383 -1.186845 q: 0.174781 0.781384 -0.251595 0.543683 
time: 1403636959.101666, t: 9.027714 -7.587865 -1.196150 q: 0.175782 0.781481 -0.254544 0.541844 
wait for imu ... 
time: 1403636959.201667, t: 9.051919 -7.605141 -1.211109 q: 0.176986 0.780836 -0.257040 0.541203 
time: 1403636959.301666, t: 9.081108 -7.623407 -1.232198 q: 0.183042 0.780536 -0.256510 0.539872 
time: 1403636959.401666, t: 9.105240 -7.637979 -1.259328 q: 0.189162 0.780801 -0.254545 0.538307 
wait for imu ... 
time: 1403636959.501667, t: 9.132857 -7.650351 -1.290310 q: 0.193358 0.779024 -0.253263 0.539992 
wait for imu ... 
time: 1403636959.601666, t: 9.167203 -7.661589 -1.321399 q: 0.189990 0.773313 -0.257273 0.547450 
time: 1403636959.701667, t: 9.196733 -7.669052 -1.345755 q: 0.187844 0.772896 -0.260456 0.547274 
time: 1403636959.801666, t: 9.229134 -7.675942 -1.360955 q: 0.186364 0.773965 -0.263220 0.544942 
time: 1403636959.901666, t: 9.258608 -7.679063 -1.370497 q: 0.185863 0.774748 -0.265444 0.542919 
time: 1403636960.001667, t: 9.290829 -7.684359 -1.373313 q: 0.185354 0.774950 -0.266719 0.542179 
wait for imu ... 
time: 1403636960.101666, t: 9.322441 -7.689710 -1.368658 q: 0.185713 0.774681 -0.266834 0.542383 
time: 1403636960.201667, t: 9.353207 -7.694158 -1.360734 q: 0.188516 0.775419 -0.265098 0.541211 
time: 1403636960.301666, t: 9.384637 -7.697132 -1.357652 q: 0.197028 0.777605 -0.259169 0.537906 
time: 1403636960.401666, t: 9.419300 -7.698925 -1.360356 q: 0.201900 0.776977 -0.255330 0.538841 
wait for imu ... 
time: 1403636960.501667, t: 9.454725 -7.695946 -1.366011 q: 0.203170 0.775358 -0.249111 0.543586 
time: 1403636960.601666, t: 9.495827 -7.690604 -1.377421 q: 0.203430 0.775287 -0.241060 0.547208 
wait for imu ... 
time: 1403636960.701667, t: 9.531561 -7.676665 -1.390171 q: 0.195744 0.774809 -0.239079 0.551541 
time: 1403636960.801666, t: 9.568599 -7.657770 -1.409305 q: 0.181445 0.776693 -0.241003 0.552940 
wait for imu ... 
time: 1403636960.901666, t: 9.603737 -7.635463 -1.430360 q: 0.166912 0.780947 -0.233796 0.554619 
time: 1403636961.001667, t: 9.639943 -7.611574 -1.447810 q: 0.153264 0.786343 -0.216586 0.557912 
time: 1403636961.101666, t: 9.670847 -7.586114 -1.458376 q: 0.137223 0.791468 -0.195739 0.562525 
wait for imu ... 
time: 1403636961.201667, t: 9.702088 -7.560416 -1.462086 q: 0.119864 0.796776 -0.173073 0.566416 
wait for imu ... 
time: 1403636961.301666, t: 9.726196 -7.536312 -1.455166 q: 0.104103 0.801574 -0.150294 0.569256 
wait for imu ... 
time: 1403636961.401666, t: 9.755805 -7.511016 -1.444018 q: 0.092868 0.804291 -0.131405 0.572035 
wait for imu ... 
time: 1403636961.501667, t: 9.780362 -7.485305 -1.425723 q: 0.082978 0.806414 -0.118578 0.573368 
time: 1403636961.601666, t: 9.805957 -7.459664 -1.400392 q: 0.072781 0.808073 -0.105704 0.574932 
time: 1403636961.701667, t: 9.831076 -7.433863 -1.368150 q: 0.061679 0.809185 -0.091445 0.577108 
wait for imu ... 
time: 1403636961.801666, t: 9.854311 -7.406381 -1.332935 q: 0.051285 0.810327 -0.075314 0.578850 
time: 1403636961.901666, t: 9.876430 -7.378927 -1.302866 q: 0.041455 0.812304 -0.057315 0.578929 
time: 1403636962.001667, t: 9.897527 -7.350096 -1.279489 q: 0.028202 0.813329 -0.039255 0.579792 
time: 1403636962.101666, t: 9.917401 -7.321690 -1.261396 q: 0.014978 0.815051 -0.020211 0.578843 
time: 1403636962.201667, t: 9.938062 -7.291908 -1.242116 q: 0.001207 0.815338 -0.000680 0.578984 
wait for imu ... 
time: 1403636962.301666, t: 9.957863 -7.261154 -1.216769 q: -0.011378 0.814164 0.021054 0.580141 
wait for imu ... 
time: 1403636962.401666, t: 9.975475 -7.229496 -1.186325 q: -0.025949 0.812991 0.042211 0.580164 
time: 1403636962.501667, t: 9.990445 -7.196375 -1.149225 q: -0.037458 0.811464 0.065630 0.579496 
time: 1403636962.601666, t: 10.005652 -7.161924 -1.108958 q: -0.046370 0.809463 0.091665 0.578115 
time: 1403636962.701667, t: 10.017179 -7.126575 -1.071717 q: -0.051994 0.806498 0.116028 0.577403 
wait for imu ... 
time: 1403636962.801666, t: 10.025228 -7.088505 -1.041232 q: -0.057040 0.804045 0.132736 0.576749 
time: 1403636962.901666, t: 10.032909 -7.045480 -1.017376 q: -0.063748 0.802177 0.140257 0.576867 
time: 1403636963.001667, t: 10.034825 -6.998685 -0.999341 q: -0.074486 0.798764 0.139151 0.580573 
wait for imu ... 
time: 1403636963.101666, t: 10.034631 -6.948841 -0.985581 q: -0.081657 0.793417 0.136050 0.587633 
time: 1403636963.201667, t: 10.028774 -6.898521 -0.978052 q: -0.083523 0.793136 0.133181 0.588406 
time: 1403636963.301666, t: 10.019669 -6.846850 -0.974261 q: -0.087177 0.797801 0.127325 0.582840 
time: 1403636963.401666, t: 10.005550 -6.796973 -0.975996 q: -0.089578 0.801247 0.121026 0.579079 
wait for imu ... 
time: 1403636963.501667, t: 9.990844 -6.748325 -0.979299 q: -0.085454 0.802812 0.119115 0.577929 
time: 1403636963.601666, t: 9.970613 -6.700859 -0.979035 q: -0.081216 0.803310 0.117301 0.578219 
time: 1403636963.701667, t: 9.951377 -6.652206 -0.972331 q: -0.076739 0.804022 0.115122 0.578279 
wait for imu ... 
time: 1403636963.901666, t: 9.907532 -6.556326 -0.934478 q: -0.071606 0.804235 0.110920 0.579461 
wait for imu ... 
time: 1403636964.001667, t: 9.881487 -6.508722 -0.907207 q: -0.070722 0.804015 0.109576 0.580131 
time: 1403636964.101666, t: 9.855753 -6.460442 -0.874504 q: -0.069832 0.803675 0.109569 0.580710 
time: 1403636964.201667, t: 9.824804 -6.412802 -0.841509 q: -0.069444 0.802178 0.110237 0.582697 
time: 1403636964.301666, t: 9.791444 -6.364893 -0.814913 q: -0.068117 0.802284 0.112197 0.582333 
time: 1403636964.401666, t: 9.753309 -6.316256 -0.796251 q: -0.067790 0.801644 0.113811 0.582939 
wait for imu ... 
time: 1403636964.501667, t: 9.716012 -6.266598 -0.781732 q: -0.066547 0.801005 0.117288 0.583272 
time: 1403636964.601666, t: 9.674573 -6.213779 -0.771967 q: -0.064204 0.799236 0.126549 0.584024 
time: 1403636964.701667, t: 9.631573 -6.157679 -0.766972 q: -0.066904 0.797615 0.139067 0.583090 
wait for imu ... 
time: 1403636964.801666, t: 9.583912 -6.101421 -0.767769 q: -0.071538 0.794316 0.155852 0.582799 
wait for imu ... 
time: 1403636964.901666, t: 9.531174 -6.037601 -0.767436 q: -0.088557 0.789657 0.170583 0.582667 
time: 1403636965.001667, t: 9.472077 -5.971031 -0.759883 q: -0.104979 0.783245 0.189645 0.582702 
time: 1403636965.101666, t: 9.409701 -5.899727 -0.745295 q: -0.121184 0.780038 0.206824 0.577995 
time: 1403636965.201667, t: 9.342726 -5.825957 -0.724897 q: -0.132465 0.780936 0.220799 0.569069 
time: 1403636965.301666, t: 9.273252 -5.751985 -0.698337 q: -0.136971 0.788122 0.232555 0.553192 
time: 1403636965.401666, t: 9.202592 -5.674893 -0.668827 q: -0.136772 0.799187 0.241393 0.533220 
wait for imu ... 
time: 1403636965.501667, t: 9.136707 -5.593363 -0.630209 q: -0.140905 0.807236 0.242258 0.519449 
time: 1403636965.601666, t: 9.081393 -5.504228 -0.579302 q: -0.152426 0.803165 0.234843 0.525871 
wait for imu ... 
time: 1403636965.701667, t: 9.030612 -5.410926 -0.519095 q: -0.167214 0.793125 0.223584 0.541297 
time: 1403636965.801666, t: 8.985454 -5.318816 -0.464190 q: -0.173041 0.786184 0.217148 0.552104 
time: 1403636965.901666, t: 8.938993 -5.232909 -0.417554 q: -0.164317 0.781636 0.220341 0.559905 
time: 1403636966.001667, t: 8.889356 -5.149881 -0.378533 q: -0.155575 0.779401 0.223875 0.564101 
wait for imu ... 
time: 1403636966.101666, t: 8.838992 -5.068628 -0.346170 q: -0.150805 0.777979 0.225062 0.566881 
time: 1403636966.201667, t: 8.788048 -4.987070 -0.322128 q: -0.148000 0.777619 0.224699 0.568256 
time: 1403636966.301666, t: 8.733637 -4.910071 -0.307920 q: -0.147265 0.776849 0.223349 0.570030 
time: 1403636966.401666, t: 8.677809 -4.831158 -0.300259 q: -0.156897 0.777997 0.214827 0.569170 
time: 1403636966.501667, t: 8.620341 -4.761358 -0.301370 q: -0.172470 0.780159 0.202061 0.566372 
time: 1403636966.601666, t: 8.564327 -4.698698 -0.308466 q: -0.176178 0.780701 0.197148 0.566215 
time: 1403636966.701667, t: 8.507483 -4.644448 -0.319472 q: -0.165105 0.780459 0.202020 0.568165 
time: 1403636966.801666, t: 8.451081 -4.591532 -0.326824 q: -0.157129 0.779130 0.205206 0.571102 
wait for imu ... 
time: 1403636966.901666, t: 8.391206 -4.544555 -0.329025 q: -0.157888 0.779606 0.203010 0.571028 
time: 1403636967.001667, t: 8.327581 -4.497137 -0.322831 q: -0.160042 0.780138 0.200194 0.570696 
time: 1403636967.101666, t: 8.264125 -4.455220 -0.318626 q: -0.154939 0.779671 0.202444 0.571946 
time: 1403636967.201667, t: 8.199755 -4.421006 -0.325065 q: -0.150501 0.779309 0.204609 0.572855 
time: 1403636967.301666, t: 8.132995 -4.386291 -0.335308 q: -0.148563 0.779365 0.204892 0.573183 
time: 1403636967.401666, t: 8.064173 -4.351329 -0.345101 q: -0.145919 0.778378 0.205864 0.574853 
time: 1403636967.501667, t: 7.991807 -4.317860 -0.345503 q: -0.146163 0.778180 0.204882 0.575410 
time: 1403636967.601666, t: 7.914884 -4.283873 -0.340215 q: -0.143985 0.777743 0.205005 0.576504 
time: 1403636967.701667, t: 7.835327 -4.252359 -0.335568 q: -0.141692 0.778012 0.205279 0.576612 
wait for imu ... 
time: 1403636967.801666, t: 7.753897 -4.221355 -0.347261 q: -0.141869 0.777862 0.204470 0.577058 
wait for imu ... 
time: 1403636967.901666, t: 7.669455 -4.192606 -0.372557 q: -0.141772 0.777174 0.204468 0.578009 
wait for imu ... 
time: 1403636968.001667, t: 7.581636 -4.162204 -0.406293 q: -0.142115 0.776882 0.204264 0.578390 
time: 1403636968.101666, t: 7.490730 -4.134180 -0.443895 q: -0.144482 0.776181 0.203568 0.578989 
wait for imu ... 
time: 1403636968.201667, t: 7.396328 -4.106796 -0.481117 q: -0.147073 0.777258 0.201806 0.577507 
time: 1403636968.301666, t: 7.296474 -4.083171 -0.510427 q: -0.146693 0.778019 0.202197 0.576441 
time: 1403636968.401666, t: 7.196877 -4.062566 -0.532340 q: -0.146231 0.779066 0.202402 0.575071 
time: 1403636968.501667, t: 7.091424 -4.041566 -0.545854 q: -0.146653 0.780027 0.201684 0.573912 
time: 1403636968.601666, t: 6.985422 -4.021758 -0.552800 q: -0.144815 0.779655 0.202653 0.574542 
time: 1403636968.701667, t: 6.874396 -4.002795 -0.551967 q: -0.138755 0.778959 0.206462 0.575624 
time: 1403636968.801666, t: 6.767183 -3.983035 -0.556404 q: -0.134498 0.778588 0.209061 0.576199 
time: 1403636968.901666, t: 6.651452 -3.962808 -0.559305 q: -0.136982 0.779216 0.206761 0.575594 
wait for imu ... 
time: 1403636969.001667, t: 6.537081 -3.938958 -0.555491 q: -0.139051 0.780048 0.204992 0.574602 
time: 1403636969.101666, t: 6.414289 -3.919782 -0.544711 q: -0.143723 0.781104 0.201768 0.573158 
time: 1403636969.201667, t: 6.296517 -3.899408 -0.530414 q: -0.143652 0.786085 0.201527 0.566411 
time: 1403636969.301666, t: 6.180465 -3.880499 -0.510234 q: -0.142853 0.792852 0.201622 0.557071 
wait for imu ... 
time: 1403636969.401666, t: 6.062302 -3.861329 -0.483962 q: -0.139568 0.797069 0.203075 0.551327 
wait for imu ... 
time: 1403636969.501667, t: 5.950695 -3.840515 -0.456948 q: -0.137067 0.799651 0.203645 0.547997 
time: 1403636969.601666, t: 5.840432 -3.817382 -0.435052 q: -0.135631 0.801374 0.203217 0.545992 
time: 1403636969.701667, t: 5.734799 -3.796113 -0.417270 q: -0.135358 0.801346 0.202409 0.546400 
time: 1403636969.801666, t: 5.631877 -3.772978 -0.403739 q: -0.134478 0.801440 0.202396 0.546485 
time: 1403636969.901666, t: 5.531175 -3.748078 -0.396164 q: -0.134926 0.801772 0.201558 0.546196 
wait for imu ... 
time: 1403636970.001667, t: 5.436327 -3.720137 -0.393444 q: -0.137251 0.799291 0.200226 0.549733 
wait for imu ... 
time: 1403636970.101666, t: 5.343702 -3.690628 -0.394453 q: -0.138831 0.793009 0.199962 0.558460 
time: 1403636970.201667, t: 5.254856 -3.659642 -0.399860 q: -0.140181 0.788333 0.199455 0.564888 
wait for imu ... 
time: 1403636970.301666, t: 5.165970 -3.628145 -0.407238 q: -0.140723 0.786867 0.199709 0.566705 
time: 1403636970.401666, t: 5.076896 -3.598671 -0.416312 q: -0.139585 0.791294 0.200804 0.560399 
time: 1403636970.501667, t: 4.987888 -3.567427 -0.422589 q: -0.138975 0.797002 0.201807 0.552039 
wait for imu ... 
time: 1403636970.601666, t: 4.901954 -3.536784 -0.426369 q: -0.137630 0.801471 0.203763 0.545145 
time: 1403636970.701667, t: 4.820357 -3.503915 -0.422469 q: -0.135975 0.805072 0.206253 0.539286 
time: 1403636970.801666, t: 4.745336 -3.467477 -0.410384 q: -0.135252 0.808748 0.207917 0.533295 
time: 1403636970.901666, t: 4.678013 -3.428934 -0.399156 q: -0.133894 0.813664 0.209857 0.525341 
wait for imu ... 
time: 1403636971.001667, t: 4.617165 -3.384463 -0.393564 q: -0.137721 0.815839 0.207819 0.521776 
time: 1403636971.101666, t: 4.564147 -3.336270 -0.392167 q: -0.147929 0.813703 0.200814 0.525050 
time: 1403636971.201667, t: 4.513585 -3.286866 -0.395786 q: -0.160712 0.809241 0.192042 0.531433 
wait for imu ... 
time: 1403636971.301666, t: 4.470989 -3.242588 -0.405299 q: -0.173243 0.804749 0.183138 0.537425 
time: 1403636971.401666, t: 4.436139 -3.204250 -0.420631 q: -0.181546 0.802647 0.177798 0.539617 
wait for imu ... 
time: 1403636971.501667, t: 4.410758 -3.172413 -0.441853 q: -0.178868 0.801618 0.180265 0.541220 
time: 1403636971.601666, t: 4.385355 -3.143224 -0.464613 q: -0.171806 0.798027 0.185935 0.546867 
time: 1403636971.701667, t: 4.368496 -3.120414 -0.491579 q: -0.162212 0.791913 0.193593 0.555952 
time: 1403636971.801666, t: 4.351238 -3.099830 -0.522775 q: -0.160364 0.786970 0.195839 0.562681 
time: 1403636971.901666, t: 4.335813 -3.084490 -0.555159 q: -0.161001 0.784619 0.196568 0.565520 
time: 1403636972.001667, t: 4.317283 -3.071067 -0.581652 q: -0.160995 0.784680 0.197480 0.565119 
time: 1403636972.101666, t: 4.295575 -3.063446 -0.599392 q: -0.159631 0.784413 0.198570 0.565495 
wait for imu ... 
time: 1403636972.201667, t: 4.268798 -3.057908 -0.612714 q: -0.160918 0.787968 0.197345 0.560595 
time: 1403636972.301666, t: 4.247294 -3.055590 -0.620452 q: -0.162400 0.793677 0.196053 0.552509 
time: 1403636972.401666, t: 4.227849 -3.056554 -0.614385 q: -0.159379 0.795811 0.197593 0.549764 
time: 1403636972.501667, t: 4.216816 -3.060084 -0.600468 q: -0.156176 0.794696 0.199447 0.551623 
time: 1403636972.601666, t: 4.209056 -3.065077 -0.579769 q: -0.153850 0.792885 0.200338 0.554552 
time: 1403636972.701667, t: 4.201056 -3.070913 -0.558199 q: -0.150838 0.790688 0.201663 0.558025 
time: 1403636972.801666, t: 4.192644 -3.076581 -0.543120 q: -0.150134 0.790671 0.201578 0.558270 
time: 1403636972.901666, t: 4.180910 -3.082181 -0.532491 q: -0.148773 0.791128 0.201124 0.558151 
time: 1403636973.001667, t: 4.171084 -3.089027 -0.531376 q: -0.143448 0.794604 0.195785 0.556503 
time: 1403636973.101666, t: 4.161220 -3.096505 -0.536441 q: -0.136899 0.799153 0.185994 0.554995 
time: 1403636973.201667, t: 4.151891 -3.104757 -0.544008 q: -0.131138 0.802010 0.177747 0.554968 
wait for imu ... 
time: 1403636973.301666, t: 4.143295 -3.112268 -0.556828 q: -0.125654 0.802448 0.171957 0.557422 
time: 1403636973.401666, t: 4.137226 -3.117916 -0.576032 q: -0.121456 0.803066 0.167718 0.558752 
time: 1403636973.501667, t: 4.130716 -3.123769 -0.598469 q: -0.119305 0.803818 0.164237 0.559169 
time: 1403636973.601666, t: 4.125477 -3.126952 -0.624583 q: -0.116965 0.802222 0.162253 0.562523 
time: 1403636973.701667, t: 4.122748 -3.128810 -0.652494 q: -0.115866 0.800587 0.161226 0.565368 
time: 1403636973.801666, t: 4.118579 -3.129568 -0.673516 q: -0.114598 0.797587 0.161555 0.569757 
wait for imu ... 
time: 1403636973.901666, t: 4.111855 -3.130265 -0.688163 q: -0.114635 0.796461 0.161933 0.571215 
time: 1403636974.001667, t: 4.107367 -3.130908 -0.694635 q: -0.116786 0.798214 0.159726 0.568949 
time: 1403636974.101666, t: 4.099465 -3.132511 -0.697657 q: -0.115576 0.802602 0.151843 0.565169 
time: 1403636974.201667, t: 4.094494 -3.135580 -0.702595 q: -0.106362 0.807962 0.138345 0.562801 
wait for imu ... 
time: 1403636974.301666, t: 4.091419 -3.140601 -0.711539 q: -0.089748 0.811427 0.121043 0.564695 
wait for imu ... 
time: 1403636974.401666, t: 4.089695 -3.145440 -0.727133 q: -0.072035 0.813629 0.099254 0.568302 
time: 1403636974.501667, t: 4.087255 -3.149658 -0.747216 q: -0.054092 0.813964 0.074125 0.573622 
time: 1403636974.601666, t: 4.082848 -3.153688 -0.771392 q: -0.034868 0.813791 0.048211 0.578104 
time: 1403636974.701667, t: 4.075613 -3.157422 -0.796307 q: -0.017440 0.816048 0.022251 0.577292 
time: 1403636974.801666, t: 4.064882 -3.160833 -0.815281 q: -0.003738 0.819594 -0.003441 0.572923 
time: 1403636974.901666, t: 4.053931 -3.164043 -0.826089 q: 0.003917 0.823600 -0.030154 0.566356 
time: 1403636975.001667, t: 4.045432 -3.168666 -0.828947 q: 0.012210 0.825955 -0.053908 0.561020 
time: 1403636975.101666, t: 4.040461 -3.176458 -0.824876 q: 0.024737 0.824157 -0.071706 0.561258 
time: 1403636975.201667, t: 4.040292 -3.186166 -0.821424 q: 0.038561 0.818976 -0.081487 0.566702 
wait for imu ... 
time: 1403636975.301666, t: 4.044943 -3.195253 -0.820120 q: 0.049919 0.813490 -0.085872 0.573034 
time: 1403636975.401666, t: 4.049988 -3.203196 -0.825807 q: 0.056519 0.810445 -0.087862 0.576424 
time: 1403636975.501667, t: 4.055889 -3.208831 -0.838910 q: 0.058929 0.811539 -0.088806 0.574495 
time: 1403636975.601666, t: 4.062190 -3.213140 -0.850600 q: 0.059981 0.815628 -0.088030 0.568687 
time: 1403636975.701667, t: 4.065508 -3.218318 -0.853532 q: 0.059059 0.818732 -0.085884 0.564637 
wait for imu ... 
time: 1403636975.801666, t: 4.075580 -3.220280 -0.847611 q: 0.057705 0.821069 -0.083896 0.561674 
time: 1403636975.901666, t: 4.086526 -3.223683 -0.837671 q: 0.061265 0.822229 -0.087661 0.559019 
time: 1403636976.001667, t: 4.099278 -3.227431 -0.831508 q: 0.065504 0.819823 -0.100349 0.559936 
time: 1403636976.101666, t: 4.112825 -3.233065 -0.829146 q: 0.068498 0.812159 -0.120920 0.566643 
time: 1403636976.201667, t: 4.128862 -3.239915 -0.832645 q: 0.070213 0.803305 -0.146832 0.572898 
time: 1403636976.301666, t: 4.139970 -3.250073 -0.839903 q: 0.077495 0.795925 -0.173078 0.574928 
wait for imu ... 
time: 1403636976.401666, t: 4.147039 -3.266242 -0.852443 q: 0.090884 0.789484 -0.197743 0.573893 
time: 1403636976.501667, t: 4.151510 -3.288725 -0.869780 q: 0.111219 0.785194 -0.216997 0.569222 
time: 1403636976.601666, t: 4.151552 -3.314937 -0.891817 q: 0.134151 0.783491 -0.226770 0.562780 
time: 1403636976.701667, t: 4.151029 -3.340182 -0.919761 q: 0.150779 0.782032 -0.233158 0.557969 
time: 1403636976.801666, t: 4.149496 -3.364642 -0.953628 q: 0.158071 0.781204 -0.240607 0.553934 
time: 1403636976.901666, t: 4.149350 -3.389799 -0.990460 q: 0.157444 0.783765 -0.248006 0.547189 
time: 1403636977.001667, t: 4.148642 -3.414799 -1.019578 q: 0.155042 0.784651 -0.252999 0.544313 
time: 1403636977.101666, t: 4.149585 -3.441843 -1.038378 q: 0.152864 0.785760 -0.255095 0.542347 
wait for imu ... 
time: 1403636977.201667, t: 4.152428 -3.473286 -1.047841 q: 0.149848 0.787020 -0.255360 0.541236 
time: 1403636977.301666, t: 4.153735 -3.506749 -1.052517 q: 0.148211 0.787640 -0.253354 0.541727 
time: 1403636977.401666, t: 4.158639 -3.544591 -1.058284 q: 0.148895 0.785578 -0.248644 0.546694 
time: 1403636977.501667, t: 4.163945 -3.584860 -1.069146 q: 0.151503 0.782560 -0.242570 0.552999 
time: 1403636977.601666, t: 4.168789 -3.625151 -1.085081 q: 0.152067 0.779679 -0.239099 0.558398 
time: 1403636977.701667, t: 4.170672 -3.667049 -1.107516 q: 0.152626 0.778729 -0.238367 0.559882 
time: 1403636977.801666, t: 4.174560 -3.710327 -1.134515 q: 0.148641 0.778923 -0.245752 0.557487 
time: 1403636977.901666, t: 4.174445 -3.752361 -1.162755 q: 0.146428 0.778834 -0.251762 0.555511 
time: 1403636978.001667, t: 4.173106 -3.798325 -1.187445 q: 0.151035 0.776909 -0.252338 0.556710 
time: 1403636978.101666, t: 4.169066 -3.843228 -1.202224 q: 0.158218 0.776784 -0.250508 0.555716 
time: 1403636978.201667, t: 4.164788 -3.891173 -1.210139 q: 0.161732 0.778263 -0.250130 0.552797 
time: 1403636978.301666, t: 4.156640 -3.938013 -1.211480 q: 0.164187 0.780674 -0.249929 0.548750 
time: 1403636978.401666, t: 4.149150 -3.986850 -1.206497 q: 0.164931 0.783754 -0.250758 0.543735 
time: 1403636978.501667, t: 4.143183 -4.033840 -1.196472 q: 0.164968 0.789167 -0.252018 0.535246 
time: 1403636978.601666, t: 4.140835 -4.086871 -1.192919 q: 0.163588 0.794395 -0.254328 0.526776 
time: 1403636978.801666, t: 4.152288 -4.193014 -1.198083 q: 0.163160 0.799662 -0.256489 0.517816 
time: 1403636978.901666, t: 4.167873 -4.244595 -1.202725 q: 0.164430 0.796421 -0.255825 0.522714 
time: 1403636979.001667, t: 4.190078 -4.303137 -1.214645 q: 0.169128 0.788673 -0.252663 0.534371 
time: 1403636979.101666, t: 4.214199 -4.356043 -1.230240 q: 0.173058 0.780880 -0.249838 0.545764 
time: 1403636979.201667, t: 4.239973 -4.408369 -1.256413 q: 0.174313 0.777985 -0.248821 0.549948 
time: 1403636979.301666, t: 4.265025 -4.454600 -1.286362 q: 0.172330 0.783195 -0.250070 0.542562 
time: 1403636979.401666, t: 4.292667 -4.503264 -1.310121 q: 0.170532 0.789280 -0.251031 0.533797 
time: 1403636979.501667, t: 4.323736 -4.552272 -1.325295 q: 0.167409 0.793298 -0.253096 0.527821 
time: 1403636979.601666, t: 4.359996 -4.599692 -1.334353 q: 0.166748 0.795244 -0.254187 0.524567 
time: 1403636979.701667, t: 4.400607 -4.649776 -1.333999 q: 0.168856 0.796373 -0.254358 0.522092 
time: 1403636979.801666, t: 4.447556 -4.697920 -1.328232 q: 0.170527 0.795020 -0.255202 0.523198 
time: 1403636979.901666, t: 4.497937 -4.745601 -1.318294 q: 0.174144 0.789568 -0.255364 0.530137 
time: 1403636980.001667, t: 4.552003 -4.791476 -1.310068 q: 0.178400 0.785602 -0.255130 0.534707 
time: 1403636980.101666, t: 4.606105 -4.836776 -1.307280 q: 0.180745 0.783532 -0.255728 0.536667 
time: 1403636980.201667, t: 4.661545 -4.881731 -1.308964 q: 0.181345 0.781951 -0.257335 0.538002 
time: 1403636980.301666, t: 4.720972 -4.929147 -1.319103 q: 0.180709 0.779210 -0.259290 0.541243 
time: 1403636980.401666, t: 4.779755 -4.972516 -1.329354 q: 0.180254 0.774657 -0.260153 0.547482 
time: 1403636980.501667, t: 4.840549 -5.014370 -1.349695 q: 0.178950 0.770943 -0.261140 0.552658 
time: 1403636980.601666, t: 4.900850 -5.054888 -1.368858 q: 0.174822 0.768495 -0.262983 0.556500 
time: 1403636980.701667, t: 4.957041 -5.094189 -1.388432 q: 0.173707 0.767702 -0.262346 0.558242 
wait for imu ... 
time: 1403636980.801666, t: 5.013022 -5.132290 -1.397808 q: 0.176177 0.771381 -0.259377 0.553766 
time: 1403636980.901666, t: 5.064546 -5.167553 -1.403752 q: 0.177623 0.777134 -0.256613 0.546501 
time: 1403636981.001667, t: 5.117422 -5.202803 -1.403918 q: 0.179557 0.780500 -0.253896 0.542325 
time: 1403636981.101666, t: 5.164816 -5.228863 -1.404524 q: 0.179010 0.779620 -0.254066 0.543690 
time: 1403636981.201667, t: 5.221742 -5.261809 -1.413191 q: 0.173548 0.776816 -0.258557 0.547345 
time: 1403636981.301666, t: 5.276407 -5.294312 -1.427116 q: 0.163870 0.771194 -0.266923 0.554220 
time: 1403636981.401666, t: 5.319871 -5.320272 -1.444403 q: 0.158089 0.767410 -0.272772 0.558288 
time: 1403636981.501667, t: 5.362533 -5.350075 -1.466236 q: 0.165192 0.769654 -0.270475 0.554246 
time: 1403636981.601666, t: 5.408860 -5.385287 -1.483234 q: 0.177127 0.775145 -0.264936 0.545513 
time: 1403636981.701667, t: 5.452843 -5.418634 -1.494013 q: 0.185077 0.779131 -0.262303 0.538422 
time: 1403636981.801666, t: 5.487374 -5.444338 -1.495783 q: 0.188819 0.781974 -0.262969 0.532646 
time: 1403636981.901666, t: 5.535367 -5.476421 -1.489698 q: 0.189615 0.786084 -0.265190 0.525159 
time: 1403636982.001667, t: 5.572163 -5.496804 -1.477647 q: 0.186578 0.790745 -0.270027 0.516717 
time: 1403636982.101666, t: 5.635071 -5.533016 -1.472513 q: 0.181916 0.794979 -0.274877 0.509272 
time: 1403636982.201667, t: 5.685757 -5.559074 -1.469329 q: 0.177416 0.795950 -0.279233 0.506968 
time: 1403636982.301666, t: 5.756739 -5.600369 -1.472545 q: 0.171477 0.793038 -0.283465 0.511208 
time: 1403636982.401666, t: 5.821412 -5.639402 -1.475417 q: 0.169850 0.786338 -0.284352 0.521505 
time: 1403636982.501667, t: 5.891570 -5.685658 -1.475006 q: 0.170712 0.777455 -0.282400 0.535417 
time: 1403636982.601666, t: 5.968086 -5.739576 -1.468532 q: 0.173537 0.773262 -0.278730 0.542458 
time: 1403636982.701667, t: 6.043299 -5.795271 -1.460388 q: 0.170870 0.773917 -0.278473 0.542502 
time: 1403636982.801666, t: 6.117834 -5.853214 -1.452034 q: 0.170664 0.778019 -0.276280 0.537801 
time: 1403636982.901666, t: 6.187882 -5.911781 -1.446562 q: 0.172324 0.781500 -0.274214 0.533263 
time: 1403636983.001667, t: 6.264667 -5.974181 -1.446782 q: 0.175020 0.781181 -0.271775 0.534100 
time: 1403636983.101666, t: 6.339227 -6.038155 -1.453982 q: 0.177891 0.777602 -0.269290 0.539605 
time: 1403636983.201667, t: 6.420733 -6.103631 -1.463973 q: 0.182547 0.775867 -0.266163 0.542092 
time: 1403636983.301666, t: 6.496459 -6.167547 -1.481506 q: 0.186490 0.774666 -0.263852 0.543595 
time: 1403636983.401666, t: 6.570723 -6.229036 -1.504405 q: 0.189008 0.773843 -0.262859 0.544379 
time: 1403636983.501667, t: 6.645525 -6.290024 -1.525546 q: 0.188962 0.773642 -0.264098 0.544081 
time: 1403636983.601666, t: 6.719189 -6.346903 -1.541275 q: 0.189959 0.775712 -0.265041 0.540315 
time: 1403636983.701667, t: 6.797097 -6.402891 -1.547696 q: 0.189649 0.779162 -0.267336 0.534294 
time: 1403636983.801666, t: 6.868704 -6.453727 -1.548204 q: 0.190807 0.781940 -0.268317 0.529310 
time: 1403636983.901666, t: 6.941589 -6.502576 -1.537338 q: 0.190828 0.784086 -0.270065 0.525223 
time: 1403636984.001667, t: 7.017270 -6.550252 -1.525108 q: 0.190384 0.785092 -0.272517 0.522609 
time: 1403636984.101666, t: 7.090520 -6.591468 -1.502379 q: 0.190508 0.785688 -0.274684 0.520528 
time: 1403636984.201667, t: 7.173520 -6.639142 -1.489973 q: 0.193029 0.783039 -0.274772 0.523536 
time: 1403636984.301666, t: 7.252830 -6.680131 -1.480353 q: 0.196191 0.778695 -0.274534 0.528937 
time: 1403636984.401666, t: 7.343108 -6.727621 -1.480075 q: 0.197670 0.775705 -0.275139 0.532453 
time: 1403636984.501667, t: 7.430299 -6.769506 -1.486456 q: 0.200003 0.773206 -0.274851 0.535358 
time: 1403636984.601666, t: 7.523760 -6.813329 -1.496194 q: 0.200837 0.772376 -0.274539 0.536403 
time: 1403636984.701667, t: 7.616694 -6.854105 -1.501551 q: 0.201263 0.772157 -0.273703 0.536986 
time: 1403636984.801666, t: 7.710795 -6.891666 -1.503031 q: 0.198406 0.770880 -0.274961 0.539236 
time: 1403636984.901666, t: 7.807742 -6.930987 -1.501141 q: 0.193042 0.770007 -0.278085 0.540826 
time: 1403636985.001667, t: 7.893699 -6.961923 -1.492702 q: 0.191533 0.769371 -0.278980 0.541807 
time: 1403636985.101666, t: 7.981565 -6.994392 -1.481747 q: 0.192758 0.770009 -0.278379 0.540773 
time: 1403636985.201667, t: 8.068959 -7.026251 -1.479077 q: 0.195326 0.771293 -0.276961 0.538746 
time: 1403636985.301666, t: 8.153313 -7.054093 -1.490720 q: 0.197754 0.771145 -0.276038 0.538546 
time: 1403636985.401666, t: 8.246760 -7.082183 -1.504057 q: 0.197391 0.772630 -0.272526 0.538339 
time: 1403636985.501667, t: 8.331219 -7.105218 -1.516159 q: 0.194906 0.776592 -0.263088 0.538239 
time: 1403636985.601666, t: 8.425735 -7.130550 -1.517720 q: 0.192160 0.778880 -0.253826 0.540364 
time: 1403636985.701667, t: 8.520949 -7.152140 -1.511039 q: 0.192306 0.780283 -0.245136 0.542297 
time: 1403636985.801666, t: 8.615143 -7.167808 -1.497849 q: 0.191903 0.779361 -0.238621 0.546653 
time: 1403636985.901666, t: 8.704931 -7.177115 -1.481426 q: 0.193448 0.770047 -0.231884 0.561993 
time: 1403636986.001667, t: 8.793929 -7.182038 -1.472633 q: 0.195960 0.759216 -0.225803 0.578104 
time: 1403636986.101666, t: 8.876721 -7.178989 -1.475475 q: 0.196268 0.758778 -0.223607 0.579426 
time: 1403636986.201667, t: 8.956971 -7.168012 -1.486473 q: 0.191539 0.766974 -0.226612 0.568956 
time: 1403636986.301666, t: 9.028662 -7.148559 -1.501486 q: 0.187617 0.776402 -0.229646 0.556114 
time: 1403636986.401666, t: 9.101732 -7.125729 -1.518723 q: 0.183468 0.781190 -0.232441 0.549594 
time: 1403636986.501667, t: 9.174035 -7.098028 -1.536638 q: 0.175975 0.782221 -0.237460 0.548431 
time: 1403636986.601666, t: 9.246206 -7.066767 -1.547090 q: 0.169187 0.781321 -0.241761 0.549968 
time: 1403636986.701667, t: 9.317580 -7.034978 -1.549806 q: 0.164269 0.780707 -0.245043 0.550878 
time: 1403636986.801666, t: 9.386576 -7.004274 -1.542107 q: 0.158852 0.779589 -0.248625 0.552443 
time: 1403636986.901666, t: 9.453112 -6.975904 -1.525748 q: 0.157906 0.778326 -0.249753 0.553983 
time: 1403636987.001667, t: 9.515994 -6.951404 -1.500151 q: 0.160548 0.777796 -0.248510 0.554528 
time: 1403636987.101666, t: 9.572925 -6.926599 -1.467582 q: 0.166184 0.777492 -0.246171 0.554336 
wait for imu ... 
time: 1403636987.201667, t: 9.625917 -6.903680 -1.438819 q: 0.168977 0.776432 -0.245871 0.555111 
time: 1403636987.301666, t: 9.676199 -6.878415 -1.416363 q: 0.170370 0.774714 -0.246784 0.556678 
time: 1403636987.401666, t: 9.724452 -6.853599 -1.399489 q: 0.170007 0.773441 -0.246097 0.558858 
time: 1403636987.501667, t: 9.772644 -6.827330 -1.388423 q: 0.171173 0.771660 -0.240579 0.563350 
wait for imu ... 
time: 1403636987.601666, t: 9.817585 -6.797844 -1.380307 q: 0.173849 0.769506 -0.234717 0.567930 
wait for imu ... 
time: 1403636987.701667, t: 9.859266 -6.764657 -1.376783 q: 0.173886 0.768700 -0.231423 0.570358 
wait for imu ... 
time: 1403636987.801666, t: 9.897332 -6.727228 -1.369978 q: 0.171222 0.768883 -0.229822 0.571562 
time: 1403636987.901666, t: 9.930814 -6.688614 -1.356773 q: 0.166039 0.769750 -0.230781 0.571538 
wait for imu ... 
time: 1403636988.001667, t: 9.959887 -6.646418 -1.338677 q: 0.156985 0.771901 -0.235210 0.569386 
time: 1403636988.101666, t: 9.983343 -6.603672 -1.323979 q: 0.148058 0.774380 -0.241164 0.565911 
time: 1403636988.201667, t: 10.002146 -6.562372 -1.314470 q: 0.142278 0.775191 -0.245755 0.564305 
wait for imu ... 
time: 1403636988.301666, t: 10.015592 -6.525060 -1.307803 q: 0.139411 0.771406 -0.248690 0.568903 
wait for imu ... 
time: 1403636988.401666, t: 10.024072 -6.491729 -1.299547 q: 0.140547 0.764870 -0.248647 0.577404 
time: 1403636988.501667, t: 10.024474 -6.460480 -1.282037 q: 0.143985 0.760629 -0.246393 0.583096 
time: 1403636988.601666, t: 10.014554 -6.429698 -1.258133 q: 0.144604 0.758301 -0.245805 0.586216 
wait for imu ... 
time: 1403636988.701667, t: 9.998613 -6.399470 -1.234630 q: 0.144443 0.757216 -0.246049 0.587555 
time: 1403636988.801666, t: 9.972444 -6.370497 -1.216001 q: 0.145820 0.755504 -0.245032 0.589839 
time: 1403636988.901666, t: 9.940487 -6.340083 -1.203483 q: 0.145561 0.752881 -0.245340 0.593120 
time: 1403636989.001667, t: 9.895075 -6.312869 -1.196246 q: 0.144871 0.750230 -0.246135 0.596309 
wait for imu ... 
time: 1403636989.101666, t: 9.842386 -6.284371 -1.194124 q: 0.144597 0.748404 -0.247187 0.598233 
time: 1403636989.201667, t: 9.774606 -6.258276 -1.193734 q: 0.144737 0.748388 -0.248076 0.597850 
wait for imu ... 
time: 1403636989.301666, t: 9.698934 -6.232373 -1.190692 q: 0.150681 0.754675 -0.246018 0.589267 
time: 1403636989.401666, t: 9.612187 -6.209136 -1.179820 q: 0.157189 0.764590 -0.243467 0.575689 
time: 1403636989.501667, t: 9.523138 -6.186330 -1.163161 q: 0.158991 0.772308 -0.243642 0.564713 
time: 1403636989.601666, t: 9.431341 -6.165189 -1.140760 q: 0.161899 0.776138 -0.242596 0.559058 
wait for imu ... 
time: 1403636989.701667, t: 9.337928 -6.143275 -1.121070 q: 0.166417 0.779226 -0.239792 0.554627 
time: 1403636989.801666, t: 9.245165 -6.120103 -1.107706 q: 0.168272 0.780627 -0.238704 0.552563 
time: 1403636989.901666, t: 9.153626 -6.095273 -1.099601 q: 0.167870 0.778997 -0.238828 0.554928 
time: 1403636990.001667, t: 9.063709 -6.071257 -1.093573 q: 0.167793 0.774296 -0.238650 0.561567 
time: 1403636990.101666, t: 8.970217 -6.043339 -1.088414 q: 0.169011 0.770237 -0.237612 0.567196 
time: 1403636990.201667, t: 8.877524 -6.015698 -1.074915 q: 0.170339 0.770043 -0.236258 0.567627 
time: 1403636990.301666, t: 8.777578 -5.982568 -1.056638 q: 0.170599 0.773461 -0.234915 0.563444 
time: 1403636990.401666, t: 8.680895 -5.952597 -1.031727 q: 0.170470 0.776934 -0.233379 0.559328 
time: 1403636990.501667, t: 8.579363 -5.914807 -1.010861 q: 0.167091 0.779771 -0.233498 0.556342 
time: 1403636990.601666, t: 8.484679 -5.879523 -0.994188 q: 0.165576 0.781538 -0.232724 0.554638 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636990.701667, t: 8.386380 -5.842700 -0.985114 q: 0.164643 0.783787 -0.231308 0.552328 
time: 1403636990.801666, t: 8.293783 -5.803716 -0.983153 q: 0.164730 0.785102 -0.229861 0.551038 
wait for imu ... 
time: 1403636990.901666, t: 8.198388 -5.763534 -0.988684 q: 0.163784 0.785435 -0.229267 0.551093 
time: 1403636991.001667, t: 8.108319 -5.725870 -0.997347 q: 0.164304 0.785533 -0.228485 0.551123 
time: 1403636991.101666, t: 8.017573 -5.686044 -1.009838 q: 0.165164 0.785066 -0.228057 0.551708 
time: 1403636991.201667, t: 7.932203 -5.646652 -1.024252 q: 0.166348 0.785125 -0.228151 0.551229 
wait for imu ... 
time: 1403636991.301666, t: 7.842567 -5.608818 -1.033606 q: 0.166023 0.785490 -0.228849 0.550517 
time: 1403636991.401666, t: 7.756219 -5.565486 -1.034643 q: 0.166112 0.785917 -0.230137 0.549343 
wait for imu ... 
time: 1403636991.501667, t: 7.673224 -5.525604 -1.025936 q: 0.167296 0.786128 -0.231055 0.548295 
time: 1403636991.601666, t: 7.596031 -5.482122 -1.007791 q: 0.165449 0.785228 -0.234463 0.548699 
time: 1403636991.701667, t: 7.518384 -5.441018 -0.984490 q: 0.159453 0.783614 -0.241425 0.549761 
time: 1403636991.801666, t: 7.442092 -5.399311 -0.953626 q: 0.154825 0.781954 -0.247851 0.550588 
time: 1403636991.901666, t: 7.362759 -5.360978 -0.927163 q: 0.152205 0.781091 -0.252786 0.550299 
time: 1403636992.001667, t: 7.282524 -5.324631 -0.903261 q: 0.154926 0.780778 -0.253830 0.549503 
wait for imu ... 
time: 1403636992.101666, t: 7.200385 -5.290570 -0.891443 q: 0.164382 0.779301 -0.249950 0.550630 
time: 1403636992.201667, t: 7.119756 -5.261864 -0.881329 q: 0.171136 0.776318 -0.247108 0.554058 
time: 1403636992.301666, t: 7.041277 -5.231586 -0.880664 q: 0.174669 0.775215 -0.246497 0.554772 
time: 1403636992.401666, t: 6.963239 -5.203712 -0.883017 q: 0.176291 0.777217 -0.247503 0.550997 
wait for imu ... 
time: 1403636992.501667, t: 6.889219 -5.175232 -0.884359 q: 0.176786 0.780134 -0.249492 0.545795 
time: 1403636992.601666, t: 6.816502 -5.145644 -0.874602 q: 0.178656 0.781727 -0.250554 0.542410 
wait for imu ... 
time: 1403636992.701667, t: 6.747013 -5.114138 -0.860544 q: 0.180112 0.782212 -0.251720 0.540685 
time: 1403636992.801666, t: 6.680094 -5.082835 -0.842456 q: 0.181927 0.782831 -0.252595 0.538771 
time: 1403636992.901666, t: 6.615553 -5.050292 -0.835187 q: 0.185006 0.782570 -0.252453 0.538167 
time: 1403636993.001667, t: 6.555792 -5.015770 -0.830515 q: 0.186888 0.780711 -0.252944 0.539984 
time: 1403636993.101666, t: 6.497197 -4.981609 -0.831396 q: 0.184230 0.776821 -0.254768 0.545620 
time: 1403636993.201667, t: 6.441797 -4.945021 -0.829117 q: 0.177006 0.774976 -0.252246 0.551772 
time: 1403636993.301666, t: 6.385870 -4.906343 -0.822676 q: 0.168484 0.776344 -0.243657 0.556359 
time: 1403636993.401666, t: 6.332391 -4.867931 -0.805044 q: 0.158401 0.779290 -0.230477 0.560801 
time: 1403636993.501667, t: 6.277194 -4.827361 -0.781270 q: 0.146551 0.783498 -0.214634 0.564434 
time: 1403636993.601666, t: 6.226466 -4.790563 -0.753083 q: 0.135026 0.788315 -0.197570 0.566827 
time: 1403636993.701667, t: 6.173977 -4.754084 -0.724642 q: 0.124717 0.793301 -0.184370 0.566681 
time: 1403636993.801666, t: 6.119125 -4.718144 -0.700191 q: 0.118042 0.796915 -0.174621 0.566128 
time: 1403636993.901666, t: 6.064341 -4.683486 -0.681836 q: 0.115303 0.799108 -0.166038 0.566183 
time: 1403636994.001667, t: 6.009508 -4.648131 -0.668332 q: 0.113101 0.797183 -0.160750 0.570848 
time: 1403636994.101666, t: 5.955441 -4.612300 -0.659556 q: 0.115035 0.792555 -0.155663 0.578267 
time: 1403636994.201667, t: 5.898031 -4.571676 -0.656774 q: 0.116324 0.787577 -0.152528 0.585599 
time: 1403636994.301666, t: 5.839480 -4.532536 -0.657980 q: 0.116897 0.783057 -0.151315 0.591829 
time: 1403636994.401666, t: 5.775874 -4.486203 -0.664788 q: 0.118171 0.780412 -0.150584 0.595245 
time: 1403636994.501667, t: 5.710757 -4.442189 -0.672199 q: 0.118183 0.781050 -0.151312 0.594220 
time: 1403636994.601666, t: 5.637423 -4.390222 -0.674731 q: 0.113956 0.784159 -0.154955 0.589998 
time: 1403636994.701667, t: 5.559028 -4.335159 -0.670747 q: 0.107036 0.789511 -0.160939 0.582507 
time: 1403636994.801666, t: 5.482069 -4.281596 -0.661417 q: 0.097394 0.794825 -0.168992 0.574639 
time: 1403636994.901666, t: 5.402342 -4.229717 -0.649256 q: 0.090187 0.796667 -0.175130 0.571417 
time: 1403636995.001667, t: 5.320458 -4.182212 -0.641952 q: 0.090141 0.794995 -0.175964 0.573493 
wait for imu ... 
time: 1403636995.101666, t: 5.238319 -4.137631 -0.638771 q: 0.097279 0.791984 -0.170742 0.578053 
wait for imu ... 
time: 1403636995.201667, t: 5.152279 -4.092657 -0.634707 q: 0.103476 0.789373 -0.165507 0.582057 
time: 1403636995.301666, t: 5.064946 -4.049065 -0.624449 q: 0.106869 0.787985 -0.161814 0.584359 
time: 1403636995.401666, t: 4.972089 -4.003207 -0.606515 q: 0.108714 0.790406 -0.158651 0.581609 
time: 1403636995.501667, t: 4.877077 -3.956644 -0.583174 q: 0.107709 0.795068 -0.158142 0.575550 
time: 1403636995.601666, t: 4.780758 -3.907696 -0.553661 q: 0.106886 0.796834 -0.157992 0.573297 
wait for imu ... 
time: 1403636995.701667, t: 4.685321 -3.861530 -0.524411 q: 0.104528 0.796856 -0.159700 0.573228 
time: 1403636995.801666, t: 4.586874 -3.813834 -0.501521 q: 0.099859 0.796836 -0.163359 0.573057 
wait for imu ... 
time: 1403636995.901666, t: 4.489049 -3.767710 -0.485244 q: 0.092482 0.797898 -0.169109 0.571146 
time: 1403636996.001667, t: 4.390359 -3.722965 -0.474536 q: 0.088967 0.798930 -0.172411 0.569271 
wait for imu ... 
time: 1403636996.101666, t: 4.293350 -3.684319 -0.467670 q: 0.094072 0.800821 -0.169351 0.566706 
time: 1403636996.201667, t: 4.194810 -3.646341 -0.461040 q: 0.099526 0.802004 -0.165817 0.565145 
time: 1403636996.301666, t: 4.098250 -3.609545 -0.448718 q: 0.103401 0.802693 -0.162411 0.564459 
time: 1403636996.401666, t: 4.004756 -3.575895 -0.426878 q: 0.101087 0.803805 -0.157843 0.564593 
time: 1403636996.501667, t: 3.915786 -3.543478 -0.395979 q: 0.096700 0.805633 -0.148632 0.565255 
time: 1403636996.601666, t: 3.828078 -3.509898 -0.360542 q: 0.089777 0.808429 -0.135736 0.565648 
time: 1403636996.701667, t: 3.742288 -3.475324 -0.327493 q: 0.079688 0.811748 -0.119703 0.566026 
time: 1403636996.801666, t: 3.659794 -3.439549 -0.309658 q: 0.067146 0.815102 -0.101967 0.566307 
wait for imu ... 
time: 1403636996.901666, t: 3.572860 -3.403682 -0.305457 q: 0.051579 0.817922 -0.083761 0.566857 
time: 1403636997.001667, t: 3.491155 -3.368216 -0.305791 q: 0.036341 0.820243 -0.064241 0.567234 
time: 1403636997.101666, t: 3.412874 -3.334678 -0.312313 q: 0.022890 0.822199 -0.046307 0.566851 
time: 1403636997.201667, t: 3.334720 -3.301945 -0.323119 q: 0.012830 0.823592 -0.033411 0.566053 
time: 1403636997.401666, t: 3.199526 -3.249097 -0.349963 q: 0.004142 0.825102 -0.017486 0.564698 
time: 1403636997.501667, t: 3.136411 -3.222807 -0.352142 q: 0.003165 0.824768 -0.014657 0.565273 
time: 1403636997.601666, t: 3.077516 -3.195571 -0.347042 q: 0.003108 0.825493 -0.014566 0.564216 
time: 1403636997.701667, t: 3.023794 -3.171858 -0.332387 q: 0.004848 0.825916 -0.014838 0.563577 
time: 1403636997.801666, t: 2.983607 -3.147877 -0.312451 q: 0.007987 0.827032 -0.015871 0.561874 
optimize pose graph 
time: 1403636997.901666, t: 2.943405 -3.125280 -0.283582 q: 0.010134 0.827250 -0.017473 0.561471 
time: 1403636998.001667, t: 2.907816 -3.104867 -0.248165 q: 0.011156 0.827208 -0.019652 0.561441 
time: 1403636998.101666, t: 2.885470 -3.084861 -0.209722 q: 0.012109 0.827454 -0.021978 0.560973 
time: 1403636998.201667, t: 2.866107 -3.067760 -0.170094 q: 0.011152 0.826175 -0.025099 0.562744 
time: 1403636998.301666, t: 2.850130 -3.052715 -0.137999 q: 0.012130 0.826703 -0.026519 0.561882 
time: 1403636998.401666, t: 2.833746 -3.032190 -0.112910 q: 0.012036 0.826281 -0.028240 0.562421 
time: 1403636998.501667, t: 2.829207 -3.018226 -0.090035 q: 0.011460 0.826080 -0.030236 0.562624 
time: 1403636998.601666, t: 2.816978 -3.000927 -0.061059 q: 0.013129 0.822866 -0.030025 0.567289 
time: 1403636998.701667, t: 2.808099 -2.984853 -0.025017 q: 0.016571 0.814697 -0.028475 0.578950 
wait for imu ... 
time: 1403636998.801666, t: 2.801959 -2.970378 0.014731 q: 0.019821 0.809506 -0.027583 0.586128 
time: 1403636998.901666, t: 2.789204 -2.954434 0.052614 q: 0.025496 0.807932 -0.024258 0.588225 
wait for imu ... 
time: 1403636999.001667, t: 2.775648 -2.937863 0.083968 q: 0.029320 0.808323 -0.022514 0.587577 
time: 1403636999.101666, t: 2.756236 -2.916926 0.108099 q: 0.032237 0.808553 -0.021033 0.587162 
time: 1403636999.201667, t: 2.736183 -2.892965 0.126628 q: 0.033863 0.810232 -0.020727 0.584762 
time: 1403636999.301666, t: 2.716670 -2.865983 0.141342 q: 0.032677 0.811914 -0.022047 0.582445 
time: 1403636999.401666, t: 2.693423 -2.836029 0.150272 q: 0.027976 0.812474 -0.025771 0.581755 
time: 1403636999.501667, t: 2.667594 -2.804239 0.153556 q: 0.025114 0.812075 -0.028038 0.582337 
time: 1403636999.601666, t: 2.640733 -2.770042 0.156988 q: 0.023557 0.809858 -0.029598 0.585404 
time: 1403636999.701667, t: 2.612748 -2.736117 0.153659 q: 0.023111 0.807938 -0.030606 0.588018 
time: 1403636999.901666, t: 2.549289 -2.666435 0.142162 q: 0.024845 0.807732 -0.030889 0.588216 
time: 1403637000.001667, t: 2.512333 -2.630058 0.137008 q: 0.029050 0.807672 -0.028413 0.588231 
time: 1403637000.101666, t: 2.472600 -2.590502 0.138053 q: 0.031340 0.808703 -0.026704 0.586774 
time: 1403637000.201667, t: 2.432812 -2.549938 0.145745 q: 0.030491 0.810442 -0.026978 0.584402 
time: 1403637000.301666, t: 2.390918 -2.507998 0.153953 q: 0.029233 0.811887 -0.027884 0.582415 
wait for imu ... 
time: 1403637000.401666, t: 2.350573 -2.465020 0.162269 q: 0.027131 0.811352 -0.029423 0.583186 
time: 1403637000.501667, t: 2.306873 -2.421753 0.163908 q: 0.029199 0.811342 -0.028141 0.583164 
time: 1403637000.601666, t: 2.264533 -2.375795 0.167130 q: 0.029996 0.811554 -0.027824 0.582843 
wait for imu ... 
time: 1403637000.701667, t: 2.218599 -2.329629 0.174301 q: 0.027880 0.810665 -0.029451 0.584105 
time: 1403637000.801666, t: 2.172879 -2.283789 0.188853 q: 0.027796 0.810049 -0.030073 0.584930 
wait for imu ... 
time: 1403637000.901666, t: 2.129722 -2.235117 0.206813 q: 0.027294 0.810121 -0.031212 0.584795 
time: 1403637001.001667, t: 2.079769 -2.188309 0.217287 q: 0.027470 0.809814 -0.031983 0.585170 
time: 1403637001.101666, t: 2.028948 -2.139288 0.221369 q: 0.027151 0.809626 -0.032745 0.585403 
time: 1403637001.201667, t: 1.978195 -2.089766 0.225927 q: 0.026256 0.808957 -0.034022 0.586295 
time: 1403637001.301666, t: 1.922515 -2.040306 0.232379 q: 0.025419 0.809084 -0.034913 0.586104 
wait for imu ... 
time: 1403637001.401666, t: 1.864529 -1.990413 0.248467 q: 0.023109 0.810379 -0.037010 0.584279 
wait for imu ... 
time: 1403637001.501667, t: 1.806282 -1.942063 0.266867 q: 0.023172 0.813355 -0.038128 0.580054 
time: 1403637001.601666, t: 1.746106 -1.894266 0.286642 q: 0.023571 0.814813 -0.039138 0.577921 
time: 1403637001.701667, t: 1.685970 -1.847675 0.302007 q: 0.023964 0.814907 -0.039851 0.577723 
wait for imu ... 
time: 1403637001.801666, t: 1.622415 -1.799706 0.311478 q: 0.023420 0.813919 -0.041276 0.579037 
time: 1403637001.901666, t: 1.562941 -1.755923 0.315320 q: 0.024402 0.815444 -0.041176 0.576854 
time: 1403637002.001667, t: 1.500117 -1.709923 0.311675 q: 0.024971 0.816220 -0.041244 0.575726 
time: 1403637002.101666, t: 1.443142 -1.667648 0.306717 q: 0.025418 0.816847 -0.041425 0.574803 
time: 1403637002.201667, t: 1.382556 -1.624980 0.293199 q: 0.025157 0.816932 -0.041630 0.574679 
time: 1403637002.301666, t: 1.328134 -1.584977 0.282079 q: 0.026531 0.816972 -0.040795 0.574620 
wait for imu ... 
time: 1403637002.401666, t: 1.268011 -1.543878 0.272272 q: 0.028456 0.817423 -0.039169 0.573999 
time: 1403637002.501667, t: 1.212718 -1.501215 0.272398 q: 0.030067 0.816887 -0.037929 0.574763 
time: 1403637002.601666, t: 1.157402 -1.459350 0.278169 q: 0.032720 0.816523 -0.036071 0.575256 
time: 1403637002.701667, t: 1.103503 -1.415421 0.293260 q: 0.034020 0.816832 -0.036016 0.574744 
time: 1403637002.801666, t: 1.047602 -1.371555 0.306267 q: 0.034190 0.816565 -0.037065 0.575046 
wait for imu ... 
time: 1403637002.901666, t: 0.992457 -1.327593 0.314976 q: 0.031069 0.815993 -0.040426 0.575809 
time: 1403637003.001667, t: 0.939536 -1.281280 0.316973 q: 0.030810 0.816234 -0.042049 0.575364 
time: 1403637003.101666, t: 0.888468 -1.237550 0.310849 q: 0.030695 0.816548 -0.043932 0.574784 
time: 1403637003.201667, t: 0.837860 -1.191610 0.301623 q: 0.031545 0.816090 -0.045067 0.575300 
time: 1403637003.301666, t: 0.786681 -1.146357 0.292204 q: 0.031533 0.814969 -0.046615 0.576766 
time: 1403637003.401666, t: 0.738079 -1.102817 0.283711 q: 0.031413 0.813642 -0.047745 0.578550 
time: 1403637003.501667, t: 0.689640 -1.056369 0.285025 q: 0.030969 0.812380 -0.049384 0.580207 
time: 1403637003.601666, t: 0.640930 -1.012275 0.288913 q: 0.030443 0.812576 -0.050694 0.579848 
time: 1403637003.701667, t: 0.590068 -0.967477 0.303484 q: 0.030345 0.812775 -0.051491 0.579503 
wait for imu ... 
time: 1403637003.801666, t: 0.526947 -0.918476 0.310342 q: 0.028761 0.813174 -0.051907 0.578987 
time: 1403637003.901666, t: 0.477677 -0.876182 0.316793 q: 0.030134 0.815032 -0.049757 0.576489 
time: 1403637004.001667, t: 0.416459 -0.831329 0.312606 q: 0.030665 0.817639 -0.047002 0.572989 
time: 1403637004.101666, t: 0.367871 -0.792351 0.304939 q: 0.031289 0.819743 -0.044689 0.570127 
time: 1403637004.201667, t: 0.315893 -0.750216 0.291589 q: 0.031582 0.820925 -0.042801 0.568553 
time: 1403637004.301666, t: 0.270031 -0.707004 0.283093 q: 0.029421 0.821053 -0.042378 0.568516 
wait for imu ... 
time: 1403637004.401666, t: 0.226429 -0.665559 0.283950 q: 0.027113 0.819921 -0.044024 0.570137 
time: 1403637004.501667, t: 0.189586 -0.626046 0.297589 q: 0.027183 0.815637 -0.045445 0.576136 
time: 1403637004.601666, t: 0.151002 -0.587742 0.322758 q: 0.026843 0.813220 -0.048369 0.579321 
time: 1403637004.701667, t: 0.112241 -0.552069 0.352222 q: 0.018659 0.812643 -0.057424 0.579625 
time: 1403637004.801666, t: 0.070023 -0.516562 0.372588 q: 0.012108 0.812103 -0.065360 0.579716 
wait for imu ... 
time: 1403637004.901666, t: 0.033648 -0.489015 0.378005 q: 0.010667 0.814866 -0.069197 0.575405 
time: 1403637005.001667, t: -0.012446 -0.456192 0.368697 q: 0.014351 0.819104 -0.069176 0.569278 
time: 1403637005.101666, t: -0.051330 -0.434079 0.352196 q: 0.022620 0.819042 -0.065239 0.569563 
time: 1403637005.201667, t: -0.090388 -0.413739 0.334679 q: 0.029892 0.817972 -0.061428 0.571187 
time: 1403637005.301666, t: -0.125958 -0.398203 0.312258 q: 0.033446 0.819901 -0.059052 0.568468 
time: 1403637005.401666, t: -0.150826 -0.393330 0.286234 q: 0.034118 0.820966 -0.057052 0.567094 
time: 1403637005.501667, t: -0.184443 -0.373367 0.264887 q: 0.035883 0.818012 -0.053207 0.571610 
time: 1403637005.601666, t: -0.216997 -0.354118 0.248690 q: 0.033883 0.817180 -0.052017 0.573030 
time: 1403637005.701667, t: -0.248045 -0.336172 0.242659 q: 0.027300 0.821346 -0.054258 0.567187 
time: 1403637005.801666, t: -0.273335 -0.321594 0.247745 q: 0.024006 0.823038 -0.055036 0.564804 
time: 1403637005.901666, t: -0.297089 -0.305557 0.260837 q: 0.027391 0.821470 -0.051760 0.567236 
time: 1403637006.001667, t: -0.318632 -0.291474 0.272625 q: 0.029937 0.819072 -0.049759 0.570744 
time: 1403637006.101666, t: -0.337477 -0.277740 0.278647 q: 0.028122 0.817116 -0.051011 0.573522 
time: 1403637006.201667, t: -0.358628 -0.262860 0.283882 q: 0.027208 0.817178 -0.051415 0.573442 
time: 1403637006.301666, t: -0.378221 -0.249007 0.285617 q: 0.024514 0.821383 -0.053334 0.567349 
time: 1403637006.401666, t: -0.395015 -0.235386 0.285981 q: 0.022286 0.827908 -0.054886 0.557727 
time: 1403637006.501667, t: -0.409550 -0.224301 0.285498 q: 0.016177 0.830559 -0.058955 0.553566 
time: 1403637006.701667, t: -0.420286 -0.211675 0.281538 q: 0.010831 0.821473 -0.046203 0.568270 
time: 1403637006.801666, t: -0.421896 -0.208465 0.275263 q: 0.005030 0.817539 -0.032104 0.574955 
time: 1403637006.901666, t: -0.423371 -0.206117 0.265671 q: -0.006730 0.817744 -0.016114 0.575317 
wait for imu ... 
time: 1403637007.001667, t: -0.424302 -0.205411 0.255571 q: -0.016647 0.819140 0.005995 0.573321 
wait for imu ... 
time: 1403637007.101666, t: -0.425319 -0.205953 0.246248 q: -0.024593 0.817852 0.025044 0.574357 
time: 1403637007.201667, t: -0.427206 -0.206708 0.236105 q: -0.031149 0.815680 0.039384 0.576320 
time: 1403637007.301666, t: -0.429762 -0.207883 0.226726 q: -0.032484 0.810507 0.051181 0.582583 
wait for imu ... 
time: 1403637007.401666, t: -0.435202 -0.208259 0.216948 q: -0.033421 0.807100 0.058013 0.586607 
wait for imu ... 
time: 1403637007.501667, t: -0.443790 -0.207110 0.205200 q: -0.035318 0.808463 0.060705 0.584342 
time: 1403637007.601666, t: -0.454586 -0.204640 0.192245 q: -0.036543 0.813012 0.061183 0.577869 
time: 1403637007.701667, t: -0.467068 -0.200520 0.177470 q: -0.036222 0.821193 0.060537 0.566273 
time: 1403637007.801666, t: -0.477825 -0.194675 0.164535 q: -0.034159 0.824776 0.060204 0.561207 
wait for imu ... 
time: 1403637007.901666, t: -0.485887 -0.186572 0.154793 q: -0.037617 0.819896 0.055633 0.568560 
time: 1403637008.001667, t: -0.493402 -0.177639 0.146128 q: -0.043570 0.813371 0.048779 0.578056 
wait for imu ... 
time: 1403637008.101666, t: -0.502477 -0.169800 0.137639 q: -0.040036 0.815104 0.047820 0.575948 
time: 1403637008.201667, t: -0.511809 -0.162301 0.129411 q: -0.035307 0.824266 0.047617 0.563092 
time: 1403637008.301666, t: -0.518925 -0.154181 0.124902 q: -0.030092 0.830431 0.048219 0.554214 
time: 1403637008.401666, t: -0.521464 -0.144482 0.124656 q: -0.027325 0.830608 0.047427 0.554161 
time: 1403637008.501667, t: -0.518903 -0.133378 0.129932 q: -0.030440 0.823196 0.043031 0.565305 
time: 1403637008.601666, t: -0.514877 -0.121930 0.135847 q: -0.033193 0.817051 0.038681 0.574309 
time: 1403637008.701667, t: -0.510552 -0.111142 0.137490 q: -0.038013 0.814956 0.033277 0.577317 
time: 1403637008.801666, t: -0.506446 -0.101604 0.134404 q: -0.050171 0.818577 0.023084 0.571736 
time: 1403637008.901666, t: -0.500468 -0.097936 0.126992 q: -0.058108 0.822770 0.016723 0.565149 
time: 1403637009.001667, t: -0.492028 -0.102381 0.114731 q: -0.054805 0.823185 0.017692 0.564845 
time: 1403637009.101666, t: -0.482305 -0.109492 0.100154 q: -0.042555 0.821978 0.024682 0.567390 
wait for imu ... 
time: 1403637009.201667, t: -0.471519 -0.119900 0.084014 q: -0.030621 0.820989 0.031982 0.569225 
time: 1403637009.301666, t: -0.459045 -0.131219 0.066121 q: -0.014381 0.820932 0.042600 0.569253 
time: 1403637009.401666, t: -0.445818 -0.139362 0.052197 q: -0.028195 0.816898 0.043930 0.574415 
wait for imu ... 
wait for imu ... 
time: 1403637009.501667, t: -0.440459 -0.146792 0.047502 q: -0.041812 0.807444 0.058037 0.585591 
wait for imu ... 
time: 1403637009.601666, t: -0.441177 -0.153087 0.040496 q: -0.084526 0.785220 0.047467 0.611582 
wait for imu ... 
time: 1403637009.701667, t: -0.449876 -0.157858 0.036899 q: -0.073256 0.790878 0.034243 0.606608 
time: 1403637009.801666, t: -0.448886 -0.156511 0.036978 q: -0.072960 0.790226 0.031056 0.607664 
time: 1403637009.901666, t: -0.450586 -0.156596 0.036574 q: -0.073533 0.789936 0.031991 0.607923 
optimize pose graph 
wait for imu ... 
time: 1403637010.001667, t: -0.449819 -0.154570 0.036739 q: -0.073824 0.789864 0.031943 0.607985 
wait for imu ... 
time: 1403637010.101666, t: -0.449753 -0.153822 0.035735 q: -0.074658 0.789972 0.031660 0.607756 
time: 1403637010.201667, t: -0.452201 -0.155725 0.035875 q: -0.075166 0.789940 0.031695 0.607734 
time: 1403637010.301666, t: -0.452088 -0.155553 0.036038 q: -0.075579 0.790165 0.031599 0.607395 
time: 1403637010.401666, t: -0.451644 -0.155926 0.036206 q: -0.075599 0.790156 0.031599 0.607404 
time: 1403637010.501667, t: -0.451583 -0.156112 0.036210 q: -0.075640 0.790264 0.031534 0.607262 

Done.
time: 1403637010.601666, t: -0.451464 -0.156044 0.036266 q: -0.075756 0.790322 0.031439 0.607177 
killing:
 * /loop_fusion
 * /rosout
 * /vins_estimator
[33m[ WARN] [1720469479.472021652]: Shutdown request received.[0m
[33m[ WARN] [1720469479.472053074]: Reason given for shutdown: [user request][0m
[33m[ WARN] [1720469479.472916110]: Shutdown request received.[0m
[33m[ WARN] [1720469479.472947863]: Reason given for shutdown: [user request][0m
killed
[1m[rosout-1] process has finished cleanly
log file: /root/.ros/log/db090d34-3d65-11ef-8bd1-0242ac110005/rosout-1*.log[0m
[rosout-1] restarting process
