07_kfinit_153 逐行注释 Wiki (PSINS)¶

配套源码: kfinit.m 关联源码: kfhk.m / kfinit0.m / kfsetting.m / etm.m 所属层级: L3 组合导航 SINS/GPS松组合 前置依赖: 00_glvf / 03_insupdate / 08_kffk / 09_kfupdate 学习目标: 读完回答3个问题

1. 15状态向量 \(\boldsymbol{X}=[\phi^T,\delta v_n^T,\delta p^T,\varepsilon_b^T,\nabla_b^T]^T\) 的索引编号1~15分别对应什么？etm.m 的F阵分块与此顺序如何对应？
2. P0对角线上 10″²、(0.1m/s)²、(0.01°/h)² 这些量级的工程依据是什么？单位如何从 deg/arcsec/°/h 换算到 SI 单位 rad/(rad/s)/(m/s²)？
3. 对于 case 153，kf.Hk 为什么是 [zeros(3,6), eye(3), zeros(3,6)]？如果把第7~9列与第1~3列写反了，会出现什么现象？

🧩 函数作用一句话¶

根据 psinsdef.kfinit 标志位（153=15状态+3位置观测）对 SINS/GPS 松组合卡尔曼滤波器进行初始化：设定系统噪声协方差 Qt、观测噪声协方差 Rk、初始均方误差阵 Pxk、观测矩阵 Hk，并调用 kfinit0 填充所有其余 KF 结构字段（Pmin/Pmax、adaptive、pconstrain等开关）。

📐 数学原理 / 物理意义¶

1. 15状态定义（状态顺序编号表）¶

PSINS 中 SINS/GPS 松组合 15 维误差状态向量严格定义为：

etm.m 分块对应（见 etm.m:67-70）：

行/列的大顺序：姿态φ(3) → 速度δv(3) → 位置δp(3) → 陀螺零偏εb(3) → 加计零偏∇b(3)。顺序错任何一个都会导致 F×P、P×Fᵀ、H×P、K×残差 全部维度对不上。

2. P0 初值为什么是这些量级？¶

初始协方差阵 \(P_0 = diag(\sigma_1^2, \sigma_2^2, \dots, \sigma_{15}^2)\) 的每条对角线工程含义：

3. Q 阵驱动噪声：ARW/VRW 的量级换算¶

SINS 的连续系统噪声 \(w(t)\) 来自陀螺角度随机游走（ARW）和加计速度随机游走（VRW）：

常用单位换算表（使用 glv 常量）：

PSINS kfinit(153)中具体写法：

kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;

4. Rk 观测噪声：GPS位置各向不同¶

松组合位置观测模型 \(\boldsymbol{Z}_k = \boldsymbol{p}_{SINS} - \boldsymbol{p}_{GPS}\)，其协方差：

工程依据： - 水平方向 GPS C/A 码单点定位 ≈ 3~5m (1σ)；DGPS/SBAS ≈ 0.5~1m；RTK ≈ 1~5cm - 高度方向 GDOP 放大 ≈ 1.5~2×，所以 5m 对应水平 3m - 如果第7~8状态（δL, δλ）用 rad，而 GPS 观测给的是米，需要先通过 poserrset 把米转成 rad（除 RMh/RNhcosL），kfsetting.m 第51行 Rk(4:5) = Rk(4:5)/glv.Re 就是这个处理。

📝 逐行注释 (4列: 行号 / 原代码 / 中文注释 / 公式备注)¶

附：kfinit0.m 中的关键字段（由第135行自动设置）¶

🔍 断点调试建议¶

调试1：初始化后P对角线单位是否正确¶

1. 在 /workspace/psins/demos/test_SINS_GPS_153_kfstat.m 第18行 kf = kfinit(...) 后立即打断点；
2. 命令窗口执行：

   format shortE
   diag_sigma = sqrt(diag(kf.Pxk))
   % 换算成人类可读单位：
   phi_sigma_deg = diag_sigma(1:3)/glv.deg * 3600  % rad → arcsec
   eb_sigma_dph = diag_sigma(10:12)/glv.dph        % rad/s → deg/h
   db_sigma_ug  = diag_sigma(13:15)/glv.ug         % m/s² → ug
   

3. 预期结果：
4. phi_sigma ≈ 10 arcsec，即约 4.848e-5 rad；
5. eb_sigma ≈ 0.01 deg/h，即约 4.848e-8 rad/s；
6. db_sigma ≈ 10 ug；
7. 若差一个 3600、180/π、或 1e6 的倍数，说明 avperrset / gabias 调用时单位没乘 glv.sec / glv.dph / glv.ug。

调试2：第一次时间更新后 P(1,1) 增量量级验证¶

1. 在第30行 kf = kfupdate(kf); 第1次执行完后断点；
2. 比较 kf.Pxk(1,1) 前后差值 ΔP11；
3. 预期量级：

若 ARW=0.1°/√h = 0.1×glv.dpsh rad/√s，则 Qt(1,1)=(0.1dpsh)²≈(4.85e-8)²≈2.35e-15 rad²/s；乘以nts=0.02s得 ΔP≈4.7e-17，所以 P(1,1) 从 (10″)²=2.35e-9 增加约 4.7e-17，增量是P0的~5e-8倍，几乎不变。 4. 如果发现 P(1,1) 一跳变几倍，那就是 Qt 里 ARW 单位换算错了（比如写成 0.1 dph 而非 0.1 dpsh，差 √3600≈60 倍 → 平方差3600倍）。

调试3：Hk矩阵的位置列是否真在第7~9列¶

