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

配套源码: cnscl.m 所属层级: L0基础 / L1纯惯导 前置依赖: 必须先读 00_glvf.md（圆锥补偿系数表glv.cs）、01_earth.md（地球参考系） 学习目标: 读完后你应能回答哪3个问题 1. 为什么双子样下 \(\boldsymbol{\omega}_{m1}+\boldsymbol{\omega}_{m2} \neq \boldsymbol{\omega}_{m,\text{total}}\)，差的那一项叫什么？ 2. 圆锥补偿 \(\Delta\boldsymbol{\phi}\)、划桨补偿 \(\Delta\boldsymbol{v}_s\)、旋转补偿 \(\Delta\boldsymbol{v}_r\) 三者物理意义分别是什么？ 3. glv.cs(n-1,:) 这一行系数为什么只乘前 \(n-1\) 个样本，最后一个样本用叉乘单独处理？

💡 学习心态：这一章涉及较多数学知识（旋转矢量、不可交换性、Bortz 方程等），初学阶段**"知道它是干嘛的、什么时候需要、怎么调用"就够了**，推导细节是研究生级别的内容。先会用，有兴趣再深究。可将重心放在 ./03_insupdate.md。

你现在需要掌握的： 1. 物理直觉：两个旋转不可交换顺序 → 直接相加有误差 → 补一个叉乘项 2. 什么时候用：高动态/振动环境必须用，低动态 MEMS 可跳过 3. 怎么调用：PSINS 里 cnscl(imu, 0) 返回 [phim, dvbm]，直接喂给 insupdate

不需要掌握的（等以后有需要再深挖）： - 旋转矢量微分方程（Bortz 方程）的推导 - \(\frac{2}{3}\)、\(\frac{9}{20}\)、\(\frac{27}{40}\) 这些系数怎么从泰勒展开解出来的 - 多子样算法的最优性证明

一句话：cnscl.m 是 PSINS 的"黑盒入口"——你把 IMU 数据扔进去，它吐出补偿后的角增量 \(\boldsymbol{\phi}_m\) 和速度增量 \(\Delta\mathbf{v}_m\)，你拿着这俩去更新姿态和速度就行。至于盒子里面怎么算的，等你需要自己写嵌入式版的时候再拆。

🧩 函数作用一句话¶

IMU多子样圆锥/划桨/旋转补偿

📐 数学原理 / 物理意义¶

1. 旋转不可交换性误差（圆锥误差来源）¶

考虑刚体在时间间隔 \([t_k, t_k+h]\) 内做两次连续转动： - 先绕 \(x\) 轴转 \(\Delta\theta_1\)：旋转矢量 \(\boldsymbol{\phi}_1 = [\Delta\theta_1, 0, 0]^T\)，单位：rad - 再绕 \(y\) 轴转 \(\Delta\theta_2\)：旋转矢量 \(\boldsymbol{\phi}_2 = [0, \Delta\theta_2, 0]^T\)，单位：rad

合成旋转矩阵用方向余弦矩阵连乘：

小角度近似下 \(\mathbf{C}(\boldsymbol{\phi}) \approx \mathbf{I} - [\boldsymbol{\phi}\times]\)，代入得：

但如果直接求和 \(\boldsymbol{\phi}_{\text{sum}} = \boldsymbol{\phi}_1+\boldsymbol{\phi}_2\)，其矩阵近似为：

两者差了一个二阶叉乘项！ 利用恒等式 \([\boldsymbol{a}\times][\boldsymbol{b}\times] - [\boldsymbol{b}\times][\boldsymbol{a}\times] = [(\boldsymbol{a}\times\boldsymbol{b})\times]\)，精确到二阶的合成旋转矢量应为：

这就是旋转不可交换性定理：有限转动的合成不满足加法交换律，必须加叉乘补偿项。

2. 双子样圆锥补偿公式推导（Bortz方程）¶

设 IMU 采样间隔为 \(h\)（单位：s），在一个更新周期内采 \(n\) 个子样。第 \(i\) 个子样角增量：

对于典型圆锥运动（绕两个正交轴做正弦/余弦耦合振荡）：

其中 \(a\) 为圆锥半角（rad），\(\Omega\) 为圆锥角频率（rad/s）。

对 \(n=2\)（双子样）积分一个周期 \(h\)，得到两个子样：

代入圆锥运动角速度精确积分，再用 Bortz 旋转矢量微分方程：

忽略三阶以上小量，积分后得到双子样圆锥补偿精确公式：

其中： - \(\Delta\boldsymbol{\theta}_1 + \Delta\boldsymbol{\theta}_2\)：直接求和的"名义"旋转矢量，单位 rad - \(\frac{2}{3}\Delta\boldsymbol{\theta}_1 \times \Delta\boldsymbol{\theta}_2\)：圆锥补偿量（coning compensation）\(\Delta\boldsymbol{\phi}_{\text{cone}}\)，单位 rad，由旋转不可交换性引起 - 系数 \(\frac{2}{3}\) 是 \(n=2\) 时在圆锥运动假设下使误差四阶小量的最优值，对应 glv.cs(1,:) = [2/3, 0, 0, 0, 0]

