diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
index 151a4fecb9..c2e8cb1a26 100644
--- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
+++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_python_setup()
 
 generate_dynamic_reconfigure_options(
+  config/OdometryOffsetReconfigure.cfg
   config/OdometryFeedbackWrapperReconfigure.cfg
   config/ConstantHeightFramePublisherReconfigure.cfg
 )
diff --git a/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml b/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml
index d2b7b70319..bb1d639880 100644
--- a/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml
+++ b/jsk_robot_common/jsk_robot_startup/config/HRP2JSKNT_odometry_params.yaml
@@ -14,6 +14,7 @@ viso_odom_calculator:
 biped_particle_odometry:
   particle_num: 80
   rate: 50
+  use_imu: true
 
 biped_odometry_offset:
   use_twist_filter: true
diff --git a/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg b/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg
index 980b335f31..bc64beca39 100755
--- a/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg
+++ b/jsk_robot_common/jsk_robot_startup/config/OdometryFeedbackWrapperReconfigure.cfg
@@ -5,12 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("sigma_x", double_t, 0, "sigma of distribution on x velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_y", double_t, 0, "sigma of distribution on y velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_z", double_t, 0, "sigma of distribution on z velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_roll", double_t, 0, "sigma of distribution on roll angular velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_pitch", double_t, 0, "sigma of distribution on pitch angular velocity", 0.3, 0.0001, 10.0)
-gen.add("sigma_yaw", double_t, 0, "sigma of distribution on yaw angular velocity", 0.3, 0.0001, 10.0)
 gen.add("force_feedback_sigma", double_t, 0, "Odometry feedback is forcely enabled when sigma is larger than this value", 0.0001, 0.3, 10.0)
 gen.add("distribution_feedback_minimum_sigma", double_t, 0, "Distribution check is enabled when sigma is larger than this value", 0.0001, 0.3, 10.0)
 
diff --git a/jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg b/jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg
new file mode 100755
index 0000000000..a0a864b83b
--- /dev/null
+++ b/jsk_robot_common/jsk_robot_startup/config/OdometryOffsetReconfigure.cfg
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+PACKAGE = "jsk_robot_startup"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("mean_x", double_t, 0, "Average for linear x", -10.0, 0.0, 10.0)
+gen.add("mean_y", double_t, 0, "Average for linear y", -10.0, 0.0, 10.0)
+gen.add("mean_z", double_t, 0, "Average for linear z", -10.0, 0.0, 10.0)
+gen.add("mean_roll", double_t, 0, "Average for angular x", -10.0, 0.0, 10.0)
+gen.add("mean_pitch", double_t, 0, "Average for angular y", -10.0, 0.0, 10.0)
+gen.add("mean_yaw", double_t, 0, "Average for angular z", -10.0, 0.0, 10.0)
+gen.add("sigma_x", double_t, 0, "Standard diviation for linear x", 1e-6, 0.05, 10.0)
+gen.add("sigma_y", double_t, 0, "Standard diviation for linear y", 1e-6, 0.05, 10.0)
+gen.add("sigma_z", double_t, 0, "Standard diviation for linear z", 1e-6, 0.05, 10.0)
+gen.add("sigma_roll", double_t, 0, "Standard diviation for angular x", 1e-6, 0.05, 10.0)
+gen.add("sigma_pitch", double_t, 0, "Standard diviation for angular y", 1e-6, 0.05, 10.0)
+gen.add("sigma_yaw", double_t, 0, "Standard diviation for angular z", 1e-6, 0.05, 10.0)
+
+exit(gen.generate(PACKAGE, "constant_height_frame_publisher_reconfigure", "OdometryOffsetReconfigure"))
diff --git a/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml b/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml
index f1907b97e7..4c12e2e565 100644
--- a/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml
+++ b/jsk_robot_common/jsk_robot_startup/config/default_odometry_params.yaml
@@ -1,11 +1,15 @@
-viso_odom_calculator:
-  rate: 10
+viso_odometry_offset:
+  overwrite_pdf: true
+  twist_proportional_sigma: true
   sigma_x: 1.0
   sigma_y: 1.0
   sigma_z: 1.0
   sigma_roll: 0.5
   sigma_pitch: 0.5
   sigma_yaw: 0.5
+
+viso_odom_calculator:
+  rate: 10
   twist_proportional_sigma: true
   force_feedback_sigma: 1.0
   distribution_feedback_minimum_sigma: 0.1
@@ -14,3 +18,4 @@ viso_odom_calculator:
 biped_particle_odometry:
   particle_num: 20
   rate: 100
+  use_imu: false
diff --git a/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch b/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch
index d49735db44..7cea89a4ca 100644
--- a/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch
+++ b/jsk_robot_common/jsk_robot_startup/launch/biped_localization.launch
@@ -1,7 +1,6 @@
 <launch>
   <arg name="set_slam_laser_params" default="true"/>
   <arg name="use_particle_odom" default="true"/>
-  <arg name="use_imu" default="false"/>
   <arg name="use_slam_feedback" default="false"/>
   <arg name="nodelet_index" default="2"/>
   <arg name="stereo_namespace" default="multisense"/>
@@ -25,16 +24,14 @@
     <arg name="stereo" default="$(arg stereo_namespace)" />
     <arg name="image" default="image_rect" />
     <arg name="use_robot_pose_ekf" default="false" />
-    <arg name="publish_viso_tf" default="true" />
+    <arg name="publish_viso_tf" default="false" />
     <arg name="invert_viso_tf" default="true" /> 
   </include>
  
   <!-- <include file="$(find jsk_robot_startup)/launch/odometry_integration.launch"/>     -->
-
   <group if="$(arg use_particle_odom)">
     <include file="$(find jsk_robot_startup)/launch/particle_odometry.launch">
       <arg name="robot" value="$(arg robot)"/>
-      <arg name="use_imu" value="$(arg use_imu)"/>
       <arg name="use_slam_feedback" value="$(arg use_slam_feedback)"/>
     </include>
   </group>
diff --git a/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch b/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch
index dea6df122d..88f4361f36 100644
--- a/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch
+++ b/jsk_robot_common/jsk_robot_startup/launch/particle_odometry.launch
@@ -1,5 +1,4 @@
 <launch>
-  <arg name="use_imu" default="false"/>
   <arg name="use_slam_feedback" default="false"/>
   <arg name="robot" default="default"/>
 
@@ -16,6 +15,7 @@
       base_odom_frame: odom_init
       odom_frame: biped_odom_offset
       base_link_frame: BODY
+      publish_tf: false
     </rosparam>
   </node>
 
@@ -28,6 +28,7 @@
       base_odom_frame: odom_init
       odom_frame: viso_odom_offset
       base_link_frame: BODY
+      publish_tf: false      
     </rosparam>
   </node>
 
@@ -43,6 +44,7 @@
     <rosparam>
       odom_frame: viso_odom_integrated
       base_link_frame: BODY
+      publish_tf: false
     </rosparam>
   </node>
 
@@ -59,7 +61,6 @@
       odom_frame: biped_odom_particle
       base_link_frame: BODY
       publish_tf: false
-      use_imu: $(arg use_imu)
     </rosparam>
   </node>
 
diff --git a/jsk_robot_common/jsk_robot_startup/launch/viso.launch b/jsk_robot_common/jsk_robot_startup/launch/viso.launch
index 4650f9c3b9..c5d572399c 100644
--- a/jsk_robot_common/jsk_robot_startup/launch/viso.launch
+++ b/jsk_robot_common/jsk_robot_startup/launch/viso.launch
@@ -37,6 +37,7 @@
     <param name="~base_link_frame" value="$(arg base_link_frame_id)" />
     <param name="~camera_frame" value="$(arg sensor_frame_id)"/>
     <param name="~odom_frame" value="$(arg odom_frame_id)"/>
+    <param name="~publish_tf" value="$(arg publish_viso_tf)" />
   </node>
   
   <!-- EKF -->
