PSINS 逐行注释 Wiki（初学者版）¶
本系列在 wiki 里的位置与用法
- 定位：这是「
04_拆解PSINS篇」下的函数级逐行拆解子系列（显微镜视角），与同篇的 P1–P4（按测试脚本拆的 demo 级「望远镜」视角）互补。先看 P 系列了解整条 pipeline，再读本系列抠 PSINS 源码细节。- 前置数学：读本文档前建议先过一遍 00_前置数学基础（坐标变换、矢量叉乘、概率/协方差）。
- 关于文中
[xxx.m]链接：已指向本仓库随附的 PSINS 源码副本（psins260314），点击即可对照阅读。行号锚点（如#L37）在静态文件下不跳转，请按函数名在文件中检索。
目标: 配合严恭敏老师的 PSINS 工具箱（站内副本见
psins260314），为其中 11 个最核心 .m 函数提供逐行中文注释 + 数学公式 + 断点调试建议 + 坑点 + 验证练习题。📌 PSINS 版本说明：本 Wiki 基于随附的 PSINS 源码副本
psins260314（版权日期 2024-04-25，psins.m L13）。如果你使用的是psins260705等更新版本，核心函数（insupdate/cnscl/earth 等）的算法逻辑一致，个别行号可能因新增代码而偏移，请对照源码调整。insupdate.m 的关键修订标注在代码注释中（如 L28 和 L38 的2014-11-30），这些修订自 2014 年起已稳定。
🚦 学习路线图（严格按这个顺序读！）¶
| 阶段 | 文件编号 | 文件名 | 对应源码 | 一句话定位 |
|---|---|---|---|---|
| L0 · 基础 (1天) | 00a | INS 基础概念入门 ⭐ | — | 新手扫盲篇：圆锥/划桨/舒勒/陀螺加计指标全谱，必须在 00 之前读 |
| L0 · 基础 | 00b | 坐标变换与旋转矩阵 ⭐ | — | 第一步坑：东北天手性/主动vs被动/\((\phi-\theta)\)推导/被动=主动转置/三步法写旋转矩阵，读完不再怕 \(-\sin\theta\) 符号 |
| L0 · 基础 | 00 | glvf 逐行注释 | glvf.m | 所有全局常量和单位换算，全局地基！ |
| L0 · 基础 | 01 | earth 逐行注释 | earth.m | 地球曲率/地转角速度/重力/重力异常 全部入口 |
| L0.5 · 航姿 AHRS (3天) | 11 | Mahony 互补滤波 ⭐ | MahonyUpdate.m | 做航姿板必看！ PI 互补滤波，简单易懂，先上这个验证硬件 |
| L0.5 · 航姿 AHRS | 12 | QEKF 四元数 EKF ⭐ | QEAHRSUpdate.m | Mahony 升级版：7 状态 EKF，精度更高，显式估零偏 |
| L1 · 纯惯导 SINS (1个周) | 02 | cnscl 逐行注释 | cnscl.m | 圆锥+划桨+旋转补偿，IMU数据进入SINS的第一扇门 |
| L1 · 纯惯导 SINS | 03 | insupdate 逐行注释⭐⭐⭐ | insupdate.m | 最核心！ 姿态-速度-位置三连更新全部实现，48行=SINS全部算法 |
| L1 · 纯惯导 SINS | 04 | insinit 逐行注释 | insinit.m | ins结构体初始化/字段全览/默认参数 |
| L1 · 纯惯导 SINS | 05 | inspure 逐行注释 | inspure.m | 纯惯导主循环封装，如何批量跑n次insupdate |
| L2 · 航位推算 DR (3天) | 06 | drupdate 逐行注释 | drupdate.m | 陀螺方位+里程计→位置，无舒勒线性发散 |
| L3 · 组合导航 KF (2个周) | 07 | kfinit(153) 逐行注释 | kfinit.m | 15状态P0/Q0/R0初始化，SINS/GPS松组合 |
| L3 · 组合导航 KF | 08 | kffk 逐行注释⭐⭐ | kffk.m | 连续误差方程F阵离散化，时间更新核心1 |
| L3 · 组合导航 KF | 09 | kfupdate 逐行注释⭐⭐⭐ | kfupdate.m | KF五公式全部实现，卡尔曼滤波的全部内容就在这103行 |
| L3 · 组合导航 KF | 10 | kffeedback 逐行注释⭐⭐ | kffeedback.m | ESKF灵魂：把误差状态反馈回主状态+清零 |
📦 文件列表（绝对路径）¶
- ./00a_INSBasics.md — 新手扫盲前置阅读：圆锥/划桨补偿推导 + 舒勒振荡物理直觉 + 陀螺加计指标全谱（ARW/VRW/零偏/SF）+ Allan方差 + 指标→位置误差换算例题
- ./00b_CoordRot.md — 坐标变换第一步：东北天/北东地手性验证 + 主动vs被动变换 + \((\phi-\theta)\)投影推导 + 被动=主动转置证明 + 三步法写旋转矩阵 + 配套双滑块交互可视化
- ./00_glvf.md — WGS84参数、单位换算、圆锥系数表、默认初始位置
- ./01_earth.md — wnie/wnen/wnin/wnien/gcc 地球参数 36行一行不漏
- ./02_cnscl.md — 圆锥/划桨/旋转 不可交换性补偿 87行
- ./03_insupdate.md — PSINS皇冠之珠，姿态/速度/位置三连更新48行
- ./04_insinit.md — SINS结构体ins字段全览+初始化
- ./05_inspure.md — 纯惯导主循环，子步解算+avp打包存储
- ./06_drupdate.md — 航位推算DR，用于SINS/DR松组合/OD辅助
- ./07_kfinit_153.md — 15状态ESKF初始化P0/Q0/R0/Hk
- ./08_kffk.md — F阵→Φ离散化，expm与一阶近似的取舍
- ./09_kfupdate.md — 卡尔曼滤波完整实现，时间/量测/自适应/对称化/P约束
- ./10_kffeedback.md — ESKF和EKF的本质区别，反馈清零机制
- ./11_Mahony.md — Mahony 互补滤波 AHRS：PI 控制器推导 + 传递函数 + 叉积误差推导 + 自适应详解
- ./12_QEAHRS.md — QEKF 四元数 EKF AHRS：7 状态 EKF + \(\Phi\)/\(H\)/\(R\) 矩阵完整推导 + 自适应 R
🎯 使用方法（严格三步走）¶
Step1 先读 Wiki 数学原理：每个 wiki 的"📐 数学原理"先读一遍，公式对照符号表看明白。不要一上来就跑代码，数学关系没搞清楚，断点看数值也不知道对不对。
Step2 设断点跑 demo：打开 test_SINS.m / test_SINS_GPS_153.m，按 Wiki 里的"🔍 断点调试建议"在指定行设断点，F10 步进，观察变量的实际数值和 wiki 说的数量级是否一致。
Step3 做配套练习：每个 wiki 最后"🎯 配套练习"必须亲手敲一遍跑通，这是最有效的巩固方式，不做题=没学会。
配套的 demo（PSINS 自带）路径： - 航姿 AHRS（做航姿板从这里开始）: test_AHRS_Mahony.m（互补滤波，简单易懂）→ test_AHRS_QEKF.m（四元数EKF，精度更高） - 核心函数: MahonyInit.m / MahonyUpdate.m / QEAHRSInit.m / QEAHRSUpdate.m - 纯惯导入门: test_SINS_trj.m → test_SINS.m - DR 基础: test_DR.m / test_SINS_DR.m - 组合导航入门: test_SINS_GPS_153.m
🧪 配套 civil_wiki 学习资料交叉验证¶
本 Wiki 与 GitHub 仓库 wild-civil/civil_wiki/惯导 assets 完美互补，建议两组代码用同一组数据跑一遍做交叉验证，保证两边理解一致：
| 本 Wiki 文件 | civil_wiki 对应代码 | 交叉验证方式 |
|---|---|---|
| 03_insupdate.md | pure_ins_demo.c | 同一组 wm/vm（比如 100Hz 直线 10s 零偏），两边各跑一遍，qnb/vn/pos 逐位对照误差 <2% |
| 02_cnscl.md | attitude_update_demo.c | 5°锥角双子样，rv2q(qerr_phi) 误差分别 <1e-6 rad |
| 07 + 08 + 09 + 10 四件套 KF | gen_ekf_vs_eskf_py.py | 4状态小车模型（位置/速度/加速度零偏 2 组），MATLAB 跑 kfinit + kffk + kfupdate + kffeedback vs Python 独立实现，两边 xk 最终 RMSE <1e-9 |
🧭 统一术语表（贯穿所有 Wiki）¶
| 符号 | 中文名 | 单位（PSINS内部约定） | 常见下标/后缀 |
|---|---|---|---|
| qnb / Cnb / att | b系到n系的四元数/方向余弦阵/欧拉角(roll,pitch,yaw) | — / — / rad | q⁻先验 / q⁺后验 |
| vn | n系速度 [vn;ve;vd]（北东地） | m/s | δvn = vn_nom − vn_true |
| pos | 位置 [L;λ;h]（纬/经/高） | rad / rad / m | δL, δλ = rad；δh = m |
| φ | 平台失准角 [φx;φy;φz] | rad | 小误差量，ESKF前3态 |
| eb (εb) | 陀螺三轴零偏 | rad/s | 第10~12态 |
| db (∇b) | 加计三轴零偏 | m/s² | 第13~15态 |
| xk | ESKF误差状态向量（先验/后验均值） | 按分量 | 反馈后对应部分=0 |
| Pxk | ESKF误差状态协方差阵 | 分量单位² | 不清零 |
| Φk (Phikk_1) | 状态转移矩阵 Φk = e^{F·nts} | — | kffk.m 输出 |
| Hk | 量测矩阵 | — | kfhk.m 输出，153 时 Hk=[0 0 I_{3×3}] |
| Kk | 卡尔曼增益 | — | K = P⁻Hᵀ(HP⁻Hᵀ+R)⁻¹ |
| fbstr | 反馈字符 | 字符 | a/v/p/e/d/A/V/P/E/D/L/T/G/C/h/H |
| T_fb | 反馈间隔 | s | 默认1秒；=0 开环不反馈 |
| coef_fb | 比例反馈系数 | (0,1] | AR(1)渐进：T_fb/xtau 或 1 |
| avp | att + vn + pos 打包 | 混合 | [att(3); vn(3); pos(3)] = 9×1 |
重要符号约定提醒：PSINS ESKF 中
δx = x_nominal − x_true，所以反馈公式是x_corrected = x_nominal − δx_hat；不要和某些教材δx = x_true − x_nominal混用，差一个符号。kffeedback.m 里速度/位置用减号，零偏/杆臂用加号，务必严格对应 wiki 里的表格，不要凭直觉改号。