inspure.m 逐行注释 Wiki (PSINS)¶

配套源码: inspure.m 所属层级: L1纯惯导 前置依赖: 00_glvf / 01_earth / 03_insupdate / 04_insinit 学习目标: 读完能回答3个问题

🧩 函数作用一句话¶

批量跑纯惯导循环，存轨迹并可选阻尼/外参约束。

📐 数学原理 / 物理意义¶

① 纯惯导主循环结构：KF步×IMU子样的分组关系¶

inspure主循环不是一行IMU跑一次insupdate，而是按KF周期分组批量跑：

时间轴：   ├────── 1个KF更新步 = nn行IMU数据 ────────┤
IMU行：   [k行  k+1行  ...  k+nn-1行] 共nn行
           ↓ 一次性取出 wvm=imu(k:k1,1:6)
           ↓ 调 insupdate(ins, wvm) 内部再拆成 nsub子步
           ↓ 积分完得到本KF步结束时刻的 avp
           ↓ 存入 avp(ki,:) 一行

关键参数关系（由 nnts(glv.ns, imu(:,end)) 返回）： | 参数 | 含义 | 典型值举例 | |------|------|-----------| | ts | IMU相邻两行的间隔(双样周期) | 0.005 s → IMU=200Hz | | nts | 单个INS子步更新间隔 | 0.01 s (双样=2子步) | | nn | 1个KF步=多少行IMU | 10 → KF=20Hz (200/10) | | 等价nts2ins | 1次KF更新=多少次INS子步 | nn×nsub=10×2=20次 |

⚠️ 代码里变量名是 nn（不是nts2ins），但语义等价：1个KF步内包含的IMU行数。

② 为什么 openloop=1 时纯惯导不反馈¶

vp_fix='O' 模式（行55-58、行111-113）下设置：

ins.vn0 = zeros(3,1);   % 参考速度设为0
ins.openloop = 1;       % 开环标志=1

其物理含义： - 闭环(openloop=0)：惯导误差会在KFs更新时被反馈修正，ins状态跳变 - 开环(openloop=1)：惯导内部状态 永远不修正，纯粹靠积分走 - 纯惯导评估时用开环，才能看到SINS本身的误差发散特性（舒勒周期、傅科周期、指数发散等） - 如果用闭环跑纯惯导，vn被强行拉回vn0，就无法评估惯导算法本身了

③ avpcvt 设计：把结构体ins打包成avp数组的设计模式¶

inspure行117核心操作：

avp(ki,:) = [ins.avp; t]';   % ki为行索引，每KF步写一行

设计模式： - ins结构体：内部高效更新，包含qnb/Cnb/att/vn/pos/eth/各种临时字段(>30个字段) - avp数组(10列)：对外输出的精简轨迹

avp = [ att(3), vn(3), pos(3), t ]  即  [pitch,roll,yaw, VE,VN,VU, lat,lon,hgt, time]

📝 逐行注释 (4列表格，行号严格对齐)¶

🔍 断点调试建议¶

在主循环 k=1 第一次迭代（处理最开头的几行IMU）时抓以下对比：

1. 进入循环前（第78行 break）：抓 ins.avp，此时 vn 全是初值，pos 是初值。
2. 第一次调完 insupdate 后（第81行后）：再次抓 ins.avp，应出现：
3. ins.vn(3) 变化量 ≈ g₀×Δt ≈ 9.796m/s² × (nn×nts) ≈ 9.796 × 0.2 ≈ 2 m/s（若nn=10，nts=0.02）
4. ins.pos(3) 变化量 ≈ 初VU×Δt + ½g₀Δt² ≈ 0 + 0.5×9.8×0.04 ≈ 0.2 m

5. vn/pos的变化量应是纯积分方向：向下(h减小？注意VU向上为正，g指向-U，所以VU(3)会负向增大即下坠)

6. 循环跑完后检查 avp 数组：

7. size(avp,1) ≈ fix(size(imu,1)/nn)，维度差nn倍
8. avp(1,10) = imu(nn,7)（第1个KF步结束时刻=第nn行IMU的时间戳）

❌ 初学者最容易踩的坑¶

坑1：以为 nn(nts2ins) 必须=1¶

症状：看到 for k=1:nn:len 以为步长必须是1，把nn强行设成1，结果insupdate一次只处理1行，速度慢10倍甚至子步计数报错。 规避：nn=10是正常配置！意思是IMU=200Hz(ts=0.005s双样，行间隔=5ms)，KF更新=20Hz，1个KF步=10行IMU=0.05s。inspure每次取10行，内部insupdate再拆成20个子步，完全正确。不要乱改nn。

坑2：画图横坐标用成imu.time而不是avp(:,10)¶

症状：plot(imu.time, avp(1:length(imu),1))，报维度不一致（avp比imu短nn倍），或者强行截取后图形完全错位。 规避：avp的时间轴是 avp(:,10)，每一行对应第nn/2nn/...行IMU的时刻，画图永远是 plot(avp(:,10), avp(:,1:3))。如果要把avp和gps对比，需要用imugpssyn做时间对齐，不能直接索引。

坑3：href传错，高度约束模式搞混¶

症状：传 href='O' 以为是自由，结果ins.vn全被置0+openloop=1，输出速度永远=0，还以为惯导积分为0了。或者 href=100 传一个标量高度，以为是固定hgt=100m，但实际默认模式'H'只用pos0(3)做高度。 规避：需要纯惯导无约束用 href='f'；需要高度固定用默认 href='H' 或不传；开环纯惯导评估误差发散才用 href='O'。

🎯 配套练习¶

练习题目： 复制 test_SINS_trj 生成的 trj/imu 数据（或用 trjsimu 自己生成一段60s静基座数据）：

步骤A：手写纯惯导循环版

nn = 1;  % 简化：一次处理1行
len = length(imu);
avp_manual = zeros(len, 10);
ins2 = insinit(avp0, ts);
for k = 1:len
    ins2 = insupdate(ins2, imu(k,1:6));
    avp_manual(k,:) = [ins2.avp; imu(k,7)]';
end
pos_final_manual = avp_manual(end,7:9);

步骤B：调用inspure函数版

avp_func = inspure(imu, avp0, 'f', 0);  % 'f'高度自由, isfig=0
pos_final_func = avp_func(end,7:9);

步骤C：对比数值等价

err_pos = norm(pos_final_manual - pos_final_func)

检查 err_pos < 1e-6（即微米级），两者应该数学等价，因为都是insupdate积分的直接结果。如果差得大，检查nn和kfsub设置是否一致，或手动循环里少加了某步约束。