diff --git a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py
index c85e827510..296a0f542a 100755
--- a/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py
+++ b/jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py
@@ -9,6 +9,7 @@
 import time
 import threading
 import copy
+from jsk_robot_startup.odometry_utils import broadcast_transform, make_homogeneous_matrix
 
 class CameraToBaseOffset(object):
     def __init__(self):
@@ -17,12 +18,13 @@ def __init__(self):
         self.rate = float(rospy.get_param("~rate", 100))
         # tf parameters
         self.publish_tf = rospy.get_param("~publish_tf", True)
-        self.invert_tf = rospy.get_param("~invert_tf", True)
+        if self.publish_tf:
+            self.invert_tf = rospy.get_param("~invert_tf", True)
+            self.broadcast = tf.TransformBroadcaster()
         self.base_link_frame = rospy.get_param("~base_link_frame", "BODY")
         self.camera_frame = rospy.get_param("~camera_frame", "left_camera_optical_frame")
         self.odom_frame = rospy.get_param("~odom_frame", "viso_odom")        
         self.tf_duration = rospy.get_param("~tf_duration", 1)
-        self.broadcast = tf.TransformBroadcaster()
         self.listener = tf.TransformListener(True, rospy.Duration(10))
         self.r = rospy.Rate(self.rate)
         self.initial_matrix = None
@@ -44,54 +46,36 @@ def source_odom_callback(self, msg):
         with self.lock:
             # calculate camera transform
             current_camera_to_base = self.calculate_camera_to_base_transform(msg.header.stamp)
+            if current_camera_to_base == None:
+                return
+
             if self.initial_matrix == None:
-                self.initial_matrix = current_camera_to_base
+                self.initial_matrix = numpy.linalg.inv(current_camera_to_base)
 
-            if self.initial_matrix != None:
-                camera_relative_base_transformation = numpy.dot(numpy.linalg.inv(self.initial_matrix), current_camera_to_base) # base_link transformation in camera coords
-    
-                # calculate offseted odometry
-                source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
-                                                                  [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
-                                                                   msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
-                new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix)
+            camera_relative_base_transformation = numpy.dot(self.initial_matrix, current_camera_to_base) # base_link transformation in camera coords
+            
+            # calculate offseted odometry
+            source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
+                                                         [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
+                                                          msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+            new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix)
 
-                # make odometry msg. twist is copied from source_odom
-                new_odom = copy.deepcopy(msg)
-                new_odom.header.frame_id = self.odom_frame
-                new_odom.child_frame_id = self.base_link_frame
-                new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3]))
-                new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix)))
-                new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6)
-                rotation_matrix = camera_relative_base_transformation[:3, :3]
-                new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix))
-                new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
-                new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist()
+            # make odometry msg. twist is copied from source_odom
+            new_odom = copy.deepcopy(msg)
+            new_odom.header.frame_id = self.odom_frame
+            new_odom.child_frame_id = self.base_link_frame
+            new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3]))
+            new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix)))
+            new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6)
+            rotation_matrix = camera_relative_base_transformation[:3, :3]
+            new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix))
+            new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
+            new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist()
                                             
-                # publish
-                self.pub.publish(new_odom)
-                if self.publish_tf:
-                    self.broadcast_transform(new_odom)
-
-    def make_homogeneous_matrix(self, trans, rot):
-        homogeneous_matrix = tf.transformations.quaternion_matrix(rot)
-        homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3)
-        return homogeneous_matrix
-
-    def broadcast_transform(self, odom):
-        position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z]
-        orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]
-        if self.invert_tf:
-            homogeneous_matrix = self.make_homogeneous_matrix(position, orientation)
-            homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix)
-            position = list(homogeneous_matrix_inv[:3, 3])
-            orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv))
-            parent_frame = odom.child_frame_id
-            target_frame = odom.header.frame_id
-        else:
-            parent_frame = odom.header.frame_id
-            target_frame = odom.child_frame_id
-        self.broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame)
+            # publish
+            self.pub.publish(new_odom)
+            if self.publish_tf:
+                broadcast_transform(self.broadcast, new_odom, self.invert_tf)
 
     def calculate_camera_to_base_transform(self, stamp):
         try:
@@ -103,7 +87,7 @@ def calculate_camera_to_base_transform(self, stamp):
             except:
                 rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame)
                 return None
-        camera_to_base_link = self.make_homogeneous_matrix(trans, rot) # camera -> base_link
+        camera_to_base_link = make_homogeneous_matrix(trans, rot) # camera -> base_link
         return camera_to_base_link
         
 if __name__ == '__main__':
diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py
index 386112d030..8683462723 100755
--- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py
+++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryFeedbackWrapper.py
@@ -16,18 +16,20 @@
 from dynamic_reconfigure.server import Server
 from jsk_robot_startup.cfg import OdometryFeedbackWrapperReconfigureConfig
 
+from odometry_utils import transform_local_twist_to_global, transform_local_twist_covariance_to_global, update_pose, update_pose_covariance, broadcast_transform, make_homogeneous_matrix
+
 class OdometryFeedbackWrapper(object):
     def __init__(self):
         rospy.init_node("OdometryFeedbackWrapper", anonymous=True)
         self.rate = float(rospy.get_param("~rate", 100))
         self.publish_tf = rospy.get_param("~publish_tf", True)
-        self.invert_tf = rospy.get_param("~invert_tf", True)
         self.odom_frame = rospy.get_param("~odom_frame", "feedback_odom")
         self.base_link_frame = rospy.get_param("~base_link_frame", "BODY")
         self.max_feedback_time = rospy.get_param("~max_feedback_time", 60) # if max_feedback_time <= 0, feedback is not occurs by time
         self.tf_cache_time = rospy.get_param("~tf_cache_time", 60) # determined from frequency of feedback_odom
-        self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False)
-        self.broadcast = tf.TransformBroadcaster()
+        if self.publish_tf:
+            self.broadcast = tf.TransformBroadcaster()
+            self.invert_tf = rospy.get_param("~invert_tf", True)
         self.listener = tf.TransformListener(True, rospy.Duration(self.tf_cache_time + 10)) # 10[sec] is safety mergin for feedback
         self.odom = None # belief of this wrapper
         self.feedback_odom = None
@@ -38,12 +40,6 @@ def __init__(self):
         self.r = rospy.Rate(self.rate)
         self.lock = threading.Lock()
         self.odom_history = []
-        self.v_sigma = [rospy.get_param("~sigma_x", 0.05),
-                        rospy.get_param("~sigma_y", 0.1),
-                        rospy.get_param("~sigma_z", 0.0001),
-                        rospy.get_param("~sigma_roll", 0.0001),
-                        rospy.get_param("~sigma_pitch", 0.0001),
-                        rospy.get_param("~sigma_yaw", 0.01)]
         self.force_feedback_sigma = rospy.get_param("~force_feedback_sigma", 0.5)
         self.distribution_feedback_minimum_sigma = rospy.get_param("~distribution_feedback_minimum_sigma", 0.5)
         self.pub = rospy.Publisher("~output", Odometry, queue_size = 1)
@@ -58,10 +54,6 @@ def execute(self):
             self.r.sleep()
 
     def reconfigure_callback(self, config, level):
