2. 状态估计基础
PX4 的 EKF2 并不是“把传感器数据简单加权平均”的模块，而是一套以概率论为语言、以贝叶斯滤波为骨架、以卡尔曼滤波为工程实现的状态估计器。理解这条主线，才能在调参和排障时知道每个参数究竟在改变什么。
一、核心原理：用概率描述“我不知道”
状态估计的本质是：在存在噪声和不确定性的条件下，推断系统真实状态。概率论提供了描述不确定性的工具——我们不再说“高度是 10 米”，而是说“高度服从均值 10 米、标准差 0.3 米的高斯分布”。
高斯分布（正态分布）由均值 μ 和方差 σ² 完全确定，具有两个关键性质：线性变换后仍是高斯分布，两个高斯分布相乘或相加后仍是高斯分布。这两条性质使高斯成为卡尔曼滤波能够“闭式递推”的数学基础。在 EKF2 中，状态向量、过程噪声、观测噪声全部用高斯分布建模，协方差矩阵 P 就是多维高斯分布的“方差”。
二、方法流程：从贝叶斯滤波到卡尔曼滤波
贝叶斯滤波是通用框架，它用两步递推更新状态的后验概率：
- 预测步：根据上一时刻后验和控制输入，计算当前时刻先验分布。
- 更新步：根据当前观测，用贝叶斯公式把先验修正为后验。
贝叶斯滤波在一般情况下需要处理任意概率分布，无法用有限参数表示。卡尔曼滤波做了三个假设：系统线性、噪声高斯、初始状态高斯。于是所有分布始终是高斯的，只需递推均值与协方差即可。
扩展卡尔曼滤波（EKF）进一步放宽“线性”假设：对非线性运动模型和观测模型，在当前估计点附近做一阶泰勒展开，用雅可比矩阵代替线性矩阵，其余递推结构与卡尔曼滤波完全一致。PX4 EKF2 正是 EKF 的多传感器融合实现。
三、案例/场景：EKF2 如何融合 IMU 与 GPS
以水平位置估计为例。IMU 加速度积分给出位置预测，但会随时间漂移；GPS 给出绝对位置，但噪声大、更新慢。EKF2 的处理逻辑是：
- 用 IMU 做预测步，状态协方差 P 随积分时间增大，表示“越来越不确定”。
- GPS 到达时做更新步，计算卡尔曼增益 K。若 P 大而 GPS 噪声 R 小，K 接近 1，更相信 GPS；反之更相信预测。
- 更新后 P 减小，表示不确定性被观测压缩。
这正是“预测—校正”循环的物理含义：协方差 P 是信任分配器。EKF2 参数 EKF2_GPS_P_NOISE、EKF2_ACC_NOISE 等，本质上就是在设定 Q 和 R，从而改变 K 的取值倾向。
# PX4 中与 EKF2 噪声模型相关的典型参数
EKF2_GPS_P_NOISE # GPS 水平位置观测噪声 R
EKF2_GPS_V_NOISE # GPS 速度观测噪声 R
EKF2_ACC_NOISE # 加速度计过程噪声 Q
EKF2_GYR_NOISE # 陀螺仪过程噪声 Q
EKF2_BARO_NOISE # 气压高度观测噪声 R
四、误区与复盘
- 误区二：噪声参数越小越好。 把 R 设得过小会让滤波器过度相信某个传感器，一旦该传感器异常，估计会被带偏。
- 误区三：忽略协方差一致性。 调参后不仅要看估计值是否平滑，还要看新息（innovation）是否为零均值白噪声；若新息有偏，说明模型或噪声设定有问题。
- 复盘要点： 出现位置跳变时，先查 Q/R 比例，再查传感器时间戳与延迟，最后查线性化点是否远离真实状态。
| 滤波方法 | 系统假设 | 分布表示 | 计算量 | PX4 对应 |
|---|---|---|---|---|
| 贝叶斯滤波 | 无线性/高斯要求 | 任意分布 | 不可解析 | 理论框架 |
| 卡尔曼滤波 | 线性 + 高斯 | 均值 + 协方差 | 低 | 线性子模块 |
| 扩展卡尔曼滤波 | 非线性可微 + 高斯 | 均值 + 协方差 | 中 | EKF2 主体 |
掌握这条“概率描述 → 贝叶斯递推 → 高斯简化 → 线性化扩展”的链条，后续理解 EKF2 的状态向量、协方差传播、传感器融合门限和故障检测，都会变得有据可依。