diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt
index 2cd8e5e..d6014bf 100644
--- a/frtos/CMakeLists.txt
+++ b/frtos/CMakeLists.txt
@@ -3,6 +3,7 @@ include(FreeRTOS_Kernel_import.cmake)
 add_subdirectory(motor)
 add_subdirectory(line_sensor)
 add_subdirectory(car)
+add_subdirectory(ultrasonic_sensor)
 
 add_executable(rtos_car rtos_car.c)
 
@@ -17,6 +18,7 @@ target_include_directories(rtos_car PRIVATE
         ${CMAKE_CURRENT_LIST_DIR}/config
         ${CMAKE_CURRENT_LIST_DIR}/motor
         ${CMAKE_CURRENT_LIST_DIR}/line_sensor
+        ${CMAKE_CURRENT_LIST_DIR}/ultrasonic_sensor
 )
 target_link_libraries(rtos_car
         pico_cyw43_arch_lwip_sys_freertos
diff --git a/frtos/config/ultrasonic_sensor_config.h b/frtos/config/ultrasonic_sensor_config.h
new file mode 100644
index 0000000..a271db0
--- /dev/null
+++ b/frtos/config/ultrasonic_sensor_config.h
@@ -0,0 +1,11 @@
+#ifndef ULTRASONIC_CONFIG_H
+#define ULTRASONIC_CONFIG_H
+
+/* ADC Configuration */
+
+#define TRIG_PIN                                ( 0 )
+#define ECHO_PIN                                ( 1 )
+
+#define ULTRASONIC_SENSOR_READ_DELAY            ( 500 )
+
+#endif //ULTRASONIC_CONFIG_H
diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c
index f5f3549..8084001 100644
--- a/frtos/rtos_car.c
+++ b/frtos/rtos_car.c
@@ -17,6 +17,8 @@
 
 #include "line_sensor.h"
 
+#include "ultrasonic_sensor.h"
+
 #define READ_LEFT_WHEEL_SPEED_PRIO      (tskIDLE_PRIORITY + 1UL)
 #define READ_RIGHT_WHEEL_SPEED_PRIO     (tskIDLE_PRIORITY + 1UL)
 
@@ -25,6 +27,8 @@
 
 #define DIRECTION_TASK_PRIORITY         (tskIDLE_PRIORITY + 3UL)
 
+#define DISTANCE_TASK_PRIORITY          (tskIDLE_PRIORITY + 4UL)
+
 /* Common Car State Structure (TODO: TBC)*/
 //static car_state_t g_car_state;
 
@@ -68,6 +72,14 @@ launch()
                 DIRECTION_TASK_PRIORITY,
                 &h_monitor_direction_task);
 
+    TaskHandle_t h_monitor_distance_task;
+    xTaskCreate(distance_task, 
+                "Monitor Distance Task", 
+                configMINIMAL_STACK_SIZE, 
+                NULL, 
+                DISTANCE_TASK_PRIORITY, 
+                &h_monitor_distance_task);
+
     vTaskStartScheduler();
 }
 