-        with self.lock:
-            for i, sigma in enumerate(["sigma_x", "sigma_y", "sigma_z", "sigma_roll", "sigma_pitch", "sigma_yaw"]):
-                self.v_sigma[i] = config[sigma]
-        rospy.loginfo("[%s]" + "velocity sigma updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_sigma), rospy.get_name())
         self.force_feedback_sigma = config["force_feedback_sigma"]
         rospy.loginfo("[%s]: force feedback sigma is %f", rospy.get_name(), self.force_feedback_sigma)
         self.distribution_feedback_minimum_sigma = config["distribution_feedback_minimum_sigma"]
@@ -86,7 +78,7 @@ def source_odom_callback(self, msg):
             self.calculate_odometry(self.odom, self.source_odom)
             self.publish_odometry()
             if self.publish_tf:
-                self.broadcast_transform()
+                broadcast_transform(self.broadcast, self.odom, self.invert_tf)
 
     def feedback_odom_callback(self, msg):
         if not self.odom:
@@ -102,9 +94,10 @@ def feedback_odom_callback(self, msg):
                     nearest_dt = dt
                     nearest_odom = copy.deepcopy(hist)
             # get approximate pose at feedback_odom timestamp (probably it is past) of nearest_odom
-            self.update_pose(nearest_odom.pose, nearest_odom.twist,
-                             nearest_odom.header.frame_id, nearest_odom.child_frame_id,
-                             nearest_odom.header.stamp, nearest_dt)
+            global_nearest_odom_twist = transform_local_twist_to_global(nearest_odom.pose.pose, nearest_odom.twist.twist)
+            nearest_odom.pose.pose = update_pose(nearest_odom.pose.pose, global_nearest_odom_twist, nearest_dt)
+            global_nearest_odom_twist_covariance = transform_local_twist_covariance_to_global(nearest_odom.pose.pose, nearest_odom.twist.covariance)
+            nearest_odom.pose.covariance = update_pose_covariance(nearest_odom.pose.covariance, global_nearest_odom_twist_covariance, nearest_dt)
             enable_feedback = self.check_covariance(nearest_odom) or self.check_distribution_difference(nearest_odom, self.feedback_odom) or self.check_feedback_time()
             # update feedback_odom to approximate odom at current timestamp using previsous velocities
             if enable_feedback:
@@ -115,20 +108,18 @@ def feedback_odom_callback(self, msg):
                     if dt > 0.0:
                         # update pose and twist according to the history
                         self.update_twist(self.feedback_odom.twist, hist.twist)
-                        self.update_pose(self.feedback_odom.pose,
-                                         hist.twist, self.feedback_odom.header.frame_id, hist.child_frame_id,
-                                         hist.header.stamp, dt) # update feedback_odom according to twist of hist
+                        global_hist_twist = transform_local_twist_to_global(hist.pose.pose, hist.twist.twist)
+                        self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose.pose, global_hist_twist, dt) # update feedback_odom according to twist of hist
                         # update covariance
-                        # this wrapper do not upgrade twist.covariance to trust feedback_odom.covariance
-                        self.update_twist_covariance(self.feedback_odom.twist)
-                        self.update_pose_covariance(self.feedback_odom.pose, self.feedback_odom.twist,
-                                                    self.feedback_odom.header.frame_id, hist.child_frame_id,
-                                                    hist.header.stamp, dt)
+                        self.feedback_odom.twist.covariance = hist.twist.covariance
+                        global_hist_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, hist.twist.covariance)
+                        self.feedback_odom.pose.covariance = update_pose_covariance(self.feedback_odom.pose.covariance, global_hist_twist_covariance, dt)
                         self.feedback_odom.header.stamp = hist.header.stamp
                 dt = (self.odom.header.stamp - self.feedback_odom.header.stamp).to_sec()
-                self.update_pose(self.feedback_odom.pose, self.feedback_odom.twist,
-                                 self.feedback_odom.header.frame_id, self.feedback_odom.child_frame_id,
-                                 self.feedback_odom.header.stamp, dt)
+                global_feedback_odom_twist = transform_local_twist_to_global(self.feedback_odom.pose.pose, self.feedback_odom.twist.twist)
+                self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose.pose, global_feedback_odom_twist, dt)
+                global_feedback_odom_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, self.feedback_odom.twist.covariance)
+                self.feedback_odom.pose.covariance = update_pose_covariance(self.feedback_odom.pose.covariance, global_feedback_odom_twist_covariance, dt)
                 self.feedback_odom.header.stamp = self.odom.header.stamp
                 # integrate feedback_odom and current odom
                 new_odom_pose, new_odom_cov = self.calculate_mean_and_covariance(self.odom.pose, self.feedback_odom.pose)
@@ -139,10 +130,10 @@ def feedback_odom_callback(self, msg):
                 self.prev_feedback_time = self.odom.header.stamp
                 self.odom_history = []
                 # update offset
-                new_pose_homogeneous_matrix = self.make_homogeneous_matrix([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z],
-                                                                           [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])
-                source_homogeneous_matrix = self.make_homogeneous_matrix([self.source_odom.pose.pose.position.x, self.source_odom.pose.pose.position.y, self.source_odom.pose.pose.position.z],
-                                                                         [self.source_odom.pose.pose.orientation.x, self.source_odom.pose.pose.orientation.y, self.source_odom.pose.pose.orientation.z, self.source_odom.pose.pose.orientation.w])
+                new_pose_homogeneous_matrix = make_homogeneous_matrix([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z],
+                                                                      [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])
+                source_homogeneous_matrix = make_homogeneous_matrix([self.source_odom.pose.pose.position.x, self.source_odom.pose.pose.position.y, self.source_odom.pose.pose.position.z],
+                                                                    [self.source_odom.pose.pose.orientation.x, self.source_odom.pose.pose.orientation.y, self.source_odom.pose.pose.orientation.z, self.source_odom.pose.pose.orientation.w])
                 # Hnew = Hold * T -> T = Hold^-1 * Hnew
                 self.offset_homogeneous_matrix = numpy.dot(numpy.linalg.inv(source_homogeneous_matrix), new_pose_homogeneous_matrix) # self.odom.header.stamp is assumed to be same as self.source_odom.header.stamp
                 
@@ -215,138 +206,26 @@ def calculate_odometry(self, odom, source_odom):
         result_odom.header.frame_id = self.odom_frame
         result_odom.child_frame_id = self.base_link_frame
 
-        # overwrite twist covariance (twist is copied from source_odom)
-        self.update_twist_covariance(result_odom.twist)
-
-        # consider only pose because twist is local
+        # consider only pose because twist is local and copied from source_odom
         position = [source_odom.pose.pose.position.x, source_odom.pose.pose.position.y, source_odom.pose.pose.position.z]
         orientation = [source_odom.pose.pose.orientation.x, source_odom.pose.pose.orientation.y, source_odom.pose.pose.orientation.z, source_odom.pose.pose.orientation.w]
 
         # calculate pose (use odometry source)
-        source_homogeneous_matrix = self.make_homogeneous_matrix(position, orientation)
+        source_homogeneous_matrix = make_homogeneous_matrix(position, orientation)
         result_homogeneous_matrix = numpy.dot(source_homogeneous_matrix, self.offset_homogeneous_matrix)
         result_odom.pose.pose.position = Point(*list(result_homogeneous_matrix[:3, 3]))
         result_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(result_homogeneous_matrix)))
 
-        # calculate covariance (do not use odometry source)
+        # calculate pose covariance (do not use odometry source)
         if self.odom != None:
             result_odom.pose.covariance =  self.odom.pose.covariance # do not use source_odom covariance in pose
             dt = (self.source_odom.header.stamp - self.odom.header.stamp).to_sec()
         else:
             # initial covariance of pose is defined as same value of source_odom
             dt = 0.0
-        self.update_pose_covariance(result_odom.pose, result_odom.twist, result_odom.header.frame_id, result_odom.child_frame_id, result_odom.header.stamp, dt)
+        global_twist_covariance = transform_local_twist_covariance_to_global(result_odom.pose.pose, result_odom.twist.covariance)
+        result_odom.pose.covariance = update_pose_covariance(result_odom.pose.covariance, global_twist_covariance, dt)
         self.odom = result_odom
 
     def update_twist(self, twist, new_twist):
         twist.twist = new_twist.twist
