4、飞控核心算法：姿态解算（互补滤波）、姿态解算（EKF）、位置解算

飞控算法里，最核心的其实就是两件事：我到底朝哪？我到底在哪？

说白了，姿态解算告诉你飞机的“脑袋”冲哪个方向，位置解算告诉你飞机在三维空间里的坐标。这两个搞不定，后面什么控制、导航全是空中楼阁。

我个人习惯把姿态解算分成两派：一派是轻量级的互补滤波，适合资源受限的MCU；另一派是EKF（扩展卡尔曼滤波），精度高但计算量大。今天咱们就把这两个都捋一遍，再把位置解算也带上。

4.1 姿态解算之互补滤波

互补滤波，名字听着挺唬人，其实原理特别朴素。你想想看，陀螺仪测角速度，积分能得到角度，但积分会漂移；加速度计能测重力方向，算出俯仰和横滚，但噪声大、动态响应慢。

互补滤波就是：低频段信加速度计，高频段信陀螺仪。两者一结合，既解决了漂移，又抑制了噪声。

离散形式的互补滤波公式很简单：

// 互补滤波核心代码（C语言风格）
angle = 0.98 * (angle + gyro * dt) + 0.02 * acc_angle;

这里的0.98和0.02就是权重系数，加起来等于1。系数越大，越信任陀螺仪；系数越小，越信任加速度计。

嗯，这里要注意：互补滤波只适用于俯仰和横滚。偏航角（Yaw）没有加速度计来修正，只能靠磁力计。所以很多低成本的飞控，偏航角会慢慢漂移，就是这个原因。

4.2 姿态解算之EKF

EKF，全称扩展卡尔曼滤波。说白了，就是卡尔曼滤波的“非线性版本”。

为什么需要EKF？因为飞控系统是非线性的。你想想，姿态角的正余弦运算、四元数的乘法，哪个是线性的？标准卡尔曼滤波处理不了这些，所以得用EKF。

EKF的核心步骤，我总结为五步：

1. 状态预测：用陀螺仪数据预测下一时刻的姿态
2. 协方差预测：预测不确定性会变大
3. 计算卡尔曼增益：决定信传感器还是信模型
4. 状态更新：用加速度计/磁力计测量值修正预测值
5. 协方差更新：修正后的不确定性变小

我曾经在PX4的EKF代码里踩过一个坑。当时发现飞机悬停时高度一直在0.5米范围内波动，查了两天，最后发现是加速度计的噪声协方差矩阵设得太小了。EKF太相信加速度计，导致高度估计跟着加速度噪声一起抖。把协方差调大一个数量级，问题就解决了。

在Simulink里搭建EKF，我一般用MATLAB Function模块手写，或者用Aerospace Blockset里的现成模块。手写的好处是灵活，能加各种约束；用现成模块的好处是快，适合快速原型验证。

4.3 位置解算

位置解算，说白了就是回答一个问题：飞机现在在哪儿？

常用的方法有两种：

• 加速度积分法：对加速度计测得的加速度做二次积分，得到位移。但这个方法有个致命问题——误差会随时间平方增长。你想想，加速度计有零偏，积分一次变成速度误差，再积分一次变成位置误差，几秒钟就漂到天边去了。
• GPS + IMU融合：用GPS提供绝对位置，用IMU提供短时间内的相对位置变化，两者通过卡尔曼滤波融合。这是目前最主流的方法。

我在做物流无人机项目时，遇到过GPS信号被高楼遮挡的情况。当时位置解算全靠IMU积分，结果飞机在返航时偏了20多米，差点撞到树。后来加了视觉里程计作为辅助，才算把这个问题解决。

位置解算的Simulink模型，我一般这样搭建：

// Simulink模型结构示意
1. IMU数据输入 → 姿态解算（EKF） → 旋转矩阵
2. 加速度计数据 → 减去重力 → 转换到导航系
3. 导航系加速度 → 积分 → 速度 → 积分 → 位置
4. GPS数据 → 位置/速度观测 → 卡尔曼滤波融合
5. 输出：位置（NED坐标系）、速度

最后说一句，位置解算里有个容易被忽略的细节——坐标系转换。IMU测量的是机体坐标系下的加速度，而位置是在导航坐标系（NED）下计算的。中间需要用一个旋转矩阵把加速度从机体系转到导航系。这个矩阵就是由姿态角算出来的。如果姿态角不准，这个转换就会引入误差。

好了，这一章的内容就这些。互补滤波适合快速原型和资源受限的平台，EKF适合对精度要求高的场景。位置解算则要结合传感器特性，做好融合。下一章咱们会讲控制分配和混控器设计，到时候这些姿态和位置信息就会派上用场了。

---

公众号:蓝海资料掘金营，微信deep3321