diff --git a/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml b/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml
new file mode 100644
index 00000000..b13c0d04
--- /dev/null
+++ b/bag_data/rosbag2_2025_03_06-20_57_16/metadata.yaml
@@ -0,0 +1,21 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 0
+  starting_time:
+    nanoseconds_since_epoch: 9223372036854775807
+  message_count: 0
+  topics_with_message_count:
+    []
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_2025_03_06-20_57_16_0.db3
+  files:
+    - path: rosbag2_2025_03_06-20_57_16_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 9223372036854775807
+      duration:
+        nanoseconds: 0
+      message_count: 0
\ No newline at end of file
diff --git a/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 b/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3
new file mode 100644
index 00000000..8d37ede6
Binary files /dev/null and b/bag_data/rosbag2_2025_03_06-20_57_16/rosbag2_2025_03_06-20_57_16_0.db3 differ
diff --git a/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml b/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml
new file mode 100644
index 00000000..eae7d682
--- /dev/null
+++ b/bag_data/rosbag2_2025_03_06-21_00_01/metadata.yaml
@@ -0,0 +1,21 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 0
+  starting_time:
+    nanoseconds_since_epoch: 9223372036854775807
+  message_count: 0
+  topics_with_message_count:
+    []
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_2025_03_06-21_00_01_0.db3
+  files:
+    - path: rosbag2_2025_03_06-21_00_01_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 9223372036854775807
+      duration:
+        nanoseconds: 0
+      message_count: 0
\ No newline at end of file
diff --git a/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 b/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3
new file mode 100644
index 00000000..8d37ede6
Binary files /dev/null and b/bag_data/rosbag2_2025_03_06-21_00_01/rosbag2_2025_03_06-21_00_01_0.db3 differ
diff --git a/scripts/run_path_planning.sh b/scripts/run_path_planning.sh
index 130ff830..27b16204 100755
--- a/scripts/run_path_planning.sh
+++ b/scripts/run_path_planning.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 source install/setup.bash
 colcon build --packages-select path_planning 
-ros2 run path_planning centerline_planner
+ros2 run path_planning fasttube
 #ros2 run path_planning trajectory_generation
 
diff --git a/scripts/run_zed_launch.sh b/scripts/run_zed_launch.sh
old mode 100644
new mode 100755
index 7bdb5ce7..22a65913
--- a/scripts/run_zed_launch.sh
+++ b/scripts/run_zed_launch.sh
@@ -1,3 +1,5 @@
 #!/bin/bash
 source install/setup.bash
-colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording2.svo2
\ No newline at end of file
+#colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node 
+## with sim
+colcon build --packages-select zed_launch && ros2 run zed_launch zed_launch_node recording1.svo2
\ No newline at end of file
diff --git a/scripts/z_recording_simulation.sh b/scripts/z_recording_simulation.sh
old mode 100644
new mode 100755
diff --git a/src/action/stanley_controller/stanley_controller/stanley_controller.py b/src/action/stanley_controller/stanley_controller/stanley_controller.py
index d0b274f9..fbc071f3 100644
--- a/src/action/stanley_controller/stanley_controller/stanley_controller.py
+++ b/src/action/stanley_controller/stanley_controller/stanley_controller.py
@@ -4,7 +4,6 @@
 import math
 from geometry_msgs.msg import PoseArray
 from geometry_msgs.msg import Pose
-from moa_msgs.msg import ConeMap
 from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped
 from std_msgs.msg import Header
 
diff --git a/src/hardware_drivers/CanTalk b/src/hardware_drivers/CanTalk
index f887c380..0d8cb207 160000
--- a/src/hardware_drivers/CanTalk
+++ b/src/hardware_drivers/CanTalk
@@ -1 +1 @@
-Subproject commit f887c38038fd4ba4a87c7075ca9f883bd60670bb
+Subproject commit 0d8cb20773093716615f7e18d7ba739931cc9ae1
diff --git a/src/perception/aquisition/zed_launch/CMakeLists.txt b/src/perception/aquisition/zed_launch/CMakeLists.txt
index 34c3682f..8052b12e 100644
--- a/src/perception/aquisition/zed_launch/CMakeLists.txt
+++ b/src/perception/aquisition/zed_launch/CMakeLists.txt
@@ -73,7 +73,7 @@ link_directories(/usr/local/cuda/lib64)
 include_directories(/usr/include/x86_64-linux-gnu/)
 link_directories(/usr/lib/x86_64-linux-gnu/)
 
