08_kffk 逐行注释 Wiki (PSINS)¶

配套源码: kffk.m 所属层级: L3组合导航 前置依赖: 00_glvf / 01_earth / 03_insupdate + 基本卡尔曼滤波概念 学习目标: 读完能独立回答3个问题

🧩 函数作用一句话¶

生成KF连续/离散状态转移矩阵

📐 数学原理 / 物理意义¶

连续时间线性误差方程 δẋ = F·δx + G·w¶

PSINS标准15状态卡尔曼滤波的状态向量定义为：

δx = [φ; δvⁿ; δp; ∇b; εb]   15×1
      3×3   3×3   3×3  3×3  3×3
     姿态  速度   位置 加计零偏 陀螺零偏

对应的连续时间误差状态方程 δẋ = F·δx + G·w 中，F阵(15×15)具有如下9×9上三角块结构简图（姿态-速度-位置的核心耦合块）：

      φ(3)   δvⁿ(3)   δp(3)   ∇b(3)   εb(3)
φ(3)  [Maa]   [Mav]    [Map]    O       Cnb
δvⁿ(3)[Mva]   [Mvv]    [Mvp]   -Cnb      O
δp(3) [ O ]   [Mpv]    [Mpp]    O        O
∇b(3) [ O ]    O        O       O        O
εb(3) [ O ]    O        O       O        O

各3×3子块物理意义： - Maa = -(ω̂ᵢₙⁿ×)：姿态误差自耦合块，地球自转角速度+牵连角速度引起的姿态漂移 - Mav：姿态→速度耦合块（即 M(1/R) 矩阵），姿态误差通过比力投影影响速度 - Map = Mp1 + Mp2：姿态→位置耦合块，由地球曲率和位置相关项构成 - Mva = (f̂ⁿ×)：速度→姿态耦合块（舒勒振荡源！），北向比力 f_N≈g 产生的 84.4min 舒勒周期 - Mvv = (v̂ⁿ×)·Mav - (ω̂ᵢₑⁿ+ω̂ₑₙⁿ)×：速度误差自耦合块 - Mvp = (v̂ⁿ×)·Map + 重力梯度项：速度→位置耦合块，含重力随高度变化修正 - Mpv = diag(1/RMh, 1/(RNh·cosL), 1)：位置微分运动学块，速度→位置直接积分关系

完整F阵生成由 etm.m 函数负责，kffk只负责组装+离散化。

离散化方法：Φ = exp(F·Δt) ≈ I + F·Δt + ½(F·Δt)²¶

矩阵指数的泰勒展开定义为：

exp(F·Δt) = Σ (F·Δt)^k / k!  = I + FΔt + (FΔt)²/2! + (FΔt)³/3! + ...

为什么PSINS只取一阶近似 I + F·Δt？

1. Δt极小：SINS惯导解算周期通常 nts=0.01~0.02s（对应IMU 50~100Hz），此时 ‖F·Δt‖ << 1
2. F阵非零元量级：姿态块≈ω_ie≈7.29e-5 rad/s，速度块≈f_N≈9.8 m/s² → (F·Δt)最大元≈0.2 m/s（对速度块），平方后≈0.04
3. 但注意：F阵元素经Δt=0.02s缩放后，(F·Δt)元≈2e-3量级，平方项≈4e-6，已低于双精度浮点舍入误差累积效应
4. 经验最优：二阶项 (FΔt)²/2 在Δt很小时，其计算引入的浮点乘法舍入误差可能反而超过其带来的精度增益
5. 高频更新补偿：SINS每0.01s更新一次，一阶截断误差O((FΔt)²)会被下一次更新的状态转移所平滑，不会累积

为什么大Δt(>0.1s)要切换到expm()？

• 当Δt=0.1s时，(F·Δt)元≈0.1量级，平方项≈0.01，相对误差~1%，已不可忽略
• 当Δt=1s时，一阶近似相对误差可达10%以上，舒勒振荡的正弦项 sin(ω_s·t) ≠ ω_s·t，必须用真实矩阵指数
• PSINS在 kffk.m:51 硬编码阈值 nts>0.1，超过就调用 MATLAB 内置 expm() 用Padé近似+缩放平方法精确计算

📝 逐行注释¶

🔍 断点调试建议¶

目标脚本: test_SINS_GPS_153.m

调试步骤：¶

1. 打开 /workspace/psins/demos/test_SINS_GPS_153.m，在 L27 kf.Phikk_1 = kffk(ins); 之前确保 kf.nts = 0.02（默认配置）
2. 在 /workspace/psins/base/kf/kffk.m L50（Fk = Ft*nts;）设断点，运行脚本
3. 第一次命中断点时，在命令行执行以下观察：

