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

配套源码: insupdate.m 所属层级: L1纯惯导 前置依赖: 必须先读 00_glvf.md（四元数/旋转辅助函数）、01_earth.md（地球模型eth）、02_cnscl.md（圆锥划桨补偿） 学习目标: 读完后你应能回答哪3个问题 1. SINS机械编排"三连更新"（姿态-速度-位置）的顺序为什么是速度→位置→姿态？调换顺序会有什么精度损失？ 2. 速度更新公式中 ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc 每一项的物理含义是什么？为什么要用rotv对比力做wnin半步旋转？ 3. 位置更新的Mpv矩阵 [0, 1/RMh, 0; 1/(cl·RNh), 0, 0; 0, 0, 1] 各元素怎么来的？为什么Mpv(2,1)必须除以cos(L)？

🧩 函数作用一句话¶

捷联惯导三连更新：姿态速度位置

📐 数学原理 / 物理意义¶

1. 坐标系与基本符号约定¶

在纯惯导（SINS）机械编排中，使用三个右手直角坐标系：

关键角度符号： - 纬度 \(L\)（Latitude）：当地水平面法线与赤道面夹角，单位rad，范围[-π/2, π/2] - 经度 \(\lambda\)（Longitude）：本初子午面到当地子午面夹角，单位rad，范围[-π, π] - 高度 \(h\)（Altitude）：沿椭球法线到参考椭球面距离，单位m

地球基本参数（WGS84）： - 地球自转角速率：\(\omega_{ie} = 7.2921151467 \times 10^{-5}\) rad/s - 椭球长半轴：\(a = 6378137.0\) m - 椭球扁率：\(f = 1/298.257223563\) - 子午圈曲率半径 \(R_M\)（沿经线）、卯酉圈曲率半径 \(R_N\)（沿纬线）：

其中 \(e^2 = 2f-f^2\) 为第一偏心率平方。含高度时：\(RMh = R_M + h\)，\(RNh = R_N + h\)，单位m。

2. SINS机械编排三连更新完整公式¶

设SINS更新周期为 \(T_s = n \cdot \Delta t\)（n子样），当前时刻 \(t_k\)，下一时刻 \(t_{k+1}=t_k+T_s\)。记半步时刻 \(t_{k+1/2} = t_k + T_s/2\)（中点积分时刻）。

已知状态：\(t_k\) 时刻姿态四元数 \(\mathbf{Q}_{nb}(t_k)\)、速度 \(\mathbf{v}^n(t_k)\)、位置 \(\mathbf{p}(t_k)=[L_k, \lambda_k, h_k]^T\)；当前周期IMU增量（经cnscl补偿后）：等效旋转矢量 \(\Delta\boldsymbol{\phi}_m\)（rad）、等效速度增量 \(\Delta\mathbf{v}_{bm}\)（m/s）。

三连更新顺序：为什么是"速度→位置→姿态"？¶

教科书上通常写"姿态→速度→位置"（先更新姿态，用新姿态投影比力，再算速度，再算位置）。但 PSINS 的实际代码顺序是反过来的：

💡 一句话直觉：姿态最后更新，是因为新姿态对速度和位置的计算没有用（速度用的是 \(t_k\) 的旧姿态）。如果先更新姿态，反而要担心"新姿态 vs 旧姿态"混用的问题。所以把姿态更新推到最后，让速度和位置都用旧姿态算完，再统一换新。

一个带数字的例子：静止状态下走一步¶

假设载体静止，IMU 无误差，\(T_s = 0.1\) s，初始 \(v=0\)，\(h=0\)，\(L=34°\)。

• 陀螺输出 ≈ 0（静止，只有地球自转 \(7.29 \times 10^{-5}\) rad/s，可忽略）
• 加计输出 \(\mathbf{f}^b \approx [0, 0, -9.8]\) m/s²（感受重力反向）
• 姿态 \(C_{nb} = I\)（水平无转角）

走一步 insupdate 会怎样？

1. L22 半步外推：\(v_{01} = 0 + 0 \times 0.05 = 0\)，\(p_{01}\) 也不变（速度为 0）