推广到 \(n\) 子样最优补偿：

其中系数向量 \([c_1(n), c_2(n), \dots, c_{n-1}(n)]\) 即为 glv.cs(n-1,:)，它使补偿后残余圆锥误差阶次最高（\(2n\) 阶）。

3. 划桨补偿（Sculling Compensation）¶

划桨运动定义为载体角振动和线振动在两个正交轴上同频耦合：

其中 \(a\)（rad）、\(b\)（m/s）分别为角/线划桨振幅，\(\Omega\) 为角频率（rad/s）。

比力积分得到子样速度增量：

划桨误差的物理来源：在机体系 \(b\) 中积分比力时，坐标系本身同时在旋转，导致"速度增量的坐标系不一致"。

双子样下划桨补偿公式：

对应代码 L66 中两项叉乘之和。推广到 \(n\) 子样最优形式：

与圆锥补偿共用同一套系数表 glv.cs(n-1,:)——这就是"圆锥-划桨对偶原理"。

4. 旋转补偿（Rotation Compensation）¶

即使完全没有圆锥/划桨振动，单纯"刚体有角速度 + 有加速度"，由于子样积分区间内机体系已经转过一个角度，前后子样的速度增量方向也不一致。这一补偿称为旋转补偿或速度转动补偿：

对应代码 L71 rotm = 1/2*cros(wmm,vmm)。物理含义：将速度增量从"平均机体系"旋转到"更新周期末机体系"所需的一半转角近似（中点积分思想）。

5. 补偿系数表 glv.cs 详解¶

glv.cs 是 \((n_s-1) \times (n_s-1)\) 下三角矩阵，其中 \(n_s\) 最大支持子样数（这里最大6子样）。第 \(i\) 行对应 \((i+1)\) 子样补偿多项式系数：

📝 逐行注释¶

主函数 L1-L73¶

子函数 imureshape L75-L87¶

🔍 断点调试建议¶

调试1：圆锥运动定量验证（验证圆锥补偿公式）¶

构造理想圆锥运动角增量：设周期 \(h=0.01\,\text{s}\)，圆锥半角 \(a=0.01\,\text{rad}\)，角频率 \(\Omega=2\pi\,\text{rad/s}\)。双子样下，两个子样角增量为：

调试步骤： 1. 在 cnscl.m 第 L41 行设断点（cm = glv.cs(n-1,1:n-1)*wm(1:n-1,:)） 2. 构造 wm = [wm1; wm2]（n=2），调用 [phim, ~, dphim] = cnscl(wm, 0) 3. 命中断点后： - 观察 glv.cs(1,1) = 2/3（双子样系数） - 观察 cm = 2/3 × wm1（第1子样加权） - 单步跳过L42：dphim = cross(cm, wm2) = (2/3)·wm1 × wm2，验证其第三分量≈\((2/3)\omega_{m1,x}\omega_{m2,y} - (2/3)\omega_{m1,y}\omega_{m2,x}\) - 继续：phim(3) 应等于 wm1(3)+wm2(3)+dphim(3)（圆锥补偿主要沿z轴） 4. 对比 sum(wm,1)(3) 与 phim(3)，两者之差就是圆锥补偿误差的直接数值体现——在高动态下差异可达10^-8 rad量级，长期积累会显著漂移

调试2：单子样+前一样模式（coneoptimal=2）¶

调试步骤： 1. 在 cnscl.m 第 L34 行设断点 2. 先调用一次 glv.wm_1 = [0;0;0]; 初始化缓存 3. 第一次调用 [~,~,dphim1] = cnscl(wm_a, 2)：因wm_1初值全零，dphim1=0；执行后glv.wm_1被设为wm_a 4. 第二次调用 [~,~,dphim2] = cnscl(wm_b, 2)：命中断点，查看 1/12*cross(glv.wm_1,wm) 应为 (1/12)·wm_a × wm_b，验证与手算一致 5. 若忘记重置 glv.wm_1，第三次调用会使用wm_b作为前一样，导致补偿量"串了"——此即"坑②"的调试重现

调试3：旋转补偿项验证¶

调试步骤： 1. 在 cnscl.m 第 L71 行设断点 2. 构造双子样：wm=[1e-4,0,0; 1e-4,0,0]（x轴纯转动），vm=[0,1e-3,0; 0,1e-3,0]（y轴纯加速） 3. 命中断点：wmm=[2e-4,0,0]，vmm=[0,2e-3,0] 4. 手算 rotm = (1/2)*cross(wmm,vmm) = [0, 0, (1/2)(2e-4·2e-3)] = [0,0,2e-7]，与运行结果对比 5. 物理理解：x轴旋转2e-4 rad ≈ 0.0115°，会把y方向速度增量"投影"一小部分到z方向，量级正好是2e-7 m/s

❌ 初学者最容易踩的坑¶

坑①：以为wm直接sum就够，高动态下误差爆炸¶

