This project contains a C++ reference implementation of a visual target tracking and control system and its Python port, designed for drone tracking and control of moving targets.

Project Structure

code-test-drone/
├── reference-c++/           # C++ reference implementation
│   ├── target_tracker/      # Target tracking library (independent sub-project)
│   │   ├── include/         # Header files directory
│   │   ├── src/             # Source files directory
│   │   └── tests/           # Unit tests
│   └── visual_track_control/# Visual tracking control library
│       ├── include/         # Header files directory
│       ├── src/             # Source files directory
│       └── tests/           # Unit tests
└── utils/                   # Python port
    ├── target_tracker/      # Target tracking (Python version)
    ├── visual_track_control/# Tracking control (Python version)
    └── tests/               # Unit tests

Main Modules

target_tracker - Target Tracking Library

Provides target tracking functionality based on Kalman filtering:

KalmanBoxFilter: 8-dimensional Kalman filter (position, velocity)
Tracklet: Target trajectory management and state updates
TargetTracker: Main tracker, containing association and election logic

Core features:

Greedy association algorithm for detection matching
Three-level fallback mechanism for main target election
Supports Coast (pure prediction) and Lost (missed) state management

visual_track_control - Visual Tracking Control Library

Generates drone control commands based on tracking results:

PIDController: Distance PID control
TrackingControlLaw: Tracking control law
MavlinkCommandGateway: MAVLink command gateway
PixelToAngle: Pixel to angle conversion
StalenessChecker: Data staleness check
SlewRateLimiter: Rate limiter

Supports multiple control modes:

Rate Mode: Angular rate control
Position Mode: Absolute heading control
Distance Control: Approach/retreat strategy

Environment Requirements

C++ Version

CMake 3.10+
C++17 Compiler
MAVLink Library (MAVSDK)

Python Version

Python 3.12+
Dependencies: numpy, pymavlink, mavsdk

Installation

Python Environment

pip install -r requirements.txt

C++ Build

cd reference-c++/target_tracker
mkdir build && cd build
cmake ..
make

cd ../../visual_track_control
mkdir build && cd build
cmake ..
make

Testing

Python Tests

cd utils
pytest

C++ Tests

cd reference-c++/target_tracker/build
ctest

cd ../../visual_track_control/build
ctest

Configuration

Both C++ and Python versions use JSON format configuration files:

{
    "control": {
        "frequency_hz": 20.0,
        "visual_stale_ms": 200,
        "radar_stale_ms": 500,
        "attitude_stale_ms": 300
    },
    "camera": {
        "image_width": 1920,
        "image_height": 1080,
        "fov_h_deg": 60.0,
        "fov_v_deg": 35.0
    },
    "heading": {
        "mode": "rate",
        "gain": 1.0,
        "yaw_rate_limit_dps": 30.0,
        "yaw_slew_limit_dps2": 60.0
    },
    "distance": {
        "d_exp_m": 10.0,
        "kp": 0.5,
        "ki": 0.05,
        "kd": 0.1,
        "integral_limit": 2.0
    }
}

Usage Examples

Python Tracking Demo

cd utils
python demo_track_control.py

C++ Tracking Demo

cd reference-c++/visual_track_control/build
./track_control_demo

Porting Notes

Please refer to the README in each subdirectory for interface differences between the Python version and the C++ version:

License

This project follows open source license agreements.

Contributing Guidelines

Feel free to submit Issues and Pull Requests to improve this project.