AI-Enhanced Sonar & Spatial Occupancy Scanner

A high-performance, AI-enhanced sonar scanning system built on ESP32-S3 with dual-core FreeRTOS architecture, featuring Extended Kalman Filter noise reduction, on-device ML classification, and real-time Python visualization with computer vision.

Features

Firmware (ESP32-S3)

• Dual-Core FreeRTOS Architecture: Dedicated cores for sensor acquisition and motor control
• Extended Kalman Filter (EKF): Real-time noise filtering for multipath interference and sensor jitter
• Edge AI Classification: On-device k-NN classifier for object categorization
• High-Frequency Sampling: 100Hz sensor acquisition with hardware interrupts
• Sinusoidal Sweep Control: Smooth servo motor control for continuous scanning
• Structured Telemetry: JSON streaming at 921600 baud

Python Visualizer

• Real-time Occupancy Grid: Polar-to-Cartesian mapping with fading radar trails
• DBSCAN Clustering: Automatic object detection and grouping
• Bounding Box Visualization: Visual object identification
• Velocity Vector Tracking: Dynamic object movement analysis
• AI Confidence Display: Real-time classification confidence metrics

Hardware Requirements

ESP32-S3 Board

• ESP32-S3-DevKitC-1 or equivalent
• Dual-core Xtensa LX7 processor
• PSRAM support (optional but recommended)

Sensors

• Ultrasonic Sensor: HC-SR04 or JSN-SR04T
  • Range: 2cm to 400cm
  • Trigger/Echo interface
• Alternative: TF-Luna Micro-LiDAR (I2C/UART)

Motor

• Servo Motor: Standard PWM servo (SG90, MG996R, etc.)
  • Range: 0-180 degrees
  • PWM control at 50Hz
• Alternative: Stepper motor with driver

Pin Configuration

TRIGGER_PIN  -> GPIO5
ECHO_PIN     -> GPIO18
SERVO_PIN    -> GPIO16 (PWM)
UART0        -> USB/Serial (921600 baud)

Software Architecture

Firmware Structure

src/
├── main.cpp                      # Main application with FreeRTOS tasks
├── extended_kalman_filter.cpp    # EKF implementation
└── object_classifier.cpp         # k-NN ML classifier

include/
├── extended_kalman_filter.h      # EKF header
└── object_classifier.h           # Classifier header

Core Assignment

• Core 0 (APP_CPU): Sensor acquisition with EKF filtering
• Core 1 (PRO_CPU): Motor control and telemetry streaming

Data Flow

Ultrasonic Sensor -> Interrupt -> EKF -> Shared Memory -> Classifier -> JSON -> Serial
                                                    ↓
                                              Servo PWM

Mathematical Foundation

Extended Kalman Filter

The EKF models the system state as:

State vector: x = [distance, velocity]^T

Prediction Step:

x_pred = F * x_prev
P_pred = F * P_prev * F^T + Q

Where F is the state transition matrix for constant velocity model:

F = [1  dt]
    [0   1]

Update Step:

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

Polar to Cartesian Transformation

x = r * cos(θ)
y = r * sin(θ)

Where:

• r: radial distance (meters)
• θ: angle in radians
• x, y: Cartesian coordinates

k-NN Classification

Feature vector includes:

• Distance (current measurement)
• Velocity (rate of change)
• Variance (measurement consistency)
• Amplitude (signal strength)
• Gradient (multi-sample rate of change)
• Consistency (inverse normalized variance)

Euclidean distance in feature space:

d = √(Σ(xi - yi)²)

Installation

Prerequisites

• PlatformIO CLI
• Python 3.8+
• ESP32-S3 development board
• USB cable for programming

Firmware Setup

1. Install PlatformIO (if not already installed):

pip install platformio

2. Install Dependencies:

cd "Sonar Scanner"
pio lib install

3. Build Firmware:

pio run

4. Upload to ESP32-S3:

pio run --target upload

5. Monitor Serial Output:

pio device monitor

Python Visualizer Setup

1. Install Python Dependencies:

pip install -r requirements.txt

2. Run Visualizer:

python visualizer.py --port COM3 --baud 921600

Adjust the port based on your system (COM3 for Windows, /dev/ttyUSB0 for Linux).

Configuration

Firmware Parameters

Edit src/main.cpp to modify:

#define TRIGGER_PIN      5      // Ultrasonic trigger pin
#define ECHO_PIN         18     // Ultrasonic echo pin
#define SERVO_PIN        16     // Servo PWM pin