2. L32 加速度：\(a^n = f^n + g_{cc} = [0, 0, -9.8] + [0, 0, +9.8] = [0, 0, 0]\)

   静止时比力 = -g，重力 = +g，两者抵消，加速度 = 0 → 符合直觉！

3. L34 新速度：\(v_{k+1} = 0 + 0 \times 0.1 = 0\) → 速度不变 ✓

4. L39 位置变化率：\(M_{pv} \cdot (0+0)/2 = 0\) → 位置不变 ✓

5. L46 姿态更新：phim ≈ 0（陀螺无输出），wnin 很小（地球自转+位移角速度），qnb 几乎不变 ✓

结论：静止时 insupdate 每一步都是"零增量"——速度不变、位置不变、姿态不变。这正是"无运动 = 无变化"的物理直觉验证。如果静止仿真时位置漂移了，说明要么 IMU 有零偏，要么 gcc 符号写错了（参见坑③）。

(1) 姿态更新：四元数递推¶

姿态微分方程（泊松方程四元数形式）：

其中 机体系b相对于导航系n的角速度在b系投影：

• \(\boldsymbol{\omega}_{in}^n = \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n\)：导航系相对于惯性系的角速度（两部分之和），单位rad/s
• \(\boldsymbol{\omega}_{en}^n\)（位移角速度/牵连角速度）：由载体相对地球运动引起导航系原点转动，公式：

离散积分采用"双子样等效旋转矢量法"（比简单一阶欧拉高两阶精度）：

其中： - \(\mathbf{q}_{\Delta}(\Delta\boldsymbol{\phi}_m)\)：右乘项，IMU等效旋转矢量对应的四元数，体现机体系相对惯性系的转动 - \(\mathbf{q}_{\omega}(-\boldsymbol{\omega}_{in}^n T_s)\)：左乘项（四元数乘法顺序对应姿态补偿反向），补偿导航系本身相对于惯性系的转动 - 负号：因\(\boldsymbol{\omega}_{in}^n\)定义为"n系相对i系的角速度"，而我们要维持姿态在n系下，所以反向扣除

PSINS使用qupdt2(qnb, phim, wnin_nts)一次性实现这一双向更新，对应L46。

(2) 速度更新：比力积分 + 有害加速度补偿¶

速度微分方程（比力方程，SINS核心）：

离散形式（中点积分，\(f_L, f_R, g, \omega\)均近似取半步时刻值）：

将四项合并写为"比力投影 + 有害加速度补偿gcc"：

其中：

各项逐项解释：

为什么中点积分精度更高？ 普通一阶欧拉积分用区间起点值：\(v_{k+1} = v_k + a_k T_s\)，截断误差 \(O(T_s^2)\)。中点积分用区间中点值：\(v_{k+1} = v_k + a_{k+1/2} T_s\)，截断误差 \(O(T_s^3)\)，精度高一阶。

数值积分三阶梯对比：PSINS 用的是哪一级？¶

梯形法和中点法精度同阶（都是 \(O(T_s^3)\)），但 PSINS 为什么选了中点法而不是梯形法？

因为梯形法需要 \(a_{k+1}\)，而它要等下一拍才算出来——下一拍的 \(a_{k+1}\) 需要下一拍的姿态 \(C_{nb}(t_{k+1})\) 和地球参数 \(\mathrm{eth}(t_{k+1})\)，这些在本拍还没算出来。

PSINS 的解法：用半步外推把 \(a_k\) 推到 \(a_{k+1/2}\)，用中点法达到和梯形法同阶的精度。这就是 L22 半步外推存在的根本原因——你不算半步，就只能用矩形法（一阶精度），误差随时间线性累积。

     a_k --------approx--------> a_{k+1/2} --------approx--------> a_{k+1}
      ↑                              ↑                               ↑
   L32 算出来                   L22 外推得到              下一拍才能算
   (用 t_k 姿态)              (用 vn01/pos01 估半步)      (现在拿不到!)
                                      ↓
                              L34: v_{k+1} = v_k + a_{k+1/2}·T_s
                              ← 中点法，O(T_s³)，不需要 a_{k+1}

