1. EKF基础回顾：从线性到非线性的思维跃迁

各位同学，欢迎来到《多旋翼飞控EKF姿态估计深度解析》的第一章。

说实话，每次讲EKF，我都习惯先带大家回顾一下卡尔曼滤波的根基。为什么？因为我在实际项目中见过太多人——包括我自己早期——直接套用EKF公式，结果飞控在天上飘得跟喝醉了似的。嗯，那感觉，真不好受。

所以这一章，咱们把地基打牢。我会用我自己的理解方式，把状态空间模型、线性卡尔曼、再到EKF的扩展，掰开了讲清楚。

1.1 状态空间模型：你总得知道系统长什么样

做飞控，说白了就是在跟「状态」打交道。什么是状态？就是能完整描述系统当前情况的一组数值。

拿四旋翼来说，它的状态包括：

  • 姿态角：横滚、俯仰、偏航
  • 角速度：三个轴的转动速率
  • 位置与速度：当然，姿态估计里我们更关注前者

状态空间模型，就是用数学语言把这些状态串起来。它由两个方程组成：

状态方程：xk = A·xk-1 + B·uk-1 + wk-1

观测方程：zk = H·xk + vk

这里：

  • xk 是k时刻的状态向量
  • A 是状态转移矩阵——描述上一时刻状态如何影响当前
  • B 是控制输入矩阵——你给油门，系统怎么响应
  • uk-1 是控制输入
  • w 是过程噪声——模型总有不完美的地方
  • zk 是观测值，比如加速度计读数
  • H 是观测矩阵——把状态映射到观测空间
  • v 是观测噪声——传感器也不是完美的

我个人习惯，把状态方程想象成「预测」，观测方程想象成「修正」。这两个步骤，就是卡尔曼滤波的灵魂。

1.2 线性卡尔曼滤波：五个公式走天下

线性卡尔曼滤波，是EKF的基石。它假设系统是线性的，噪声是高斯白噪声。在这个前提下，五个公式就能搞定最优估计。

我当年第一次看这五个公式，觉得像天书。后来做项目多了，发现其实就是两个阶段：

预测阶段（时间更新）

  1. 状态预测：x̂k|k-1 = A·x̂k-1|k-1 + B·uk-1
  2. 协方差预测：Pk|k-1 = A·Pk-1|k-1·AT + Q

更新阶段（测量更新）

  1. 卡尔曼增益：Kk = Pk|k-1·HT·(H·Pk|k-1·HT + R)-1
  2. 状态更新：x̂k|k = x̂k|k-1 + Kk·(zk - H·x̂k|k-1)
  3. 协方差更新：Pk|k = (I - Kk·H)·Pk|k-1

我的小技巧：卡尔曼增益K，你可以理解为「相信传感器多一点，还是相信模型多一点」。K大，信传感器；K小，信模型。我在调参时，经常盯着这个值看，它能告诉你系统是不是快发散。

你想想看，这五个公式其实就干了一件事：用观测数据修正模型预测。但问题来了——现实世界哪有那么多线性系统？

1.3 从线性到非线性：为什么我们需要EKF

四旋翼的姿态动力学，是典型的非线性系统。为什么？

  • 姿态角的变化涉及三角函数（sin、cos）
  • 陀螺仪的测量模型包含角速度的乘积项
  • 加速度计在非水平状态下，重力分量会耦合到各个轴

线性卡尔曼滤波处理不了这些。你硬套的话，估计出来的姿态角会慢慢漂移，最后完全不可信。我早期踩过这个坑——飞控悬停时，明明飞机是平的，姿态角却显示有10度倾斜。查了半天，发现就是非线性没处理好。

所以，我们需要一种方法，把非线性问题「近似」成线性问题来处理。这就是EKF的核心思想。

1.4 EKF的核心思想：线性化 + 卡尔曼滤波

EKF的做法，说白了就是：在当前估计点附近，用泰勒展开把非线性函数线性化。

具体来说：

  • 对于非线性状态方程 f(x)，我们在 x̂k-1|k-1 处做一阶泰勒展开
  • 对于非线性观测方程 h(x)，我们在 x̂k|k-1 处做一阶泰勒展开
  • 然后用展开后的雅可比矩阵替代原来的A和H

数学框架长这样：

非线性系统模型：

xk = f(xk-1, uk-1) + wk-1

zk = h(xk) + vk

线性化后的EKF递推：

Fk-1 = ∂f/∂x |x̂k-1|k-1

Hk = ∂h/∂x |x̂k|k-1

然后，把F和H代入标准卡尔曼滤波的五个公式中，就得到了EKF。

注意：EKF的线性化是一阶近似。如果系统非线性太强，或者你的估计点离真实值太远，这个近似就会失效。我曾经在无人机做大机动飞行时遇到过——角速度太大，线性化误差累积，最后EKF直接发散。所以，EKF不是万能的，它要求系统「弱非线性」。

1.5 知识体系总览

为了让你对本章内容有个整体印象，我画了一张图。它把状态空间模型、线性KF、EKF的关系串了起来：

第一章：EKF基础回顾 · 知识体系 状态空间模型 xk = f(xk-1, uk-1) + wk-1   |   zk = h(xk) + vk 线性卡尔曼滤波 假设：线性系统 + 高斯噪声 五个公式：预测 + 更新 非线性挑战 四旋翼姿态动力学 三角函数 · 耦合项 · 大机动 EKF：线性化 + 卡尔曼滤波 泰勒展开 → 雅可比矩阵 F = ∂f/∂x , H = ∂h/∂x 代入标准KF五公式 → 非线性状态估计 多旋翼飞控姿态估计 · 实战应用

从这张图你可以看到，整个知识脉络是：状态空间模型 → 线性KF → 发现非线性问题 → EKF线性化解决。这也是我设计这门课的逻辑——不跳步，一步步来。

1.6 本章小结

好了，第一章的内容就到这里。我们回顾了：

  • 状态空间模型的两个方程——预测和观测
  • 线性卡尔曼滤波的五个公式——预测两步，更新三步
  • 为什么飞控需要EKF——因为姿态动力学是非线性的
  • EKF的核心思想——在当前估计点做泰勒展开，用雅可比矩阵替代线性模型

下一章，我们会深入飞控的传感器模型——陀螺仪、加速度计、磁力计，它们各自有什么特性？噪声怎么建模？这些是EKF能跑起来的前提。咱们到时候细聊。

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

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