#define SENSOR_SAMPLE_RATE_HZ    100    // Sensor sampling frequency
#define TELEMETRY_RATE_HZ        50     // Telemetry output frequency
#define SWEEP_PERIOD_MS          2000   // Sweep period

#define MAX_DISTANCE_M          4.0f    // Maximum detection range

EKF Tuning

Adjust EKF parameters in main.cpp:

ExtendedKalmanFilter ekf(0.1f, 0.3f);
//                        ^      ^
//                        |      |
//                 Process noise  Measurement noise

• Process noise (Q): Higher values allow faster adaptation to changes
• Measurement noise (R): Higher values trust measurements less

Motor Control

Adjust sweep parameters in motorControlTask:

motorCommand.sweepAmplitude = 90.0f;  // ±90 degrees
motorCommand.sweepFrequency = 0.5f;   // 0.5Hz sweep

Visualizer Parameters

Edit visualizer.py to modify:

self.grid_size = 400      # Grid resolution (pixels)
self.scale = 100          # Pixels per meter
self.decay_rate = 0.98    # Radar trail decay factor

DBSCAN clustering parameters:

eps = 0.3                # Cluster radius (meters)
min_samples = 3           # Minimum points per cluster

Object Classes

The system classifies objects into 5 categories:

1. Wall/Flat: Consistent reflections, low variance
2. Corner/Edge: High variance, discontinuous returns
3. Dynamic/Moving: Significant velocity, moderate variance
4. Human/Soft: Low amplitude, absorbing materials
5. Unknown: Insufficient data for classification

Telemetry Format

JSON telemetry format (921600 baud):

{
  "t": 1234567890,        // Timestamp (microseconds)
  "d": 1.23,              // Distance (meters)
  "v": 0.05,              // Velocity (m/s)
  "a": 45.0,              // Angle (degrees)
  "c": 0.95,              // Confidence (0-1)
  "oc": 1,                // Object class (0-4)
  "cc": 0.87              // Classification confidence (0-1)
}

Performance Characteristics

Firmware

• Sensor Sampling: 100 Hz
• Telemetry Rate: 50 Hz
• EKF Update Rate: 100 Hz
• Classification Rate: 50 Hz
• Memory Usage: ~50KB RAM
• CPU Utilization: ~60% (dual-core)

Visualizer

• Frame Rate: 30-60 FPS (depending on CPU)
• Latency: <50ms
• Grid Resolution: 400x400 pixels
• Maximum Range: 4 meters

Troubleshooting

Firmware Issues

No serial output:

• Check baud rate matches (921600)
• Verify USB driver installation
• Check TX/RX connections

Servo not moving:

• Verify SERVO_PIN configuration
• Check PWM frequency (50Hz)
• Ensure external power supply for servo

Sensor readings invalid:

• Check TRIGGER_PIN and ECHO_PIN connections
• Verify sensor power supply (5V)
• Check for electrical noise (add decoupling capacitors)

Visualizer Issues

No serial connection:

• Verify correct COM port
• Check baud rate matches firmware
• Ensure ESP32 is powered and running

Poor clustering results:

• Adjust DBSCAN eps parameter
• Increase sensor sample rate
• Improve EKF tuning for better noise filtering

High latency:

• Reduce grid resolution
• Decrease decay rate
• Optimize clustering parameters

Optimization Tips

Firmware

• Enable PSRAM for larger buffers
• Adjust task priorities based on requirements
• Use hardware timers for precise timing
• Optimize EKF parameters for your environment

Visualizer

• Use GPU acceleration if available
• Implement multi-threading for clustering
• Reduce grid resolution for real-time performance
• Use binary protocol instead of JSON for higher throughput

Safety Considerations

• Electrical Safety: Use appropriate voltage levels for sensors
• Mechanical Safety: Secure servo motor to prevent injury
• Eye Safety: Avoid pointing ultrasonic sensors at eyes
• Heat Management: Ensure adequate ventilation for ESP32

License

This project is provided as-is for educational and research purposes.

Contributing

Contributions are welcome! Areas for improvement:

• Additional sensor support (LiDAR, ToF)
• Web-based visualization interface
• Advanced ML models (neural networks)
• Multi-sensor fusion
• SLAM implementation

References

• Extended Kalman Filter: Welch & Bishop, 2001
• DBSCAN Clustering: Ester et al., 1996
• ESP32-S3 Technical Reference Manual
• FreeRTOS Documentation
• ArduinoJson Library

Contact

For questions or issues, please open an issue on the project repository.