断点下执行 spy(kf.Hk) 看稀疏图：3行×15列矩阵的第(1,7)(2,8)(3,9)位置必须是1，其余为0。如果发现1出现在(1,1)(2,2)(3,3)，说明状态顺序与Hk错位了（比如误以为φ是第7~9），那观测残差会直接把姿态当位置观测，KF会完全失效。

❌ 初学者最容易踩的坑¶

坑1：15状态顺序搞混（φ/εb混序 或 δp与δv颠倒）¶

现象：kfupdate运行不报错，但kf.xk(1:3)姿态估计极大（几十度），位置误差几十km不收敛，残差 Z-Hk*xk 一开始就是巨大值。 典型错误顺序： - ❌ 错误：[φ(3); εb(3); δv(3); δp(3); ∇b(3)] → 后12号状态全错位 - ✅ 正确：[φ(3); δv(3); δp(3); εb(3); ∇b(3)] → etm.m分块顺序严格对应 验证方式：第1次kffk后，size(kf.Phikk_1)=15×15，且 sum(diag(kf.Phikk_1)) ≈15（F阵对角≈I，短Δt下离散化Φ≈I+FΔt对角≈15+trace(F)Δt）。

坑2：Pmin 设置太高（≥P0）导致 P 永远缩不下去¶

现象：KF跑很久后 sqrt(diag(kf.Pxk)) 基本不变，和P0一样大，状态估计xk虽然小但不是靠观测收的，而是"盲猜"。 原因：kfsetting里Pmin默认值如果写得和P0同量级（或者pconstrain=1时设置错），每次更新完 Pxk = max(Pxk, Pmin) 会把刚被观测缩小的P又"顶回去"。 正确范围：通常 Pmin ≈ P0 × 1e-3 ~ 1e-2。比如P0(φ)=10″，则Pmin(φ)=0.01″~0.1″，是物理上能收敛到的极限。

坑3：KF状态维度15与etm.m的F阵维度不一致¶

现象：kffk返回时直接报错 Inner matrix dimensions must agree，或者 Matrix dimensions must agree。 排查步骤： 1. 检查 psinsdef.kffk 是否=15（kfinit:25设了，但之前如果跑过别的demo可能还留18/19等）； 2. etm.m 第16行默认 n=15，但kffk如果显式传n=18等就不对； 3. kf.Hk 维度：3×15对，但 Hk×Pxk×Hk' 报错说明 Pxk 不是15×15 → 往回找 Pxk 初始化错在哪。

坑4：观测噪声Rk单位没把"米"换成"rad"对经纬度¶

现象：纬度/经度误差收敛极慢（比预期慢1e6倍），高度正常。 原因：δL和δλ的单位是rad，而rk输入是米，需要 rk(1:2) = rk(1:2)/Re 预处理。kfsetting.m第51行 Rk(4:5) = Rk(4:5)/glv.Re 就是干这个。而 poserrset 函数也会自动转。如果手动写 rk=[3;3;5] 忘了除Re，Rk(1,1)=9 rad²≈(1000km)²，GPS观测几乎被KF"完全不信"。

坑5：adaptive=1开启但初始Q/R数量级差太远¶

现象：KF前几十秒Q阵或R阵被Sage-Husa自适应"跑飞"，变成0或Inf。 原因：Sage-Husa自适应需要初始Q、R是同一量纲体系下大致正确的，否则Q太小→自适应补Q但一步调太大；R太大→自适应把R调0→残差=0→增益无穷大。 稳妥使用：前100秒 kf.adaptive=0 让P先自然收敛到合理值，之后再 kf.adaptive=1 开启自适应。

🎯 配套练习¶

练习1：把P0(1:3)从10″改到10°，观察收敛速度变慢多少¶

1. 打开 /workspace/psins/demos/test_SINS_GPS_153_kfstat.m；
2. 第14行改为：

   davp0 = avperrset([10;-10;10]*glv.deg, 0.1, [1;1;3]);  % 姿态初误差10°（×10000倍）
   

3. 第17-18行保持rk不变，跑完整仿真；
4. 对比原版（10″初误差）和修改版（10°初误差）：
5. 画出 φ_x(1号状态) vs t 两条曲线同一图上；
6. 原版可能在前10秒φ就被观测拉到<1′；
7. 修改版因为P0的φ分量大10⁸倍，KF前几秒几乎完全相信模型而非观测（因为 K=PHᵀ(HPHᵀ+R)⁻¹，H对应φ的部分=0，只能通过F阵耦合到δp才能被位置观测修正），可能要 300~1000秒 才能把10°误差逐步"传"到位置再收回来。
8. 思考题：如果把GPS位置观测换成156速度+位置双观测，收敛速度会加快多少倍？（提示：Hk的δv对应位置非零，F中φ→δv是直接耦合Mva，比φ→δp快一个积分阶次）。

练习2：改变Rk量级观测权值，画出位置误差 vs Rk的曲线¶

1. 固定P0和Qt，分别设 rk = poserrset([1;1;2])、[3;3;5]、[10;10;20]、[30;30;50] 共4组；
2. 每组跑完取最后60s内位置RMSE平均 mean(sqrt(sum(xkpk(end-600:end,7:9).^2,2)))（注意xkpk里是xk还是diag(Pxk)，看列索引）；
3. 理论预期：当Rk≥P0×(HPHᵀ)量级后，KF"不信观测"→误差趋近纯SINS发散值；当Rk→0时，误差≈纯GPS精度+系统不可观部分（姿态/零偏由耦合定），不会随Rk减小继续下降。把4个(Rk_水平, RMSE_水平)点画在loglog图上，看是否存在一个"拐点"对应 Rk≈1~3m 正好是GPS单点精度区间。