4、传感器数据融合：卡尔曼滤波入门、扩展卡尔曼滤波（EKF）、Mahony互补滤波

各位同学，欢迎来到传感器数据融合这一章。

说实话，这是整个飞控系统里最“玄学”的部分。为什么这么说？因为传感器本身就不完美。加速度计有振动噪声，陀螺仪有零偏漂移，磁力计容易被干扰。你想想看，如果只用其中一个，那飞控基本就是个“瞎子”。

我刚开始做飞控那会儿，就吃过这个亏。当时图省事，直接用陀螺仪积分算姿态，结果飞机飞了不到30秒，姿态就歪到姥姥家去了。嗯，从那以后，我再也不敢轻视数据融合了。

4.1 为什么需要数据融合？

说白了，每个传感器都有自己的“脾气”：

  • 陀螺仪：动态响应快，但会漂移。积分时间越长，误差越大。
  • 加速度计：长期稳定，但短期噪声大。飞机一震动，数据就跳得厉害。
  • 磁力计：能提供绝对航向，但容易受环境磁场干扰。

数据融合的目的，就是取长补短。用陀螺仪的短期精度，配合加速度计和磁力计的长期稳定性，得到一套干净、可靠的姿态数据。

核心思想：用高频但会漂移的传感器（陀螺仪）做预测，用低频但稳定的传感器（加速度计/磁力计）做修正。

4.2 卡尔曼滤波入门

卡尔曼滤波，很多人一听就头大。其实它的逻辑很简单，就两步：预测和更新。

我习惯用一个比喻来解释：你开车进隧道，GPS信号断了。这时候你只能靠车速和方向盘角度，估算自己大概在什么位置——这就是预测。等出了隧道，GPS信号恢复，你发现实际位置和估算位置差了50米——这就是更新，用测量值去修正估算值。

4.2.1 基本公式

卡尔曼滤波的五个核心公式，我建议你背下来，但不用死记硬背，理解逻辑就行：

// 预测阶段
x_pred = A * x_prev + B * u          // 状态预测
P_pred = A * P_prev * A^T + Q        // 协方差预测

// 更新阶段
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)  // 卡尔曼增益
x_updated = x_pred + K * (z - H * x_pred)       // 状态更新
P_updated = (I - K * H) * P_pred                // 协方差更新

这里面的字母，我解释一下：

  • x：你要估计的状态量，比如角度、角速度。
  • P：状态的不确定性，也就是协方差。
  • Q：过程噪声，代表你对预测模型的信任程度。
  • R：测量噪声，代表你对传感器数据的信任程度。
  • K：卡尔曼增益，它决定了你是更相信预测值，还是更相信测量值。

我的经验：调参的时候，Q和R是最关键的。Q设得小，系统就“固执”，不太相信测量值；R设得小，系统就“敏感”，容易被噪声带偏。我一般先固定R，然后慢慢调Q，直到响应速度和平滑度都满意为止。

4.2.2 一维卡尔曼滤波示例

我们用一个最简单的例子——估计电池电压。假设你有一个电压传感器，但读数有噪声。你想得到一个平滑的电压值。

# 一维卡尔曼滤波（Python伪代码）
import numpy as np

# 初始化
x = 12.0          # 初始电压估计值
P = 1.0           # 初始估计误差协方差
Q = 0.01          # 过程噪声
R = 0.1           # 测量噪声

# 测量值序列（带噪声）
measurements = [12.1, 12.3, 11.9, 12.0, 12.2]

for z in measurements:
    # 预测
    x_pred = x
    P_pred = P + Q
    
    # 更新
    K = P_pred / (P_pred + R)
    x = x_pred + K * (z - x_pred)
    P = (1 - K) * P_pred
    
    print(f"测量值: {z:.2f}, 滤波后: {x:.2f}")

你看，滤波后的值比原始测量值平滑多了。这就是卡尔曼滤波的基本原理。

4.3 扩展卡尔曼滤波（EKF）

标准的卡尔曼滤波假设系统是线性的。但现实世界哪有那么多线性？姿态估计、GPS定位，基本都是非线性的。

这时候就需要扩展卡尔曼滤波了。它的思路很简单：把非线性函数线性化。怎么线性化？用泰勒展开，取一阶近似。

4.3.1 EKF的核心区别

和标准卡尔曼滤波相比，EKF就改了两个地方：

  • 状态预测时，用非线性函数 f(x) 代替线性矩阵 A。
  • 测量更新时，用非线性函数 h(x) 代替线性矩阵 H。
  • 计算协方差时，需要用到雅可比矩阵（也就是 f 和 h 的偏导数）。