💡 一句话记忆：梯形法要"两头加速度"（\(a_k\) 和 \(a_{k+1}\)），但未来拿不到；中点法只要"中间加速度"（\(a_{k+1/2}\)），半步外推就能估出来。PSINS = 用半步外推 + 中点法，绕开了梯形法"需要未来值"的困境，精度还一样高。

(3) 位置更新：经纬度积分¶

位置（纬度L、经度λ、高度h）与导航系速度(v_E, v_N, v_U)的微分关系（由球面几何或椭球面第一基本形式）：

写成矩阵形式 \(\dot{\mathbf{p}} = \mathbf{M}_{pv} \mathbf{v}^n\)：

这就是Mpv矩阵。关键要点： - Mpv(1,2)=1/RMh：北速v_N→纬度变化率，沿子午圈（经线）方向，所以除以RMh - Mpv(2,1)=1/(RNh·cosL)：东速v_E→经度变化率，沿卯酉圈（纬线）方向。纬线圈半径=RNh·cosL（赤道半径RNh，在纬度L处按cosL缩放），所以经度变化=东速/(纬线圈半径) - Mpv(3,3)=1：天速直接变高度率（地心地固近似下）

离散中点积分：速度取前后时刻平均 \(\bar{\mathbf{v}}^n = (\mathbf{v}^n(t_k) + \mathbf{v}^n(t_{k+1}))/2\)

位置积分用的就是梯形法！¶

你可能听说过"惯导里位置用梯形积分"——就是这里。对照上一节的"三阶梯"表：

为什么位置能用梯形法而速度不能？ 因为位置积分时，\(v_{k+1}\) 已经在本拍算出来了（L34 算出 vn1），两头速度都有，可以直接取平均。而速度积分时 \(a_{k+1}\) 要等下一拍才有——所以速度被迫用中点法，位置可以大方用梯形法。

💡 你之前听说的"速度外推 + 位置梯形积分"完全正确，对应关系： - "速度外推" = L22 的 vn01 = vn + an*nts2（半步外推估 \(v_{k+1/2}\)）→ 配合 L32 估出 \(a_{k+1/2}\) → L34 中点法积分 - "位置梯形积分" = L39 的 Mpv*(vn+vn1)/2（新旧速度取平均）→ L40 全步长积分

为什么需要Mpv半步外推？ Mpv中的RMh、RNh、cosL都是位置的函数。如果用t_k的L计算Mpv，一阶精度；如果用半步位置L(t_{k+1/2}) 计算，二阶精度。所以L22用vn, an外推出pos01再更新eth。

3. 地球参数半步外推的精度机制¶

insupdate最精妙的"数值小技巧"集中在L22-L24：

朴素做法（一阶精度）：

PSINS做法（二阶精度，L22）：

用vn01, pos01调用ethupdate得到半步时刻的eth参数，再用这些参数做后续积分。本质是预估-校正法的预测步：先用旧状态预测半步状态，再用半步状态做高精度积分。

📝 逐行注释¶

insupdate共48行，每一行详细注释，一行不落下。标⚠️为关键易错行，标★为算法核心行。

🔍 断点调试建议¶

调试0：完整流程逐步手算（推荐新手必做）¶

前置准备：运行 test_SINS_trj.m 或 trjsimu.m 生成一条轨迹（或用insinit造初始状态），得到ins结构体和前一帧imu数据。

在insupdate L19设断点，运行到断住后，在Command Window手动执行以下每一步，并跟代码比较：

💡 怎么读这些调试代码？——_man 变量是什么意思？

_man 后缀 = "manual"（手算的）。调试的套路是： 1. 代码已经执行到断点，此时代码里的变量（如 ins.fn、vn1）已经有了值 2. 你在 Command Window 里用同样的公式手算一遍，存到 xxx_man 变量里 3. 用 norm(代码变量 - 手算变量) 比对，如果 <1e-14 说明你理解的公式和代码一致

代码里的变量	你手算的变量	对应关系	比对命令
phim（L19 代码算出来的）	phim2（你手动调 cnscl 算的）	同一个公式，两次计算	norm(phim - phim2)
ins.fn（L30 代码算出来的）	fn_man（你用 qmulv 手算的）	同一个公式	norm(ins.fn - fn_man)
ins.an（L32 代码算出来的）	an_man（你用 rotv+gcc 手算的）	同一个公式	norm(ins.an - an_man)
vn1（L34 代码算出来的）	vn1_man（你用 vn+an*nts 手算的）	同一个公式	norm(vn1 - vn1_man)

