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

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

🧩 函数作用一句话¶

初始化SINS结构体，装配初值与默认参数。

📐 数学原理 / 物理意义¶

① ins结构体字段总览¶

② avp2qvp / vpverify 的数据一致性检查¶

在组合导航中，avp(attitude-velocity-position)是核心9维状态向量。PSINS约定：

avp = [ att(3); vn(3); pos(3) ]
    = [ pitch; roll; yaw;  V_E; V_N; V_U;  lat; lon; hgt ]

• 顺序严格: att→vn→pos，共9维；pos是[lat;lon;hgt]而不是[x;y;z]地心地固坐标
• 单位统一: att用rad(非deg)，lat/lon用rad(非deg)，hgt用m
• vpverify() 函数会检查 vn 的量级(≈0~1000 m/s)、pos的lat∈[-π/2,π/2]、lon∈[-π,π]等

③ att → quaternion 的转换 (a2qua → qnb)¶

姿态角→四元数使用 a2qua() 函数，采用 ZYX(航向-横滚-俯仰) 旋转顺序：

先按Z轴转航向yaw → 再按Y轴转横滚roll → 最后按X轴转俯仰pitch

对应公式（见 /workspace/psins/base/base0/a2qua.m:23-31）：

att2 = att/2;  [sp,sr,sy] = sin(att2);  [cp,cr,cy] = cos(att2);
q0 = cp*cr*cy - sp*sr*sy
q1 = sp*cr*cy - cp*sr*sy
q2 = cp*sr*cy + sp*cr*sy
q3 = cp*cr*sy + sp*sr*cy

确保 q0>0（即实部为正），保证四元数双值唯一性。

④ 为什么初始化 Kg=I₃, Ka=I₃, eb=0, db=0 这些默认量级¶

• Kg/Ka=单位阵：标度因子误差默认"已理想校准"，即标称值=真值。后续KFsysclbt标定才会修改为非对角矩阵。
• eb=0 (≈0.001°/h量级)：光纤陀螺典型零偏约 0.001~1°/h；如果后续要加入 imuadderr(imu, eb, db)，可在外部覆盖。
• db=0 (≈100μg量级)：MEMS加速度计零偏约 100μg~1mg，战术级约 10~50μg。
• tauG/tauA=inf：随机游走模型中 inf 表示零偏为常值（一阶马尔可夫退化为常值偏置）。

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

🔍 断点调试建议¶

在 test_SINS.m 里调完 insinit 后，在Workspace里点开ins结构体看字段：

1. ins.qnb 应该是4维归一化：
2. ‖qnb‖ = 1（误差<1e-15）
3. q0≈1，q1~q3≈sin(θ/2)小角度量级
4. 若 θ=1°，|q1~q3| ≈ 0.0087 rad
5. ins.Cnb 应该是3×3正交：
6. det(Cnb) = 1.0（误差<1e-14）
7. Cnb·Cnbᵀ ≈ I₃（各元素误差<1e-14）
8. ins.eth.RMh / ins.eth.RNh 应该≈6.4×10⁶ m量级：
9. WGS84赤道半径Re=6378137m
10. 在中纬度 RM≈RN≈6.38e6 m
11. ins.nts：
12. 若IMU=100Hz双样→ts=0.01→nts=0.02
13. 若IMU=200Hz双样→ts=0.005→nts=0.01

❌ 初学者最容易踩的坑¶

坑1：avp顺序/维度错了¶

症状：avp写成 [vn;att;pos] 或者 pos 填 [x;y;z] ECEF坐标，导致 insinit 一运行就炸，qnb出现NaN，ethinit报错纬度超出[-π/2,π/2]。 规避：永远记死顺序 [pitch; roll; yaw; VE; VN; VU; lat; lon; hgt]，9维，前3姿态、中3速度、后3位置(lat/lon用rad，hgt用m)。

坑2：att用deg不是rad¶

症状：a2qua输入的att是度数(如30°直接传30)，而a2qua内部sin/cos按rad计算。结果qnb的q0≈0.15而不是≈0.9999，姿态全错57.3倍。 规避：所有角度在传进PSINS函数之前一律 ×deg（即π/180）。调试时看 ins.att(1)：如果>3.14肯定是deg没转。

坑3：ts / nts 与IMU实际数据不匹配¶

症状：后面 insupdate 跑完，vn积分误差超大，或 cnscl 子步计数报维度错。 规避：insinit的ts = IMU一行数据对应的"双样"时间。例如imu以100Hz存一行(Δt=0.01s双样)，ts就传0.01。nts会自动变成2×ts=0.02子步。不要传成单样周期。

🎯 配套练习¶

练习题目： 自己构造 avp=[30*deg,0,90*deg; 100;0;0; 34*deg,108*deg,0]（即俯仰30°/横滚0/航向90°，东向速度100m/s，位置西安34°N/108°E/高度0），调用：

ins = insinit(avp, imuerrset(1,10,0,0), 5, 0, 0, 0);

再手动用 a2qua 算出 qnb：

att0 = avp(1:3);
qnb_manual = a2qua(att0);

对比 ins.qnb 与 qnb_manual 的误差：

max(abs(ins.qnb - qnb_manual))  < 1e-12

扩展验证：再从qnb反算Cnb，手动验证 det(Cnb)=1、Cnb·Cnb'=I。