结论：`MultiWheelC\Fleet` 当前的数学链路作为“第一版组件”是成立的，现有 Fleet 数学测试共47个场景全部通过，没有发现明显的坐标系或符号错误。但它还不是可以直接上双车实车的完整工程方案，`FleetStateEstimator` 的等权平均尤其需要增加鲁棒性。

## 正负平均为0是不是问题

不一定，很多时候这正是期望结果。

假设两辆车反算出的车队中心分别为：

```text
车辆1候选中心：+20mm
车辆2候选中心：-20mm
```

平均得到：

```text
车队中心：0mm
```

但代码随后还会计算每辆车相对于这个中心的布局误差：

```text
车辆1相对误差：+20mm
车辆2相对误差：-20mm
```

所以并不是误差消失了，而是被分成了：

```text
公共分量：车队整体中心运动
相对分量：成员之间的队形变形
```

这正好符合当前架构：

- 公共分量交给 `FleetController` 控制车队中心；
- 相对分量交给 `FleetCoordinator` 减速，以及 `FleetMemberCommandCorrector` 小幅纠偏。

如果两辆车反算结果都是 `+20mm`：

```text
平均中心：+20mm
成员相对误差：接近0
```

这表示整个车队共同移动了20mm，而队形没有变，也符合物理意义。

## 当前不是“无条件直接平均”

当前 [FleetStateEstimator.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Fleet/FleetStateEstimator.cs:277) 实际执行的是：

1. 将成员状态对齐到统一时刻。
2. 每辆车根据固定布局反算候选车队中心。
3. 检查任意两辆车反算的中心位置差和航向差。
4. 差异超过阈值，直接返回状态不可用。
5. 只有所有候选结果基本一致，才平均：
   - X/Y算术平均；
   - Yaw圆周平均。
6. 根据融合后的中心重新计算每辆车的布局误差。

所以如果：

```text
车辆1：+200mm
车辆2：-200mm
```

两者相差400mm，只要超过 `_maximumPositionDisagreementMeters`，代码会在平均前拒绝本周期状态，不会把它们平均成0后继续运行。

Yaw也不是普通平均。例如：

```text
+179°和-179°
```

圆周平均会得到接近 `±180°`，而不是错误地得到 `0°`。

## 当前平均真正的问题

它隐含了一个假设：

> 所有成员定位精度相同、可信度相同，而且没有异常值。

对你目前的 Detour 情况，这个假设不够可靠。

假如真实车队中心是0，而：

```text
车辆1定位正常：0mm
车辆2发生小范围跳变：+60mm
```

当前等权平均会得到：

```text
估计中心：+30mm
```

如果60mm还没有超过候选差异阈值，这个错误会进入车队中心控制。

当前估计器存在三个主要限制：

- 所有成员等权，没有利用定位质量、数据年龄和历史稳定性；
- 单个异常成员超过阈值时，会使整个车队状态不可用，不能隔离坏成员；
- 它是单帧估计，没有利用上一周期车队中心和轮速预测判断哪辆车更可信。

而且你目前主要是双车。两辆车互相矛盾时不存在“多数票”：

```text
车1说中心在A
车2说中心在B
```

只看这两个当前观测，无法判断谁正确。必须引入：

- 上一周期车队状态；
- 轮速运动预测；
- 每辆车定位健康状态；
- 数据新鲜度；
- 连续多帧确认。

## 更成熟、适合你的方案

我建议采用：

> 带时间预测和异常门控的鲁棒加权 SE(2) 融合。

不需要现在就上复杂的因子图或完整EKF。

### 第一步：保留当前反算方法

继续让每辆车计算：

```text
候选车队位姿
= 成员实际世界位姿
× 成员固定布局位姿的逆
```

这部分当前是正确的，不需要推翻。

### 第二步：增加上一状态预测

根据上一周期车队中心和轮组速度预测：

```text
PredictedFleetPose
```

然后每辆车的候选中心分别与预测值比较：

```text
位置创新
航向创新
```

这样双车出现分歧时，可以判断：

```text
车1与预测连续
车2突然跳变
→ 暂时拒绝车2，而不是两车平均或全队立即失败
```

### 第三步：成员单独门控

每个成员分别检查：

- 状态是否可用；
- 数据是否过期；
- 相对预测的位置创新是否合理；
- 相对预测的航向创新是否合理；
- 是否连续多帧异常；
- 后续是否连续多帧恢复。

