168、飞控中的多传感器融合：无迹卡尔曼滤波（UKF）

飞控中的多传感器融合：无迹卡尔曼滤波（UKF）

从一次炸机说起

去年夏天，我在调试一架四轴飞行器的室内悬停。IMU数据看起来干净，GPS信号在室外也正常，但一进室内，飞控就开始抽风——姿态偶尔跳变，高度漂移，最后直接翻倒炸机。事后复盘，问题出在传感器融合上：标准EKF（扩展卡尔曼滤波）在强非线性场景下，雅可比矩阵的线性近似误差被放大了，尤其是当无人机靠近地面时，气压计和加速度计的非线性耦合让EKF直接“失智”。

那次之后，我彻底把EKF换成了UKF。不是EKF不能用，而是UKF在处理飞控这种强非线性系统时，容错率更高，调试起来更省心。今天这篇笔记，就聊聊UKF在飞控多传感器融合中的落地细节。

UKF的核心思想：用采样代替线性化

EKF的痛点在于：你必须对非线性函数求导，得到雅可比矩阵。飞控里的状态方程和观测方程，比如四元数旋转、加速度计投影、磁力计倾斜补偿，全是非线性。求导不仅麻烦，而且一旦模型复杂，雅可比算错一个符号，整个滤波就崩了。

UKF的思路很暴力：我不去线性化你的函数，我直接在你的状态分布上撒一堆点（Sigma点），把这些点通过非线性函数传过去，再统计传过去之后的均值和协方差。只要点撒得够聪明，结果就足够逼近真实分布。

这里有个坑：Sigma点的数量是2n+1（n是状态维度）。飞控状态通常包括位置、速度、姿态四元数、陀螺零偏、加速度计零偏，至少16维。2*16+1=33个点，每个点都要跑一遍状态预测和观测预测，计算量比EKF大一个量级。但别慌，现代STM32H7或F7系列，浮点运算能力足够，实测200H