Python Radar and CAN Data Fusion Tracker This project is a complete Python port of a MATLAB-based system for processing raw radar point cloud data and vehicle CAN bus logs to detect and track objects in a dynamic environment. It is designed for post-processing of recorded data files and leverages an advanced Interacting Multiple Model (IMM) filter combined with a Joint Probabilistic Data Association (JPDA) algorithm for robust tracking.
Key Functionality The system provides an end-to-end pipeline that performs the following key functions:
Data Synchronization: Loads raw radar data (fHist.mat) and a separate CAN log (.mat), then synchronizes the two sources by interpolating CAN signals to match each radar frame's timestamp, correctly handling extrapolation for data continuity.
Ego-Motion Estimation & Classification:
Estimation: In each frame, it calculates the host vehicle's own velocity by fusing sensor data from CAN, IMU, and a RANSAC-based radar velocity estimate into a 5D Extended Kalman Filter. This step is critical for distinguishing static points from moving ones and produces an isOutlier flag for every radar point.
Classification: It classifies the ego vehicle's motion state (e.g., straight, turning left, turning right) based on yaw rate data, allowing downstream logic to adapt to the vehicle's behavior.
Dynamic Environment Filtering: It identifies static barriers (like guardrails or walls) using the static radar points identified during ego-motion estimation. This barrier information is used to create a dynamic "stationary box" and a "dead zone" to enable the tracking of relevant stationary objects while filtering out clutter near the barriers.
Object Detection (Clustering): It groups raw radar points into distinct clusters representing potential objects using a custom, grid-accelerated DBSCAN algorithm. This algorithm uses a multi-modal distance metric, considering both spatial proximity and the difference in radial velocity.
Pre-tracking Filters: Applies critical filters to reduce false detections before tracking, including:
Ignoring low-speed clusters when the ego vehicle is stationary.
Calculating an isOutlierCluster flag for each cluster based on the percentage of dynamic points it contains.
Calculating an isStationary_inBox flag for stationary clusters that fall within the dynamic stationary box, marking them as important objects to track (e.g., a guardrail).
Dynamic Object Tracking: Manages the entire lifecycle of a track (creation, update, deletion) using a hierarchical assignment logic.
State Estimation: Uses an Interacting Multiple Model (IMM) filter to accurately estimate an object's state by dynamically blending three different motion models: Constant Velocity (CV), Constant Turn (CT), and Constant Acceleration (CA).
Data Association: For confirmed tracks, it uses the Joint Probabilistic Data Association (JPDA) algorithm to handle ambiguous detections and calculate the marginal probability of association between tracks and measurements in dense scenarios.
Dual-Format Export: Saves the final processing results to two separate files:
A track_history.json file with a schema optimized for web-based visualization tools.
A track_history_python.mat file with allTracks and fHist variables structured to be identical to the original MATLAB output, enabling direct side-by-side comparison and debugging.
Project Structure python-radar-tracker/ │ ├── data/ │ └── (Input .mat files for radar and CAN logs) │ ├── src/ │ ├── algorithms/ # Core algorithms (DBSCAN, RANSAC, Ego-Motion) │ ├── filters/ # EKF and IMM filter implementations │ ├── track_management/ # Logic for track lifecycle (assign, update, delete, JPDA) │ ├── utils/ # Helper functions (coordinate transforms, etc.) │ ├── data_loader.py # Loads and synchronizes radar and CAN data │ ├── export_to_json.py # Formats data for the JSON output schema │ ├── main.py # Main entry point of the application │ └── perform_track_assignment_master.py # High-level orchestrator for tracking │ └── readme.md Control Flow The simulation runs in a frame-by-frame loop orchestrated by main.py.
Initialization (main.py):
Sets up a file-based logger to capture all console output.
Initializes the 5D EKF for ego-motion and filters for motion classification and barrier detection.
Prompts the user for the paths to the radar fHist.mat and CAN log .mat files.
Calls data_loader.py to load both files and synchronize the CAN signals to the radar's time frame.
Main Processing Loop (for each frame in main.py):
CAN Interpolation: Interpolates all required CAN/IMU signals to match the current radar frame's timestamp.
Ego-Motion Classification: The classify_vehicle_motion function determines if the ego vehicle is turning.
Ego-Motion Estimation: The estimate_ego_motion function calculates the ego vehicle's state and produces the isOutlier boolean array, flagging every point as static or dynamic.
Dynamic Barrier Detection: The detect_side_barrier function uses the static points to update the position of the dynamic stationary box.
Clustering: my_dbscan.py groups all radar points into clusters.
Centroid Calculation & Filtering: The centroid of each cluster is calculated, and the crucial pre-tracking flags (isOutlierCluster and isStationary_inBox) are computed.
Master Tracking Call (perform_track_assignment_master.py): The list of filtered, valid detections is passed to the master tracking function, which executes a strict, hierarchical logic:
Predict: Calls imm_predict() for all active (confirmed and tentative) tracks to predict their next state.
Maintain Confirmed Tracks: Uses jpda_assignment() for robust data association. History and stationaryCount are updated for successfully associated tracks.
Update Tentative Tracks: Uses a greedy assignment for new tracks, checks for promotion to "confirmed", and updates their stationaryCount.
Reassign Lost Tracks: Attempts to revive lost tracks with new detections by re-initializing their state.
Create New Tracks: Creates a new tentative track only if a detection's cluster is deemed "trackable" (i.e., it is a dynamic isOutlierCluster OR it is a stationary cluster flagged as isStationary_inBox).
Handle Missed Tracks: Manages unassigned tracks by increasing their miss counter and marking them as "lost" if they exceed a threshold.
Finalization (main.py):
After the loop finishes, update_and_save_history.py is called.
This function serializes the final all_tracks and fhist data into both the track_history.json and track_history_python.mat files.
How to Run Setup Environment: It is recommended to use a virtual environment.
Bash
python -m venv radar_env
.\radar_env\Scripts\activate
source radar_env/bin/activate Install Dependencies:
Bash
pip install numpy scipy matplotlib Place Data: Put your input fHist_...mat and CAN log .mat files in a data/ directory at the project root.
Run the Script: Open your terminal in the project's root directory and run the main module.
Bash
python -m src.main Follow Prompts: The script will prompt you to enter the full path to your radar history file and your CAN log file.
Outputs The simulation will produce three files in the root directory:
sim_log_...txt: A detailed log of the entire processing run, capturing all console output.
track_history.json: The final output data, formatted for use in downstream visualization applications.
track_history_python.mat: A MATLAB-compatible file containing allTracks and fHist variables, structured identically to the original MATLAB script's output for direct comparison and debugging.