姿态解算（四元数、欧拉角、互补滤波、卡尔曼）
第一部分：从“人”到“机器”——姿态的本质（第一性原理）
1. 生活中的例子（闭眼走直线）
请你现在闭眼，向前走10步。你会发现自己大概率走歪了。
原因：你的肌肉（陀螺仪）告诉你“我在直走”，但地面的微小倾斜和左右脚发力不均（积分漂移）导致误差累积。
解决：你睁开眼看一眼远方（加速度计/磁力计），用绝对参照系校正一下方向。
结论：姿态解算的本质，就是“闭眼走”（高频积分）和“睁眼看”（低频绝对）的博弈。
2. 数学物理建模（刚体旋转）
在三维空间中，物体绕质心旋转。描述这个旋转，大学物理告诉我们有三个工具：
| 工具 | 数学本质 | 物理直观 | 致命缺陷 |
|---|---|---|---|
| 欧拉角 | 3个独立变量 (Roll/Pitch/Yaw) | 像炮塔的方位角、俯仰角 | 万向锁（当Pitch=±90°，Yaw和Roll耦合，丢失一个自由度） |
| 旋转矩阵 | 3x3 正交矩阵 (9个参数) | 绝对精确，无奇点 | 计算量大，9个参数必须满足6个约束条件，漂移后极难维护正交性 |
| 四元数 | 4维复数(a+bi+cj+dk) | 数学上的“四维球面”，完美避免万向锁 | 抽象，反直觉 |
给小白的第一条铁律：工程上绝不直接存欧拉角做积分，内存里只存四元数，只在给用户看或控制舵机输出时，才把四元数转为欧拉角。
第二部分：传感器的“自私”与“偏见”（物理与统计）
要让机器感知姿态，只有三种“感官”：
陀螺仪（角速度计）：
物理原理：科里奥利力（谐振质量偏移）。
数学模型：ω=dθdt。姿态 = ∫ω⋅dt。
性格：短时天才，长时白痴。高频响应极准，但积分1分钟，漂移几十度（因为直流偏置bias被积分）。
加速度计（比力计）：
物理原理：牛顿第二定律 F=ma。静止时，它测的是重力 g 在机体轴上的分量。
性格：低频绝对准，高频像疯子。一旦有震动（运动加速度），它测出的方向就变成了“重力+运动加速度”的合矢量，完全不可信。
磁力计（指南针）：
物理原理：地磁场强度分量。
性格：绝对航向（Yaw）唯一来源，但极易受电机、高压线、金属家具干扰（软硬磁误差）。
第三部分：核心算法 (一) —— 互补滤波（Complementary Filter）
1. 第一性原理（频率互补）
既然陀螺仪高频准（像收音机的调频），加速度计低频准（像调幅）。我们不需要复杂数学，只需要一个高通滤波器处理陀螺仪，一个低通滤波器处理加速度计，然后把结果加起来。因为 1=0.98+0.02，这就是“互补”的由来。
2. 数学模型（一阶低通与高通）
设角度为 θ，采样周期为 dt，陀螺仪角速度为 ω，加速度计算出的角度为 θacc。
θ融合(t)=α×[θ融合(t−1)+ω⋅dt]+(1−α)×θacc
这里的 α=0.98（时间常数T=α⋅dt1−αT，工程上通常取 T=0.1s∼0.5s）。
3. 生活案例（手机屏幕旋转）
你横着拿手机看视频，屏幕跟着转。
极快旋转时，陀螺仪占98%，屏幕秒切横屏（实时性好）。
你手轻微发抖时，加速度计占2%，屏幕纹丝不动（抑制高频抖动噪声）。
4. 实用工作建议（针对互补滤波）
适用场景：低成本MCU（如STM32F1）、高动态飞行器（穿越机）。因为计算量极小。
调参秘诀：α越接近1，跟踪越灵敏但漂移大；α越接近0，越抗震动但延迟大。先固定 T=0.2s，反算 α=T/(T+dt)α，这是最科学的调法。
第四部分：核心算法 (二) —— 卡尔曼滤波（Kalman Filter）
1. 第一性原理（最优数据融合——高斯分布）
如果加速度计有噪声 R（方差），陀螺仪有噪声 Q（方差）。我们不拍脑袋定 0.98，而是根据当前震动程度动态计算权重（卡尔曼增益 K）。
大学数学直观：两个独立高斯分布相乘，得到的新高斯分布均值，就是加权平均，权重反比于方差。谁的噪声小（方差小），谁就更可信。
2. 物理模型（经典五步法）——用“司机出隧道”解释
状态量：真实角度 x 和陀螺仪零偏 bias（把误差也当状态来估计！）。
预测（先验）：xk∣k−1=xk−1+(ω−bias)⋅dt。（司机在隧道内完全靠速度计推算位置，噪声 QQ 代表对速度计的信心）。
更新（后验）：xk∣k=xk∣k−1+K⋅(zk−xk∣k−1)。（出隧道口看到GPS，zk 就是加速度计测的角度，噪声 R 代表对GPS的信心）。
灵魂解读卡尔曼增益 K：
如果隧道内速度计极差（Q大），一出隧道，K 自动变大，疯狂相信GPS。
如果GPS信号差（R大，比如加速度计剧烈震动），K自动变小，我宁可相信陀螺仪积分。
3. 图表模型（状态观测器逻辑）
第五部分：四元数的“降维打击”（数学落地）
既然我们知道了如何融合，那么卡尔曼滤波里的“预测”怎么推演？
解法：四元数微分方程
如果机体以角速度 ω=[ωx,ωy,ωz]旋转，四元数 q随时间的变化率为（大学物理刚体运动学）：
q˙=12⋅q⊗ω
在代码中，我们利用一阶龙格-库塔（泰勒展开）近似更新：
（AI生成）
python
# 伪代码 (忽略小量二阶项) q += 0.5 * dt * q * omega q = q / norm(q) # 强制归一化，防止数学漂移（关键！）
给小白的大实话：千万不要在代码里直接对欧拉角做积分更新姿态！必须用四元数更新。你不需要理解 i,j,ki,j,k 的乘法表，只需要会用库函数
quaternion_multiply。但在工程面试中，一定要说出“单位四元数归一化是保证旋转矩阵正交性的关键”。
第六部分：实战避坑指南（建议）
如果现在让你去写飞控或云台的姿态代码，请收好这5条建议：
| 序号 | 场景痛点 | 第一性原理解决方案 |
|---|---|---|
| 1 | 磁力计干扰导致Yaw乱飘 | 磁力计只用于修正Yaw（偏航），绝不修正Roll/Pitch。且判断磁力计模长magx2+magy2+magz2，若偏离地磁场强度（通常50~60uT）超过5%，立即将卡尔曼的 RR 设为无穷大（即抛弃磁力计测量值）。 |
| 2 | 震动导致加速度计“饱和” | 检测加速度计模长 accx2+accy2+accz2，如果偏离 9.8m/s29.8m/s2 超过 0.50.5，说明机体在做剧烈机动。此时增大卡尔曼的 RR 参数（量测噪声），让滤波器纯靠陀螺仪。 |
| 3 | 陀螺仪零偏随时间变化（温度漂移） | 静态启动时，求前200帧陀螺仪数据的平均值作为零偏 bias。如果在动态中，必须靠卡尔曼滤波器中的增广状态量（Bias）来在线估计，否则积分一天漂移180度。 |
| 4 | 代码灵魂三问（调参顺序） | ① 原始数据过低通滤波了吗？（硬件滤波或滑动窗口滤波，去除高频尖刺） ②坐标系对齐了吗？（加速度计Z轴朝地，陀螺仪正负方向是否符合右手定则） ③ 如果只用互补滤波，时间常数TT 宁愿取大一点（0.5s），稳比快重要！ |
| 5 | 终极建议 | 除非你是做航天或超高速军用无人机，否则别自己手撕卡尔曼矩阵求逆！工程中直接用Madgwick 滤波算法（基于梯度下降）或Mahony 滤波（互补滤波变种）。这两者开源代码成熟，且计算量仅为卡尔曼的 1/10，收敛效果极佳。 |
结语（总结）
数学上：用四元数避免万向锁，用高斯分布理解噪声。
物理上：陀螺仪提供“趋势”，加速度计/磁力计提供“锚点”。
工程上：固定机动的用互补滤波（代码简单），强震动高精度的用卡尔曼（动态调整权重），但无论哪种，记得先把原始数据里的“直流偏置”和“高频震动”用最朴素的均值滤波干掉！
葡萄美酒夜光杯，欲饮琵琶马上催。——王翰《凉州词·其一》