// Set up tracker
        tracker_->setReferencePath(path);

        // 修复: 从路径起点获取初始状态，确保完美匹配
        const auto& path_points = path.getPathPoints();
        AGVModel::State initial_state;
        if (!path_points.empty()) {
            const PathPoint& start = path_points[0];
            initial_state = AGVModel::State(start.x, start.y, start.theta);
        } else {
            initial_state = AGVModel::State(0.0, 0.0, 0.0);
        }
        tracker_->setInitialState(initial_state);