表现：跳过cnscl直接用sum(wm)做姿态更新，在有角振动（圆锥运动）时姿态z轴漂移远大于理论值。

为什么错：旋转不可交换性是二阶固有误差，不补偿时每步误差量级为 \(O(\|\Delta\boldsymbol{\theta}_1\| \cdot \|\Delta\boldsymbol{\theta}_2\|)\)。对100Hz IMU做10Hz更新（n=10子样），每子样1e-3 rad，每步圆锥误差≈(2/3)·1e-3·1e-3≈7e-7 rad，1小时累计=36000步×7e-7≈0.025 rad≈1.4°——这是不能接受的。

正确做法：永远通过cnscl处理多子样IMU数据，不要自己sum。

坑②：coneoptimal=2时忘记初始化/重置glv.wm_1，初期结果不对¶

表现：仿真前几步姿态误差突然跳变，之后又慢慢收敛，或跑完后重启仿真但glv全局变量没清，结果第二次跑一开始就有补偿量。

为什么错：coneoptimal=2依赖上一个SINS更新周期的glv.wm_1/vm_1做跨周期拼接。若第一次运行前wm_1=[0,0,0]，则第一步补偿量为0（正确）；但若中途重启而不清空wm_1，第一步就会携带上一次运行的最后一个样本做叉乘，产生虚假补偿。

正确做法：每次仿真开始前调用一次 glvf 重置全局变量，或手动 glv.wm_1=[0;0;0]; glv.vm_1=[0;0,0];。

坑③：输入imu只有3列（仅wm）时，dvbm输出全零，误以为函数坏了¶

表现：只传入 imu = [wx, wy, wz] 调用cnscl，发现dvbm=[0;0;0]，以为"划桨补偿没生效"。

为什么错：L53 if m>=6 决定了只有列数≥6（含加计dv列）时才进入速度补偿分支。3列输入时函数只做姿态圆锥补偿，dvbm被L51初始化为零且永远不会被覆盖。

正确做法：如果需要速度补偿，IMU数据必须是6列格式 [wm1,wm2,wm3, vm1,vm2,vm3]；若只有陀螺数据，则dvbm=0是正确行为。

坑④（补充）：n>6不报错但结果不同，因为imureshape自动压缩¶

表现：传入n=12子样，预期用12子样系数，但实际输出=6子样结果。

为什么错：L26-L28的imureshape会把n从12压缩到6（因为glv.csmax=6且12能被6整除）。补偿公式用的是6子样系数，不是12子样。

正确做法：SINS更新频率和IMU采样频率配合，保证一个更新周期内子样数n≤6；如果IMU频率极高，先在前端硬件/预处理器中做累加压缩。

🎯 配套练习¶

练习题：双子样圆锥补偿手算 vs cnscl对比¶

题目：手工构造两个连续角增量子样：

手算步骤要求： 1. 直接求和得名义旋转矢量：\(\boldsymbol{\phi}_{\text{sum}} = \boldsymbol{\omega}_{m1} + \boldsymbol{\omega}_{m2}\) 2. 计算叉乘：\(\boldsymbol{\omega}_{m1} \times \boldsymbol{\omega}_{m2} = \begin{vmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ 1e-4 & 0 & 0 \\ 0 & 1e-4 & 0 \end{vmatrix} = [0, 0, 1e-8]^T\) 3. 双子样补偿公式：\(\Delta\boldsymbol{\phi} = \boldsymbol{\phi}_{\text{sum}} + \frac{2}{3}(\boldsymbol{\omega}_{m1} \times \boldsymbol{\omega}_{m2})\) 4. 得到第三分量：\(\Delta\phi_z = 0 + \frac{2}{3} \times 1e-8 = 6.666666666666667\text{e-9}\,\text{rad}\)

Matlab验证代码：

glvf;  % 初始化glv全局变量，尤其是glv.cs和glv.csCompensate
wm1 = [1e-4, 0, 0];
wm2 = [0, 1e-4, 0];
imu_input = [wm1; wm2];  % n=2子样，m=3列（无加计）
[phim_cnscl, ~, dphim] = cnscl(imu_input, 0);

% 手算对比
phi_sum  = (wm1 + wm2)';
phi_man  = phi_sum + (2/3) * cross(wm1, wm2)';

% 验证1：cnscl输出phim与手算phi_man之差应<1e-15
diff1 = norm(phim_cnscl - phi_man);
fprintf('手算phim与cnscl输出差 = %.2e (应<1e-15)\n', diff1);
assert(diff1 < 1e-15, '圆锥补偿公式手算验证失败！');

% 验证2：dphim（圆锥补偿分项）应等于 (2/3)*cross(wm1,wm2)
diff2 = norm(dphim' - (2/3)*cross(wm1,wm2)');
fprintf('圆锥补偿分项dphim误差 = %.2e (应<1e-15)\n', diff2);
assert(diff2 < 1e-15, '圆锥补偿分项dphim验证失败！');

disp('✅ 双子样圆锥补偿验证全部通过！');

预期结果：两个差值均在1e-16量级（Matlab双精度舍入误差），验证cnscl核心公式实现完全正确。