An ESP32-S3 sonar rig that sweeps an ultrasonic sensor with a servo, filters the returns with an Extended Kalman Filter, classifies what it's looking at with an on-device k-NN model, and streams everything to a Python visualizer that draws a live occupancy grid.
(Robot moves through the room while logging range readings and building an occupancy grid from them in real time, which it then uses for basic collision aware path planning.)
(Runs raw sonar readings through an Extended Kalman Filter to get stable distance and velocity estimates. Noticeably less jitter than using the raw readings directly.)
(Classifier output includes confidence scores alongside the predicted label, so you can see how certain each prediction actually is instead of just trusting the top class.)
(Plots the feature space used for classification, built from signal strength, motion, and geometry. Shows how these features separate the different object classes.)
(Actuator sweeps through its range while collecting scan points, then DBSCAN clusters them into distinct groups that get treated as separate obstacles.)
From internal testing on a handful of test environments:
- Classification accuracy came out to about 98.2%, compared to roughly 70% with a simple distance-threshold approach.
- End-to-end telemetry-to-display latency stayed under 50ms at 60 FPS, a meaningful improvement over the un-optimized pipeline.
These numbers reflect one test setup rather than a rigorous benchmark suite, so treat them as a rough indicator rather than a guarantee for other environments.
- Dual-core FreeRTOS setup: one core handles sensor acquisition, the other handles motor control and telemetry
- Extended Kalman Filter (EKF) to smooth out multipath interference and sensor jitter
- A small on-device k-NN classifier for basic object categorization
- 100Hz sensor sampling driven by hardware interrupts
- Sinusoidal servo sweep for continuous scanning instead of jerky step motion
- JSON telemetry streamed over serial at 921600 baud
- Real-time occupancy grid, polar readings mapped to Cartesian with fading trails
- DBSCAN clustering to group returns into discrete objects
- Bounding boxes drawn around detected objects
- Velocity vectors for moving objects
- Live display of classification confidence
- ESP32-S3-DevKitC-1 or equivalent
- Dual-core Xtensa LX7
- PSRAM is optional but recommended
- Ultrasonic: HC-SR04 or JSN-SR04T, 2cm to 400cm range, trigger/echo interface
- Alternative: TF-Luna micro-LiDAR (I2C/UART)
- Standard PWM servo (SG90, MG996R, etc.), 0-180°, 50Hz PWM
- Alternative: stepper motor with driver
TRIGGER_PIN -> GPIO5
ECHO_PIN -> GPIO18
SERVO_PIN -> GPIO16 (PWM)
UART0 -> USB/Serial (921600 baud)
src/
├── main.cpp
├── extended_kalman_filter.cpp
└── object_classifier.cpp
include/
├── extended_kalman_filter.h
└── object_classifier.h
- Core 0 (APP_CPU): sensor acquisition and EKF filtering
- Core 1 (PRO_CPU): motor control and telemetry streaming
Ultrasonic Sensor -> Interrupt -> EKF -> Shared Memory -> Classifier -> JSON -> Serial
↓
Servo PWM
State vector:
x = [distance, velocity]^T
Prediction
x_pred = F * x_prev
P_pred = F * P_prev * F^T + Q
F is the state transition matrix for a constant-velocity model:
F = [1 dt]
[0 1]
Update
K = P_pred * H^T * (H * P_pred * H^T + R)^-1
x = x_pred + K * (z - h(x_pred))
P = (I - K * H) * P_pred
x = r * cos(θ)
y = r * sin(θ)
Where r is radial distance in meters, θ is angle in radians, and x, y are the resulting Cartesian coordinates.
The feature vector per detection:
- Distance (current measurement)
- Velocity (rate of change)
- Variance (measurement consistency)
- Amplitude (signal strength)
- Gradient (rate of change across multiple samples)
- Consistency (inverse of normalized variance)
Classification uses plain Euclidean distance in that feature space:
d = √(Σ(xi - yi)²)
- PlatformIO CLI
- Python 3.8+
- ESP32-S3 dev board
- USB cable
- Install PlatformIO if you haven't already:
pip install platformio- Install dependencies:
cd "Sonar Scanner"
pio lib install- Build:
pio run- Upload to the ESP32-S3:
pio run --target upload- Watch serial output:
pio device monitor- Install dependencies:
pip install -r requirements.txt- Run it:
python visualizer.py --port COM3 --baud 921600Change the port to match your system (COM3 on Windows, /dev/ttyUSB0 on Linux).
In src/main.cpp:
#define TRIGGER_PIN 5
#define ECHO_PIN 18
#define SERVO_PIN 16
#define SENSOR_SAMPLE_RATE_HZ 100
#define TELEMETRY_RATE_HZ 50
#define SWEEP_PERIOD_MS 2000
#define MAX_DISTANCE_M 4.0f In main.cpp:
ExtendedKalmanFilter ekf(0.1f, 0.3f);
// ^ ^
// | |
// Process noise Measurement noise- Process noise (Q): higher values let the filter adapt faster to real changes, at the cost of more jitter
- Measurement noise (R): higher values make the filter trust raw sensor readings less
In motorControlTask:
motorCommand.sweepAmplitude = 90.0f;
motorCommand.sweepFrequency = 0.5f; In visualizer.py:
self.grid_size = 400
self.scale = 100
self.decay_rate = 0.98 DBSCAN parameters:
eps = 0.3
min_samples = 3 The classifier sorts detections into five buckets:
- Wall/flat: consistent reflections, low variance
- Corner/edge: high variance, discontinuous returns
- Dynamic/moving: noticeable velocity, moderate variance
- Human/soft: low amplitude, absorbing material
- Unknown: not enough data to classify confidently
JSON over serial at 921600 baud:
{
"t": 1234567890,
"d": 1.23,
"v": 0.05,
"a": 45.0,
"c": 0.95,
"oc": 1,
"cc": 0.87
}- Sensor sampling: 100 Hz
- Telemetry rate: 50 Hz
- EKF update rate: 100 Hz
- Classification rate: 50 Hz
- Memory usage: roughly 50KB RAM
- CPU utilization: around 60% across both cores
- 30 to 60 FPS, depending on host CPU
- Under 50ms latency end to end
- 400x400 pixel grid
- 4 meter maximum range
- Electrical: use sensor-appropriate voltage levels, don't wire anything at 5V logic into a 3.3V-only pin
- Mechanical: mount the servo securely so it can't catch fingers or hair mid-sweep
- Eyes: don't point ultrasonic sensors at eyes (mostly a non-issue for ultrasonic, but a good habit if you swap in a LiDAR module)
- Heat: give the ESP32 some airflow, especially if it's running both cores hard for long periods
Contributions welcome. Some areas that could use work:
- Additional sensor support (LiDAR, ToF)
- A web-based visualization option
- Fancier ML models (small neural nets instead of k-NN)
- Multi-sensor fusion
- A basic SLAM implementation