add vo publishing
publish odom with current timestamp
publish odom with current timestamp
publish odom with current timestamp
publish odom with current timestamp
publish odom with current timestamp
fix position estimation
add slam optimization test