3. 状态估计与滤波：卡尔曼滤波在高度与速度估计中的应用
各位同学，今天我们来聊聊飞控系统里一个绕不开的话题——状态估计。说白了，就是怎么让飞控知道它自己飞得有多高、跑得有多快。
你可能会问：传感器不是直接能测吗？嗯，理论上是的。但实际项目中你会发现，传感器数据全是“噪声”。气压计受风影响，加速度计有漂移，GPS在树下就飘。我当年调试第一架无人直升机时，就吃过这个亏——传感器读数跳得像心电图，飞机差点翻跟头。
所以，我们需要一个“大脑”，把多个不完美的传感器数据融合起来，给出一个靠谱的估计值。这个大脑，就是卡尔曼滤波器。
3.1 为什么需要卡尔曼滤波？
先看一个简单场景。你想知道直升机当前的高度。
- 气压计：能测高度，但受气流扰动，噪声大。
- 加速度计：能测垂直加速度，但积分后漂移严重。
- GPS：高度精度差，更新频率低。
单独用任何一个，都不靠谱。卡尔曼滤波的思路是：用模型预测 + 用测量修正。它像一个聪明的裁判，既相信物理规律，也参考传感器读数，最终给出一个“最优估计”。
核心思想：卡尔曼滤波不是“消除噪声”，而是“在噪声中找出最可能的真实值”。
3.2 卡尔曼滤波的五个核心公式
别被公式吓到。我拆开讲，你就懂了。
我们以高度估计为例。状态变量设为：
x = [高度, 垂直速度]系统模型（预测步）：
- 状态预测：根据上一时刻的状态，推算当前时刻的状态。
- 协方差预测：估计这个预测的“不确定度”有多大。
测量更新（修正步）：
- 卡尔曼增益计算：决定“更相信模型”还是“更相信传感器”。
- 状态更新：用测量值修正预测值。
- 协方差更新：更新不确定度。
我的经验：卡尔曼增益 K 是关键。K 大，说明传感器可信；K 小，说明模型更准。我在项目中调试时，经常盯着 K 值看——它就像飞控的“自信心指数”。
3.3 高度估计的完整实现
下面是一个简化版的一维卡尔曼滤波代码，用于高度估计。我建议你亲手跑一遍。
// 一维卡尔曼滤波 - 高度估计
typedef struct {
float x; // 状态：高度 (m)
float P; // 估计误差协方差
float Q; // 过程噪声协方差
float R; // 测量噪声协方差
} Kalman1D;
void Kalman_Init(Kalman1D *kf, float init_height) {
kf->x = init_height;
kf->P = 1.0f; // 初始不确定度
kf->Q = 0.01f; // 模型噪声（调参重点）
kf->R = 0.1f; // 传感器噪声（可从数据手册查）
}
float Kalman_Update(Kalman1D *kf, float z, float dt) {
// 1. 预测
float x_pred = kf->x; // 假设高度不变（简化模型）
float P_pred = kf->P + kf->Q; // 不确定度增加
// 2. 更新
float K = P_pred / (P_pred + kf->R); // 卡尔曼增益
kf->x = x_pred + K * (z - x_pred); // 修正高度
kf->P = (1 - K) * P_pred; // 更新不确定度
return kf->x;
}避坑指南：我曾经在调参时把 Q 设得太大，结果滤波器完全跟着传感器跑，噪声一点没滤掉。后来把 R 设得太小，滤波器反应迟钝，飞机落地了高度还没更新。记住：Q 和 R 的比值决定了滤波器的“脾气”。
3.4 速度估计：加入加速度计
实际项目中，我们不会只用气压计。我会把加速度计也加进来，构成一个二维状态向量。
状态向量：
x = [高度, 垂直速度]控制输入：
u = 加速度计测量的垂直加速度测量值：
z = 气压计高度这样，加速度计提供“趋势”，气压计提供“基准”。两者互补，效果远好于单一传感器。
我当年在做一个植保无人机项目时，就用这个方案。飞机在田里作业，气流乱得很，气压计跳得厉害。但融合了加速度计后，高度估计稳得像钉在地上一样。
3.5 知识体系图
下面这张图，是我自己总结的卡尔曼滤波在飞控中的应用逻辑。你把它记在脑子里，调参时就不会迷路。
3.6 调参实战经验
调卡尔曼滤波，说白了就是调两个数：Q 和 R。
| 参数 | 含义 | 调大效果 | 调小效果 |
|---|---|---|---|
| Q | 过程噪声（模型信任度） | 更相信传感器，响应快但噪声大 | 更相信模型，平滑但滞后 |
| R | 测量噪声（传感器信任度） | 更相信模型，平滑但滞后 | 更相信传感器，响应快但噪声大 |
我的调参口诀：先固定 R（从传感器数据手册查），再调 Q。Q 从 0.001 开始，逐步增大，直到估计值“跟得上”但又不“太跳”。
3.7 避坑总结
最后，分享几个我踩过的坑：
- 初始化要谨慎：P 的初始值别设太大，否则滤波器刚开始会“乱跳”。我一般设 1.0。
- 注意单位统一：高度用米，速度用米/秒，加速度用米/秒²。单位错了，滤波器直接发散。
- 别忘了 dt：卡尔曼滤波是离散的，dt 必须准确。我见过有人把 dt 写死成 0.01，结果实际跑 0.005，估计值直接崩了。
- 传感器延时：气压计和加速度计的时间戳要对齐。不对齐的话，融合出来的高度会有相位差。
好了，这一章的内容就到这里。卡尔曼滤波是飞控的“基本功”，你把它吃透了，后面做自主起降、航线飞行都会轻松很多。