Sensor Modeling: IMU / GPS Electronic Face

> Aligned with PCR Master Blueprint v1.0 — see Blueprint §1.1（Device 视角）、§2.4、§7.4。 > 职责：传感器（IMU / GPS）是只有电子面的 device。它们的"物理输入"就是 body 自身的真值状态（body.aux / body.spatial），无需独立 Mech。本文档规约传感器 step 函数的 fidelity 切换、量化/噪声机制、Bus 输出形态。 > C-Distillation note：fidelity 当前用 enum + switch，蒸馏阶段直接退化为函数指针表或多版本编译。

1. 在双面拓扑中的位置

参考 Device_Dual_Face.md §2：传感器属于"只有电子面（含传感）"一类。

text

body.aux (真值)        ┌─────────────────────┐
body.spatial (真值) ─→ │ avionics::device::  │ ─→ Bus
                       │ imu::step / gps::step│
                       └─────────────────────┘
                              ImuState / GpsState
                              （只持电子状态：bias、残差、量化累计）

与执行机构的对比：

项	传感器（IMU / GPS）	执行机构（Engine / Servo / Fin）
物理面	❌ 无 Mech	✅ 有 Mech
物理输入	body 自身 (aux / spatial)	自身 Mech
物理输出	无（只发 Bus）	写回 Mech，再被 plant::physics 读
Bus 方向	Device → FCC	FCC → Device

> 把"传感器 Mech"独立出来是冗余的——传感器并不修改物理，它只是观察。"观察行为本身"才是 fidelity 的关键。

2. 实际签名（与代码对齐）

2.1 IMU

cpp

// src/avionics/devices/Imu.h
namespace avionics::device::imu {

struct ImuState {
    Vec3_T&lt;dynamics::frame::BODY&gt; di_theta;       // 累计角增量（量化前）
    Vec3_T&lt;dynamics::frame::BODY&gt; di_v;           // 累计速度增量
    Vec3_T&lt;dynamics::frame::BODY&gt; bias_g;         // 陀螺零偏
    Vec3_T&lt;dynamics::frame::BODY&gt; bias_a;         // 加速度计零偏
    Vec3_T&lt;dynamics::frame::BODY&gt; last_di_theta;  // 上拍角增量（双采样 coning 补偿用）
};

struct ImuConfig {
    enum class Fidelity : uint16_t {
        Transparent,    // 直接转写真值，不引入量化与噪声
        Realistic,      // 加噪 + 量化 + 残差累积 + bias drift
        Fault           // 故障注入：丢点、卡死、漂移失控
    } fidelity = Fidelity::Transparent;
};

void step(ImuState& imu,
          const dynamics::AuxState& aux_truth,
          bus::IBus& bus,
          const ImuConfig& cfg,
          Time dt);
}

2.2 GPS

cpp

// src/avionics/devices/Gps.h
namespace avionics::device::gps {

struct GpsState {
    Vec3_T&lt;dynamics::frame::ECF&gt; last_pos_ecef_noisy;
    Vec3_T&lt;dynamics::frame::ECF&gt; last_vel_ecef_noisy;
    uint32_t                     sat_count;
};

struct GpsConfig {
    enum class Fidelity : uint16_t { Transparent, Realistic, Fault } fidelity = Fidelity::Transparent;
};

void step(GpsState& gps,
          const dynamics::SpatialState& spatial_truth,
          const plant::model::FrameConfig& frame_cfg,   // LIC↔ECF 装配期固定
          bus::IBus& bus,
          const GpsConfig& cfg,
          Time dt);
}

注意：

GPS 的输出是 ECF 系（Vec3_T<dynamics::frame::ECF>），不是 LIC。FCC 的 GPS 滤波要直接消费 ECF 真值。
IMU 的输出是 BODY 系（Vec3_T<dynamics::frame::BODY>），角增量与速度增量。

3. IMU 量化与残差守恒

3.1 Delta-V / Delta-Theta 机制

真实 IMU 输出累计脉冲（angular increments / velocity increments），不是瞬时角速率 / 加速度。建模时：

text

truth_d_theta  = aux_truth.body_rate * dt
quantized      = floor(truth_d_theta / gyro_lsb) * gyro_lsb
residue        = truth_d_theta - quantized
imu.di_theta  += quantized
imu.last_di_theta += residue   // 累计到下拍

关键：截断产生的余量必须保留到下拍，否则会有累计漂移。

3.2 双采样下采样

物理拍 body_dt = 5ms (200Hz)，FCC 通常以 100Hz / 50Hz 工作。IMU 累计两拍 5ms 后形成一帧 10ms 双采样：

text

拍 k     拍 k+1
 │5ms    │5ms
 ↓       ↓
 di_theta_k  di_theta_{k+1}
       │
       └──→ 合并为 ImuPayload { delta_theta_body = di_theta_k + di_theta_{k+1}, ... }
            发送 Bus，并支持 FCC 端的 coning / sculling 补偿

3.3 Noise / Bias drift

白噪声：均值 0 的高斯，σ 由 ImuConfig 提供
Bias drift：1/τ 一阶马尔可夫，状态保存在 ImuState.bias_g / bias_a
量化：gyro_lsb / accel_lsb 由配置给出

4. GPS 噪声与时延

GPS 比 IMU 低频得多，建模重点不同：

维度	IMU	GPS
工作频率	100-400 Hz	1-10 Hz
主要建模点	量化、残差、coning	位置/速度噪声、卫星数量、丢星
输出系	BODY	ECF
是否量化	✅ 关键	一般不量化（数值已粗）

GPS 步骤：

text