-
-    def convert_local_twist_to_global_twist(self, local_twist, pose_frame, twist_frame, stamp):
-        try:
-            (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, stamp)
-        except:
-            try:
-                # rospy.loginfo("timestamp %f of tf (%s to %s) is not correct. use rospy.Time(0).",  stamp.to_sec(), pose_frame, twist_frame)
-                (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, rospy.Time(0))
-            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
-                rospy.logwarn("failed to solve tf: %s to %s", pose_frame, twist_frame)
-                return None
-        rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3]
-        global_velocity = numpy.dot(rotation_matrix, numpy.array([[local_twist.twist.linear.x],
-                                                                  [local_twist.twist.linear.y],
-                                                                  [local_twist.twist.linear.z]]))
-        global_omega = numpy.dot(rotation_matrix, numpy.array([[local_twist.twist.angular.x],
-                                                               [local_twist.twist.angular.y],
-                                                               [local_twist.twist.angular.z]]))
-        return Twist(Vector3(*global_velocity[:3, 0]), Vector3(*global_omega[:3, 0]))
-
-    def update_pose(self, pose, twist, pose_frame, twist_frame, stamp, dt):
-        global_twist = self.convert_local_twist_to_global_twist(twist, pose_frame, twist_frame, stamp)
-        if global_twist == None:
-            rospy.logwarn("Cannot calculate global twist. return.")
-            return
-        # calculate trapezoidal integration
-        pose.pose.position.x += global_twist.linear.x * dt
-        pose.pose.position.y += global_twist.linear.y * dt
-        pose.pose.position.z += global_twist.linear.z * dt
-        pose.pose.orientation = self.calculate_quaternion_transform(pose.pose.orientation, twist.twist.angular, dt)
-
-    def calculate_quaternion_transform(self, orientation, angular, dt): # angular is assumed to be global
-        # quaternion calculation
-        quat_vec = numpy.array([[orientation.x],
-                                [orientation.y],
-                                [orientation.z],
-                                [orientation.w]])
-        # skew_omega = numpy.matrix([[0, angular.z, -angular.y, angular.x],
-        #                            [-angular.z, 0, angular.x, angular.y],
-        #                            [angular.y, -angular.x, 0, angular.z],
-        #                            [-angular.x, -angular.y, -angular.z, 0]])
-        skew_omega = numpy.matrix([[0, -angular.z, angular.y, angular.x],
-                                   [angular.z, 0, -angular.x, angular.y],
-                                   [-angular.y, angular.x, 0, angular.z],
-                                   [-angular.x, -angular.y, -angular.z, 0]])
-        new_quat_vec = quat_vec + 0.5 * numpy.dot(skew_omega, quat_vec) * dt
-        norm = numpy.linalg.norm(new_quat_vec)
-        if norm == 0:
-            rospy.logwarn("norm of quaternion is zero")
-        else:
-            new_quat_vec = new_quat_vec / norm # normalize
-        return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist())
-        
-    def update_twist_covariance(self, twist):
-        twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z]
-        if self.twist_proportional_sigma == True:
-            current_sigma = [x * y for x, y in zip(twist_list, self.v_sigma)]
-        else:
-            current_sigma = self.v_sigma
-        twist.covariance = numpy.diag([max(x**2, 0.001*0.001) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular
-
-    def update_pose_covariance(self, pose, twist, pose_frame, twist_frame, stamp, dt):
-        # make matirx from covariance array
-        prev_pose_cov_matrix = numpy.matrix(pose.covariance).reshape(6, 6)
-        twist_cov_matrix = numpy.matrix(twist.covariance).reshape(6, 6)
-        # twist is described in child_frame_id coordinates
-        try:
-            (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, stamp)
-        except:
-            try:
-                # rospy.logwarn("timestamp %f of tf (%s to %s) is not correct. use rospy.Time(0).",  stamp.to_sec(), pose_frame, twist_frame)
-                (trans,rot) = self.listener.lookupTransform(pose_frame, twist_frame, rospy.Time(0)) # todo: lookup odom.header.stamp
-            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
-                rospy.logwarn("failed to solve tf: %s to %s", pose_frame, twist_frame)
-                return
-        rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3]
-        global_twist_cov_matrix = numpy.zeros((6, 6))
-        global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix))
-        global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
-        # jacobian matrix
-        # elements in pose and twist are assumed to be independent on global coordinates
-        jacobi_pose = numpy.diag([1.0] * 6)
-        jacobi_twist = numpy.diag([dt] * 6)
-        # covariance calculation
-        pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T))
-        # update covariances as array type (twist is same as before)
-        pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
-
-    def broadcast_transform(self):
-        if not self.odom:
-            return
-        position = [self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z]
-        orientation = [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w]
-        if self.invert_tf:
-            homogeneous_matrix = self.make_homogeneous_matrix(position, orientation)
-            homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix)
-            position = list(homogeneous_matrix_inv[:3, 3])
-            orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv))
-            parent_frame = self.odom.child_frame_id
-            target_frame = self.odom.header.frame_id
-        else:
-            parent_frame = self.odom.header.frame_id
-            target_frame = self.odom.child_frame_id
-        self.broadcast.sendTransform(position, orientation, self.odom.header.stamp, target_frame, parent_frame)
-
-    def make_homogeneous_matrix(self, trans, rot):
-        homogeneous_matrix = tf.transformations.quaternion_matrix(rot)
-        homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3)
-        return homogeneous_matrix
-        
diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py
index 2ab1eed98d..4a866ca756 100644
--- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py
+++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/OdometryOffset.py
@@ -9,42 +9,65 @@
 import time
 import threading
 import copy
+from dynamic_reconfigure.server import Server
+from jsk_robot_startup.cfg import OdometryOffsetReconfigureConfig
+from odometry_utils import make_homogeneous_matrix, update_twist_covariance, update_pose, update_pose_covariance, broadcast_transform, transform_local_twist_to_global, transform_local_twist_covariance_to_global
 
 class OdometryOffset(object):
     def __init__(self):
         rospy.init_node("OdometryFeedbackWrapper", anonymous=True)
+        self.listener = tf.TransformListener(True, rospy.Duration(120))
+        self.offset_matrix = None
+        self.prev_odom = None
+        self.lock = threading.Lock()
         # execute rate
         self.rate = float(rospy.get_param("~rate", 100))
+        self.r = rospy.Rate(self.rate)
         # tf parameters
         self.publish_tf = rospy.get_param("~publish_tf", True)
-        self.invert_tf = rospy.get_param("~invert_tf", True)
+        if self.publish_tf:
+            self.broadcast = tf.TransformBroadcaster()
+            self.invert_tf = rospy.get_param("~invert_tf", True)
         self.odom_frame = rospy.get_param("~odom_frame", "offset_odom")
         self.base_odom_frame = rospy.get_param("~base_odom_frame", "odom_init")
         self.base_link_frame = rospy.get_param("~base_link_frame", "BODY")
         self.tf_duration = rospy.get_param("~tf_duration", 1)
         # for filter (only used when use_twist_filter is True)
         self.use_twist_filter = rospy.get_param("~use_twist_filter", False)
-        self.filter_buffer_size = rospy.get_param("~filter_buffer_size", 5)
-        self.filter_buffer = []
-        # for covariance calculation (only used when calculate_covariance is True)
-        self.calculate_covariance = rospy.get_param("~calculate_covariance", False)
-        self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False)
-        self.v_sigma = [rospy.get_param("~sigma_x", 0.05),
-                        rospy.get_param("~sigma_y", 0.1),
-                        rospy.get_param("~sigma_z", 0.0001),
-                        rospy.get_param("~sigma_roll", 0.0001),
-                        rospy.get_param("~sigma_pitch", 0.0001),
-                        rospy.get_param("~sigma_yaw", 0.01)]
-        self.broadcast = tf.TransformBroadcaster()
-        self.listener = tf.TransformListener(True, rospy.Duration(120))
-        self.r = rospy.Rate(self.rate)
-        self.offset_matrix = None
-        self.prev_odom = None
-        self.lock = threading.Lock()
+        if self.use_twist_filter:
+            self.filter_buffer_size = rospy.get_param("~filter_buffer_size", 5)
+            self.filter_buffer = []
+        # to overwrite probability density function (only used when overwrite_pdf is True)
+        self.overwrite_pdf = rospy.get_param("~overwrite_pdf", False)
+        if self.overwrite_pdf:
+            self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False)
+            self.v_err_mean = [rospy.get_param("~mean_x", 0.0),
+                               rospy.get_param("~mean_y", 0.0),
+                               rospy.get_param("~mean_z", 0.0),
+                               rospy.get_param("~mean_roll", 0.0),
+                               rospy.get_param("~mean_pitch", 0.0),
+                               rospy.get_param("~mean_yaw", 0.0)]
+            self.v_err_sigma = [rospy.get_param("~sigma_x", 0.05),
+                                rospy.get_param("~sigma_y", 0.1),
+                                rospy.get_param("~sigma_z", 0.0001),
+                                rospy.get_param("~sigma_roll", 0.0001),
+                                rospy.get_param("~sigma_pitch", 0.0001),
+                                rospy.get_param("~sigma_yaw", 0.01)]
+            self.reconfigure_server = Server(OdometryOffsetReconfigureConfig, self.reconfigure_callback)
         self.source_odom_sub = rospy.Subscriber("~source_odom", Odometry, self.source_odom_callback)
         self.init_signal_sub = rospy.Subscriber("~init_signal", Empty, self.init_signal_callback)
         self.pub = rospy.Publisher("~output", Odometry, queue_size = 1)
 