核心思想：代码算了一遍，你再手算一遍，两个结果一致 → 说明你理解对了。如果不一致 → 说明你的公式理解和代码不同，要找哪里想错了。

步骤A——圆锥补偿验证（对照02_cnscl.md练习）：

% 命中断点后
[phim2, dvbm2] = cnscl(imu, 0);   % 手动调用cnscl
norm(phim - phim2)  % 应当 <1e-15，说明L19输入输出关系正确

步骤B——初始量检查：

% 断点停在L20之后，L21之前
nts_exp = size(imu,1) * ins.ts;   % 手算nts
abs(nts_exp - nts) < 1e-12        % 验证L18
% 检查ins.an, ins.vn初始字段存在
keyboard

步骤C——半步外推L22手算验证：

vn01_man = ins.vn + ins.an * nts2;
pos01_man = ins.pos + ins.Mpv * vn01_man * nts2;
norm(vn01 - vn01_man)  % <1e-15
norm(pos01 - pos01_man)  % <1e-15

步骤D——速度更新L30-L34手算：

% L30 比力投影
% dvbm/nts = 速度增量/时间 = 比力 fb（和 L25 的 ins.fb 等价）
fn_man = qmulv(ins.qnb, dvbm/nts);  % 用 dvbm/nts 更直观：展示了 fb 的来源
norm(ins.fn - fn_man)  % <1e-14
% L32 速度更新加速度
wnin_half = - ins.eth.wnin * nts2;
fn_rot = rotv(wnin_half, fn_man);
an_man = fn_rot + ins.eth.gcc;
norm(ins.an - an_man)  % <1e-14
% L34 新速度
vn1_man = ins.vn + an_man * nts;
norm(vn1 - vn1_man)  % <1e-14

步骤E——位置更新L36-L40手算：

% L37 Mpv构造
Mpv_man = zeros(3);
Mpv_man(1,2) = 1/ins.eth.RMh;
Mpv_man(2,1) = 1/ins.eth.clRNh;  % clRNh = cosL * RNh
Mpv_man(3,3) = 1;
norm(ins.Mpv - Mpv_man, 'fro')  % 非零元素应<1e-15
% L39 位置变化率
Mpvvn_man = Mpv_man * (ins.vn + vn1_man) / 2;
norm(ins.Mpvvn - Mpvvn_man)  % <1e-15
% L40 新位置
pos_man = ins.pos + Mpvvn_man * nts;

步骤F——姿态更新L44-L47手算：

% L46 qupdt2展开
qnb_man = qupdt(qupdt(ins.qnb, phim),  -ins.eth.wnin*nts);
% 注意qupdt2的顺序和精度更高，但差应<1e-12
[qnb2, att2, Cnb2] = attsyn(qnb_man);
norm(q2mat(ins.qnb) - q2mat(qnb2), 'fro')  % <1e-10

调试1：去掉半步外推看精度差（配套练习预演）¶

在insupdate L22设断点，命中断点后手动强制：

vn01 = ins.vn;    % 不外推，直接用旧速度
pos01 = ins.pos;  % 不外推，直接用旧位置
dbstep in 2;      % 单步跳入ethupdate，观察输入就是旧pos/vn

❌ 初学者最容易踩的坑¶

坑①：Mpv(2,1)漏掉cos(L)，高纬度经度飞速发散¶

表现：修改Mpv时按直觉写 Mpv(2,1)=1/RNh（忘记除以cosL），在纬度L=70°仿真时经度λ 600s内漂移几十度，正常应当几海里。

为什么错：纬线（东向行驶）所在的平行圆半径不是RNh，而是RNh·cos(L)！东向行驶1 m对应的经度变化不是1/RNh rad，而是1/(RNh·cosL) rad。在L=70°，cosL≈0.342，所以漏掉cosL会导致经度变化率被放大约2.92倍——短期就明显偏，长期发散。

验证：L=0°（赤道）cosL=1没问题；L=80°cosL≈0.1736→漏除以放大5.76倍→几小时就漂一圈。

