diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index 8bd041b0ed8e426d55f8fbabc34b50501c3a35de..7da006515acef2554deb51c55751b820bffc090c 100644
--- a/config/omniwheels_config.yaml
+++ b/config/omniwheels_config.yaml
@@ -1,6 +1,6 @@
 # configuration file for all the omniwheels 
-name: "omniwheel2"
-id: 2
+name: "omniwheel1"
+id: 1
 channel: 90 # change to the correct one
 type: default
 position_offset: 0.11 # ofsset from crazyflie module to the cenyter of the omniwheels
diff --git a/scripts/Buffer.py b/scripts/Buffer.py
new file mode 100644
index 0000000000000000000000000000000000000000..6313d5b24281c2e6ad5d816d8a4997cb37e70f89
--- /dev/null
+++ b/scripts/Buffer.py
@@ -0,0 +1,14 @@
+import numpy as np 
+
+
+class Buffer:
+    def __init__(self, buffer_size):
+        self.buf = np.zeros(buffer_size)
+        
+    def get_mean(self):
+        return self.buf.mean()
+    def fill(self, data):
+        self.buf[0:-2] = self.buf[1:-1]
+        self.buf[-1] = data
+    def print_buffer(self):
+        print(self.buf)
diff --git a/scripts/Buffer.pyc b/scripts/Buffer.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..243ba8424069612b9e7ad7825fa08ae9c9b1a785
Binary files /dev/null and b/scripts/Buffer.pyc differ
diff --git a/scripts/__pycache__/Buffer.cpython-37.pyc b/scripts/__pycache__/Buffer.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b063a054839cc29247a2074c1200ab964583dbd5
Binary files /dev/null and b/scripts/__pycache__/Buffer.cpython-37.pyc differ
diff --git a/scripts/__pycache__/SimplePI.cpython-37.pyc b/scripts/__pycache__/SimplePI.cpython-37.pyc
index 367ecb7e4cf6ad3b1c9d85960607279f4cfee9b2..e97544cd15523b4b1ed1322cad87a432aedc926b 100644
Binary files a/scripts/__pycache__/SimplePI.cpython-37.pyc and b/scripts/__pycache__/SimplePI.cpython-37.pyc differ
diff --git a/scripts/crazyflie_logger_node.py b/scripts/crazyflie_logger_node.py
index cce46f6f2627afb0acb0d3ba5562d398629aaf84..6798272f8327628b7645a77cc9e5b0593d38aa8c 100755
--- a/scripts/crazyflie_logger_node.py
+++ b/scripts/crazyflie_logger_node.py
@@ -22,7 +22,7 @@ from omniwheels_controller.msg import motorCommand
 from geometry_msgs.msg import Twist
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Vector3
-
+from Buffer import Buffer
 
 uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB
 
@@ -58,6 +58,14 @@ class ParameterLogging:
         self._pose_data_ = Pose()
         self._vel_data_ = Twist()
         self._acc_data_ = Vector3()
+        # Buffer
+        self.bs = 30
+        self.pos_x_buf = Buffer(self.bs)
+        self.pos_y_buf = Buffer(self.bs)
+        self.pos_yaw_buf = Buffer(self.bs)
+        self.vel_x_buf = Buffer(self.bs)
+        self.vel_y_buf = Buffer(self.bs)
+        self.vel_z_buf = Buffer(self.bs)
         # create publishers 
         self._position_pub = rospy.Publisher("pose", Pose,queue_size=100)
         self._velocity_pub = rospy.Publisher("vel", Twist,queue_size=100)
@@ -205,14 +213,14 @@ class ParameterLogging:
         print('Error when logging %s: %s' % (logconf.name, msg))
 
     def _pos_log_data(self, timestamp, data, logconf):
-        x = data['stateEstimateZ.x']/1000.0
-        y = data['stateEstimateZ.y']/1000.0
+        self.pos_x_buf.fill(data['stateEstimateZ.x']/1000.0)
+        self.pos_y_buf.fill(data['stateEstimateZ.y']/1000.0)
         z = data['stateEstimateZ.z']/1000.0
         yaw = (data['stateEstimate.yaw'])
-        yaw_ = np.mod(yaw-30, 360);
+        self.pos_yaw_buf.fill(np.mod(yaw-30, 360));
 
        
-        quat_val = get_quaternion_from_euler(0,0,np.deg2rad(yaw_))
+        quat_val = get_quaternion_from_euler(0,0,np.deg2rad(self.pos_yaw_buf.get_mean()))
         
 
         
@@ -220,17 +228,21 @@ class ParameterLogging:
         self._pose_data_.orientation.y =quat_val[1]
         self._pose_data_.orientation.z = quat_val[2]
         self._pose_data_.orientation.w = quat_val[3] 
-        self._pose_data_.position.x =x+ np.sin(np.deg2rad(yaw))*self._position_offset;
-        self._pose_data_.position.y = y- np.cos(np.deg2rad(yaw))*self._position_offset
+        self._pose_data_.position.x =self.pos_x_buf.get_mean()+ np.sin(np.deg2rad(yaw))*self._position_offset;
+        self._pose_data_.position.y = self.pos_x_buf.get_mean()- np.cos(np.deg2rad(yaw))*self._position_offset
         self._pose_data_.position.z = z;
         
              
         self._position_pub.publish(self._pose_data_)
        
     def _vel_log_data(self, timestamp,data, logconf):
-        self._vel_data_.linear.x = data['stateEstimateZ.vx']/1000.0
-        self._vel_data_.linear.y = data['stateEstimateZ.vy']/1000.0
-        self._vel_data_.angular.z = data['stateEstimateZ.rateYaw']/1000.0
+        self.vel_x_buf.fill(data['stateEstimateZ.vx']/1000.0)
+        self.vel_y_buf.fill(data['stateEstimateZ.vy']/1000.0)
+        self.vel_z_buf.fill(data['stateEstimateZ.rateYaw']/1000.0)
+
+        self._vel_data_.linear.x = self.vel_x_buf.get_mean()
+        self._vel_data_.linear.y = self.vel_y_buf.get_mean()
+        self._vel_data_.angular.z = self.vel_z_buf.get_mean()
         self._velocity_pub.publish(self._vel_data_)
         
     def _acc_log_data(self, timestamp,data, logconf):