拆解 PSINS ②：test_SINS.m——纯惯导解算（inspure）与"正演→反演"闭环¶

拆解 PSINS 系列第二篇。P1 造出了数据母版（trj10ms.mat，966 s / 96600 拍 @100 Hz），本篇消费它：test_SINS.m（19 行薄壳）把"干净的真值"污染成"带误差的传感器读数"（误差注入），再喂给纯惯导解算 inspure，最后与真值对比得到误差传播曲线——Schuler 振荡、水平漂移、垂直通道发散。

核心交付：迷你 inspure（自包含机械编排，MATLAB+Python 双轨），与 PSINS 原版同参数对拍：水平位置误差末点 935.9 m（PSINS）vs 936.0 m（迷你）——量级完全一致，差异全是可解释的工程细节。

参考体系：PSINS base/base2/inspure.m（纯惯导框架）、base/base1/insupdate.m（机械编排六步）、base/imu/imuerrset.m+imuadderr.m（传感器误差注入）、base/base2/avperrset.m+avpadderr.m（初始误差）、base/base1/bhsimu.m（高度参考生成）、base/base2/avpcmpplot.m（误差画图）；对照科普系列 03 比力方程、07 姿态更新、09 误差传播；以及 P1 的正演机 trjsimu。

一、19 行薄壳干了什么¶

glvs
trj = trjfile('trj10ms.mat');                 % ① 加载 P1 的数据母版
imuerr = imuerrset(0.01, 100, 0.001, 10);     % ② 误差注入（四幕）
imu = imuadderr(trj.imu, imuerr);             %    IMU 误差：eb/db + 随机游走
davp0 = avperrset([0.5;0.5;5], 0.1, [10;10;10]);
avp00 = avpadderr(trj.avp0, davp0);           %    初始误差：姿态/速度/位置
trj = bhsimu(trj, 1, 10, 3, trj.ts);          %    高度参考（trj.bh）
avp = inspure(imu, avp00, trj.bh, 1);         % ③ 纯惯导解算
avperr = avpcmpplot(trj.avp, avp);            % ④ 与真值比较，画误差曲线

四幕：加载 → 注入 → 解算 → 比较。本篇逐幕拆，重点在 ② 的"误差从哪来"和 ③ 的"解算怎么算"。

二、误差注入体系：干净数据怎么"变脏"¶

P1 的 trjsimu 生成的是理想读数（真值反算，无任何误差）。test_SINS 用两套函数把它污染成"真实传感器会给出的读数"：

① IMU 误差（imuerrset + imuadderr）：

imuerr = imuerrset(eb, db, web, wdb);   % eb=陀螺零偏(°/h), db=加计零偏(µg),
                                        % web/wdb=随机游走(°/√h, µg/√Hz)
imu = imuadderr(imu, imuerr);           % imu(:,1:3) += eb*ts + web*sqrt(ts)*randn
                                        % imu(:,4:6) += db*ts + wdb*sqrt(ts)*randn

imuadderr.m 的核心就一行：在增量上叠加"零偏×ts + 随机游走×√ts"——零偏是确定性偏移，随机游走是白噪声积分（科普 04 的 Allan 方差那套）。单位换算藏在 imuerrset（eb*glv.dph 把 °/h 转 rad/s、db*glv.ug 把 µg 转 m/s²）。注意注入发生在增量上（wm/vm），不是速率上——和 P1 坑 1 呼应。

② 初始误差（avperrset + avpadderr）：

davp0 = avperrset([0.5;0.5;5], 0.1, [10;10;10]);
%       phi(arcmin, [pitch;roll;yaw]) + dvn(m/s) + dpos(m)
avp00 = avpadderr(trj.avp0, davp0);