+    def reconfigure_callback(self, config, level):
+        with self.lock:
+            for i, mean in enumerate(["mean_x", "mean_y", "mean_z", "mean_roll", "mean_pitch", "mean_yaw"]):
+                self.v_err_mean[i] = config[mean]
+            for i, sigma in enumerate(["sigma_x", "sigma_y", "sigma_z", "sigma_roll", "sigma_pitch", "sigma_yaw"]):
+                self.v_err_sigma[i] = config[sigma]
+        rospy.loginfo("[%s]" + "velocity mean updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_err_mean), rospy.get_name())                
+        rospy.loginfo("[%s]" + "velocity sigma updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_err_sigma), rospy.get_name())
+        return config
+
     def execute(self):
         while not rospy.is_shutdown():
             self.r.sleep()
@@ -56,10 +79,10 @@ def calculate_offset(self, odom):
         except:
             rospy.logwarn("[%s] failed to solve tf in initialize_odometry: %s to %s", rospy.get_name(), self.base_odom_frame, odom.child_frame_id)
             return None
-        base_odom_to_base_link = self.make_homogeneous_matrix(trans, rot) # base_odom -> base_link
-        base_link_to_odom = numpy.linalg.inv(self.make_homogeneous_matrix([odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z],
-                                                                          [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
-                                                                           odom.pose.pose.orientation.z, odom.pose.pose.orientation.w])) # base_link -> odom
+        base_odom_to_base_link = make_homogeneous_matrix(trans, rot) # base_odom -> base_link
+        base_link_to_odom = numpy.linalg.inv(make_homogeneous_matrix([odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z],
+                                                                     [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
+                                                                      odom.pose.pose.orientation.z, odom.pose.pose.orientation.w])) # base_link -> odom
         return base_odom_to_base_link.dot(base_link_to_odom) # base_odom -> odom
         
     def init_signal_callback(self, msg):
@@ -67,38 +90,49 @@ def init_signal_callback(self, msg):
         with self.lock:
             self.offset_matrix = None
             self.prev_odom = None
-            self.filter_buffer = []
+            if self.use_twist_filter:
+                self.filter_buffer = []
             
     def source_odom_callback(self, msg):
         with self.lock:
             if self.offset_matrix != None:
-                source_odom_matrix = self.make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
-                                                                  [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
-                                                                   msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+                source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
+                                                             [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
+                                                              msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
                 new_odom = copy.deepcopy(msg)
                 new_odom.header.frame_id = self.odom_frame
                 new_odom.child_frame_id = self.base_link_frame
+                twist_list = [new_odom.twist.twist.linear.x, new_odom.twist.twist.linear.y, new_odom.twist.twist.linear.z, new_odom.twist.twist.angular.x, new_odom.twist.twist.angular.y, new_odom.twist.twist.angular.z]
 
                 # use median filter to cancel spike noise of twist when use_twist_filter is true
-                if self.use_twist_filter:
-                    vel = [new_odom.twist.twist.linear.x, new_odom.twist.twist.linear.y, new_odom.twist.twist.linear.z, new_odom.twist.twist.angular.x, new_odom.twist.twist.angular.y, new_odom.twist.twist.angular.z]
-                    vel = self.median_filter(vel)
-                    new_odom.twist.twist = Twist(Vector3(*vel[0:3]), Vector3(*vel[3: 6]))
+                if self.use_twist_filter:                    
+                    twist_list = self.median_filter(twist_list)
+                    new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6]))
 
                 # overwrite twist covariance when calculate_covariance flag is True
-                if self.calculate_covariance:
-                    self.update_twist_covariance(new_odom.twist)
+                if self.overwrite_pdf:
+                    if all([abs(x) < 1e-6 for x in twist_list]):
+                        # shift twist according to error mean when moving (stopping state is trusted)
+                        twist_list = [x + y for x, y in zip(twist_list, self.v_err_mean)]
+                        new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6]))
+                    # calculate twist covariance according to standard diviation
+                    if self.twist_proportional_sigma:
+                        current_sigma = [x * y for x, y in zip(twist_list, self.v_err_sigma)]
+                    else:
+                        current_sigma = self.v_err_sigma
+                    new_odom.twist.covariance = update_twist_covariance(new_odom.twist, current_sigma)
                     
                 # offset coords
                 new_odom_matrix = self.offset_matrix.dot(source_odom_matrix)
                 new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3]))
                 new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix)))
 
-                if self.calculate_covariance:
+                if self.overwrite_pdf:
                     if self.prev_odom != None:
                         dt = (new_odom.header.stamp - self.prev_odom.header.stamp).to_sec()
-                        global_twist_with_covariance = self.transform_twist_with_covariance_to_global(new_odom.pose, new_odom.twist)
-                        new_odom.pose.covariance = self.update_pose_covariance(self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt)
+                        global_twist_with_covariance = TwistWithCovariance(transform_local_twist_to_global(new_odom.pose.pose, new_odom.twist.twist),
+                                                                           transform_local_twist_covariance_to_global(new_odom.pose.pose, new_odom.twist.covariance))
+                        new_odom.pose.covariance = update_pose_covariance(self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt)
                     else:
                         new_odom.pose.covariance = numpy.diag([0.01**2] * 6).reshape(-1,).tolist() # initial covariance is assumed to be constant
                 else:
@@ -112,7 +146,7 @@ def source_odom_callback(self, msg):
                 # publish
                 self.pub.publish(new_odom)
                 if self.publish_tf:
-                    self.broadcast_transform(new_odom)
+                    broadcast_transform(self.broadcast, new_odom, self.invert_tf)
 
                 self.prev_odom = new_odom
                     
@@ -121,74 +155,9 @@ def source_odom_callback(self, msg):
                 if current_offset_matrix != None:
                     self.offset_matrix = current_offset_matrix
 
-    def make_homogeneous_matrix(self, trans, rot):
-        homogeneous_matrix = tf.transformations.quaternion_matrix(rot)
-        homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3)
-        return homogeneous_matrix
-
-    def broadcast_transform(self, odom):
-        position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z]
-        orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]
-        if self.invert_tf:
-            homogeneous_matrix = self.make_homogeneous_matrix(position, orientation)
-            homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix)
-            position = list(homogeneous_matrix_inv[:3, 3])
-            orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv))
-            parent_frame = odom.child_frame_id
-            target_frame = odom.header.frame_id
-        else:
-            parent_frame = odom.header.frame_id
-            target_frame = odom.child_frame_id
-        self.broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame)
-
     def median_filter(self, data):
         self.filter_buffer.append(data)
         ret = numpy.median(self.filter_buffer, axis = 0)
         if len(self.filter_buffer) >= self.filter_buffer_size:
             self.filter_buffer.pop(0) # filter_buffer has at least 1 member
         return ret