% ① 观察姿态-速度耦合块：Ft(1:3,4:6) = Mav
Ft(1:3,4:6)   % 应全≈0附近（因Mav元≈1/R≈1.5e-7 m⁻¹量级 × Δt可忽略但F阵中可见）

% ② ★核心观察：舒勒振荡源 Mva = askew(fⁿ) = Ft(4:6,1:3)
Ft(4:6,1:3)   % 非对角元素非零！
              % Ft(5,1)≈-fU≈9.8 m/s²，Ft(6,2)≈fN≈9.8 m/s²
              % 这两个就是 (f̂ⁿ×) 块，产生舒勒振荡的根源！

% ③ 验证一阶近似精度
A = Ft * 0.02;               % F·Δt
Phi1 = eye(15) + A;          % 一阶近似（PSINS实际用的）
Phi2 = expm(A);              % 精确矩阵指数
rel_err = norm(Phi1-Phi2) / norm(Phi2)   % 应 < 1e-6 !!
% 结论：nts=0.02s时一阶近似完全ok，二阶确实没必要

1. 继续运行到第二次或第三次命中（确认Ft随位置vn/fn变化），再执行：

   % 把nts调大到1s看误差暴涨
   A_big = Ft * 1.0;
   Phi1_big = eye(15) + A_big;
   Phi2_big = expm(A_big);
   rel_err_big = norm(Phi1_big-Phi2_big) / norm(Phi2_big)   % 通常 > 0.1 !!
   % 这就是为什么kffk L51要有nts>0.1切expm的分支
   

❌ 初学者最容易踩的坑¶

坑①：擅自打开二阶展开反而更差¶

很多人看到 L54 注释里有 + Fk*Fk*0.5，觉得"加了二阶肯定更准"，就把它取消注释。结果在nts=0.01s时反而出现细微的数值误差累积： - (F·Δt)² 的元素≈(1e-2)²=1e-4，乘以0.5得5e-5 - 但双精度乘法对接近0的数舍入相对误差大 - 经验：PSINS作者注释掉二阶是大量测试后的最优，不要乱改

坑②：psinsdef.kffk 维度与kfinit不一致直接崩¶

场景：kfinit 时设 kffk=15（15状态），但某次改代码时误把psinsdef.kffk设成18
后果：kffk返回的Ft=18×18，kf.Phikk_1=18×18，但kf.xk=15×1
      → 下一步 kfupdate L37 做 Phikk_1*xk 时报维度不匹配错
排查：看kfinit.m里psinsdef.kffk设置的值，必须与L21 switch的case标签完全一致

坑③：传参时nts单位搞错（秒vs毫秒）¶

有人把惯导周期20ms写作kffk(ins, 20)（毫秒思维），结果 kffk L51判 20>0.1 成立，走expm分支——矩阵指数虽然精度高但20×大了1000倍的FΔt，状态转移矩阵Φ直接变成指数爆炸型数值，KF瞬间发散。牢记：PSINS所有时间单位都是秒。

🎯 配套练习¶

练习题目：一阶近似 vs 精确矩阵指数 的相对误差验证

% 在MATLAB命令行或单独脚本中执行
%% 1) 构造随机F阵（模拟真实PSINS的Ft量级）
rng(42);  % 固定种子可复现
Ft = randn(15,15) * 0.01;   % 元素~1e-2量级，模拟F阵实际非零元大小
nts = 0.02;                 % SINS标准解算周期 20ms

%% 2) 分别计算两种离散化
Phi1 = eye(15) + Ft * nts;           % PSINS默认一阶近似
Phi2 = expm(Ft * nts);               % 精确矩阵指数（MATLAB内置）

%% 3) 计算Frobenius范数相对误差
rel_err = norm(Phi1 - Phi2, 'fro') / norm(Phi2, 'fro')
% 预期输出：rel_err ≈ 1e-5 ~ 1e-6 量级  →  一阶近似完全OK

%% 4) 把Δt调大到1秒，再看误差暴涨
nts_big = 1.0;
Phi1_big = eye(15) + Ft * nts_big;
Phi2_big = expm(Ft * nts_big);
rel_err_big = norm(Phi1_big - Phi2_big, 'fro') / norm(Phi2_big, 'fro')
% 预期输出：rel_err_big ≈ 0.05 ~ 0.5  →  误差>5%，必须用expm!

%% 5) 思考题：临界Δt在哪里？
% 写个for循环扫nts从0.001到1.0，画rel_err-nts曲线
% 观察：当nts≈0.1时rel_err刚好跨过1e-3量级——这就是L51阈值0.1的来源！

进阶挑战：把Ft换成真实的。取test_SINS_GPS_153.m仿真中间某时刻的ins结构体，真实调用Ft=etm(ins,15)，重复上面计算，验证结论一致。