4. 状态估计基础：卡尔曼滤波原理、EKF、UKF简介
各位同学，欢迎来到状态估计这一章。说实话，这是整个自主降落系统里最核心、也最容易让人头疼的部分。我当年刚接触多旋翼时，看到卡尔曼滤波的公式也是一脸懵。但别怕，今天我用最接地气的方式，带大家把这几个滤波器搞明白。
4.1 为什么需要状态估计？
你想想看，无人机在降落过程中，传感器数据是直接能用的吗？
当然不是。GPS有噪声，IMU有漂移，视觉里程计偶尔会丢帧。每个传感器都有自己的脾气。我们需要一个「大脑」，把这些乱七八糟的数据融合起来，得到一个靠谱的估计值。
这个大脑，就是状态估计器。而卡尔曼滤波，是其中最经典、最实用的工具。
核心思想： 用预测 + 测量，得到最优估计。说白了，就是「我不完全相信传感器，也不完全相信模型，我取一个折中，而且这个折中是最优的」。
4.2 卡尔曼滤波（KF）原理
卡尔曼滤波适用于线性系统、高斯噪声的场景。我刚开始做飞控时，用的就是标准KF做高度融合。嗯，这里要注意，它有几个关键假设：
- 系统是线性的
- 噪声是高斯白噪声
- 初始状态已知
它的工作流程，我习惯分成两步：预测和更新。
4.2.1 预测步骤
根据上一时刻的状态，预测当前时刻的状态。比如，我知道上一秒无人机在10米高度，速度是1m/s，那这一秒我猜它在11米左右。当然，这个预测有误差。
// 预测状态
x_pred = A * x_prev + B * u
// 预测协方差
P_pred = A * P_prev * A^T + Q4.2.2 更新步骤
拿到传感器测量值后，修正预测值。比如，激光测距仪说现在是10.8米，那我们就结合预测和测量，算出一个最优值。
// 卡尔曼增益
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个人经验： 调参时，Q和R的比值决定了你更相信模型还是传感器。我曾经在调试时把R设得太小，结果无人机跟着噪声乱跳，差点炸机。后来我学乖了，先让Q大一点，让滤波器平滑些，再慢慢调。
4.3 扩展卡尔曼滤波（EKF）
现实世界哪有那么多线性系统？无人机降落时，视觉观测模型是非线性的，IMU的旋转也是非线性的。这时候，标准KF就不够用了。
EKF的思路很简单：把非线性函数线性化。怎么线性化？用泰勒展开，取一阶近似。
举个例子，假设我们有非线性函数 f(x) 和 h(x)：
// 状态方程
x_k = f(x_{k-1}, u_k) + w_k
// 观测方程
z_k = h(x_k) + v_kEKF的做法是：在估计点附近，计算雅可比矩阵，把f和h近似成线性函数。
避坑指南： 我曾经在视觉SLAM项目中，因为雅可比矩阵算错了，导致滤波器发散。检查了半天才发现是求导时漏了一项。所以，写EKF时，雅可比矩阵一定要手算一遍，别完全依赖自动求导。
EKF的流程和KF类似，只是把A和H换成了雅可比矩阵：
// 预测
x_pred = f(x_prev, u)
F = df/dx (雅可比矩阵)
P_pred = F * P_prev * F^T + Q
// 更新
H = dh/dx (雅可比矩阵)
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_pred4.4 无迹卡尔曼滤波（UKF）
EKF有个问题：线性化会引入误差。如果非线性很强，一阶近似就不够用了。这时候，UKF登场了。
UKF的思想很巧妙：我不去近似非线性函数，而是用一堆采样点（Sigma点）去逼近概率分布。说白了，就是「用点云代替解析解」。
4.4.1 Sigma点采样
根据当前状态的均值和协方差，生成2n+1个Sigma点（n是状态维度）。这些点能很好地代表原分布。
// 生成Sigma点
λ = α^2 * (n + κ) - n
X0 = x_mean
Xi = x_mean + sqrt((n+λ)*P) (i=1..n)
Xi = x_mean - sqrt((n+λ)*P) (i=n+1..2n)4.4.2 预测与更新
把每个Sigma点通过非线性函数传播，然后加权平均得到预测值。更新时也是类似，用Sigma点计算协方差和卡尔曼增益。
我的建议： 如果你的系统非线性很强（比如大角度姿态估计），优先用UKF。它比EKF更鲁棒，而且不需要算雅可比矩阵。我在做视觉惯性里程计时，从EKF换成UKF后，精度提升了约15%。
4.5 三种滤波器的对比
| 特性 | KF | EKF | UKF |
|---|---|---|---|
| 适用系统 | 线性 | 弱非线性 | 强非线性 |
| 计算复杂度 | 低 | 中 | 较高 |
| 是否需要雅可比 | 否 | 是 | 否 |
| 精度 | 最优（线性） | 一阶近似 | 二阶近似 |
| 实现难度 | 简单 | 中等 | 中等 |
4.6 知识体系总览
下面这张图，是我自己总结的三种滤波器选择逻辑。你一看就明白什么时候该用哪个。
4.7 实战中的选择建议
说了这么多理论，到底怎么选？我给大家一个实战清单：
- GPS + IMU融合： 用标准KF就够了，因为GPS位置和IMU加速度的关系是线性的。
- 视觉惯性里程计（VIO）： 推荐UKF。视觉观测模型是非线性的，UKF比EKF更稳定。
- 自主降落中的高度估计： 如果只用激光测距，KF就行。如果融合视觉深度，建议UKF。
我的习惯： 先写一个标准KF框架，然后根据实际效果决定是否升级。不要一开始就上UKF，调试起来更复杂。记住，简单够用就好。
好了，状态估计的基础就讲到这里。这三种滤波器是后续视觉定位和自主降落的基石。下一节我们会把这些理论用到实际的降落场景中，到时候你会看到它们是如何协同工作的。