Micro-ROS ESP32 Ultrasonic Distance Sensor with Kalman Filter

A professional-grade distance measurement system using ESP32, HC-SR04 ultrasonic sensor, and advanced Kalman filtering, seamlessly integrated with ROS2 through micro-ROS.

🚀 Features

Real-time distance measurement using HC-SR04 ultrasonic sensor (20Hz)
Advanced Kalman filtering for noise reduction and signal stability
Voltage divider protection for safe 3.3V operation on ESP32
Dual-topic publishing: raw and filtered sensor data
Professional PlatformIO integration with micro-ROS
Comprehensive error handling and diagnostic output
RViz2 visualization support with proper frame configuration

Hardware Requirements

ESP32 Development Board
HC-SR04 Ultrasonic Distance Sensor
2x Resistors: 1kΩ (R1) and 2kΩ (R2) for voltage divider
Jumper wires
Breadboard

Circuit Connections & Voltage Divider

Why Voltage Divider is Needed

The HC-SR04 operates at 5V logic levels, but the ESP32 GPIO pins are designed for 3.3V. The ECHO pin outputs 5V when HIGH, which could potentially damage the ESP32. A voltage divider reduces this to a safe 3.3V level.

Voltage Divider Calculation

Using resistors R1 = 1kΩ and R2 = 2kΩ:

Vout = Vin × (R2 / (R1 + R2))
Vout = 5V × (2kΩ / (1kΩ + 2kΩ))
Vout = 5V × (2/3) = 3.33V

This safely converts the 5V ECHO signal to 3.33V for the ESP32.

Circuit Schematic

HC-SR04 Ultrasonic Sensor        ESP32
    ┌─────────────┐              ┌─────────────┐
    │             │              │             │
    │        VCC──┼──────────────┼──5V (VIN)   │
    │        GND──┼──────────────┼──GND        │
    │       TRIG──┼──────────────┼──GPIO2      │
    │             │              │             │
    │       ECHO──┼────┐         │             │
    └─────────────┘    │         │             │
                       │         │             │
                    R1 │ 1kΩ     │             │
                       │         │             │
                       ├─────────┼──GPIO4      │
                       │         │             │
                    R2 │ 2kΩ     │             │
                       │         │             │
                      GND────────┼──GND        │
                                 └─────────────┘

Legend:
- R1: 1kΩ resistor (Brown-Black-Red)
- R2: 2kΩ resistor (Red-Black-Red)
- Voltage divider output = 3.33V (safe for ESP32)

Pin Connections

HC-SR04	ESP32 Pin	Connection Type	Notes
VCC	5V (VIN)	Direct	Power supply (5V required)
GND	GND	Direct	Ground connection
TRIG	GPIO2	Direct	Trigger pin (3.3V compatible)
ECHO	GPIO4	Via Voltage Divider	Through R1(1kΩ) + R2(2kΩ)

Software Setup

Prerequisites

PlatformIO (VS Code extension or CLI)
ROS2 (Humble/Iron/Jazzy recommended)
micro-ROS agent

PlatformIO Configuration

Create a platformio.ini file in your project root:

[env:upesy_wroom]
platform = espressif32
board = upesy_wroom
framework = arduino
board_microros_distro = humble
lib_deps =
    https://github.com/micro-ROS/micro_ros_platformio

Required Libraries (Auto-installed via PlatformIO)

#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/range.h>

Installation Steps

Install PlatformIO:
- VS Code: Install PlatformIO IDE extension
- CLI: pip install platformio
Create new project:
```
pio project init --board upesy_wroom
```

Build and upload:

pio run --target upload
pio device monitor

Set up micro-ROS agent:

# Install micro-ROS agent (one-time setup)
sudo apt install ros-humble-micro-ros-agent

# Run the agent (replace /dev/ttyUSB0 with your ESP32 port)
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -b 115200

ROS2 Integration

Published Topics

The system publishes distance data on the following ROS2 topics:

Filtered Topic: /ultrasonic_sensor/range
- Message Type: sensor_msgs/msg/Range
- Frame ID: ultrasonic_filtered
- Frequency: ~20 Hz
Raw Topic: /ultrasonic_sensor/range_raw
- Message Type: sensor_msgs/msg/Range
- Frame ID: ultrasonic_raw
- Frequency: ~20 Hz

ROS2 Commands

View Available Topics

ros2 topic list

Echo Ultrasonic Data (Filtered)

ros2 topic echo /ultrasonic_sensor/range

Echo Raw Ultrasonic Data

ros2 topic echo /ultrasonic_sensor/range_raw

Compare Data Rates

ros2 topic hz /ultrasonic_sensor/range
ros2 topic hz /ultrasonic_sensor/range_raw