不要只做当前这种“成员之间两两比较”。

### 第四步：对通过门控的候选加权融合

权重可以先用简单工程等级：

```text
健康且新鲜        → 权重1.0
轻度退化或较旧    → 较低权重
正在异常确认      → 权重0
```

以后 Detour 如果能提供协方差，再改成按协方差加权。

对残差使用 Huber 一类鲁棒损失会让正常小误差仍按最小二乘处理，而异常大残差的影响被压低；这是成熟估计库采用的标准做法，[GTSAM官方文档](https://gtsam.org/doxygen/a04439.html)也明确给出了 Huber 在迭代重加权最小二乘中的权重形式。

### 第五步：输出融合健康等级

建议以后区分：

```text
Healthy
所有有效成员一致，正常融合

Degraded
有一个成员被拒绝，暂时依赖剩余成员和预测，整队限速

Unavailable
所有成员都不可信，或双车分歧且无法判断谁正确
短时保持轮速预测，随后停车
```

## 更严格的数学形式

成熟版本可以直接求一个最符合全部刚体观测的车队位姿：

\[
T_f^*=
\arg\min_{T_f}
\left[
w_p\|\log(T_{pred}^{-1}T_f)\|^2+
\sum_i w_i\,\rho\left(
\|\log((T_fT_{layout,i})^{-1}T_{actual,i})\|^2
\right)
\right]
\]

它表示：

- `T_f`：要求解的车队实际中心；
- `T_layout,i`：第i辆车的固定布局；
- `T_actual,i`：第i辆车当前测量位姿；
- `T_pred`：根据历史和轮速得到的预测中心；
- `w_i`：成员可靠性；
- `ρ`：Huber等鲁棒损失。

这是“鲁棒刚体拟合”，比单纯平均更完整，同时利用位置和航向。普通最小二乘刚体配准本身就是成熟方法；鲁棒损失是在异常观测下限制单个数据影响的标准扩展。[刚体点集最小二乘原始问题说明](https://ntrl.ntis.gov/NTRL/dashboard/searchResults/titleDetail/PB91135772.xhtml)

如果将来各车能提供可信协方差，但不同SLAM估计之间的相关性未知，可以考虑 Covariance Intersection；它专门处理未知交叉相关性的估计融合。[Julier与Uhlmann的SLAM融合论文](https://www.sciencedirect.com/science/article/abs/pii/S0921889006001436) 但你目前拿不到Detour协方差，所以现在直接上CI没有足够输入。

## 对当前项目的建议

当前先不要推翻 `FleetStateEstimator`。合理的推进顺序是：

1. 保留现有布局反算、时间对齐、圆周Yaw平均和成员误差计算。
2. 增加“上一车队状态＋轮速”的中心预测。
3. 把两两一致性检查改成“每成员相对预测的独立门控”。
4. 对通过门控的成员做加权圆周平均或小规模Huber融合。
5. 允许单个成员短时降级，不要一帧异常就让整个估计不可用。
6. 双车无法判断谁正确时，短时间用预测，仍无法恢复再停车。
7. 最后再考虑完整的SE(2)鲁棒最小二乘，不需要一开始引入GTSAM或QP。

对整个 `MultiWheelC\Fleet` 的当前评价是：

- 布局定义、刚体速度分解、中心控制、相对误差纠偏、最差成员统一减速和全员Ready屏障，结构上没有明显问题；
- 当前最大算法缺口就是车队中心融合还不够鲁棒；
- 最大工程缺口仍是运行入口、通信、成员超时看门狗、报警汇总和零速命令的实际下发；
- `FleetMemberAgent` 目前还缺少独立自动化测试；
- 各种阈值目前只在测试构造中出现，尚未通过双车实车数据标定。

所以，平均本身不是错误；“只做等权平均，并在冲突时整队不可用”才是需要下一阶段改进的部分。






将 FleetMemberReport 转成 FleetMemberStateSample 和 FleetMemberSafetyStatus；
将准备命令映射到 FleetMemberAgent；
将成员速度命令映射成 FleetCommand；
ShouldStop 时广播 Stop 并让主车本地停车；
命令序号、报告序号和任务编号检查；
M层报警和底盘执行失败映射到 FailureCode；
无线端口初始化；
报文序列化、分包、校验和接收循环；
主车本地单调时钟和报文接收时间记录。