06_drupdate 逐行注释 Wiki (PSINS)¶

配套源码: drupdate.m 所属层级: L2 DR 航位推算 前置依赖: 00_glvf / 01_earth / 03_insupdate / 08_kffk / 09_kfupdate 学习目标: 读完回答3个问题

1. DR的3个核心误差状态是什么？为什么位置误差是线性发散（无舒勒振荡）？
2. pos = pos + Mpv·Cnb·[0;0;s] 这一行中，b系前向为什么是z轴而不是x轴？
3. 为什么SINS/DR组合中通常SINS做参考（给姿态），DR做观测，而不是反过来？

🧩 函数作用一句话¶

基于陀螺角增量积分更新姿态四元数，结合里程计距离增量 dS，通过方向余弦矩阵将 b 系前向里程增量投影到 n 系，再经 Mpv 位置变换矩阵累加得到纬度/经度/高度，完成一次 Dead Reckoning（航位推算）更新。

📐 数学原理 / 物理意义¶

1. DR = 纯陀螺方位 + 里程计距离增量¶

航位推算（Dead Reckoning）的核心思想是：已知上一时刻位置和航向，通过测量航向变化和行进距离，递推当前时刻位置。

离散时间更新方程（二维平面简化版）：

其中 \(\psi\) 为方位航向角，\(S\) 为累计行驶距离，\((x,y)\) 为平面位置。

在 PSINS 中，完整的三维更新为：

其中 \(d\boldsymbol{s}^b = [0;0;\Delta S]\) 表示里程计增量沿 b 系前向（z 轴方向约定）。

2. DR的误差方程（3状态）与线性发散特性¶

DR误差状态向量取为 \(\boldsymbol{X}_{DR} = [\varepsilon_\psi, \delta x, \delta y]^T\)，即方位角误差、北向位置误差、东向位置误差。

误差传播方程：

该系统的关键特征： - F 阵特征值全为 0：系统是临界稳定的，没有振荡模态； - 无舒勒振荡：SINS 中位置误差通过比力反馈到姿态形成 84.4 分钟舒勒周期，但 DR 中没有重力/比力观测，位置误差和姿态误差之间不存在闭环反馈； - 位置误差线性发散：\(\delta x(t) \approx V \cdot \varepsilon_\psi \cdot t\)，随时间 \(t\) 线性增长，而非 SINS 中的振荡。直观理解：恒定航向偏差 \(\varepsilon_\psi\) 导致每米行驶产生 \(V\varepsilon_\psi\) 的横向偏移，总偏移随路程线性累积。

3. 为什么通常 SINS 做观测源给 DR，而不是反过来¶

组合策略： - SINS → DR：SINS 提供完整的俯仰/滚转姿态（dr.qnb = ins.qnb，见 test_SINS_DR.m 第27行），DR只负责用里程计补位置短期精度； - DR → SINS：DR位置作为观测输入到KF，修正SINS的位置/速度/姿态误差（松组合位置观测）。

如果反过来用DR给SINS姿态，车辆加减速时DR没有独立的俯仰参考，姿态会严重发散。

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

🔍 断点调试建议¶

调试1：纯DR位置线性发散验证¶

在 /workspace/psins/demos/test_SINS_DR.m 中操作：

1. 运行前修改：第18行 DR初始化处，将 dkod = 0.01（ODO标度+1%误差）保持不变，把第17行 dinst = [15;0;10]*glv.min 中的方位安装误差改为 dinst(3)=0 只保留标度误差；
2. 断点设置：在第28行 dr = drupdate(dr, wvm(:,1:3), dS); 后打断点；
3. 观察变量：记录 dr.pos（DR位置）与 trj.avp(k1,7:9)（真值），每 10 秒打印一次差值 dr.pos - trj.avp(k1,7:9) 转米；
4. 预期量级：设车速 \(V\approx20m/s\)，跑 \(T=300s\)，纯ODO标度误差 \(\delta k=1\%\)，则位置误差应约为 \(\delta k \cdot V \cdot T = 0.01×20×300=60m\)，随时间线性增长。若实际误差偏离该量级，检查 kod 单位或 Mpv 矩阵方向是否搞反。