1. 读 spatial_truth.pos_lic / vel_lic
2. LIC → ECF 转换（用 frame_cfg）
3. 注入 Gaussian 噪声（pos σ ~ 5m, vel σ ~ 0.1 m/s）
4. 更新 sat_count（如有丢星模型）
5. 仅在 update_hz 边界 publish 到 Bus

GPS 的 Bus payload 形态 = GpsPayload { pos_ecf, vel_ecf }，对应 src/bus/BusPayloads.h:16-19。

5. 三档 fidelity 的语义

Fidelity	含义	适用场景
Transparent	真值直转，零噪声零延迟	单元测试、控制算法回归、debug 初期
Realistic	全套：量化 + 噪声 + bias drift + 时延	闭环仿真、性能评估
Fault	故意失效：丢点 / 漂移 / 卡死 / 跳变	容错测试、FDI 测试

切换由配置决定，不需要换 step 函数：

yaml

# devices.yaml
imu:
  fidelity: Realistic
  gyro_lsb: 0.0001            # rad/pulse
  accel_lsb: 0.001            # (m/s²)/pulse
  sample_rate_hz: 200
  bias_g_init_std: 1e-5
  bias_a_init_std: 1e-4
  tau_bias_g_s: 3600
gps:
  fidelity: Realistic
  pos_std_dev: 5.0            # meters (1σ)
  vel_std_dev: 0.1            # m/s
  update_hz: 1

> Blueprint §7.4：device fidelity 是局部决策，不走 interpreter 模式。这里 enum + switch 就够用。

6. 装配与执行（在 simulation 中）

传感器与执行机构同住 RocketBody：

cpp

// simulation/state/RocketBody.h
struct ImuUnit { contracts::ImuId id; ImuState state; ImuConfig cfg; };
struct GpsUnit { contracts::GpsId id; GpsState state; GpsConfig cfg; };

struct RocketBody {
    // ...
    std::vector&lt;ImuUnit&gt; imus;
    std::vector&lt;GpsUnit&gt; gpss;
    // ...
};

在 sim::body_tick 拍头执行：

cpp

// simulation/pipeline/BodyTick.cpp
for (auto& imu : body.imus) {
    avionics::device::imu::step(imu.state, body.aux, body.bus, imu.cfg, dt);
}
for (auto& gps : body.gpss) {
    if (should_run_at(world.clock, gps.cfg.update_hz)) {
        avionics::device::gps::step(gps.state, body.spatial, world_env.frame_cfg,
                                     body.bus, gps.cfg, dt);
    }
}

注意：

传感器 step 先于 FCC tick 与执行机构 step（拍头采样，详见 Device_Dual_Face.md §4）
GPS 的 update_hz 用 world.clock 的 tick 边界判断，避免飘移

7. 测试策略

测试目标	级别	描述
Transparent 直转	bench	喂 `aux_truth = (1,0,0) rad/s`，验证 Bus 输出 `delta_theta_body = (0.005, 0, 0)`
量化残差守恒	bench	跑 1000 拍非整数倍 LSB，验证累计无漂移
Bias drift 一阶马尔可夫	bench	跑 3600s，验证 bias_g 的 PSD ≈ 1/(1+(τω)²)
GPS 1σ pos error	bench	1e4 拍蒙卡，验证 std(pos_ecf - truth) ≈ pos_std_dev
Fault 模式	fault	注入丢点，验证 FCC FDI 状态机切换

8. 反模式

反模式	为什么不行
在 IMU step 里直接修改 `body.aux`	传感器是观察者，不修改物理
不保留量化残差	累计漂移 → 长时仿真发散
GPS 输出 LIC 系	与 ECF-Centric 真值约定相悖（Blueprint §1.2.2 / `Coordinate_Frames.md`）
Fidelity 切换通过 #ifdef / 编译期开关	应是运行时 cfg.fidelity，便于 mission 切档
在 step 内调用 `IBus::poll`	传感器只 publish，不 poll
给 IMU 加 Mech	冗余抽象，违反 §7.4

9. Cross References

传感器在双面拓扑中的位置 → Device_Dual_Face.md §2、§3.2
Bus payload 定义（ImuPayload / GpsPayload）→ Semantic_Bus_Pattern.md §2
ECF / LIC 坐标系约定 → 01_Foundation/Coordinate_Frames.md
Frames 强类型保证 → 01_Foundation/Contracts_PerBodyFrames.md
FCC 侧 SINS / GPS 滤波 → 04_FCC/Theoretical_vs_SINS_Navigation.md

Sensor Modeling: IMU / GPS Electronic Face ​

1. 在双面拓扑中的位置 ​

2. 实际签名（与代码对齐） ​

2.1 IMU ​

2.2 GPS ​

3. IMU 量化与残差守恒 ​

3.1 Delta-V / Delta-Theta 机制 ​

3.2 双采样下采样 ​

3.3 Noise / Bias drift ​

4. GPS 噪声与时延 ​

5. 三档 fidelity 的语义 ​

6. 装配与执行（在 simulation 中） ​

7. 测试策略 ​

8. 反模式 ​

9. Cross References ​

Sensor Modeling: IMU / GPS Electronic Face

1. 在双面拓扑中的位置

2. 实际签名（与代码对齐）

2.1 IMU

2.2 GPS

3. IMU 量化与残差守恒

3.1 Delta-V / Delta-Theta 机制

3.2 双采样下采样

3.3 Noise / Bias drift

4. GPS 噪声与时延

5. 三档 fidelity 的语义

6. 装配与执行（在 simulation 中）

7. 测试策略

8. 反模式

9. Cross References