// EKF预测阶段
x_pred = f(x_prev, u)          // 非线性状态预测
F = df/dx                      // 计算雅可比矩阵
P_pred = F * P_prev * F^T + Q

// EKF更新阶段
H = dh/dx                      // 计算测量函数的雅可比矩阵
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_updated = x_pred + K * (z - h(x_pred))
P_updated = (I - K * H) * P_pred

注意：EKF的线性化过程会引入近似误差。如果系统非线性太强，或者初始估计偏差太大，EKF可能会发散。我曾经在无人机高速机动时遇到过这个问题，后来改用无迹卡尔曼滤波（UKF）才解决。

4.3.2 姿态估计中的EKF

在飞控中，EKF常用于融合IMU（惯性测量单元）和GPS数据。状态量通常包括：位置、速度、姿态四元数、陀螺仪零偏等。

我简单说一下思路：

  • 预测：用陀螺仪角速度积分，更新四元数；用加速度积分，更新速度和位置。
  • 更新：用加速度计测量值修正俯仰和横滚；用磁力计修正航向；用GPS修正位置和速度。

嗯，这里要注意，四元数的更新是非线性的，所以必须用EKF。如果用标准卡尔曼滤波，姿态很快就会跑偏。

4.4 Mahony互补滤波

如果你觉得卡尔曼滤波太复杂，或者计算资源有限，那Mahony互补滤波是个不错的选择。

互补滤波的思路更直观：陀螺仪提供高频姿态变化，加速度计和磁力计提供低频姿态修正。两者通过一个比例-积分（PI）控制器融合在一起。

4.4.1 算法原理

Mahony滤波的核心是：用加速度计和磁力计的测量值，计算出一个“期望”的姿态，然后和陀螺仪积分的姿态做差，用这个误差去修正陀螺仪的零偏。

// Mahony互补滤波（简化版）
// 输入：陀螺仪角速度(gyro)，加速度计(acc)，磁力计(mag)
// 输出：四元数(q)

// 1. 用加速度计和磁力计计算期望姿态
// 2. 计算当前姿态与期望姿态的误差
// 3. 用PI控制器修正陀螺仪零偏
// 4. 用修正后的角速度更新四元数

// 关键参数
float Kp = 2.0;   // 比例增益，决定收敛速度
float Ki = 0.1;   // 积分增益，消除稳态误差

// 误差累积
error_integral += error * dt;

// 修正角速度
gyro_corrected = gyro + Kp * error + Ki * error_integral;

// 更新四元数
q += 0.5 * omega_matrix(q) * gyro_corrected * dt;

我的建议：如果你做的是玩具级飞控，或者计算资源紧张（比如STM32F103），用Mahony滤波就够了。它的计算量只有EKF的十分之一，效果在大多数场景下也够用。但如果你做的是工业级或竞赛级飞控，还是老老实实用EKF吧。

4.4.2 参数调优

Mahony滤波有两个关键参数：Kp和Ki。

参数 作用 调大 调小
Kp 比例增益，控制收敛速度 响应快，但噪声大 平滑，但响应慢
Ki 积分增益，消除陀螺仪零偏 零偏补偿快，但可能过冲 零偏补偿慢，但稳定

我一般先调Kp，让姿态能快速跟上实际变化。然后加一点Ki，慢慢消除陀螺仪的零偏误差。Ki不要太大，否则飞控会“晃”。

4.5 三种方法的对比

最后，我用一张表格总结一下这三种方法的特点：

方法 精度 计算量 适用场景
标准卡尔曼滤波 中等 低 线性系统，如一维信号滤波
扩展卡尔曼滤波（EKF） 高 高 非线性系统，如姿态+位置融合
Mahony互补滤波 中等 极低 资源受限，仅需姿态估计

避坑指南：我曾经在一个项目中，为了追求精度，直接上了EKF。结果发现MCU跑不动，帧率掉到50Hz，飞控直接炸机。后来换成Mahony滤波，帧率回到400Hz，飞行效果反而更好。所以，选方法之前，先算算你的算力够不够。

传感器数据融合方法对比 陀螺仪 加速度计 磁力计 数据融合 卡尔曼滤波 扩展卡尔曼滤波 Mahony互补滤波 根据算力和精度需求 选择合适的方法 输出 姿态四元数 位置/速度 反馈修正

好了，这一章的内容就到这里。数据融合是飞控的“大脑”，选对方法、调好参数，你的飞机才能飞得稳、飞得准。下一章我们会聊聊更具体的传感器校准和标定，到时候见。

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

← 上一章 📖 返回目录 下一章 →