坑②：姿态更新四元数左乘(-wnin)和右乘(phim)顺序写反¶

表现：自行改写insupdate时写成 qnb = qmul(rv2q(-wnin*nts), qmul(qnb, rv2q(phim)))（wnin乘在qnb左边而不是右边），结果姿态漂移超快，10°/h以上。

为什么错：四元数姿态的物理定义是"n→b的旋转"：

IMU增量\(\Delta\boldsymbol{\phi}_m\)是b系本身的旋转（体固连），所以要从右侧乘\(\mathbf{Q}_{nb}\)：

而wnin·T_s是n系本身的旋转（导航系在惯性空间转动了一个角度），我们需要把Qnb调整为"新n→旧b→新b"之间的新关系，对应右侧再乘一个反向旋转：

左右顺序错 = 把坐标系旋转作用在错误的参考系 = 姿态漂移量级等于ω_ie·T≈15°/h完全没补偿。

坑③：ins.an里gcc已包含重力+科里奥利+离心，又额外加-g¶

表现：为了"加重力"在L32后面补写 ins.an(3) = ins.an(3) - 9.8，结果高度h在几十秒内从0掉到几百米，速度天向vU快速累积负号。

为什么错：gcc字段（重力+科里奥利+离心合成项）是这样定义的（参考earth.m/ethupdate.m）：

其中\(\mathbf{g}_l^n\)是"正常重力"，方向沿ENU的U轴负方向，NED惯例下是正z（地向）。PSINS中gcc在NED下已经是"应该加到速度微分上的全部有害加速度"——不需要再手动加任何-g项。再减一次9.8等于把重力算了两遍，方向相反地叠加，直接把天向速度"向下加速"。

坑④（补充）：vn1先覆盖ins.vn再做位置更新，丢失中点速度¶

表现：把L34 vn1 = ins.vn + ins.an*nts; 直接写成 ins.vn = ins.vn + ins.an*nts;，然后L39里(ins.vn+vn1)/2改成ins.vn，结果位置RMS变大。

为什么错：位置积分中点平均需要用到旧速度(t_k)和新速度(t_{k+1})的平均值。如果先覆盖ins.vn就丢了t_k的速度值，只剩t_{k+1}。梯形法则变成左矩形法则，精度降一阶，误差O(T_s²)而非O(T_s³)。

🎯 配套练习¶

练习题：去掉L22半步外推，看全程RMS退化多少¶

题目：将insupdate中第22行 vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2; 修改为直接用旧状态：

vn01 = ins.vn;   % 不外推，用t_k时刻速度代替t_{k+1/2}
pos01 = ins.pos; % 不外推，用t_k时刻位置代替t_{k+1/2}

实验步骤：

1. 备份原版insupdate：

   cd /workspace/psins/base/base1
   cp insupdate.m insupdate_original.m
   

2. 修改insupdate.m L22（直接用Edit工具）：把vn01和pos01外推替换为旧状态直接赋值（即把预估步跳过，相当于一阶精度）。

3. 跑原版基线baseline：先恢复original版本跑一遍，保存结果：

   % 在Matlab命令行
   % 先切到原版文件（如果已经改过，用cp覆盖回）
   ! cp insupdate_original.m insupdate.m
   run('test_SINS.m');  % 假设会生成trj真值和SINS解算结果inserr
   % 保存误差RMS
   rms_pos_baseline = sqrt(mean(inserr.pos_err.^2, 1));  % 3×1向量 [N;E;U] 误差RMS，单位m
   fprintf('【原版】位置RMS N/E/U = %.3f / %.3f / %.3f m\n', rms_pos_baseline);
   rms_3d_baseline = norm(rms_pos_baseline);
   save /tmp/ins_baseline.mat ins err rms_pos_baseline rms_3d_baseline;
   

4. 跑修改版本（无半步外推）：

   % 修改L22为vn01=ins.vn; pos01=ins.pos;后
   run('test_SINS.m');
   rms_pos_modified = sqrt(mean(inserr.pos_err.^2, 1));
   rms_3d_modified = norm(rms_pos_modified);
   fprintf('【修改】位置RMS N/E/U = %.3f / %.3f / %.3f m\n', rms_pos_modified);
   fprintf('【退化倍数】3D RMS = %.2f 倍 (原版%.2f → 修改后%.2f)\n', ...
       rms_3d_modified / rms_3d_baseline, rms_3d_baseline, rms_3d_modified);
   

