重构横向控制器输出前后GCP目标转角并简化命令分配器

docs/数据融合.txt

@@ -0,0 +1,94 @@
+是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。
+为什么需要系统辨识？
+卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现：
+
+预测步持续往错误方向跑
+滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型
+速度估计系统性偏大/偏小
+原地自转时航向纠正效果变差
+
+你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。
+建议辨识的主要参数
+针对四轮差速车，优先考虑这些：
+
+运动学参数
+有效轮半径（或左右轮半径）
+轮距（左右轮中心距离）
+可能的轴距相关项（如果不是纯差速）
+
+速度相关
+电机速度到实际车速的缩放系数（你提到的“偏大”问题）
+左右轮速度的不对称性（如果存在）
+低速段的非线性（静摩擦、死区）
+
+可选但有用的
+速度偏置 / 零点漂移
+简单的打滑系数（如果经常在特定地面跑）
+
+
+怎么做比较实际？
+不需要一开始就上很复杂的在线辨识，可以分两步：
+离线标定（推荐先做）
+
+让车在平坦地面以不同速度直线行驶、原地旋转，记录：
+四个电机反馈速度
+SLAM 给出的位置和航向变化
+
+用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等
+原地自转特别适合标定轮距和左右轮差异
+
+在线微调（可选）
+
+把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF）
+或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子
+
+实际建议顺序
+
+先建立基本运动学模型 + EKF 框架
+做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉
+再跑融合，看残差（innovation）是否变小、速度估计是否更合理
+如果还有残余误差，再考虑把个别参数做成在线估计
+
+总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。
+
+有没有必要做卡尔曼/传感器融合？
+分两层看：
+
+如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳
+→ 性价比一般。
+你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。
+如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等）
+→ 值得做，性价比会变高。
+原因是：
+绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。
+电机反馈是更直接的速度源，频率高、相位更好。
+把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到：
+更低的噪声
+更小的滞后
+对电机“偏大”问题的一定校正能力
+
+
+
+实际建议（按性价比排序）
+
+先低成本优化现有绿色线（最快）
+检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。
+可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。
+看看闭环时车的实际表现，如果抖动已经可接受，就先用着。
+
+中等成本：做简单融合（推荐优先考虑）
+不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度：
+高频信任电机反馈速度
+低频用处理后的 SLAM 差分速度去校正偏差
+
+实现简单，效果通常比单纯低通好一截，滞后也更小。
+
+完整 EKF + 系统辨识
+只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。
+工作量明显更大，但长期收益最高。
+
+
+总结
+从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。
+但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。
+你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。

docs/记录.txt

@@ -55,3 +55,10 @@ ParkingGeometricController.cs
 增加起步航向对齐状态。
 第一阶段建议采用第二种，简单、安全。
 
+把前后GCP转角分解成两个模态：
+共同转角 = (前GCP转角 + 后GCP转角) / 2
+差动转角 = (前GCP转角 - 后GCP转角) / 2
+
+
+
+