-FILE(GLOB_RECURSE SRC_FILES cone_detection/cone_detection.cpp cone_detection/yolo.cpp zed_launch/zed_launch.cpp localisation/localisation.cpp) 
+FILE(GLOB_RECURSE SRC_FILES cone_detection/cone_detection.cpp cone_detection/yolo.cpp zed_launch/zed_launch.cpp localisation/localisation.cpp velocity/velocity.cpp) 
 FILE(GLOB_RECURSE HDR_FILES cone_detection/*.h* zed_launch/*.h*)
 
 cuda_add_executable(zed_launch_node ${HDR_FILES} ${SRC_FILES})
@@ -135,4 +135,9 @@ install(TARGETS
   DESTINATION lib/zed_launch
 )
 
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_package()
diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp
index 778baa73..0ae218a4 100644
--- a/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp
+++ b/src/perception/aquisition/zed_launch/cone_detection/cone_detection.cpp
@@ -15,8 +15,8 @@
 #include <NvInfer.h>
 
 using namespace nvinfer1;
-#define NMS_THRESH 0.4
 #define CONF_THRESH 0.8
+#define NMS_THRESH 0.4
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -84,7 +84,7 @@ void ZedLaunchNode::cone_detection_loop()
     auto camera_info = zed.getCameraInformation(pc_resolution).camera_configuration;
 
     // Creating the inference engine class
-    std::string engine_name = "cone_detection_model.engine";
+    std::string engine_name = model_name;
     Yolo detector;
     if (detector.init(engine_name)) {
         std::cerr << "Detector init failed!" << std::endl;
diff --git a/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp b/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp
index 1bdedcea..78a8258d 100644
--- a/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp
+++ b/src/perception/aquisition/zed_launch/cone_detection/cone_subscriber.cpp
@@ -35,7 +35,11 @@ class ConeSubscriber : public rclcpp::Node
 private:
   void cone_detection_callback(const moa_msgs::msg::Detections & msg) const
   {
-    std::cout << "detections: " << msg.car_pose.position.x << " " << msg.car_pose.position.y << " " << msg.car_pose.position.z << " " << std::endl;
+    for (auto i = 0u; i < msg.blue.size(); i++)
+    {
+      float distance = sqrt(pow(msg.blue[i].x, 2) + pow(msg.blue[i].y, 2));
+      std::cout << "blue: " << i << " " << msg.blue[i].x << " " << msg.blue[i].y << " " << distance << std::endl;
+    }
   }
   rclcpp::Subscription<moa_msgs::msg::Detections>::SharedPtr subscription_;
 };
diff --git a/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py b/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py
new file mode 100644
index 00000000..f2629de8
--- /dev/null
+++ b/src/perception/aquisition/zed_launch/launch/zed_launch.launch.py
@@ -0,0 +1,38 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # Declare the launch argument for recording name
+    recording_name_arg = DeclareLaunchArgument(
+        'recording_name',
+        default_value='default_recording',
+        description="""
+    This launch file is used to start the ZED camera node with or 
+    without recording functionality.
+    Usage:
+        - Without recording: ros2 launch zed_launch.launch.py
+        - With recording: ros2 launch zed_launch.launch.py recording_name:=<NAME>
+    
+    The 'recording_name' argument is optional. If provided, the camera node will 
+    initiate a recording session using the specified file name.
+    """
+    )
+
+    # Access the recording name configured by the user
+    recording_name = LaunchConfiguration('recording_name')
+
+    # Define the node, including the recording name as a command-line argument
+    camera_node = Node(
+        package='zed_launch',
+        executable='zed_launch_node',
+        name='zed_camera',
+        output='screen',
+        arguments=[recording_name]
+    )
+
+    return LaunchDescription([
+        recording_name_arg,
+        camera_node
+    ])
\ No newline at end of file
diff --git a/src/perception/aquisition/zed_launch/package.xml b/src/perception/aquisition/zed_launch/package.xml
index 2b27b0e7..58be6bd6 100644
--- a/src/perception/aquisition/zed_launch/package.xml
+++ b/src/perception/aquisition/zed_launch/package.xml
@@ -22,6 +22,7 @@
 	<exec_depend>geometry_msgs</exec_depend>
 	<exec_depend>sensor_msgs</exec_depend>
 	<exec_depend>cv_bridge</exec_depend>
+	<exec_depend>ros2launch</exec_depend>
 
 	<export>
 		<build_type>ament_cmake</build_type>
diff --git a/src/perception/aquisition/zed_launch/velocity/velocity.cpp b/src/perception/aquisition/zed_launch/velocity/velocity.cpp
new file mode 100644
index 00000000..aa196c38
--- /dev/null
+++ b/src/perception/aquisition/zed_launch/velocity/velocity.cpp
@@ -0,0 +1,33 @@
+#include <iostream>
+#include <cmath>
+
+#include "sl/Camera.hpp"
+#include "../zed_launch/zed_launch.hpp"
+
+#include "geometry_msgs/msg/vector3.hpp" 
+
+void ZedLaunchNode::car_velocity() {
+
+    geometry_msgs::msg::Vector3 car_velocity;
+    sl::SensorsData sensor_data;
+    sl::SensorsData::IMUData imu_data;
+
+    car_velocity.x = 0;
+    car_velocity.y = 0;
+    car_velocity.z = 0;
+
+    while (camera_running) {
+        std::this_thread::sleep_for(10ms);
+
+        zed.getSensorsData(sensor_data, sl::TIME_REFERENCE::IMAGE);
+        imu_data = sensor_data.imu;
+
+        car_velocity.x += imu_data.linear_acceleration.x * 0.01;
+        car_velocity.y += imu_data.linear_acceleration.y * 0.01;
+
+        car_velocity_publisher->publish(car_velocity);
+    
+    }
+}
+
+//  ### THIS DOES NOT OUTPUT ACCURATE VELOCITY DATA ###
\ No newline at end of file
diff --git a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp
index 334d20ba..e563d7ff 100644
--- a/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp
+++ b/src/perception/aquisition/zed_launch/zed_launch/zed_launch.hpp
@@ -11,6 +11,7 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "moa_msgs/msg/detections.hpp"
 #include "sensor_msgs/msg/image.hpp"
+#include "geometry_msgs/msg/vector3.hpp"
 
 using namespace std::chrono_literals;
 
@@ -26,6 +27,7 @@ class ZedLaunchNode : public rclcpp::Node
     // Create the publishers
     cone_detection_publisher = this->create_publisher<moa_msgs::msg::Detections>("cone_detection", 10);
     car_position_publisher = this->create_publisher<geometry_msgs::msg::Pose>("car_position", 10);
+    car_velocity_publisher = this->create_publisher<geometry_msgs::msg::Vector3>("car_velocity", 10);
     image_publisher = this->create_publisher<sensor_msgs::msg::Image>("image", 10);
 
     // Start the threads
@@ -33,6 +35,8 @@ class ZedLaunchNode : public rclcpp::Node
 
     std::thread t2(&ZedLaunchNode::car_position, this);
 
+    std::thread t3(&ZedLaunchNode::car_velocity, this);
+
     t1.join();
   }
 
@@ -41,13 +45,16 @@ class ZedLaunchNode : public rclcpp::Node
     sl::Camera& zed;
     sl::Pose cam_w_pose;
     bool camera_running = true;
-    bool visualisation = false;
+    bool visualisation = true;
+    std::string model_name = "cone_detection_model.engine";
 
     // Publishers
     rclcpp::Publisher<moa_msgs::msg::Detections>::SharedPtr cone_detection_publisher;
     rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr car_position_publisher;
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher;
+    rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr car_velocity_publisher;
         
     void cone_detection_loop();
     void car_position();
+    void car_velocity();
 };
\ No newline at end of file
diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py
new file mode 100644
index 00000000..8476729d
--- /dev/null
+++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter copy.py	
@@ -0,0 +1,389 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+
+from std_msgs.msg import Float32MultiArray
+from moa_msgs.msg import Detections, Track
+from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance
+
+import math
+import numpy as np
+import time
+import threading
+
+import kdtree
+
+class Item(object):
+    def __init__(self, x, y, data):
+        self.coords = (x, y)
+        self.data = data
+
+    def __len__(self):
+        return len(self.coords)
+
+    def __getitem__(self, i):
+        return self.coords[i]
+
+    def __repr__(self):
+        return 'Item({}, {}, {})'.format(self.coords[0], self.coords[1], self.data)
+
+
+class Cone_Mapper(Node):
+    def __init__(self):
+        super().__init__('cone_mapper')
+        qos_profile = QoSProfile(
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=10
+        )
+
+        # Create cone detection subscriber
+        self.cones_subscription = self.create_subscription(
+            Detections,
+            'cone_detection',
+            self.cones_callback,
+            qos_profile)
+
+        # Create left track publisher
+        self.left_track_publisher = self.create_publisher(Track, 'left_track', 10)
+
+        # Create right track publisher
+        self.right_track_publisher = self.create_publisher(Track, 'right_track', 10)
+
+        # times_modified publisher
+        self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10)
+
+        # Existing cone map
+        self.left_track = Track()
+        self.right_track = Track()
+
+        self.left_track.cones = []
+        self.right_track.cones = []
+
+        # KDTrees for searching
+        self.left_tree = None
+        self.right_tree = None
+
+        self.times_modified_counter = 0
+
+        # recording sleep time
+        time.sleep(12)
+
+################################################################################ (parameters to tune)
+
+        # Initial error in the estimate
+        self.default_error_in_estimate = 4.0
+
+        # Error in measurement
+        self.error_in_measurement = 2.5
+
+        # Cone match radius
+        self.match_radius = 1.0
+
+        # Counter to remove points
+        self.remove_point_counter = 200
+
+        # value for the times_modified
+        self.times_modified_limit = 300000
+
+        # Modify rate
+        self.modify_rate = 1.1
+
+################################################################################ (parameters to tune)
+
+    def cones_callback(self, msg: Detections) -> None:
+        """This function updates the existing cone map with newly detected cones and 
+        publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic.
+
+        Args:
+            msg (Detections): car's position and input cones from the /cone_detection topic
+        """
+        # Update the existing cone map with new measurements
+        self.update_existing_cone_map(msg)
+
+        # Publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic
+        # print("Left track length: ", len(self.left_track.cones))
+        # print("Right track length: ", len(self.right_track.cones))
+        self.times_modified_counter = self.times_modified_counter + 1
+        if self.times_modified_counter > self.remove_point_counter:
+            self.times_modified_counter = 0
+            self.remove_points()
+
+        self.left_track_publisher.publish(self.left_track)
+        self.right_track_publisher.publish(self.right_track)
+
+
+    def update_existing_cone_map(self, msg: Detections) -> None:
+        """This function updates the existing cone map using new cone measurementsf
+
+        Args:
+            msg (Detections): Contains the car's position and lists of detected cones
+        """
+        # Get all cones' global positions
+        car_position = msg.car_pose
+        global_blue_cone_positions = self.cones_local_to_global(msg.blue, car_position)
+        global_yellow_cone_positions = self.cones_local_to_global(msg.yellow, car_position)
+
+        # Update the left track and the right track
+        self.update_left_track(global_blue_cone_positions)
+        self.update_right_track(global_yellow_cone_positions)
+
+
+    def update_left_track(self, points: list) -> None:
+        """This function updates the left track of the cone map
+
+        Args:
+            points (list): A list of blue cones' global (x, y) coordinates
+        """
+        if self.left_track.cones == []:
+            # If the left track is empty, pack all cone measurements and update the left track
+            self.left_track = self.produce_track_message(points)
+            # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)])
+            times_modified = 1
+            self.left_tree = kdtree.create(
+                 [Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2)  # For 2D points
+        else:
+            # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track
+            for coord in points:
+                point, distance = self.left_tree.search_nn(coord)
+                # print(self.left_tree.search_nn(coord))
+                # kdtree.visualize(self.left_tree)
+                # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates
+                if distance <= self.match_radius:
+                    cone = point.data.data[0]
+                    error_in_estimate = point.data.data[1]
+                    times_modified = point.data.data[2] * self.modify_rate
+                    new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate)
+                    point.data.coords = (new_estimate_x, new_estimate_y)
+                    point.data.data = (cone, new_error_in_estimate, times_modified)
+                else:                    # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map
+                    cone = Point()
+                    cone.x = coord[0]
+                    cone.y = coord[1]
+                    self.left_track.cones.append(cone)
+                    times_modified = 1
+                    self.left_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified)))
+
+    
+
+    def update_right_track(self, points: list) -> None:
+        """This function updates the right track of the cone map
+
+        Args:
+            points (list): A list of yellow cones' (x, y) coordinates
+        """
+        if self.right_track.cones == []:
+            # If the right track is empty, pack all cone measurements and update the right track
+            self.right_track = self.produce_track_message(points)
+            times_modified = 1
+            self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, times_modified]) for index, coord in enumerate(points)], dimensions=2)
+        else:
+            # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track
+            for coord in points:
+                point, distance = self.right_tree.search_nn(coord)
+                # print(self.right_tree.search_nn(coord))
+                # If the newly measured cone is in the match radius, this cone already exists in the right track, update its coordinates
+                # check if distance is nan
+                # kdtree.visualize(self.right_tree)
+                if distance <= self.match_radius:
+                    cone = point.data.data[0]
+                    error_in_estimate = point.data.data[1]
+                    times_modified = point.data.data[2] * self.modify_rate
+                    new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate)
+                    point.data.coords = (new_estimate_x, new_estimate_y)
+                    point.data.data = (cone, new_error_in_estimate, times_modified)
+                else:
+                    # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map
+                    cone = Point()
+                    cone.x = coord[0]
+                    cone.y = coord[1]
+                    self.right_track.cones.append(cone)
+                    times_modified = 1
+                    self.right_tree.add(Item(coord[0], coord[1], (cone, self.default_error_in_estimate, times_modified)))
+
+    
+    def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple:
+        """This function update the x and y coordinate of of the input cone using kalman filtering
+
+        Args:
+            cone (Point): input cone's location
+            coord (tuple): new measurement of (x, y) of the input cone
+            error_in_estimate (float): error in estimate of the cone's actual position
+        
+        Returns:
+            new_estimate_x (float): cone's new estimated x coordinate
+            new_estimate_y (float): cone's new estimated y coordinate
+            new_error_in_estimate (float): cone's new error in estimate
+        """
+        # Get the previous error_in_estimate, estimated x and estimated y coordinate
+        prev_error_in_estimate = error_in_estimate
+        prev_estimate_x = cone.x
+        prev_estimate_y = cone.y
+
+        # Calculate the kalman gain
+        kalman_gain = prev_error_in_estimate / (prev_error_in_estimate + self.error_in_measurement)
+
+        # Calculate the new error_in_estimate, estimated x and estimated y coordinate
+        new_estimate_x = prev_estimate_x + kalman_gain * (coord[0] - prev_estimate_x)
+        new_estimate_y = prev_estimate_y + kalman_gain * (coord[1] - prev_estimate_y)
+        new_error_in_estimate = (1 - kalman_gain) * (prev_error_in_estimate)
+
+        # update x and y coordinate
+        cone.x = new_estimate_x
+        cone.y = new_estimate_y
+
+        return new_estimate_x, new_estimate_y, new_error_in_estimate
+
+
+    def cones_local_to_global(self, cones: list, car_position: Pose) -> list:
+        """This function converts all cones from the local frame to the global frame
+
+        Args:
+            cones ([Point]): contains all the cones' positions in local frame
+            car_position (Pose): car's current position
+
+        Returns:
+            global_cone_positions (list): a list contains each cone's global (x, y)
+        """
+        # If no cones are detected, return an empty np.array
+        if len(cones) == 0:
+            return []
+        
+        # Extract car's x coordinate, y coordinate and rotation angle
+        x, y, theta = self.extract_data_from_car(car_position)
+
+        # Convert cones positions into a np.array contains each cone's local x coordinate, y coordinate
+        list_of_cones = self.convert_cones_to_data(cones)
+
+        # Use cart's (x, y and theta) to get the cart's position vector and rotation matrix
+        position_vector, rotation_matrix = self.get_coordinate_conversion_matrices(x, y, theta)
+
+        # Convert cone's coordinates from local frame to global frame
+        global_cone_columns = self.local_to_global(position_vector, rotation_matrix, list_of_cones)
+
+        # Convert global_cone_columns to a list of cones' (x,y) position
+        global_cone_positions = [tuple(global_cone_columns[:,i]) for i in range(len(cones))]
+
+        return global_cone_positions
+    
+
+    def extract_data_from_car(self, car_position: Pose) -> tuple:
+        """This function extracts car's x coordinate, y coordinate and rotation angle from car_position
+
+        Args:
+            car_position (Pose): car's current position
+
+        Returns:
+            x (float): car's x coordinate
+            y (float): car's y coordinate
+            theta (float): car's rotation angle
+        """
+        x = car_position.position.x
+        y = car_position.position.y
+        theta = car_position.orientation.w
+        return x, y, theta
+
+
+    def convert_cones_to_data(self, cones: list) -> np.array:
+        """Convert a list of cones' positions into a 2*n np.array contains each cone's local x coordinate and y coordinate
+
+        Args:
+            cones ([Point]): A list of cones, each cone is represented as a Point
+
+        Returns:
+            list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate
+        """
+        # Extract x and y coordinate from all cones and store them in a 2*n np.arra
+        list_of_local_cones_x = [cone.x for cone in cones]
+        list_of_local_cones_y = [cone.y for cone in cones]
+        list_of_cones = np.array([list_of_local_cones_x, list_of_local_cones_y])       
+        return list_of_cones
+ 
+
+    def get_coordinate_conversion_matrices(self, x: float, y: float, theta: float) -> tuple:
+        """This function gets car's gloabl position vector and local coordinate rotation matrix
+
+        Args:
+            x (float): car's x coordinate
+            y (float): car's y coordinate
+            theta (float): car's rotation angle
+
+        Returns:
+            position_vector (np.array): car's global position vector
+            rotation_matrix (np.array): rotation matrix for cone's local coordinates
+        """
+        position_vector = np.array([[x],[y]])
+        rotation_matrix = np.array([[math.cos(theta), -math.sin(theta)],[math.sin(theta), math.cos(theta)]])
+
+        return position_vector, rotation_matrix
+    
+
+    def local_to_global(self, position_vector: np.array, rotation_matrix: np.array, list_of_cones: np.array) -> np.array:
+        """This function converts all cones' (x, y) from the local frame to the global frame
+
+        Args:
+            position_vector (np.array): car's global position vector
+            rotation_matrix (np.array): rotation matrix for cone's local coordinates
+            list_of_cones (np.array): 2*n np.array contains each cone's local x coordinate and y coordinate
+
+        Returns:
+            list_of_cones_output (np.array): 2*n np.array contains each cone's global x coordinate and y coordinate
+        """
+        list_of_cones_unrotated = np.matmul(rotation_matrix, list_of_cones)
+        list_of_cones_output = list_of_cones_unrotated + position_vector
+        return list_of_cones_output
+    
+
+    def produce_track_message(self, points: list) -> Track:
+        """This function packs all cones' (x, y) coordinates into a cone map message
+
+        Args:
+            points (list): A list of cones' (x, y) coordinates
+
+        Returns:
+            output_track (Track): Track message
+        """
+        output_track = Track()
+        for p in points:
+            point = Point()
+            point.x = p[0]
+            point.y = p[1]
+            output_track.cones.append(point)
+        return output_track
+
+    def remove_points(self):
+        if self.left_track is None or self.right_track is None:
+            return
+        
+        times_modified_list = Float32MultiArray()
+        
+        left_tree = self.left_tree
+        right_tree = self.right_tree
+        for point in kdtree.level_order(self.left_tree):
+            # print(point)
+            times_modified_list.data.append(point.data.data[2])
+            if point.data.data[2] < self.times_modified_limit:
+                self.left_track.cones.remove(point.data.data[0])
+                self.left_tree.remove(point.data)
+        for point in kdtree.level_order(self.right_tree):
+            times_modified_list.data.append(point.data.data[2])
+            if point.data.data[2] < self.times_modified_limit:
+                self.right_track.cones.remove(point.data.data[0])
+                self.right_tree.remove(point.data)
+            
+        self.times_modified_publisher.publish(times_modified_list)
+
+        # kdtree.visualize(self.left_tree)
+        # kdtree.visualize(self.right_tree)
+        
+
+def main(args=None):
+    rclpy.init(args=args)
+    cone_mapper = Cone_Mapper()
+    rclpy.spin(cone_mapper)
+    cone_mapper.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py
index 60b436a6..07c2dd28 100644
--- a/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py
+++ b/src/perception/sythesis/cone_mapping/cone_mapping/kalman_filter.py
@@ -2,11 +2,14 @@
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
 
+from std_msgs.msg import Float32MultiArray
 from moa_msgs.msg import Detections, Track
 from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance
 
 import math
 import numpy as np
+import time
+import threading
 
 import kdtree
 
@@ -47,13 +50,20 @@ def __init__(self):
         # Create right track publisher
         self.right_track_publisher = self.create_publisher(Track, 'right_track', 10)
 
+        # times_modified publisher
+        self.times_modified_publisher = self.create_publisher(Float32MultiArray, 'times_modified', 10)
+
         # Existing cone map
-        self.left_track = None
-        self.right_track = None
+        self.left_track = Track()
+        self.left_track.cones = []
+        self.right_track = Track()
+        self.right_track.cones = []
 
         # KDTrees for searching
-        self.left_tree = None
-        self.right_tree = None
+        self.left_tree = kdtree.create(None, dimensions=2)
+        self.right_tree = kdtree.create(None, dimensions=2)
+
+        self.times_updated_counter = 0
 
 ################################################################################ (parameters to tune)
 
@@ -66,6 +76,15 @@ def __init__(self):
         # Cone match radius
         self.match_radius = 1.0
 
+        # Counter to remove points
+        self.remove_point_counter = 200
+
+        # value for the times_modified
+        self.times_modified_limit = 100
+
+        # Modify rate
+        self.modify_rate = 1.08
+
 ################################################################################ (parameters to tune)
 
     def cones_callback(self, msg: Detections) -> None:
@@ -78,15 +97,17 @@ def cones_callback(self, msg: Detections) -> None:
         # Update the existing cone map with new measurements
         self.update_existing_cone_map(msg)
 
-        # Publishes left-track cones to the /left_track topic and right-track cones to the /right_track topic
-        # print("Left track length: ", len(self.left_track.cones))
-        # print("Right track length: ", len(self.right_track.cones))                                                                                                                                                                          
+        self.times_updated_counter += 1
+        if self.times_updated_counter > self.remove_point_counter:
+            self.times_updated_counter = 0
+            self.remove_cones()
+
         self.left_track_publisher.publish(self.left_track)
         self.right_track_publisher.publish(self.right_track)
 
 
     def update_existing_cone_map(self, msg: Detections) -> None:
-        """This function updates the existing cone map using new cone measurementsf
+        """This function updates the existing cone map using new cone measurements
 
         Args:
             msg (Detections): Contains the car's position and lists of detected cones
@@ -96,9 +117,11 @@ def update_existing_cone_map(self, msg: Detections) -> None:
         global_blue_cone_positions = self.cones_local_to_global(msg.blue, car_position)
         global_yellow_cone_positions = self.cones_local_to_global(msg.yellow, car_position)
 
-        # Update the left track and the right track
-        self.update_left_track(global_blue_cone_positions)
-        self.update_right_track(global_yellow_cone_positions)
+        # Update the left track and the right track if cones got detected
+        if len(global_blue_cone_positions) != 0:
+            self.update_left_track(global_blue_cone_positions)
+        if len(global_yellow_cone_positions) != 0:
+            self.update_right_track(global_yellow_cone_positions)
 
 
     def update_left_track(self, points: list) -> None:
@@ -107,33 +130,30 @@ def update_left_track(self, points: list) -> None:
         Args:
             points (list): A list of blue cones' global (x, y) coordinates
         """
-        if self.left_track == None:
+        if len(self.left_track.cones) == 0:
             # If the left track is empty, pack all cone measurements and update the left track
-            self.left_track = self.produce_track_message(points)
-            # self.left_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)])
-            self.left_tree = kdtree.create(
-                 [Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)], dimensions=2)  # For 2D points
+            self.add_cones_to_left_track(points)
+            self.left_tree = kdtree.create([Item(coord[0], coord[1], [self.left_track.cones[index], self.default_error_in_estimate, 1]) for index, coord in enumerate(points)]) # !!!!!!
         else:
             # If the left track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track
             for coord in points:
                 point, distance = self.left_tree.search_nn(coord)
-                # print(self.left_tree.search_nn(coord))
-                # kdtree.visualize(self.left_tree)
                 # If the newly measured cone is in the match radius, this cone already exists in the left track, update its coordinates
                 if distance <= self.match_radius:
-                    index = point.data.data[0]
-                    cone = self.left_track.cones[index]
+                    cone = point.data.data[0]
                     error_in_estimate = point.data.data[1]
+                    times_modified = point.data.data[2] * self.modify_rate
                     new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate)
                     point.data.coords = (new_estimate_x, new_estimate_y)
-                    point.data.data = (index, new_error_in_estimate)
+                    point.data.data[1] = new_error_in_estimate
+                    point.data.data[2] = times_modified
+                # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map
                 else:
-                    # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map
                     cone = Point()
                     cone.x = coord[0]
                     cone.y = coord[1]
                     self.left_track.cones.append(cone)
-                    self.left_tree.add(Item(coord[0], coord[1], (len(self.left_track.cones) - 1, self.default_error_in_estimate)))
+                    self.left_tree.add(Item(coord[0], coord[1], [cone, self.default_error_in_estimate, 1]))
 
     
 
@@ -141,34 +161,32 @@ def update_right_track(self, points: list) -> None:
         """This function updates the right track of the cone map
 
         Args:
-            points (list): A list of yellow cones' (x, y) coordinates
+            points (list): A list of blue cones' global (x, y) coordinates
         """
-        if self.right_track == None:
+        if len(self.right_track.cones) == 0:
             # If the right track is empty, pack all cone measurements and update the right track
-            self.right_track = self.produce_track_message(points)
-            self.right_tree = kdtree.create([Item(coord[0], coord[1], (index, self.default_error_in_estimate)) for index, coord in enumerate(points)], dimensions=2)
+            self.add_cones_to_right_track(points)
+            self.right_tree = kdtree.create([Item(coord[0], coord[1], [self.right_track.cones[index], self.default_error_in_estimate, 1]) for index, coord in enumerate(points)]) # !!!!!!
         else:
             # If the right track is not empty, find the closest cone for each newly measured cone and update its coordinates or add it to the track
             for coord in points:
                 point, distance = self.right_tree.search_nn(coord)
-                # print(self.right_tree.search_nn(coord))
                 # If the newly measured cone is in the match radius, this cone already exists in the right track, update its coordinates
-                # check if distance is nan
-                # kdtree.visualize(self.right_tree)
                 if distance <= self.match_radius:
-                    index = point.data.data[0]
-                    cone = self.right_track.cones[index]
+                    cone = point.data.data[0]
                     error_in_estimate = point.data.data[1]
+                    times_modified = point.data.data[2] * self.modify_rate
                     new_estimate_x, new_estimate_y, new_error_in_estimate = self.kalman_filtering(cone, coord, error_in_estimate)
                     point.data.coords = (new_estimate_x, new_estimate_y)
-                    point.data.data = (index, new_error_in_estimate)
+                    point.data.data[1] = new_error_in_estimate
+                    point.data.data[2] = times_modified
+                # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map
                 else:
-                    # If the newly measured cone is not in the match radius, this cone does not exist in the existing cone map, add it to the existing cone map
                     cone = Point()
                     cone.x = coord[0]
                     cone.y = coord[1]
                     self.right_track.cones.append(cone)
-                    self.right_tree.add(Item(coord[0], coord[1], (len(self.right_track.cones) - 1, self.default_error_in_estimate)))
+                    self.right_tree.add(Item(coord[0], coord[1], [cone, self.default_error_in_estimate, 1]))
 
     
     def kalman_filtering(self, cone: Point, coord: tuple, error_in_estimate: float) -> tuple:
@@ -303,23 +321,50 @@ def local_to_global(self, position_vector: np.array, rotation_matrix: np.array,
         return list_of_cones_output
     
 
-    def produce_track_message(self, points: list) -> Track:
-        """This function packs all cones' (x, y) coordinates into a cone map message
+    def add_cones_to_left_track(self, points: list) -> None:
+        """This function packs all cones' (x, y) coordinates and add them to the left track
 
         Args:
             points (list): A list of cones' (x, y) coordinates
-
-        Returns:
-            output_track (Track): Track message
         """
-        output_track = Track()
         for p in points:
             point = Point()
             point.x = p[0]
             point.y = p[1]
-            output_track.cones.append(point)
-        return output_track
-
+            self.left_track.cones.append(point)
+
+    def add_cones_to_right_track(self, points: list) -> None:
+            """This function packs all cones' (x, y) coordinates and add them to the right track
+
+            Args:
+                points (list): A list of cones' (x, y) coordinates
+            """
+            for p in points:
+                point = Point()
+                point.x = p[0]
+                point.y = p[1]
+                self.right_track.cones.append(point)
+
+    def remove_cones(self):
+        if len(self.left_track.cones) == 0 or len(self.right_track.cones) == 0:
+            return
+        
+        times_modified_list = Float32MultiArray()
+        
+        for point in kdtree.level_order(self.left_tree):
+            if point.data.data[2] < self.times_modified_limit:
+                times_modified_list.data.append(point.data.data[2])
+                self.left_track.cones.remove(point.data.data[0])
+                self.left_tree = self.left_tree.remove(point.data)
+                
+        for point in kdtree.level_order(self.right_tree):
+            if point.data.data[2] < self.times_modified_limit:
+                times_modified_list.data.append(point.data.data[2])
+                self.right_track.cones.remove(point.data.data[0])
+                self.right_tree = self.right_tree.remove(point.data)
+        
+        self.times_modified_publisher.publish(times_modified_list)
+        
 
 def main(args=None):
     rclpy.init(args=args)
diff --git a/src/planning/path_planning/path_planning/fasttube_planner.py b/src/planning/path_planning/path_planning/fasttube_planner.py
new file mode 100644
index 00000000..53852b98
--- /dev/null
+++ b/src/planning/path_planning/path_planning/fasttube_planner.py
@@ -0,0 +1,166 @@
+import rclpy
+from rclpy.node import Node
+
+from moa_msgs.msg import Track
+from geometry_msgs.msg import Pose, PoseArray
+
+import numpy as np
+import matplotlib.pyplot as plt
+from fsd_path_planning import PathPlanner, MissionTypes, ConeTypes
+import math
+
+
+class centerline_planner(Node):
+    def __init__(self):
+        super().__init__("centerline_planner")
+
+        # parameters
+        self._plot = True
+        self.look_forward = 4
+        self.left_cones = []
+        self.right_cones = []
+        self.path_planner = PathPlanner(MissionTypes.trackdrive)
+
+        # subscribers
+        self.subscription_left_cone_map = self.create_subscription(Track, 'left_track',  self.left_cone_map_callback, 10)
+        self.subscription_right_cone_map = self.create_subscription(Track, 'right_track',  self.right_cone_map_callback, 10)
+        self.create_subscription(Pose, "car_position", self.set_car_position, 10)   # car pose
+
+        # publishers
+        self.centerline_publisher = self.create_publisher(PoseArray, "moa/selected_trajectory", 10)
+    
+    def set_car_position(self, msg:Pose) -> None: 
+        self.car_pose = msg
+        self.loop()
+
+    def left_cone_map_callback(self, msg:Track) -> None:
+        self.left_cones = msg.cones
+
+    def right_cone_map_callback(self, msg:Track) -> None:
+        self.right_cones = msg.cones
+    
+    def loop(self) -> None:
+        self.get_logger().info(f"car pose recieved = {hasattr(self,'car_pose')}")   
+        
+        if not hasattr(self,'car_pose'):
+            return 
+        
+        lb, rb = self.get_boundaries()   # get boundaries (list of [x,y] points)
+
+        if not len(lb) > 0 or not len(rb) > 0:
+            return
+
+        lblocal, _, _= self.get_next_points(lb,self.look_forward)  # get local points (list of [x,y] points) close to car
+        rblocal, car_position, car_orientation = self.get_next_points(rb,self.look_forward)
+
+        if not len(lblocal) > 0 or not len(rblocal) > 0:
+            return
+        
+        global_cones = self.get_global_cones(lblocal, rblocal)
+        car_direction = self.get_car_direction(car_orientation)
+        
+        path = self.path_planner.calculate_path_in_global_frame(global_cones, car_position, car_direction) # compute centerline
+        
+        centerline = path[:, 1:3]
+
+        if not len(centerline) > 0:
+            return
+
+        # publish
+        msg = self.get_posearray_msg(centerline)
+        self.centerline_publisher.publish(msg)
+
+        self.plot(lb,rb,centerline,self._plot) # plot
+        
+        
+    def get_global_cones(self, lb, rb):
+        cones_by_type = [np.zeros((0, 2)) for _ in range(5)]
+        cones_by_type[ConeTypes.LEFT] = lb
+        cones_by_type[ConeTypes.RIGHT] = rb
+        return cones_by_type
+        
+        
+    def get_car_direction(self, car_orientation):
+        return np.array([-math.sin(car_orientation), math.cos(car_orientation)])
+        
+        
+    # MAIN FUNCTIONS
+    def get_boundaries(self):
+        """Retrieves the left and right boundary from msg and returns as a [x,y] list"""
+        left_cones = np.array([[P.x, P.y] for P in self.left_cones])
+        right_cones = np.array([[P.x, P.y] for P in self.right_cones])
+
+        return left_cones, right_cones
+
+    def get_next_points(self,line,look_forward):
+        """Retrieves the points closest to the car
+        *ASSUMES THE points ARE SORTED/ORDERED
+        """
+        car_point = np.array([self.car_pose.position.x,self.car_pose.position.y])
+        car_orientation = self.car_pose.orientation.w
+        points = np.array(line)
+        
+        min_indx = np.argmin(np.linalg.norm(car_point-points, axis=1))
+        # min_indx += self.is_behind_car(points[min_indx], car_point, car_orientation)
+        points = self.get_local_points(min_indx,points,look_forward)
+
+        return points, car_point, car_orientation
+    
+    
+    def get_posearray_msg(self, line):
+        pose_array = PoseArray()
+
+        for P in line:
+            pose = Pose()
+            pose.position.x = P[0]  # assing x,y values to position of pose
+            pose.position.y = P[1]
+            pose_array.poses.append(pose)
+        
+        return pose_array
+    
+    
+    def plot(self,lb,rb,centerline,to_plot=False):
+        """Plots the boundary and centerline points"""
+        if to_plot:
+            # x, y points
+            lb = np.array(lb)
+            rb = np.array(rb)
+            centerline = np.array(centerline)
+
+            lbx, lby = lb[:,0], lb[:,1]
+            rbx, rby = rb[:,0], rb[:,1]
+            centx, centy = centerline[:,0], centerline[:,1]
+            car_x, car_y = [self.car_pose.position.x, self.car_pose.position.y]
+
+            # plot
+            plt.ion()
+            plt.clf()
+
+            plt.plot(lbx,lby,'*b',label='left')
+            plt.plot(rbx,rby,'*y',label='right')
+            plt.plot(centx,centy,'-r',label='centerline')
+            plt.plot(car_x,car_y,'*k',label='car position')
+
+            plt.pause(0.1)
+            plt.legend()
+            plt.show()
+    
+    
+    def get_local_points(self,min_indx,points,look_forward):
+        remaining_points = len(points) - (min_indx+1) # number of points forwards the car has detected
+        if remaining_points < look_forward: look_forward = remaining_points+1
+        points = points[min_indx:min_indx+look_forward] # local points based on closest distance to car
+
+        return points
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = centerline_planner()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/src/planning/path_planning/setup.py b/src/planning/path_planning/setup.py
index 52cedbb5..f0527522 100644
--- a/src/planning/path_planning/setup.py
+++ b/src/planning/path_planning/setup.py
@@ -27,6 +27,7 @@
             'centerline_planner = path_planning.centerline_planner:main',
             'trajectory_optimisation = path_planning.trajectory_optimisation_CS:main',
             'center_line = path_planning.simple_centerline_planner:main',
+            'fasttube = path_planning.fasttube_planner:main'
         ],
     },
 )