diff --git a/frtos/ultrasonic_sensor/CMakeLists.txt b/frtos/ultrasonic_sensor/CMakeLists.txt
new file mode 100644
index 0000000..1a9bf1b
--- /dev/null
+++ b/frtos/ultrasonic_sensor/CMakeLists.txt
@@ -0,0 +1,18 @@
+add_executable(
+        ultrasonic_sensor
+        ultrasonic_sensor.c
+        )
+
+target_link_libraries(
+        ultrasonic_sensor
+        hardware_adc
+        pico_stdlib
+        FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
+        )
+
+target_include_directories(ultrasonic_sensor 
+        PRIVATE ../config
+        )
+
+pico_enable_stdio_usb(ultrasonic_sensor 1)
+pico_add_extra_outputs(ultrasonic_sensor)
\ No newline at end of file
diff --git a/frtos/ultrasonic_sensor/FreeRTOSConfig.h b/frtos/ultrasonic_sensor/FreeRTOSConfig.h
new file mode 100644
index 0000000..f715e60
--- /dev/null
+++ b/frtos/ultrasonic_sensor/FreeRTOSConfig.h
@@ -0,0 +1,143 @@
+/*
+ * FreeRTOS V202111.00
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+#ifndef FREERTOS_CONFIG_H
+#define FREERTOS_CONFIG_H
+
+/*-----------------------------------------------------------
+ * Application specific definitions.
+ *
+ * These definitions should be adjusted for your particular hardware and
+ * application requirements.
+ *
+ * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
+ * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
+ *
+ * See http://www.freertos.org/a00110.html
+ *----------------------------------------------------------*/
+
+/* Scheduler Related */
+#define configUSE_PREEMPTION                    1
+#define configUSE_TICKLESS_IDLE                 0
+#define configUSE_IDLE_HOOK                     0
+#define configUSE_TICK_HOOK                     0
+#define configTICK_RATE_HZ                      ( ( TickType_t ) 1000 )
+#define configMAX_PRIORITIES                    32
+#define configMINIMAL_STACK_SIZE                ( configSTACK_DEPTH_TYPE ) 256
+#define configUSE_16_BIT_TICKS                  0
+
+#define configIDLE_SHOULD_YIELD                 1
+
+/* Synchronization Related */
+#define configUSE_MUTEXES                       1
+#define configUSE_RECURSIVE_MUTEXES             1
+#define configUSE_APPLICATION_TASK_TAG          0
+#define configUSE_COUNTING_SEMAPHORES           1
+#define configQUEUE_REGISTRY_SIZE               8
+#define configUSE_QUEUE_SETS                    1
+#define configUSE_TIME_SLICING                  1
+#define configUSE_NEWLIB_REENTRANT              0
+// todo need this for lwip FreeRTOS sys_arch to compile
+#define configENABLE_BACKWARD_COMPATIBILITY     1
+#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5
+
+/* System */
+#define configSTACK_DEPTH_TYPE                  uint32_t
+#define configMESSAGE_BUFFER_LENGTH_TYPE        size_t
+
+/* Memory allocation related definitions. */
+#define configSUPPORT_STATIC_ALLOCATION         0
+#define configSUPPORT_DYNAMIC_ALLOCATION        1
+#define configTOTAL_HEAP_SIZE                   (128*1024)
+#define configAPPLICATION_ALLOCATED_HEAP        0
+
+/* Hook function related definitions. */
+#define configCHECK_FOR_STACK_OVERFLOW          0
+#define configUSE_MALLOC_FAILED_HOOK            0
+#define configUSE_DAEMON_TASK_STARTUP_HOOK      0
+
+/* Run time and task stats gathering related definitions. */
+#define configGENERATE_RUN_TIME_STATS           0
+#define configUSE_TRACE_FACILITY                1
+#define configUSE_STATS_FORMATTING_FUNCTIONS    0
+
+/* Co-routine related definitions. */
+#define configUSE_CO_ROUTINES                   0
+#define configMAX_CO_ROUTINE_PRIORITIES         1
+
+/* Software timer related definitions. */
+#define configUSE_TIMERS                        1
+#define configTIMER_TASK_PRIORITY               ( configMAX_PRIORITIES - 1 )
+#define configTIMER_QUEUE_LENGTH                10
+#define configTIMER_TASK_STACK_DEPTH            1024
+
+/* Interrupt nesting behaviour configuration. */
+/*
+#define configKERNEL_INTERRUPT_PRIORITY         [dependent of processor]
+#define configMAX_SYSCALL_INTERRUPT_PRIORITY    [dependent on processor and application]
+#define configMAX_API_CALL_INTERRUPT_PRIORITY   [dependent on processor and application]
+*/
+
+#if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS
+/* SMP port only */
+#define configNUM_CORES                         1
+#define configTICK_CORE                         0
+#define configRUN_MULTIPLE_PRIORITIES           1
+#define configUSE_CORE_AFFINITY                 0
+#endif
+
+/* RP2040 specific */
+#define configSUPPORT_PICO_SYNC_INTEROP         1
+#define configSUPPORT_PICO_TIME_INTEROP         1
+
+#include <assert.h>
+/* Define to trap errors during development. */
+#define configASSERT(x)                         assert(x)
+
+/* Set the following definitions to 1 to include the API function, or zero
+to exclude the API function. */
+#define INCLUDE_vTaskPrioritySet                1
+#define INCLUDE_uxTaskPriorityGet               1
+#define INCLUDE_vTaskDelete                     1
+#define INCLUDE_vTaskSuspend                    1
+#define INCLUDE_vTaskDelayUntil                 1
+#define INCLUDE_vTaskDelay                      1
+#define INCLUDE_xTaskGetSchedulerState          1
+#define INCLUDE_xTaskGetCurrentTaskHandle       1
+#define INCLUDE_uxTaskGetStackHighWaterMark     1
+#define INCLUDE_xTaskGetIdleTaskHandle          1
+#define INCLUDE_eTaskGetState                   1
+#define INCLUDE_xTimerPendFunctionCall          1
+#define INCLUDE_xTaskAbortDelay                 1
+#define INCLUDE_xTaskGetHandle                  1
+#define INCLUDE_xTaskResumeFromISR              1
+#define INCLUDE_xQueueGetMutexHolder            1
+
+/* A header file that defines trace macro can be included here. */
+
+#endif /* FREERTOS_CONFIG_H */
+
diff --git a/frtos/ultrasonic_sensor/ultrasonic_init.h b/frtos/ultrasonic_sensor/ultrasonic_init.h
new file mode 100644
index 0000000..d2db86d
--- /dev/null
+++ b/frtos/ultrasonic_sensor/ultrasonic_init.h
@@ -0,0 +1,26 @@
+/**
+ * @file ultrasonic_init.h
+ * @brief define the constants and initialize the ultrasonic sensor
+ * @author Poon Xiang Yuan
+ */
+
+#ifndef ULTRASONIC_INIT_H
+#define ULTRASONIC_INIT_H
+
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "ultrasonic_sensor_config.h"
+
+void
+init_ultrasonic(void)
+{
+    // Set up the echo pin
+    gpio_init(ECHO_PIN);
+    gpio_set_dir(ECHO_PIN, GPIO_IN);
+
+    // Set up the trigger pin
+    gpio_init(TRIG_PIN);
+    gpio_set_dir(TRIG_PIN, GPIO_OUT);
+}
+
+#endif /* ULTRASONIC_INIT_H */
\ No newline at end of file
diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.c b/frtos/ultrasonic_sensor/ultrasonic_sensor.c
new file mode 100644
index 0000000..1c89e06
--- /dev/null
+++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.c
@@ -0,0 +1,113 @@
+#include "pico/stdlib.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include <stdio.h>
+
+#define TRIG_PIN 0
+#define ECHO_PIN 1
+
+int16_t counter = 0;
+
+void
+init_ultrasonic(void)
+{
+    // Set up the echo pin
+    gpio_init(ECHO_PIN);
+    gpio_set_dir(ECHO_PIN, GPIO_IN);
+
+    // Set up the trigger pin
+    gpio_init(TRIG_PIN);
+    gpio_set_dir(TRIG_PIN, GPIO_OUT);
+}
+
+float
+KalmanFilter(float U)
+{
+    static float R = 10;    // noise convariance can be 10, higher better smooth
+    static float H = 1;     // Measurement Map scalar
+    static float Q = 10;    // initial estimated convariance
+    static float P = 0;     // initial error covariance
+    static float U_hat = 0; // initial estimated state
+    static float K     = 0; // initial Kalman gain
+
+    // Predict
+    //
+    K     = P * H / (H * P * H + R);     // Update Kalman gain
+    U_hat = U_hat + K * (U - H * U_hat); // Update estimated state
+
+    // Update error covariance
+    //
+    P = (1 - K * H) * P + Q;
+
+    return U_hat;
+}
+
+void
+distance_task(__unused void *params)
+{
+    while (true)
+    {
+        vTaskDelay(1000);
+
+        // Trigger the ultrasonic sensor
+        gpio_put(TRIG_PIN, 1);
+        sleep_us(10); // Keep the trigger on for 10 microseconds
+        gpio_put(TRIG_PIN, 0);
+
+        // Wait for the echo pulse to start
+        while (gpio_get(ECHO_PIN) == 0)
+            tight_loop_contents();
+
+        // Measure the pulse width (time taken for the echo to return)
+        uint32_t start_time = time_us_32();
+        while (gpio_get(ECHO_PIN) == 1)
+            tight_loop_contents();
+        uint32_t end_time = time_us_32();
+
+        // Calculate the distance (in centimeters)
+        uint32_t pulse_duration = end_time - start_time;
+        float distance = pulse_duration * 0.017; // Speed of sound at ~343 m/s
+
+        // printf("Distance: %.2f cm\n", distance);
+        // printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
+        printf("%d,%.2f,%.2f\n", counter++, distance, KalmanFilter(distance));
+
+        // If gonna bang wall
+        //
+        if (distance < 5)
+        {
+            printf("Collision Imminent!\n");
+            // Proc stop
+            //
+        }
+    }
+}
+
+void
+vLaunch(void)
+{
+    TaskHandle_t disttask;
+    xTaskCreate(distance_task,
+                "TestDistThread",
+                configMINIMAL_STACK_SIZE,
+                NULL,
+                1,
+                &disttask);
+
+    // Start the tasks and timer running.
+    //
+    vTaskStartScheduler();
+}
+
+int
+main(void)
+{
+    stdio_init_all();
+    init_ultrasonic();
+    sleep_ms(3000);
+    vLaunch();
+
+    return 0;
+}
+/*** end of file ***/
+//
\ No newline at end of file
diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.h b/frtos/ultrasonic_sensor/ultrasonic_sensor.h
new file mode 100644
index 0000000..b61fc76
--- /dev/null
+++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h
@@ -0,0 +1,49 @@
+/**
+ * @file ultrasonic_sensor.h
+ * @brief Monitor the distance between the car and the wall
+ * @author Poon Xiang Yuan
+ */
+
+#ifndef ULTRASONIC_SENSOR_H
+#define ULTRASONIC_SENSOR_H
+
+#include "ultrasonic_init.h"
+
+void distance_task(__unused void *params)
+{
+    while (true)
+    {
+        vTaskDelay(500);
+
+        // Trigger the ultrasonic sensor
+        gpio_put(TRIG_PIN, 1);
+        sleep_us(10); // Keep the trigger on for 10 microseconds
+        gpio_put(TRIG_PIN, 0);
+
+        // Wait for the echo pulse to start
+        while (gpio_get(ECHO_PIN) == 0)
+            tight_loop_contents();
+
+        // Measure the pulse width (time taken for the echo to return)
+        uint32_t start_time = time_us_32();
+        while (gpio_get(ECHO_PIN) == 1)
+            tight_loop_contents();
+        uint32_t end_time = time_us_32();
+
+        // Calculate the distance (in centimeters)
+        uint32_t pulse_duration = end_time - start_time;
+        float distance = pulse_duration * 0.017; // Speed of sound at ~343 m/s
+
+        printf("Distance: %.2f cm\n", distance);
+
+        // If gonna bang wall
+        //
+        if (distance < 5)
+        {
+            printf("Collision Imminent!\n");
+            // Proc stop
+            //
+        }
+    }
+}
+#endif /* ULTRASONIC_SENSOR_H */