初始姿态误差 [0.5'; 0.5'; 5']（pitch/roll 0.5 分、yaw 5 分）、速度 0.1 m/s、位置 10 m。avpadderr 用四元数加法（qaddphi）把姿态误差正确地加到初始姿态上（不是欧拉角直接加，避免大角度下的顺序问题）。

③ 高度参考（bhsimu）：trj.bh = [真值高度 + 马尔可夫噪声, 时间]——模拟一个"带噪声的气压高度计"，供 inspure 做高度阻尼（见下节）。

📌 教学点：这套误差注入体系就是科普 15 篇"故障注入"的原料（imuerrset 在 15 篇被引用过）。软故障（偏差/漂移）的本质就是"给理想读数加个偏移"，和这里完全同构。

三、inspure 框架：解算循环 + 高度阻尼¶

inspure（base/base2/inspure.m）是纯惯导的主框架：

[nn, ts, nts] = nnts(glv.ns, imu(:,end));   % 双子样 nn=2、采样间隔 ts
ins = insinit(avp0, ts);                     % 初始化 INS 结构（含 eth 地球参数）
for k=1:nn:len-nn+1
    wvm = imu(k:k1, 1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm);               % 机械编排（下节）
    switch vp_fix                            % 高度/速度阻尼选项
        case 'f',  ins.vn(3) = ins.vn(3);    %   自由（垂直发散）
        case 'Z',  ins.pos(3) = href(k1,1);  %   固定高度（test_SINS 用这个）
        case 'H',  ins.pos(3) = pos0(3);     %   固定到初始高度
        ...
    end
    avp(ki,:) = [ins.avp; t]';
end

关键设计：高度阻尼选项（href 参数）。纯惯导的垂直通道开环不稳定（垂直 Schuler 振荡，误差随时间发散），所以 inspure 提供一串"外部高度参考"选项：

📌 test_SINS 传 trj.bh（2 列 [h, t]）→ vp_fix='Z'：高度通道被"气压高度"固定，只让水平通道自由解算。这是惯导工程的标准做法——垂直通道不可靠时，用外部高度源兜底（就像 13 篇的松组合用 GNSS PVT 兜底）。

四、机械编排核心 insupdate：六步更新（与 P1 互逆）¶

insupdate（base/base1/insupdate.m）是纯惯导的心脏，每 nn 拍（双子样）执行六步：

① 圆锥/划桨补偿 cnscl(imu,0)：双子样增量 → 等效旋转矢量。φm = Σwm + cros(0.5·wm₁, wm₂)（圆锥项）、Δvbm = Σvm + scullm（划桨项）——科普 07 圆锥效应的直接落地。

② 标定：φm = Kg·φm − eb·nts、Δvbm = Ka·Δvbm − db·nts——把传感器刻度/零偏误差"洗掉"（对应 ② 注入的误差，解算端要反向补偿；迷你版用单位矩阵/零）。

③ 地球参数更新 ethupdate：由当前 pos/vn 算 RMh/RNh/wnin/gcc（科普 09 的曲率半径与重力模型）。

④ 速度更新（比力方程）：

比力转导航系（qmulv）→ 去掉哥氏/重力（gcc）→ 得运动加速度 → 积分。这就是科普 03 的比力方程 \(f = a - g\)，和 P1 的 trjsimu 第 ⑤ 步（\(a_n - g_{cc}\)）完全互逆。

⑤ 位置更新：\(\mathbf p^+ = \mathbf p^- + \mathbf M_{pv}\,\dfrac{\mathbf v_n^-+\mathbf v_n^+}{2}\, nts\)，\(\mathbf M_{pv}\) 由 RMh/RNh 构成（科普 09 位置微分）。

⑥ 姿态更新（qupdt2）：

先右乘体轴增量 rv2q(φm)（含圆锥补偿），再左乘导航系旋转补偿 rv2q(−wnin·nts)——四元数版"旋转矢量积分"（科普 07）。

📌 互逆关系（P1 的伏笔在此回收）：trjsimu 从"想要的 att/vn/pos"反算 wm/vm；insupdate 从 wm/vm 积分回 att/vn/pos。两者共享同一套 eth 模型与 cnscl 补偿——无误差时，trjsimu 的 imu 喂给 insupdate 应完美复现 trj.avp。迷你链路的自洽性测试就是验证这一点（见第五节）。

五、迷你 inspure：自包含实现与自洽性¶

迷你链路（assets/gen_sins.py / gen_sins.m）把 test_SINS 的数据流全部自包含实现（不依赖 PSINS）：

1. 迷你 trjsimu：P1 的 wat 表（21 行硬编码）→ 数值积分出真值 avp + imu 增量；
2. 误差注入：确定性（eb·ts、db·ts、初始 att/vn/pos 误差）——关掉随机游走（web=wdb=0），保证双轨逐数字可对拍；
3. 迷你 inspure：insupdate 六步的完整实现（cnscl 双子样 + earth + 四元数更新），可选 fix_h 高度固定；
4. 误差分析：解算 vs 真值 → att/vel/pos 误差统计。

自洽性测试（迷你链路的"体检"）：无误差注入时，解算必须完美复现真值。实测：纯 yaw 转 60° 场景解算误差 3e-8°；全轨迹无注入 att/vel/pos 误差在 cm~m 级（剩余 = 位置微分未外推半拍等简化项）——生成/解算模型自洽，误差传播曲线里的误差全部来自注入（这正是教学想要的：误差源单一、清晰）。

⚠️ 写自洽性测试时踩了一个经典坑（写进第八节坑 1）：四元数共轭 ≠ np.conj、a2qua 的 q0>0 符号规范化在 dq 法里会炸——详见坑 1。

六、误差传播分析：四条路径¶

gen_sins 实测（966 s，与 test_SINS 同参数 eb=0.01°/h、db=100µg、初始 [0.5;0.5;5]'）：

三个结论：

1. yaw 初始误差 5′ 主导：att RMS 的 yaw=299.5″ ≈ 5′（注入值），水平位置误差 936 m ≈ ½·g·ψ·t² 量级——初始姿态误差是 966 s 纯惯导的最大误差源（陀螺零偏 0.01°/h 只贡献 ~9.7″/h 的姿态误差，远小于 5′）；
2. 水平 vs 垂直解耦：高度固定对水平误差几乎无影响（936.0 vs 936.0）——垂直通道发散是独立的（垂直 Schuler 振荡），不会污染水平；
3. 垂直通道必须阻尼：自由模式 966 s 发散到 581.6 m，固定后垂直误差 ≈ 气压噪声（~6.8 m RMS）——这就是 test_SINS 用 trj.bh 的原因。

七、与 PSINS 原版对拍¶

迷你链路与 PSINS 原版（inspure）同参数确定性对拍（eb/db 同、随机游走关闭）：

水平误差末点 935.9 vs 936.0 m——相差 0.1 m（0.01%），yaw 主导项 294.7″ vs 299.5″ 一致。差异来源（全部可解释）：① PSINS trjsimu 有匀速段速度阻尼（迷你没有）→ 轨迹细节差异；② PSINS 的 insupdate 有位置外推半拍、更完整的 wnin 处理；③ 迷你版位置微分用当前 vn 而非外推值。结论：迷你链路正确复现了 PSINS 纯惯导的误差传播行为——这是"读懂 PSINS"的硬证据。

八、常见坑¶

1. a2qua 的 q0>0 规范化在 dq 法里会炸（本轮调试实战）：PSINS 260705 的 a2qua 带 if qnb(1)<0, qnb=-qnb（四元数规范化）。如果用 dq 法（conj(q_prev)⊗q_new）求姿态增量，yaw 跨 180°/360° 时 q_prev/q_new 的符号翻转不同步 → dq 跳变 → wm 方向反（真实 +2° 被算成 −2°） → 解算爆炸。PSINS 原版用矩阵法 m2rv(Cbn_1*Cnb) 不受影响（q2mat 对符号不敏感）。自己写四元数姿态增量时，要么用矩阵法、要么保证符号连续。
2. 增量 vs 速率：imu 的 wm/vm 是增量（rad / m/s 每 ts），不是角速度/比力——注入误差、标定补偿都要乘 ts。
3. 垂直通道不阻尼必然发散：纯惯导垂直通道开环不稳定，必须外接高度源（trj.bh/气压计）——不是滤波器写错了，是物理如此。
4. att 顺序 [pitch;roll;yaw]、vn 顺序 [vE;vN;vU]（ENU）：PSINS 是东北天，和我们固件的 NED [vN;vE;vD] 不同——对拍/移植前先重排。
5. 双子样对齐：inspure 每 nn=2 拍输出一行，与真值比较要 trj.avp(2:2:end,:) 对齐，否则错位比较。
6. 随机游走破坏确定性对拍：imuadderr 的 randn 让每次运行结果不同——双轨/对比必须关掉（web=wdb=0）。
7. 误差注入的单位：imuerrset 参数是 °/h、µg、°/√h、µg/√Hz，内部乘 glv.dph/glv.ug 换算——传错单位误差差 1000 倍。
8. q 未归一化漂移：长时间纯积分四元数范数会漂（~1e-12 级），工程上每 N 拍归一化一次；迷你版 966 s 内影响可忽略，但长航时要处理。

九、自测题¶

1. test_SINS.m 的误差注入和 15 篇的"故障注入"是什么关系？软故障的本质是什么？
2. inspure 的高度阻尼选项里 'Z' 和 'f' 差在哪？为什么 test_SINS 用 'Z'？
3. insupdate 六步里，哪一步对应科普 03 的比力方程？哪一步对应科普 09 的位置微分？
4. 为什么说 trjsimu（P1）和 insupdate（P2）互逆？自洽性测试验证的是什么？
5. 966 s 纯惯导的误差主导项是什么（陀螺零偏还是初始 yaw 误差）？用 gen_sins 的数字说明。
6. 高度固定为什么几乎不影响水平误差？这说明了什么？
7. 迷你链路 vs PSINS 原版：水平误差末点 935.9 vs 936.0 m，差异可能来自哪些简化？

十、可复现：跑通本篇验证脚本¶

本篇数值与可视化（迷你链路自洽 + 误差传播 + 高度对比 + 绘图）来自 assets/gen_sins.py（Python）与 assets/gen_sins.m（MATLAB）双轨脚本，确定性（无随机），两轨逐数字一致。

# Python（只需 numpy，96600 步解算，约 10-30 s）
python docs/惯性导航/assets/gen_sins.py

# MATLAB（无工具箱依赖）
cd docs/惯性导航/assets
gen_sins

参考体系

• PSINS：demos/test_SINS.m（本篇拆解对象）、base/base2/inspure.m（纯惯导框架+高度阻尼）、base/base1/insupdate.m（机械编排六步）、base/base1/cnscl.m（圆锥/划桨）、base/base1/earth.m（地球参数）、base/imu/imuerrset.m+imuadderr.m、base/base2/avperrset.m+avpadderr.m、base/base1/bhsimu.m（高度参考）
• 科普系列对照：03 比力方程（insupdate ④）、07 圆锥效应/四元数更新（①⑥）、09 位置微分/误差传播（③⑤ 与全篇）、15 故障注入（误差注入体系）
• 本篇验证：assets/gen_sins.py（Python）与 assets/gen_sins.m（MATLAB）双轨——迷你链路 + 自洽性 + 误差传播 + 与 PSINS 对拍（935.9 vs 936.0 m）；两张 SVG（机械编排_数据流.svg、误差传播_路径.svg）
• 上一篇：拆解 PSINS ①：test_SINS_trj.m——数据母版（P1 造数据，本篇消费）
• 下一篇：拆解 PSINS ③：test_DR.m——航位推算（drupdate），"航向×里程"的第二条独立定位链
• 系列首页：惯性导航与惯导解算 · 自学科普系列