Show Topic Information

ros2 topic info /ultrasonic_sensor/range
ros2 interface show sensor_msgs/msg/Range

Range Message Structure

# Single range reading from an active ranger that emits energy and reports
# one range reading that is valid along an arc at the distance measured.
# This message is  not appropriate for laser scanners. See the LaserScan
# message if you are working with a laser scanner.
#
# This message also can represent a fixed-distance (binary) ranger.  This
# sensor will have min_range===max_range===distance of detection.
# These sensors follow REP 117 and will output -Inf if the object is detected
# and +Inf if the object is outside of the detection range.

std_msgs/Header header # timestamp in the header is the time the ranger
                             # returned the distance reading

# Radiation type enums
# If you want a value added to this list, send an email to the ros-users list
uint8 ULTRASOUND=0
uint8 INFRARED=1

uint8 radiation_type    # the type of radiation used by the sensor
                        # (sound, IR, etc) [enum]

float32 field_of_view   # the size of the arc that the distance reading is
                        # valid for [rad]
                        # the object causing the range reading may have
                        # been anywhere within -field_of_view/2 and
                        # field_of_view/2 at the measured range.
                        # 0 angle corresponds to the x-axis of the sensor.

float32 min_range       # minimum range value [m]
float32 max_range       # maximum range value [m]
                        # Fixed distance rangers require min_range==max_range

float32 range           # range data [m]
                        # (Note: values < range_min or > range_max should be discarded)
                        # Fixed distance rangers only output -Inf or +Inf.
                        # -Inf represents a detection within fixed distance.
                        # (Detection too close to the sensor to quantify)
                        # +Inf represents no detection within the fixed distance.
                        # (Object out of range)

Kalman Filter Implementation

The system implements a 1D Kalman filter to smooth ultrasonic distance measurements and reduce sensor noise.

Filter Algorithm

// Prediction Step
P_predicted = P_previous + Q

// Update Step  
K = P_predicted / (P_predicted + R)
x_estimated = x_previous + K * (measurement - x_previous)
P_updated = (1 - K) * P_predicted

Default Parameters

struct KalmanFilter {
  float Q = 0.01;    // Process noise - how much distance expected to change
  float R = 0.1;     // Measurement noise - sensor uncertainty
  float P = 1.0;     // Initial error covariance
};

Parameter Tuning Guide

Parameter	Low Value (0.005-0.01)	Medium Value (0.01-0.05)	High Value (0.05-0.2)
Q (Process)	Static environment	Normal indoor use	Moving robot/dynamic
R (Measurement)	Perfect mounting	Standard HC-SR04	Noisy environment

Performance Benefits

Noise Reduction: Filters out electrical and acoustic noise
Outlier Rejection: Smooths sudden spikes in readings
Stability: Provides consistent distance measurements
Adaptability: Automatically adjusts to measurement confidence

Visualization and Analysis

RViz2 Setup

Launch RViz2:
```
rviz2
```
Add Range displays for both topics:
- Filtered Data: Add → By topic → /ultrasonic_sensor/range → Range
- Raw Data: Add → By topic → /ultrasonic_sensor/range_raw → Range
Important: Change the Fixed Frame:
- In Global Options, change "Fixed Frame" from "map" to "ultrasonic_filtered"
- This matches the frame_id used in the sensor messages
Configure display colors:
- Set different colors for filtered vs raw data for easy comparison
- Filtered data should appear smoother and more stable

Data Analysis with PlotJuggler

Monitor and compare raw vs filtered sensor data:

# Install PlotJuggler (if not already installed)
sudo apt install ros-humble-plotjuggler-ros

# Launch PlotJuggler with ROS2 support
ros2 run plotjuggler plotjuggler

# In PlotJuggler:
# 1. Start ROS2 Topic Subscriber
# 2. Select both /ultrasonic_sensor/range and /ultrasonic_sensor/range_raw
# 3. Plot range values to visualize Kalman filter performance

Serial Monitor (PlatformIO)

Monitor debug output and sensor readings:

# Using PlatformIO CLI
pio device monitor

# Using PlatformIO IDE
# Click on "Monitor" button in PlatformIO toolbar

Expected serial output:

Ultrasonic sensor with Kalman filter initialized!
Publishing filtered data to: /ultrasonic_sensor/range
Publishing raw data to: /ultrasonic_sensor/range_raw
Kalman Filter Parameters:
  Process Noise (Q): 0.01
  Measurement Noise (R): 0.1
Raw: 0.156m, Filtered: 0.158m, Gain: 0.091
Raw: 0.159m, Filtered: 0.159m, Gain: 0.083

Troubleshooting

PlatformIO Issues

Build errors with micro-ROS:

# Clean and rebuild
pio run --target clean
pio run