-        
-    def update_twist_covariance(self, twist):
-        twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z]
-        if self.twist_proportional_sigma == True:
-            current_sigma = [x * y for x, y in zip(twist_list, self.v_sigma)]
-        else:
-            if all([abs(x) < 1e-6 for x in twist_list]):
-                current_sigma = [1e-6] * 6 # trust "completely stopping" state
-            else:
-                current_sigma = self.v_sigma
-        twist.covariance = numpy.diag([max(x**2, 0.001*0.001) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular
-
-    def update_pose_covariance(self, pose_cov, global_twist_cov, dt):
-        ret_pose_cov = []
-        # make matirx from covariance array
-        prev_pose_cov_matrix = numpy.matrix(pose_cov).reshape(6, 6)
-        global_twist_cov_matrix = numpy.matrix(global_twist_cov).reshape(6, 6)
-        # jacobian matrix
-        # elements in pose and twist are assumed to be independent on global coordinates
-        jacobi_pose = numpy.diag([1.0] * 6)
-        jacobi_twist = numpy.diag([dt] * 6)
-        # covariance calculation
-        pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T))
-        # update covariances as array type (twist is same as before)
-        ret_pose_cov = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
-        return ret_pose_cov
-        
-    def transform_twist_with_covariance_to_global(self, pose, twist):
-        trans = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
-        rot = [pose.pose.orientation.x, pose.pose.orientation.y,
-               pose.pose.orientation.z, pose.pose.orientation.w]
-        rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3]
-        twist_cov_matrix = numpy.matrix(twist.covariance).reshape(6, 6)
-        global_velocity = numpy.dot(rotation_matrix, numpy.array([[twist.twist.linear.x],
-                                                                  [twist.twist.linear.y],
-                                                                  [twist.twist.linear.z]]))
-        global_omega = numpy.dot(rotation_matrix, numpy.array([[twist.twist.angular.x],
-                                                               [twist.twist.angular.y],
-                                                               [twist.twist.angular.z]]))
-        global_twist_cov_matrix = numpy.zeros((6, 6))
-        global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix))
-        global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
-
-        return TwistWithCovariance(Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0])),
-                                   global_twist_cov_matrix.reshape(-1,).tolist())
diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py
index 79265d570a..7e1a4de45a 100644
--- a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py
+++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/ParticleOdometry.py
@@ -3,7 +3,6 @@
 import rospy
 import numpy
 import scipy.stats
-import math
 import random
 import threading
 import itertools
@@ -15,46 +14,8 @@
 from std_msgs.msg import Empty
 from sensor_msgs.msg import Imu
 from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, Twist, Pose, Point, Quaternion, Vector3
+from odometry_utils import norm_pdf_multivariate, transform_quaternion_to_euler, transform_local_twist_to_global, transform_local_twist_covariance_to_global, update_pose, update_pose_covariance
 
-# scipy.stats.multivariate_normal only can be used after SciPy 0.14.0
-# input: x(array), mean(array), cov_inv(matrix) output: probability of x
-# covariance has to be inverted to reduce calculation time
-def norm_pdf_multivariate(x, mean, cov_inv):
-    size = len(x)
-    if size == len(mean) and (size, size) == cov_inv.shape:
-        inv_det = numpy.linalg.det(cov_inv)
-        if not inv_det > 0:
-            rospy.logwarn("Determinant of inverse cov matrix {0} is equal or smaller than zero".format(inv_det))
-            return 0.0
-        norm_const = math.pow((2 * numpy.pi), float(size) / 2) * math.pow(1 / inv_det, 1.0 / 2) # determinant of inverse matrix is reciprocal
-        if not norm_const > 0 :
-            rospy.logwarn("Norm const {0} is equal or smaller than zero".format(norm_const))
-            return 0.0
-        x_mean = numpy.matrix(x - mean)
-        exponent = -0.5 * (x_mean * cov_inv * x_mean.T)
-        if exponent > 0:
-            rospy.logwarn("Exponent {0} is larger than zero".format(exponent))
-            exponent = 0
-        result = math.pow(math.e, exponent)
-        return result / norm_const
-    else:
-        rospy.logwarn("The dimensions of the input don't match")
-        return 0.0
-
-# tf.transformations.euler_from_quaternion is slow because the function calculates matrix inside.
-# cf. https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
-def transform_quaternion_to_euler(quat):
-    zero_thre = numpy.finfo(float).eps * 4.0 # epsilon for testing whether a number is close to zero
-    roll_numerator = 2 * (quat[3] * quat[0] + quat[1] * quat[2])
-    if abs(roll_numerator) < zero_thre:
-        roll_numerator = numpy.sign(roll_numerator) * 0.0
-    yaw_numerator = 2 * (quat[3] * quat[2] + quat[0] * quat[1])
-    if abs(yaw_numerator) < zero_thre:
-        yaw_numerator = numpy.sign(yaw_numerator) * 0.0
-    return (numpy.arctan2(roll_numerator, 1 - 2 * (quat[0] ** 2 + quat[1] ** 2)),
-            numpy.arcsin(2 * (quat[3] * quat[1] - quat[2] * quat[0])),
-            numpy.arctan2(yaw_numerator, 1 - 2 * (quat[1] ** 2 + quat[2] ** 2)))
-    
 class ParticleOdometry(object):
     ## initialize
     def __init__(self):
@@ -79,6 +40,7 @@ def __init__(self):
         self.source_odom = None
         self.measure_odom = None
         self.imu = None
+        self.imu_rotation = None
         self.particles = None
         self.weights = []
         self.measurement_updated = False
@@ -90,9 +52,10 @@ def __init__(self):
                            rospy.get_param("~init_sigma_yaw", 0.05)]
         # tf
         self.listener = tf.TransformListener(True, rospy.Duration(10))
-        self.broadcast = tf.TransformBroadcaster()
-        self.publish_tf = rospy.get_param("~publish_tf", True)
-        self.invert_tf = rospy.get_param("~invert_tf", True)
+        self.publish_tf = rospy.get_param("~publish_tf", True)        
+        if self.publish_tf:
+            self.broadcast = tf.TransformBroadcaster()
+            self.invert_tf = rospy.get_param("~invert_tf", True)
         # publisher
         self.pub = rospy.Publisher("~output", Odometry, queue_size = 1)
         # subscriber
@@ -127,6 +90,7 @@ def initialize_odometry(self):
             self.measure_odom = None
             self.measurement_updated = False
             self.imu = None
+            self.imu_rotation = None
             
     ## particle filter functions
     # input: particles(list of pose), source_odom(control input)  output: list of sampled particles(pose)
@@ -134,8 +98,7 @@ def sampling(self, particles, source_odom):
         global_twist_with_covariance = self.transform_twist_with_covariance_to_global(source_odom.pose, source_odom.twist)
         sampled_velocities = self.state_transition_probability_rvs(global_twist_with_covariance.twist, global_twist_with_covariance.covariance) # make sampeld velocity at once because multivariate_normal calculates invert matrix and it is slow
         dt = (source_odom.header.stamp - self.odom.header.stamp).to_sec()
-        # return self.calculate_pose_transform(prev_x, Twist(Vector3(*vel_list[0:3]), Vector3(*vel_list[3:6])), dt)
-        return [self.calculate_pose_transform(prt, Twist(Vector3(*vel[0:3]), Vector3(*vel[3:6])), dt) for prt, vel in zip(particles, sampled_velocities)]
+        return [update_pose(prt, Twist(Vector3(*vel[0:3]), Vector3(*vel[3:6])), dt) for prt, vel in zip(particles, sampled_velocities)]
         
     # input: particles(list of pose), min_weight(float) output: list of weights
     def weighting(self, particles, min_weight):
@@ -207,7 +170,8 @@ def imu_error_pdf(self, prt):
             rospy.logwarn("[%s]: use_imu is True but imu is not subscribed", rospy.get_name())
             return 1.0 # multiply 1.0 make no effects to weight
         prt_euler = self.convert_pose_to_list(prt)[3:6]
