4. 状态估计模块：卡尔曼滤波、扩展卡尔曼滤波、粒子滤波在飞控中的实现架构
状态估计，说白了就是飞控的「眼睛」和「耳朵」。
传感器数据有噪声，IMU会漂，GPS会跳。你想想看，如果直接把原始数据拿来做控制，飞机会抖成什么样？我见过一个新手，直接把加速度计数据当姿态用，结果飞机在天上跳了一支「霹雳舞」。
所以我们需要一个模块，能把各种不靠谱的传感器数据融合起来，给出一个靠谱的状态估计。这就是状态估计模块干的事。
4.1 卡尔曼滤波：线性系统的最优解
卡尔曼滤波，说白了就是「预测+修正」的循环。我习惯把它理解成一个「有脑子的滤波器」——它不光看当前测量值，还根据系统模型预测下一步会怎样。
核心思想：
- 预测步：根据上一时刻的状态，预测当前时刻的状态和误差协方差
- 更新步：用当前测量值修正预测结果，得到最优估计
在飞控里，卡尔曼滤波最经典的应用就是GPS+IMU融合。我记得有一次做无人机编队，GPS信号被遮挡，纯靠卡尔曼滤波的预测值撑了3秒，飞机居然没偏太多。嗯，这就是预测步的威力。
// 卡尔曼滤波核心代码（简化版）
// 预测步
x_pred = A * x_est + B * u; // 状态预测
P_pred = A * P_est * A' + Q; // 协方差预测
// 更新步
K = P_pred * H' * inv(H * P_pred * H' + R); // 卡尔曼增益
x_est = x_pred + K * (z - H * x_pred); // 状态更新
P_est = (I - K * H) * P_pred; // 协方差更新避坑指南：我曾经在调Q矩阵（过程噪声协方差）时，把值设得太小，结果滤波器对测量值过度信任，GPS一抖动，姿态就跟着跳。后来我总结了一个经验：Q矩阵先给一个保守值，然后慢慢调大，直到响应速度和噪声抑制达到平衡。
4.2 扩展卡尔曼滤波：非线性系统的利器
卡尔曼滤波假设系统是线性的。但飞控里哪有那么多线性系统？姿态四元数、地球坐标系转换，全是非线性。这时候就得请出扩展卡尔曼滤波（EKF）了。
EKF的思路很简单：把非线性函数在估计点附近做一阶泰勒展开，也就是求雅可比矩阵。说白了就是「用切线近似曲线」。
EKF vs KF 的关键区别：
| 对比项 | 卡尔曼滤波 | 扩展卡尔曼滤波 |
|---|---|---|
| 系统模型 | 线性 | 非线性（需线性化） |
| 状态转移 | 矩阵乘法 | 非线性函数+雅可比 |
| 计算量 | 低 | 中（需计算雅可比） |
| 适用场景 | GPS+IMU融合 | 姿态估计、视觉惯性里程计 |
我建议在飞控里，姿态估计用EKF是标配。为什么呢？因为四元数动力学方程是非线性的，用线性KF会引入不可忽略的误差。我曾经试过用线性KF做姿态估计，结果在高速旋转时，估计值直接发散——嗯，那次炸机让我记住了这个教训。
// EKF中雅可比矩阵的计算示例（姿态估计）
// 状态向量: [q0, q1, q2, q3, bgx, bgy, bgz] 四元数+陀螺零偏
// 状态转移函数 f(x) 对状态 x 求雅可比
// 简化后的雅可比矩阵（部分）
F[0][0] = 1; F[0][1] = -dt/2 * wx; F[0][2] = -dt/2 * wy; F[0][3] = -dt/2 * wz;
F[1][0] = dt/2 * wx; F[1][1] = 1; F[1][2] = dt/2 * wz; F[1][3] = -dt/2 * wy;
// ... 实际代码会更复杂，这里只展示思路注意：EKF的线性化误差在强非线性系统中会累积。如果你发现滤波器在高动态场景下性能下降，可以考虑用无迹卡尔曼滤波（UKF）或粒子滤波。我个人习惯在四旋翼飞控中用EKF，但在固定翼或倾转旋翼机中，我会切换到UKF。
4.3 粒子滤波：非线性非高斯问题的终极方案
粒子滤波，说白了就是「用一堆随机点去逼近真实状态分布」。你想想看，如果系统是非线性、非高斯的，EKF的线性化假设就不成立了。这时候粒子滤波就派上用场了。
粒子滤波的核心思想：用N个带权重的粒子来表示状态的后验概率分布。每个粒子就是一个假设的状态，权重表示这个假设的可信度。
粒子滤波的四个步骤：
- 初始化：在状态空间撒N个粒子，权重均匀
- 预测：每个粒子根据系统模型向前传播
- 更新：根据测量值更新每个粒子的权重
- 重采样：根据权重重新采样，权重高的粒子多复制，权重低的粒子被淘汰
我在项目中遇到过一个问题：用粒子滤波做室内定位，粒子数设了1000个，结果在STM32F4上跑，帧率只有5Hz。后来我把粒子数降到200个，配合自适应重采样策略，帧率提到了20Hz，精度损失不到5%。
// 粒子滤波重采样（系统重采样法）
void resample(Particle* particles, int N) {
float cum_weights[N];
cum_weights[0] = particles[0].weight;
for (int i = 1; i < N; i++) {
cum_weights[i] = cum_weights[i-1] + particles[i].weight;
}
float step = cum_weights[N-1] / N;
float start = rand_float() * step;
Particle new_particles[N];
int idx = 0;
for (int i = 0; i < N; i++) {
float target = start + i * step;
while (cum_weights[idx] < target) idx++;
new_particles[i] = particles[idx];
new_particles[i].weight = 1.0 / N; // 重置权重
}
memcpy(particles, new_particles, sizeof(Particle) * N);
}经验之谈：我曾经在无人机避障系统中用粒子滤波做障碍物跟踪。一开始粒子数设了500个，效果很好但CPU占用太高。后来我发现，在障碍物运动平缓时，100个粒子就够了；只有在快速机动时才需要500个。于是我做了动态粒子数调整——嗯，这个技巧让CPU占用降了60%。
4.4 三种滤波器的选型架构
在实际飞控中，我不会只用一种滤波器。我习惯把三种滤波器组合起来，各司其职。
从架构图可以看出，我习惯把三种滤波器并行部署：
- KF 处理线性度好的传感器融合（GPS+IMU位置估计）
- EKF 处理非线性姿态估计（核心模块，必须稳定）
- PF 处理强非线性、多模态的跟踪问题（避障、室内定位）
性能考量：粒子滤波的计算量是O(N)，N是粒子数。在嵌入式平台上，我建议N不要超过500。如果资源紧张，可以考虑用UKF替代PF——UKF的计算量是O(L³)，L是状态维度，对于低维状态（L<10），UKF比PF更高效。
4.5 实现架构中的关键设计模式
在实际代码中，我习惯用「策略模式」来管理不同的滤波器。这样切换滤波器时，上层代码完全不用改。
// 策略模式实现滤波器切换
typedef struct {
void (*init)(void* self, float* init_state);
void (*predict)(void* self, float* control, float dt);
void (*update)(void* self, float* measurement);
void (*get_state)(void* self, float* state_out);
} FilterOps;
// KF、EKF、PF各自实现这个接口
FilterOps kf_ops = { kf_init, kf_predict, kf_update, kf_get_state };
FilterOps ekf_ops = { ekf_init, ekf_predict, ekf_update, ekf_get_state };
FilterOps pf_ops = { pf_init, pf_predict, pf_update, pf_get_state };
// 使用时只需切换ops指针
FilterOps* current_filter = &ekf_ops; // 默认用EKF
current_filter->predict(filter_instance, control, dt);我的习惯：在飞控启动时，我会先跑一段「滤波器自检」——给滤波器输入已知的测试数据，检查输出是否在预期范围内。如果EKF的残差（innovation）持续偏大，我会自动切换到KF或者触发报警。这个机制救过我一次，那次IMU的零偏突然漂移，EKF差点发散，自检及时发现了问题。
好了，状态估计模块就讲到这里。三种滤波器各有千秋，选型时记住一句话：线性用KF，弱非线性用EKF，强非线性用PF。但实际工程中，我建议优先用EKF——它在大多数场景下够用，而且实现复杂度适中。粒子滤波虽然强大，但计算代价高，只在必要时才用。
公众号:蓝海资料掘金营，微信deep3321