1. 陀螺仪原始数据读取与时间对齐

为什么“时间对齐”是方位角解算的第一道关口

陀螺定向仪导出文件通常同时包含三类记录：时间戳（采样时刻）、角速度（陀螺敏感轴的瞬时输出，单位 °/s 或 °/h）、角度增量（相邻采样间的积分角，单位 °）。这三类数据往往来自不同采样率或不同起始时刻的通道，若直接按行号拼接，会造成“角速度与角度增量错位”，积分后方位角出现系统性偏差。本节任务是把导出文件解析成统一时间基准的干净序列。

导出文件的典型结构与解析要点

假设某型陀螺定向仪导出的文本文件为逐行记录，字段以逗号分隔，首行为表头。示例（教学假设数据）：

# t_ms,gyro_rate_dps,angle_inc_deg,temp_c
0,0.0121,0.0000,25.1
10,0.0118,0.0001,25.1
20,0.0125,0.0001,25.1
...

解析时必须处理三件事：①跳过注释行与表头；②把 t_ms 统一换算为秒；③识别缺失值（空字段或 NaN）并标记，而不是静默填 0。下面给出可直接运行的 Python 解析与对齐实现。

import numpy as np

def load_gyro_export(path):
    """解析陀螺定向仪导出文件，返回统一时间基准的字典。"""
    t_ms, rate, dtheta, temp = [], [], [], []
    with open(path, "r", encoding="utf-8") as f:
        for line in f:
            line = line.strip()
            if not line or line.startswith("#"):
                continue
            parts = [p.strip() for p in line.split(",")]
            if parts[0].lower() in ("t_ms", "time"):   # 表头
                continue
            try:
                t_ms.append(float(parts[0]))
                rate.append(float(parts[1]) if parts[1] else np.nan)
                dtheta.append(float(parts[2]) if parts[2] else np.nan)
                temp.append(float(parts[3]) if len(parts) > 3 and parts[3] else np.nan)
            except ValueError:
                continue  # 跳过损坏行
    return {
        "t_s": np.array(t_ms) / 1000.0,
        "rate_dps": np.array(rate),
        "dtheta_deg": np.array(dtheta),
        "temp_c": np.array(temp),
    }

def align_to_common_grid(data, fs=100.0):
    """把角速度与角度增量重采样到同一等间隔时间栅格。"""
    t = data["t_s"]
    t0, t1 = t[0], t[-1]
    grid = np.arange(t0, t1 + 1e-9, 1.0 / fs)

    def interp(series):
        valid = ~np.isnan(series)
        return np.interp(grid, t[valid], series[valid])

    rate_g = interp(data["rate_dps"])
    dtheta_g = interp(data["dtheta_deg"])
    # 用角速度积分重建角度增量，作为对齐后的校验量
    dtheta_from_rate = rate_g / fs
    return {
        "t_grid": grid,
        "rate_dps": rate_g,
        "dtheta_deg": dtheta_g,
        "dtheta_from_rate": dtheta_from_rate,
    }

对齐结果的自检：用积分角与增量角互相印证

对齐是否可信，不能只看“程序没报错”。一个稳定的自检方法是：把对齐后的角速度按采样间隔积分，得到 dtheta_from_rate，再与文件中的 dtheta_deg 逐点比较。若两者在静止段差异应接近 0；在转动段差异应小于一个采样周期的量化误差。示例判读（教学假设）：

常见错位与处理

• 单位混用：角速度字段为 °/h 而角度增量为 °，直接积分会差 3600 倍。解析时统一到 °/s。
• 起始时刻不同：角速度通道从 0 ms 开始、增量角通道从 5 ms 开始，按行号拼接会整体错位。必须按时间戳插值。
• 时间戳回绕：部分设备用 16 位毫秒计数，约 65.5 s 回绕一次。检测到负差值时加 65536 ms 修正。
• 缺失值：不要用 0 填充，否则会在积分中引入台阶。用 np.nan 标记后仅对有效点插值。

交付物：对齐检查清单

完成以上解析、对齐与自检后，得到的 t_grid / rate_dps / dtheta_deg 即为后续方位角解算的干净输入。