-        imu_euler = transform_quaternion_to_euler([self.imu.orientation.x, self.imu.orientation.y, self.imu.orientation.z, self.imu.orientation.w]) # imu.orientation is assumed to be global
+        imu_matrix = tf.transformations.quaternion_matrix([self.imu.orientation.x, self.imu.orientation.y, self.imu.orientation.z, self.imu.orientation.w])[:3, :3]
+        imu_euler = tf.transformations.euler_from_matrix(numpy.dot(self.imu_rotation, imu_matrix))
         roll_pitch_pdf = scipy.stats.norm.pdf(prt_euler[0] - imu_euler[0], loc = 0.0, scale = self.roll_error_sigma) * scipy.stats.norm.pdf(prt_euler[1] - imu_euler[1], loc = 0.0, scale = self.pitch_error_sigma)
         if self.use_imu_yaw:
             return roll_pitch_pdf * scipy.stats.norm.pdf(prt_euler[2] - imu_euler[2], loc = 0.0, scale = self.yaw_error_sigma)
@@ -241,7 +205,7 @@ def publish_odometry(self):
         self.odom.pose.covariance = list(itertools.chain(*cov))
         self.pub.publish(self.odom)
         if self.publish_tf:
-            self.broadcast_transform()
+            broadcast_transform(self.broadcast, self.odom, self.invert_tf)
 
     ## callback functions
     def source_odom_callback(self, msg):        
@@ -259,6 +223,15 @@ def init_signal_callback(self, msg):
 
     def imu_callback(self, msg):
         with self.lock:
+            try:
+                (trans,rot) = self.listener.lookupTransform(msg.header.frame_id, self.odom_frame, msg.header.stamp)
+            except:
+                try:
+                    (trans,rot) = self.listener.lookupTransform(msg.header.frame_id, self.odom_frame, rospy.Time(0)) # retry to get newest tf data
+                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+                    rospy.logwarn("[%s] failed to solve tf in calculate imu_transorm: %s to %s", rospy.get_name(), self.base_link_frame, msg.header.frame_id)
+                    return # imu is not updated when imu_rotation cannot be calculated
+            self.imu_rotation = tf.transformations.quaternion_matrix(rot)[:3, :3] # trans does not affects to orientation 
             self.imu = msg
         
     # main functions
@@ -312,94 +285,13 @@ def guess_normal_distribution(self, particles, weights):
         
         return (mean.tolist(), cov.tolist())
 
-    def broadcast_transform(self):
-        if not self.odom:
-            return
-        position = [self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z]
-        orientation = [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w]
-        if self.invert_tf:
-            homogeneous_matrix = tf.transformations.quaternion_matrix(orientation)
-            homogeneous_matrix[:3, 3] = numpy.array(position).reshape(1, 3)
-            homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix)
-            position = list(homogeneous_matrix_inv[:3, 3])
-            orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv))
-            parent_frame = self.odom.child_frame_id
-            target_frame = self.odom.header.frame_id
-        else:
-            parent_frame = self.odom.header.frame_id
-            target_frame = self.odom.child_frame_id
-        self.broadcast.sendTransform(position, orientation, rospy.Time.now(), target_frame, parent_frame)
-
-    def transform_twist_with_covariance_to_global(self, pose, twist):
-        trans = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
-        rot = [pose.pose.orientation.x, pose.pose.orientation.y,
-               pose.pose.orientation.z, pose.pose.orientation.w]
-        rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3]
-        twist_cov_matrix = numpy.matrix(twist.covariance).reshape(6, 6)
-        global_velocity = numpy.dot(rotation_matrix, numpy.array([[twist.twist.linear.x],
-                                                                  [twist.twist.linear.y],
-                                                                  [twist.twist.linear.z]]))
-        global_omega = numpy.dot(rotation_matrix, numpy.array([[twist.twist.angular.x],
-                                                               [twist.twist.angular.y],
-                                                               [twist.twist.angular.z]]))
-        global_twist_cov_matrix = numpy.zeros((6, 6))
-        global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix))
-        global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
-
-        return TwistWithCovariance(Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0])),
-                                   global_twist_cov_matrix.reshape(-1,).tolist())
+    def transform_twist_with_covariance_to_global(self, pose_with_covariance, twist_with_covariance):
+        global_twist = transform_local_twist_to_global(pose_with_covariance.pose, twist_with_covariance.twist)
+        global_twist_cov = transform_local_twist_covariance_to_global(pose_with_covariance.pose, twist_with_covariance.covariance)
+        return TwistWithCovariance(global_twist, global_twist_cov)
 
     def update_pose_with_covariance(self, pose_with_covariance, twist_with_covariance, dt):
         global_twist_with_covariance = self.transform_twist_with_covariance_to_global(pose_with_covariance, twist_with_covariance)
-        # calculate current pose as integration
-        ret_pose = self.calculate_pose_transform(pose_with_covariance.pose, global_twist_with_covariance.twist, dt)
-        # update covariance
-        ret_pose_cov = self.calculate_pose_covariance_transform(pose_with_covariance.covariance, global_twist_with_covariance.covariance, dt)
+        ret_pose = update_pose(pose_with_covariance.pose, global_twist_with_covariance.twist, dt)
+        ret_pose_cov = update_pose_covariance(pose_with_covariance.covariance, global_twist_with_covariance.covariance, dt)
         return PoseWithCovariance(ret_pose, ret_pose_cov)
