commit for push

examples/initial_state_fix.txt

@@ -0,0 +1,13 @@
+        // 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);