3. 捷联惯导解算

好，咱们进入正题。捷联惯导解算，说白了就是回答三个问题：我现在朝哪看？我现在在哪？我现在跑多快？ 这三个问题分别对应姿态、位置和速度的更新。

我个人习惯把姿态更新放在第一位。为什么？因为位置和速度的推算，全都依赖姿态。方向错了，后面全白搭。我在项目里见过太多这样的案例——陀螺数据漂移一点点，几分钟后位置误差就飞了。

3.1 姿态更新：四元数 vs 欧拉角

姿态更新的核心，就是利用陀螺仪测到的角速度，不断推算载体当前的朝向。常用的数学工具有两种：四元数和欧拉角。

3.1.1 欧拉角法

欧拉角最直观。俯仰、横滚、航向，三个角度一摆，谁都看得懂。但有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和航向会耦合在一起，没法区分。我刚开始做飞控时就踩过这个坑，飞机垂直爬升时姿态直接炸了。

所以，欧拉角只适合用在俯仰角变化不大的场景，比如地面车辆或平飞状态。对于无人机、机器人这种全姿态运动的，千万别用。

3.1.2 四元数法

四元数没有奇异性，计算也高效。它用四个参数表示旋转，避免了欧拉角的三角函数运算。更新公式也很简洁：

// 四元数更新（一阶龙格-库塔法）
// q = [q0, q1, q2, q3]^T
// ω = [ωx, ωy, ωz]^T 为陀螺仪角速度

double dq[4];
double norm = 0.5 * dt;

dq[0] = -q[1]*ωx - q[2]*ωy - q[3]*ωz;
dq[1] =  q[0]*ωx - q[3]*ωy + q[2]*ωz;
dq[2] =  q[3]*ωx + q[0]*ωy - q[1]*ωz;
dq[3] = -q[2]*ωx + q[1]*ωy + q[0]*ωz;

q[0] += norm * dq[0];
q[1] += norm * dq[1];
q[2] += norm * dq[2];
q[3] += norm * dq[3];

// 归一化，防止累积误差
double inv_norm = 1.0 / sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] *= inv_norm;
q[1] *= inv_norm;
q[2] *= inv_norm;
q[3] *= inv_norm;

3.2 速度与位置更新

姿态算准了，接下来就是速度和位置。说白了就是加速度计测到的比力，去掉重力，再积分。

公式其实不复杂：

// 速度更新（导航坐标系下）
// v_n 为导航系速度，f_b 为加速度计比力，g_n 为重力矢量
// C_b^n 为姿态矩阵（由四元数转换而来）

// 1. 将比力从载体系转换到导航系
double f_n[3];
f_n[0] = C_b_n[0][0]*f_b[0] + C_b_n[0][1]*f_b[1] + C_b_n[0][2]*f_b[2];
f_n[1] = C_b_n[1][0]*f_b[0] + C_b_n[1][1]*f_b[1] + C_b_n[1][2]*f_b[2];
f_n[2] = C_b_n[2][0]*f_b[0] + C_b_n[2][1]*f_b[1] + C_b_n[2][2]*f_b[2];

// 2. 去除重力，更新速度
v_n[0] += (f_n[0] + g_n[0]) * dt;
v_n[1] += (f_n[1] + g_n[1]) * dt;
v_n[2] += (f_n[2] + g_n[2]) * dt;

// 3. 位置更新（简单梯形积分）
p_n[0] += (v_n[0] + v_n_prev[0]) * 0.5 * dt;
p_n[1] += (v_n[1] + v_n_prev[1]) * 0.5 * dt;
p_n[2] += (v_n[2] + v_n_prev[2]) * 0.5 * dt;

3.3 圆锥误差与划船误差补偿

这两个误差，是捷联惯导里最容易被忽视的坑。说白了，它们都源于一个事实：陀螺和加速度计的采样是离散的，而真实运动是连续的。

3.3.1 圆锥误差

圆锥误差发生在姿态更新中。当载体同时存在两个正交方向的角振动时，如果只用简单的角速度积分，算出来的姿态会慢慢偏离真实值。形象点说，就像圆锥的母线绕轴旋转，你只采样了几个点，结果算出来的方向一直在漂。

补偿方法也很经典——多子样算法。我常用的三子样补偿：

// 三子样圆锥误差补偿
// Δθ1, Δθ2, Δθ3 为三个子样间隔的角增量

double Δθ_sum = Δθ1 + Δθ2 + Δθ3;
double Δθ_cross = (1.0/3.0) * (Δθ1×Δθ2 + Δθ2×Δθ3) 
                + (2.0/3.0) * (Δθ1×Δθ3);

// 补偿后的等效旋转矢量
double Φ = Δθ_sum + Δθ_cross;

3.3.2 划船误差

划船误差是速度更新里的对应问题。当载体同时存在线振动和角振动时，比力积分也会产生类似的漂移。名字很形象——就像划船时，船身的晃动和桨的摆动耦合在一起。

补偿公式和圆锥误差很像，只是把角增量换成了速度增量：

// 三子样划船误差补偿
// Δv1, Δv2, Δv3 为三个子样间隔的速度增量
// Δθ1, Δθ2, Δθ3 为对应的角增量

double Δv_sum = Δv1 + Δv2 + Δv3;
double Δv_cross = (1.0/3.0) * (Δθ1×Δv2 + Δθ2×Δv3) 
                + (2.0/3.0) * (Δθ1×Δv3);

// 补偿后的速度增量
double Δv_comp = Δv_sum + Δv_cross;

3.4 知识体系总览

下面这张图，把本章的核心逻辑串起来了。你可以把它当作一个速查地图：

嗯，这张图把整个流程串起来了。你从左边陀螺仪和右边加速度计出发，分别走姿态和速度两条线，中间用姿态矩阵耦合，最后汇合到位置更新。圆锥和划船补偿就像两个「纠偏员」，在关键节点上把离散采样带来的误差拉回来。

我个人建议，在实际写代码时，先把不带补偿的版本跑通，再逐步加上圆锥和划船补偿。这样出了问题也好定位。我曾经一上来就全加上，结果某个子样符号写反了，查了三天才找到bug。

好了，这一章的内容就到这。记住：姿态是灵魂，补偿是细节，两者缺一不可。