-
-    def calculate_pose_transform(self, pose, global_twist, dt):
-        ret_pose = Pose()
-        # calculate current pose as integration
-        ret_pose.position.x = pose.position.x + global_twist.linear.x * dt
-        ret_pose.position.y = pose.position.y + global_twist.linear.y * dt
-        ret_pose.position.z = pose.position.z + global_twist.linear.z * dt
-        ret_pose.orientation = self.calculate_quaternion_transform(pose.orientation, global_twist.angular, dt)
-        return ret_pose
-
-    def calculate_quaternion_transform(self, orientation, angular, dt): # angular is assumed to be global
-        # quaternion calculation
-        quat_vec = numpy.array([[orientation.x],
-                                [orientation.y],
-                                [orientation.z],
-                                [orientation.w]])
-        # skew_omega = numpy.matrix([[0, angular.z, -angular.y, angular.x],
-        #                            [-angular.z, 0, angular.x, angular.y],
-        #                            [angular.y, -angular.x, 0, angular.z],
-        #                            [-angular.x, -angular.y, -angular.z, 0]])
-        skew_omega = numpy.matrix([[0, -angular.z, angular.y, angular.x],
-                                   [angular.z, 0, -angular.x, angular.y],
-                                   [-angular.y, angular.x, 0, angular.z],
-                                   [-angular.x, -angular.y, -angular.z, 0]])
-        new_quat_vec = quat_vec + 0.5 * numpy.dot(skew_omega, quat_vec) * dt
-        norm = numpy.linalg.norm(new_quat_vec)
-        if norm == 0:
-            rospy.logwarn("norm of quaternion is zero")
-        else:
-            new_quat_vec = new_quat_vec / norm # normalize
-        return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist())
-
-    def calculate_pose_covariance_transform(self, pose_cov, global_twist_cov, dt):
-        ret_pose_cov = []
-        # make matirx from covariance array
-        prev_pose_cov_matrix = numpy.matrix(pose_cov).reshape(6, 6)
-        global_twist_cov_matrix = numpy.matrix(global_twist_cov).reshape(6, 6)
-        # jacobian matrix
-        # elements in pose and twist are assumed to be independent on global coordinates
-        jacobi_pose = numpy.diag([1.0] * 6)
-        jacobi_twist = numpy.diag([dt] * 6)
-        # covariance calculation
-        pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T))
-        # update covariances as array type (twist is same as before)
-        ret_pose_cov = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
-        return ret_pose_cov
-        
diff --git a/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py
new file mode 100644
index 0000000000..9ff1c29b9b
--- /dev/null
+++ b/jsk_robot_common/jsk_robot_startup/src/jsk_robot_startup/odometry_utils.py
@@ -0,0 +1,149 @@
+#! /usr/bin/env python
+
+import rospy
+import tf
+import numpy
+import scipy.stats
+import math
+from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, Twist, Pose, Point, Quaternion, Vector3
+
+# twist transformation
+def transform_local_twist_to_global(pose, local_twist):
+    trans = [pose.position.x, pose.position.y, pose.position.z]
+    rot = [pose.orientation.x, pose.orientation.y,
+           pose.orientation.z, pose.orientation.w]
+    rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3]
+    global_velocity = numpy.dot(rotation_matrix, numpy.array([[local_twist.linear.x],
+                                                              [local_twist.linear.y],
+                                                              [local_twist.linear.z]]))
+    global_omega = numpy.dot(rotation_matrix, numpy.array([[local_twist.angular.x],
+                                                           [local_twist.angular.y],
+                                                           [local_twist.angular.z]]))
+    return Twist(Vector3(*global_velocity[:, 0]), Vector3(*global_omega[:, 0]))
+
+def transform_local_twist_covariance_to_global(pose, local_twist_with_covariance):
+    trans = [pose.position.x, pose.position.y, pose.position.z]
+    rot = [pose.orientation.x, pose.orientation.y,
+           pose.orientation.z, pose.orientation.w]
+    rotation_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3]
+    twist_cov_matrix = numpy.matrix(local_twist_with_covariance).reshape(6, 6)
+    global_twist_cov_matrix = numpy.zeros((6, 6))
+    global_twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(twist_cov_matrix[:3, :3].dot(rotation_matrix))
+    global_twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
+    return global_twist_cov_matrix.reshape(-1,).tolist()
+
+# pose calculation
+def update_pose(pose, global_twist, dt):
+    ret_pose = Pose()
+    # calculate current pose as integration
+    ret_pose.position.x = pose.position.x + global_twist.linear.x * dt
+    ret_pose.position.y = pose.position.y + global_twist.linear.y * dt
+    ret_pose.position.z = pose.position.z + global_twist.linear.z * dt
+    ret_pose.orientation = update_quaternion(pose.orientation, global_twist.angular, dt)
+    return ret_pose
+
+def update_quaternion(orientation, angular, dt): # angular is assumed to be global
+    # quaternion calculation
+    quat_vec = numpy.array([[orientation.x],
+                            [orientation.y],
+                            [orientation.z],
+                            [orientation.w]])
+    skew_omega = numpy.matrix([[0, -angular.z, angular.y, angular.x],
+                               [angular.z, 0, -angular.x, angular.y],
+                               [-angular.y, angular.x, 0, angular.z],
+                               [-angular.x, -angular.y, -angular.z, 0]])
+    new_quat_vec = quat_vec + 0.5 * numpy.dot(skew_omega, quat_vec) * dt
+    norm = numpy.linalg.norm(new_quat_vec)
+    if norm == 0:
+        rospy.logwarn("norm of quaternion is zero")
+    else:
+        new_quat_vec = new_quat_vec / norm # normalize
+    return Quaternion(*numpy.array(new_quat_vec).reshape(-1,).tolist())
+
+# covariance calculation
+def update_twist_covariance(twist, v_sigma, min_sigma = 1e-3): # twist is assumed to be local
+    twist_list = [twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z, twist.twist.angular.x, twist.twist.angular.y, twist.twist.angular.z]
+    if all([abs(x) < 1e-6 for x in twist_list]):
+        current_sigma = [min_sigma] * 6 # trust "completely stopping" state
+    else:
+        current_sigma = v_sigma
+    return numpy.diag([max(x ** 2, min_sigma ** 2) for x in current_sigma]).reshape(-1,).tolist() # covariance should be singular
+
+def update_pose_covariance(pose_cov, global_twist_cov, dt):
+    ret_pose_cov = []
+    # make matirx from covariance array
+    prev_pose_cov_matrix = numpy.matrix(pose_cov).reshape(6, 6)
+    global_twist_cov_matrix = numpy.matrix(global_twist_cov).reshape(6, 6)
+    # jacobian matrix
+    # elements in pose and twist are assumed to be independent on global coordinates
+    jacobi_pose = numpy.diag([1.0] * 6)
+    jacobi_twist = numpy.diag([dt] * 6)
+    # covariance calculation
+    pose_cov_matrix = jacobi_pose.dot(prev_pose_cov_matrix.dot(jacobi_pose.T)) + jacobi_twist.dot(global_twist_cov_matrix.dot(jacobi_twist.T))
+    # update covariances as array type (twist is same as before)
+    ret_pose_cov = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
+    return ret_pose_cov
+
+# tf broadcast
+def broadcast_transform(broadcast, odom, invert_tf):
+    if not odom or not broadcast:
+        return
+    position = [odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z]
+    orientation = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]
+    if invert_tf:
+        homogeneous_matrix = make_homogeneous_matrix(position, orientation)
+        homogeneous_matrix_inv = numpy.linalg.inv(homogeneous_matrix)
+        position = list(homogeneous_matrix_inv[:3, 3])
+        orientation = list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_inv))
+        parent_frame = odom.child_frame_id
+        target_frame = odom.header.frame_id
+    else:
+        parent_frame = odom.header.frame_id
+        target_frame = odom.child_frame_id
+    broadcast.sendTransform(position, orientation, odom.header.stamp, target_frame, parent_frame)
+
+# mathmatical tools    
+def make_homogeneous_matrix(trans, rot):
+    homogeneous_matrix = tf.transformations.quaternion_matrix(rot)
+    homogeneous_matrix[:3, 3] = numpy.array(trans).reshape(1, 3)
+    return homogeneous_matrix
+    
+# scipy.stats.multivariate_normal only can be used after SciPy 0.14.0
+# input: x(array), mean(array), cov_inv(matrix) output: probability of x
+# covariance has to be inverted to reduce calculation time
+def norm_pdf_multivariate(x, mean, cov_inv):
+    size = len(x)
+    if size == len(mean) and (size, size) == cov_inv.shape:
+        inv_det = numpy.linalg.det(cov_inv)
+        if not inv_det > 0:
+            rospy.logwarn("Determinant of inverse cov matrix {0} is equal or smaller than zero".format(inv_det))
+            return 0.0
+        norm_const = math.pow((2 * numpy.pi), float(size) / 2) * math.pow(1 / inv_det, 1.0 / 2) # determinant of inverse matrix is reciprocal
+        if not norm_const > 0 :
+            rospy.logwarn("Norm const {0} is equal or smaller than zero".format(norm_const))
+            return 0.0
+        x_mean = numpy.matrix(x - mean)
+        exponent = -0.5 * (x_mean * cov_inv * x_mean.T)
+        if exponent > 0:
+            rospy.logwarn("Exponent {0} is larger than zero".format(exponent))
+            exponent = 0
+        result = math.pow(math.e, exponent)
+        return result / norm_const
+    else:
+        rospy.logwarn("The dimensions of the input don't match")
+        return 0.0
+
+# tf.transformations.euler_from_quaternion is slow because the function calculates matrix inside.
+# cf. https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+def transform_quaternion_to_euler(quat):
+    zero_thre = numpy.finfo(float).eps * 4.0 # epsilon for testing whether a number is close to zero
+    roll_numerator = 2 * (quat[3] * quat[0] + quat[1] * quat[2])
+    if abs(roll_numerator) < zero_thre:
+        roll_numerator = numpy.sign(roll_numerator) * 0.0
+    yaw_numerator = 2 * (quat[3] * quat[2] + quat[0] * quat[1])
+    if abs(yaw_numerator) < zero_thre:
+        yaw_numerator = numpy.sign(yaw_numerator) * 0.0
+    return (numpy.arctan2(roll_numerator, 1 - 2 * (quat[0] ** 2 + quat[1] ** 2)),
+            numpy.arcsin(2 * (quat[3] * quat[1] - quat[2] * quat[0])),
+            numpy.arctan2(yaw_numerator, 1 - 2 * (quat[1] ** 2 + quat[2] ** 2)))
+