5. 预期结果分析：

6. 短航段（60s内）：退化可能不明显，RMS增幅<5%，因为半步外推是二阶项，小T_s下T_s²很小。
7. 600s中长航段：3D RMS通常退化1.3～2.5倍。因为每一步误差O(T_s²)累计，随时间T（而非T²）增长。在高动态转弯或变加速运动场景下退化更严重（>3倍），因为an本身快速变化，旧an与半步an偏差大。

8. 高纬度（L>60°）和高航速（>300m/s）下退化显著，因为wnin和Mpv对位置强敏感。

9. 进阶思考题：如果把L22改成"vn01用外推但pos01不用"，或"pos01外推vn01不用"，分别跑一次，看哪一项的贡献更大？（提示：位置积分Mpv中RMh/RNh随纬度变化慢，主要由vn01驱动的外推更关键；但高动态下两者都重要）

📎 附录 A：飞控 vs 惯导——解算策略对比¶

这个附录是为做 MEMS 飞控/低精度惯导的读者写的。虽然我们在拆解 PSINS（200Hz、二阶精度、全状态解算），但很多读者实际做的是 1kHz+ 的 MEMS 系统，甚至只做姿态解算。这里把两者的工程取舍讲清楚。

A.1 解算什么？——取决于你的应用¶

为什么飞控只解算姿态？

• 多旋翼/固定翼的位置靠 GPS，不需要惯导位置推算（MEMS 位置漂移 1km/h，飞 10 分钟就漂 10km，没用）
• 但姿态必须高频更新（400Hz+），否则飞机翻过来都来不及纠正
• 所以飞控的核心是"陀螺+加计 → 姿态"，位置交给 GPS

A.2 "姿态→速度→位置" vs "速度→位置→姿态"——先算姿态会怎样？¶

关键在于：速度更新需要姿态来投影比力，用旧姿态还是新姿态？

先算姿态会怎样？ 如果先更新姿态得到 \(C_{nb}(t_{k+1})\)，再用它投影比力算速度，相当于用区间末点的姿态代表整个区间——这是后向欧拉，只有一阶精度 \(O(T_s^2)\)。要达到二阶精度，还得做半步补偿（用 \(C_{nb}(t_{k+1/2})\) 而非 \(C_{nb}(t_{k+1})\)），那就又回到 PSINS 的思路了。

一句话：PSINS 用"旧姿态 + rotv 半步旋转"达到二阶精度；先算姿态用"新姿态"只有一阶精度，要二阶还得补偿半步——绕了一圈又回来了。但如果你只做姿态解算（不关心速度/位置），那"先姿态"就是唯一选择，没有这个问题。

A.3 飞控的姿态积分方法——和 PSINS 对照¶

以下是飞控嵌入式场景常见的姿态积分方法，和 PSINS 对照：

飞控选①一阶欧拉的工程理由¶

• IMU 输出 1-8kHz，\(\Delta t\) 极小（0.125-1ms）
• 一阶欧拉截断误差 \(\sim O(\Delta t^2)\)，在 \(\Delta t = 0.125\) ms 下 \(\sim 1.6 \times 10^{-8}\)，已经远小于 MEMS 陀螺本身的噪声
• 算力有限（STM32 F4/H7），每步省一次叉乘+归一化就是省几百周期
• 圆锥误差在 8kHz 下可忽略：\((\omega h)^2 \sim (5° \times 0.000125)^2 \sim 4 \times 10^{-10}\) rad² → 比陀螺零偏小 6 个数量级

积分后必须归一化¶

浮点累积会导致 \(\|\mathbf{q}\| \neq 1\)，不归一化后续所有旋转计算都会带误差。PSINS 在 attsyn.m 里做归一化（L47），飞控在每次积分后做。