调试2：AOS侧滑修正效果¶

1. 修改 drinit 第18行输入中的 inst(2)=aos，比如设 aos=0.3；
2. 在drupdate第25行前后各断点，比较 dSn 修正前后的差异：转弯运动时（phim(3)≠0），修正后的东向位移应比修正前多出一个横向分量，量级约 aos×|Δs|。

❌ 初学者最容易踩的坑¶

坑1：里程计od单位搞错（脉冲没转米）¶

现象：DR跑完后位置误差正好差一个固定倍数 k（如100倍、1000倍），或者位置几乎不动。 原因：dr.kod（米/脉冲）没乘对。ODO通常输出是脉冲数，需要 kod=轮周长/每圈脉冲数 换算。比如轮胎周长2m，每圈2000脉冲，则 kod=0.001m/pulse。如果把脉冲当米直接用，结果差3个数量级。 验证：看 dr.distance 是否与 trj.distance 量级一致，前者应该≈后者。

坑2：DR航向与b系前向方向不匹配¶

现象：车辆直行但DR位置轨迹方向偏90°或180°，或者横纵向误差耦合。 原因：dr.prj 的b系前向轴默认是y轴（见drinit第28行 dr.prj = dr.Cbo*[0;1;0]），如果实际车辆ODO前向是x轴或z轴，投影方向完全错。车辆侧滑严重时AOS系数不匹配也会放大此问题。 验证：静止+IMU只给绕z轴正方向旋转，看yaw是否正确增加。然后直行时看n系位移是否沿yaw指向的方向。

坑3：位置更新时Mpv矩阵行列方向弄反¶

现象：向北运动但经度（λ）在变、纬度（L）不变，完全和直觉相反。 原因：Mpv的定义是 [δL; δλ; δh] = Mpv · [δvE; δvN; δvU]，即第1列对应东向速度（产出纬度变化=0），第2列对应北向速度（产出纬度变化），第1行第2列是1/RMh，第2行第1列才是东向→经度的系数。不要按"行1=北对应列1=北"的直觉来套。 验证：手动构造纯向北vN=20m/s，Δt=1s代入，看Mpv×[0;20;0]的第1项（纬度）是否=20/RM≈3.14e-4度，对的再继续。

🎯 配套练习¶

练习：NED下匀速直线运动的DR误差累积¶

步骤： 1. 在 /workspace/psins/demos/ 新建脚本，手动造轨迹： - L0=34°N, λ0=108°E, h0=0m, 航向 yaw0=90°（正东） - 匀速 vn=[0;20;0]m/s（北向速度？不！正东对应 vE=20,vN=0，即 vn=[20;0;0]） - T=600s（10分钟），ts=0.01s 2. 陀螺数据造零均值噪声：wm = randn(N,3) * 0.01°/h × ts，即 \(\sigma_{ARW}=0.01°/\sqrt{h}\) 换算成 rad/√s × √ts 3. ODO增量造 od = 20*ts * (1 + 0.01*randn)（1%高斯标度误差+白噪声） 4. 用 drinit 初始化，循环调用 drupdate 跑 10 次 Monte Carlo 5. 理论公式验证：最终东向位置误差的 STD 应约为：

其中航向随机游走 \(\sigma_{\varepsilon_\psi}(t)=\sigma_{ARW}\cdot\sqrt{t}\)，积分后时间指数从 1/2 变 3/2，开平方后是 √(T³/3) 量级。 6. 实际跑出来10次的终位误差STD是否与理论公式在同一数量级？如果差太多，检查ARW单位换算（deg/√h → rad/√s 要除以 √3600 × 180/π）。