1. EKF基础回顾：状态空间模型、线性化与雅可比矩阵、预测与更新步骤
各位同学，欢迎来到《无人机EKF状态估计器参数调优实战》的第一章。
说实话，EKF（扩展卡尔曼滤波）这东西，刚接触时容易让人头大。我当年第一次在无人机上跑EKF，看着那些矩阵公式，心里直打鼓。但后来我发现，只要把核心逻辑捋顺了，它其实没那么玄乎。
这一章，咱们就一起把EKF的底裤扒开看看。我会尽量用大白话，把我这些年踩过的坑、总结的经验，一股脑倒给你们。
1.1 状态空间模型：你在估计什么？
先问个问题：无人机在天上飞，你作为飞控，最想知道什么？
说白了，就是三个东西：位置、速度、姿态。这些就是我们要估计的状态。
状态空间模型，就是用一个数学框架，把这些状态随时间怎么变化给描述出来。它由两个方程组成：
- 状态方程：描述状态如何随时间演变。比如，速度积分一下就是位置变化。
- 观测方程：描述传感器（比如GPS、IMU）能测到什么。比如，GPS能测位置，但测不了姿态。
用数学语言写出来，大概是这个样子：
x_k = f(x_{k-1}, u_k) + w_k // 状态方程
z_k = h(x_k) + v_k // 观测方程这里，x_k是k时刻的状态，u_k是控制输入（比如油门），w_k是过程噪声。 z_k是观测值，v_k是观测噪声。
核心要点：状态空间模型就是你给EKF搭的“舞台”。舞台搭对了，戏才能唱好。我见过不少新手，状态变量选少了，结果估计出来的位置飘得离谱。
1.2 线性化与雅可比矩阵：EKF的精髓
你想想看，无人机飞行的模型，是线性的吗？
当然不是。姿态变化、空气动力学，全是非线性。标准的卡尔曼滤波（KF）只适用于线性系统，那怎么办？
EKF的聪明之处在于：线性化。
怎么线性化？用泰勒展开。把非线性的函数f和h，在当前估计值附近，用一阶导数近似成直线。这个一阶导数，就是雅可比矩阵。
举个例子，假设状态方程是：
x_k = sin(x_{k-1}) + u_k这是个非线性函数。它的雅可比矩阵 F 就是它对 x_{k-1} 的导数：
F = cos(x_{k-1})嗯，这里要注意：雅可比矩阵是EKF的“灵魂”。它决定了你线性化的方向准不准。如果雅可比算错了，EKF会直接发散。
个人经验：我在项目中遇到过，因为IMU的安装角度标定不准，导致雅可比矩阵里的旋转矩阵算错，结果无人机悬停时，高度估计值一直在漂。后来花了整整两天，才定位到是雅可比矩阵里一个符号写反了。所以，写雅可比矩阵时，一定要逐项检查。
1.3 预测与更新步骤：EKF的两步舞
EKF的工作流程，其实就是两步：预测和更新。像个舞步，循环往复。
第一步：预测（Time Update）
这一步，我们用状态方程，根据上一时刻的状态，猜一下当前时刻的状态大概在哪。
- 状态预测：
x_pred = f(x_est, u) - 协方差预测：
P_pred = F * P_est * F^T + Q
这里的 Q 是过程噪声协方差矩阵。它代表你对模型的信任程度。Q越大，说明你觉得模型不准，更相信传感器。
第二步：更新（Measurement Update）
这一步，我们用传感器观测值，来修正预测的结果。
- 计算卡尔曼增益：
K = P_pred * H^T * (H * P_pred * H^T + R)^{-1} - 状态更新：
x_est = x_pred + K * (z - h(x_pred)) - 协方差更新：
P_est = (I - K * H) * P_pred
这里的 R 是观测噪声协方差矩阵。它代表你对传感器的信任程度。R越大，说明传感器噪声大，你更相信模型预测。
避坑指南：我曾经在调参时，把Q和R设得特别极端。Q设得极小，R设得极大，结果EKF完全不听传感器的，只信模型，最后位置估计值直接飞出了地球。记住，Q和R的比值，决定了EKF的“性格”。
1.4 核心逻辑流程图
为了让你更直观地理解EKF的整个流程，我画了一张图。你看一遍，基本就通了。
1.5 本章小结
好了，这一章的内容就这些。我们来捋一捋：
- 状态空间模型：是EKF的“骨架”，定义了你要估计什么，以及传感器能测什么。
- 线性化与雅可比矩阵：是EKF的“魔法”，让非线性问题能用线性方法解。
- 预测与更新：是EKF的“舞步”，两步循环，不断修正估计。
说实话，这些概念光看文字可能有点干。但别急，后面几章我们会把这些东西全部用到实际的无人机数据上。到时候你回头再看这一章，会发现——哦，原来就这么回事。
一句话总结：EKF就是“用模型猜一下，用传感器修一下，再猜，再修”。循环往复，直到估计值收敛到真实值附近。