快速逆平方根（飞控嵌入式技巧）：归一化需要算 \(1/\sqrt{x}\)，经典方法用"魔数"近似（Quake III 源码中的 0x5f3759df），一次牛顿迭代即达浮点精度。PSINS 在 MATLAB 里不需要这个（直接 q/norm(q)）。

A.4 飞控姿态积分 vs PSINS 姿态更新——关键区别¶

关键直觉：飞控 8kHz 下圆锥误差 \((\omega h)^2\) 项可以忽略，一阶欧拉 + 归一化就够了。但放到 PSINS 的 100-200Hz 场景，就必须加圆锥补偿，否则高动态下姿态会漂。方法没有好坏，只有"频率+精度要求+算力"的取舍。

A.5 卡尔曼滤波 vs 机械编排——到底谁在"算导航"？¶

常见疑问："卡尔曼滤波不是滤波/融合数据用的吗？怎么还能用来惯导解算？"

你说得对——KF 是滤波/融合用的，惯导解算靠的是机械编排。 但不同系统的架构选择不同，这里讲清楚。

两个概念¶

PSINS 的做法：机械编排 + KF 分开¶

    IMU数据
      ↓
  insupdate.m        ← 机械编排：算出姿态/速度/位置（会漂移）
      ↓
  kfupdate.m          ← KF：拿 GNSS 量测和 insupdate 的结果对比
      ↓                   → 估出误差（δatt, δv, δpos, 零偏...）
  kffeedback.m        ← 反馈：把误差扣回 ins 结构体
      ↓
  修正后的姿态/速度/位置（不再漂移）

机械编排是"干活的主力"，KF 是"纠偏的监工"。 KF 不算导航解——它只估误差，然后反馈给机械编排去修正。

PX4 EKF2 的做法：合并成一个 EKF¶

PX4 EKF2 把所有状态（姿态+速度+位置+零偏）放进一个 24+ 维的状态向量，用 EKF 的预测步直接推演所有状态：

这里 \(\Phi\) 里的姿态更新部分用的还是同一套物理方程（四元数微分、比力方程），只是写成状态空间形式。物理没有变，代码结构变了。

对比表¶

一句话：PSINS = 机械编排（insupdate）+ KF（kfupdate）分开做；PX4 EKF2 = 把两者合并成一个 EKF。物理一样，代码架构不同。PX4 并没有"用 KF 代替机械编排"，而是把机械编排的方程塞进了 KF 的预测模型里。

📎 附录 B：半步外推不是"鸡生蛋"——\(a_k\) 来自上一拍¶

常见疑问¶

"半步外推需要 \(a_{k+1/2}\)，但 \(a_{k+1/2}\) 不也要算吗？这不就是鸡生蛋蛋生鸡？"

不是！关键在于 \(a_k\) 是上一拍已经算好的，不是本拍要算的。

数据流图¶

上一拍 L32: 算出 a_k，存到 ins.an     ← 已知！不是本拍算的
    ↓
本拍 L22: v_{k+1/2} = v_k + a_k × T_s/2   ← 用上一拍的 a_k 外推
         p_{k+1/2} = p_k + Mpv × v_{k+1/2} × T_s/2
    ↓
本拍 L23: eth(t_{k+1/2}) = ethupdate(p_{k+1/2}, v_{k+1/2})  ← 半步地球参数
    ↓
本拍 L30: f^n = C_nb(t_k) × f^b          ← 用旧姿态投影比力
    ↓
本拍 L32: a_{k+1/2} = rotv(-wnin×T_s/2, f^n) + gcc(t_{k+1/2})
         ← 用旧姿态 + 半步地球参数 + rotv旋转 = 真正的 a_{k+1/2}
    ↓
本拍 L34: v_{k+1} = v_k + a_{k+1/2} × T_s   ← 中点法积分

没有循环依赖：\(a_k\) 来自上一拍的 L32（存在 ins.an 里），不是本拍要算的。本拍用 \(a_k\) 做半步外推 → 得到半步地球参数 → 再用旧姿态 + rotv 算出真正的 \(a_{k+1/2}\)。

梯形法需要 \(a_{k+1}\)（本拍算不出来的未来值）→ 死路 中点法需要 \(a_{k+1/2}\)（用上一拍的 \(a_k\) 外推得到的）→ 活路