# Check library installation
pio lib list

Upload fails:

# Check available ports
pio device list

# Specify port manually
pio run --target upload --upload-port /dev/ttyUSB0

Serial monitor not working:

# Set correct baud rate
pio device monitor --baud 115200

Hardware Issues

No distance readings:
- Verify 5V power supply to HC-SR04
- Check voltage divider output (should read 3.3V when ECHO is HIGH)
- Test connections with multimeter
- Ensure proper grounding
Erratic readings:
- Check for loose connections
- Verify voltage divider resistor values (1kΩ and 2kΩ)
- Move away from acoustic interference sources
- Check sensor mounting (should be stable)
ESP32 not responding:
- ESP32 may be damaged by 5V on GPIO pin
- Always use voltage divider on ECHO pin
- Check if built-in LED blinks (error pattern)

ROS2 Integration Issues

micro-ROS agent not connecting:

# Check serial permissions
sudo chmod 666 /dev/ttyUSB0

# Try different baud rates
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -b 115200

# Reset ESP32 and restart agent

No topic data:

# Verify agent connection
ros2 topic list | grep ultrasonic

# Check message frequency
ros2 topic hz /ultrasonic_sensor/range

# Monitor for errors
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -v6

RViz2 display issues:
- Ensure Fixed Frame matches sensor frame_id (ultrasonic_filtered)
- Check Range display configuration
- Verify topic names and message types
- Try resetting RViz2 configuration

Debugging Tools

# Monitor serial output
pio device monitor

# Check ROS2 topics and types
ros2 topic list -t
ros2 topic info /ultrasonic_sensor/range

# Test with command line
ros2 topic echo /ultrasonic_sensor/range --once

Development Workflow

# 1. Clone the repository
git clone git@github.com:Adem-Aoun/micro-ros-esp32-ultrasonic-kalman.git
cd micro-ros-esp32-ultrasonic-kalman

# 2. Build the project
pio run

# 3. Upload to ESP32
pio run --target upload

# 4. Monitor serial output
pio device monitor

# 5. In another terminal, start micro-ROS agent
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -b 115200

Configuration Notes

Important RViz2 Configuration

⚠️ Critical: Change the Fixed Frame in RViz2 from "map" to "ultrasonic_filtered" to properly visualize the sensor data.

Sensor Specifications (HC-SR04)

Parameter	Value	Unit
Operating Range	2 - 400	cm
Accuracy	±3	mm
Resolution	0.3	cm
Measuring Angle	15	degrees
Operating Frequency	40	kHz
Supply Voltage	5	V DC
Operating Current	15	mA
Standby Current	<2	mA

Performance Characteristics

Update Rate: 20 Hz (50ms intervals)
Timeout Protection: 50ms pulse timeout
Range Validation: Automatic out-of-range detection
Filter Latency: <1ms (real-time processing)
Memory Usage: <2KB RAM for Kalman filter state

License

MIT License - Feel free to use and modify for your projects.

Contributing

Fork the repository
Create a feature branch
Commit your changes
Push to the branch
Create a Pull Request

Name		Name	Last commit message	Last commit date
Latest commit History 5 Commits
include		include
lib		lib
src		src
test		test
.gitignore		.gitignore
LICENSE		LICENSE
README.md		README.md
platformio.ini		platformio.ini

License

Adem-Aoun/Arduino-micro-ROS-Ultrasonic-driver-

Folders and files

Latest commit

History

Repository files navigation

Micro-ROS ESP32 Ultrasonic Distance Sensor with Kalman Filter

🚀 Features

Hardware Requirements

Circuit Connections & Voltage Divider

Why Voltage Divider is Needed

Voltage Divider Calculation

Circuit Schematic

Pin Connections

Software Setup

Prerequisites

PlatformIO Configuration

Required Libraries (Auto-installed via PlatformIO)

Installation Steps

ROS2 Integration

Published Topics

ROS2 Commands

View Available Topics

Echo Ultrasonic Data (Filtered)

Echo Raw Ultrasonic Data

Compare Data Rates

Show Topic Information

Range Message Structure

Kalman Filter Implementation

Filter Algorithm

Default Parameters

Parameter Tuning Guide

Performance Benefits

Visualization and Analysis

RViz2 Setup

Data Analysis with PlotJuggler

Serial Monitor (PlatformIO)

Troubleshooting

PlatformIO Issues

Hardware Issues

ROS2 Integration Issues

Debugging Tools

Development Workflow

Configuration Notes

Important RViz2 Configuration

Sensor Specifications (HC-SR04)

Performance Characteristics

License

Contributing

About

Resources

License

Uh oh!

Stars

Watchers

Forks

Releases

Packages 0

Languages

Packages