传感器与执行器：滤波器设计（Kalman/互补）

可视化演示

展示卡尔曼滤波预测-更新循环、噪声协方差整定与卡尔曼增益调节，以及互补滤波在传感器融合中的对比效果。

滤波思想概述

窗外明明在下雨，可你的耳朵里还混着隔壁工地洒水车的哗哗声、楼下空调外机的嗡嗡声——你听到的“雨声”从来不是纯粹的雨声。传感器的世界也一样：陀螺仪用久了会像老手表的发条一样慢慢漂移，加速度计被电机一震就像被拍了一下的旧收音机，读数乱跳。我们想知道无人机真实的倾斜角、汽车真实的速度，可拿到手的只有被噪声污染的测量值。所谓滤波，就是从这锅“噪声粥”里把真实状态重新捞出来的手艺。

技术上看，传感器测量可以写成这样一个简单关系：测量值 = 真实值 + 噪声。如果我们知道一些物理规律，比如“无人机上一秒的角速度是这么多，那这一秒角度大概转过这么多”，就不要浪费这些信息。把物理规律称为“模型”，把传感器读数称为“观测”，滤波要做的事就是：模型先猜一个，观测再修正一下。下面这个式子就是观测模型的数学表达：

所以，滤波器的本质就是“天气预报员”的工作台：一边是物理课本（模型），一边是实时观测（传感器），你的判断力就是滤波算法——给模型和观测分别打分，再决定信几分。明白了这个目标，我们就能进入卡尔曼滤波的世界了。

卡尔曼原理

老预报员每天的开工流程其实无比固定：早晨先用气象模型推演“今天应该是什么天气”——这叫预测；中午雷达和温度计的读数回传，他对照实况修正自己的判断——这叫更新。第二天，把修正过的最优判断作为新起点，再次推演、再次修正。日复一日，这个“预测-修正”的循环就是卡尔曼滤波的骨架。

卡尔曼滤波把“状态”看成一个待估计的数或向量，比如无人机当前的角度。它维护两样东西：状态的估计值 $\hat{x}$，以及对这份估计的“信心”——协方差 $P$。每个周期分成两步：预测步用运动模型推演状态向前走一步，更新步用传感器观测把状态的偏差掰回来。掰回来的力度由卡尔曼增益 $K$ 决定：$K$ 越大，越听传感器；$K$ 越小，越信模型。先看预测步：

预测毕竟只是“猜”，接下来要用观测修正。更新步的公式是：

你看，这个循环和“天气预报员的日常”完全同构：模型推演是预测，看实况是更新，而卡尔曼增益就是老预报员的“经验系数”——他知道哪个测站可靠、哪个设备该打折，于是自动分配信任度。预测与更新周而复始，估计就越来越接近真相。但这套“信任分配”到底怎么算出 K？这正是下一节数学推导要回答的问题。

数学推导

预报员有多相信自己昨天的判断？如果昨天报得极准、设备也稳定，今天他的初判就有底气；可如果昨晚来了一场强对流，他会猛然意识到“原来我的模型也会漏掉突发情况”，信心随之下滑。卡尔曼滤波里，这个“信心”由协方差矩阵 P 表达——P 越小，估计越确定；P 越大，心里越没底。预测步里，信心会自然流失；更新步里，观测会重新注入信心。

预测步的协方差传播公式如下：

更新步的信心变化则依赖于卡尔曼增益：

$$\ \displaystyle P_{k|k} = \left( I - K_k H \right) P_{k|k-1}$$

参数整定

\displaystyle K = \frac{P + Q}{P + Q + R}

互补滤波

如果卡尔曼滤波是“精算师”，那互补滤波就是“草台班子里的黄金搭档”。陀螺仪是个“急性子”：响应极快，高频段数据漂亮，可惜记性不好，角度靠积分算，积分久了就漂得没边。加速度计是个“慢性子”：长期水准极稳，因为无论你怎么晃，重力方向就在那里，它总能把你拉回正确答案；但它受不了高频振动，无人机电机一震它就哆嗦。互补滤波的做法很朴素：让急性子负责高频段，让慢性子负责低频段，两者一拼就是全频段的真相。

互补滤波在姿态估计里只用一个系数 α（通常取 0.95~0.99）。它把陀螺仪的角度积分结果高通处理，把加速度计算出的倾角低通处理，再相加。离散形式写出来就是：

互补滤波的哲学是“扬长避短”——它不需要建立精确的噪声模型，不需要矩阵求逆，一个 α 就能在绝大多数嵌入式平台上跑得飞快。与卡尔曼滤波相比，它损失了最优性和不确定度输出，却换来了可观的简单与鲁棒。在算力紧张的飞控单片机上，这性价比让人无法拒绝。

工程实践

回到无人机航拍现场。一架四旋翼悬停在空中，被风吹得微微摇摆，飞控必须知道“现在的俯仰角是多少”。它手里有两份情报：陀螺仪说“角速度这么转”，积分可得角度，短时准、长时漂；加速度计说“重力方向在那”，分解可得倾角，平稳准、动态抖。姿态滤波要做的，就是把这两份各有缺陷的情报合成一份可信的角度。

在小四轴场景，MPU6050 加互补滤波是经典组合：互补滤波计算量小，一个 α 调完，悬停稳、打舵跟手。而在高精度惯性导航中，GPS 与 IMU 的组合导航偏爱卡尔曼滤波——GPS 低频高精度但会中断，IMU 高频但会漂移，卡尔曼滤波能用 IMU 填补 GPS 中断的空窗期。其融合框架可以简化为：

工程调试的经验之谈：静止调 R，动态调 Q。先让无人机静止，把 R 调到静态偏差小于 0.5°；再手动晃动机体，观察动态跟随——“发飘”就增大 Q，“发钝”就减小 R。这就像老预报员的笔记本，密密麻麻记录着每个传感器的脾气。无论是互补滤波还是卡尔曼滤波，真正的功夫都在理解传感器性格和模型适用边界上，滤波器本身只是一把好用的工具罢了。

AI 应用场景

Python 演示

# -*- coding: utf-8 -*-# 无人机姿态估计演示: 互补滤波 vs 卡尔曼滤波# 模拟 MPU6050 的陀螺仪角速度和加速度计倾角数据import mathimport random  def kalman_update(z, x_est, p_est, Q, R):    # 一维卡尔曼滤波: 预测 + 更新    x_pred = x_est    p_pred = p_est + Q    K = p_pred / (p_pred + R)    x_new = x_pred + K * (z - x_pred)    p_new = (1 - K) * p_pred    return x_new, p_new  def complementary(angle, gyro_rate, accel_angle, dt, alpha):    # 互补滤波: 陀螺仪积分(高通) + 加速度计倾角(低通)    return alpha * (angle + gyro_rate * dt) + (1 - alpha) * accel_angle  def simulate():    random.seed(7)    dt = 0.01    T = 500    alpha = 0.98    q, r = 0.001, 0.05     true_angle = 0.0    gyro_int = 0.0    kf_angle = 0.0    kf_p = 1.0    cf_angle = 0.0     print("t     真实角   陀螺仪积分  互补滤波  卡尔曼滤波")    for i in range(T):        true_angle = 30.0 * math.sin(i * dt * 2.0)        # 陀螺仪测量: 真实角速度 + 恒定偏差 + 噪声        gyro_rate = 60.0 * math.cos(i * dt * 2.0) + 0.5 + random.uniform(-0.3, 0.3)        gyro_int += gyro_rate * dt        # 加速度计测倾角: 真实角 + 振动噪声        accel_angle = true_angle + random.uniform(-2.5, 2.5)         cf_angle = complementary(cf_angle, gyro_rate, accel_angle, dt, alpha)        kf_angle, kf_p = kalman_update(accel_angle, kf_angle, kf_p, q, r)         if i % 50 == 0:            print("%5.2f  %7.2f  %8.2f  %8.2f  %8.2f" %                  (i * dt, true_angle, gyro_int, cf_angle, kf_angle))  if __name__ == "__main__":    simulate()

1. gyro_rate = 60.0 * math.cos(i * dt * 2.0) + 0.5 + random.uniform(-0.3, 0.3)陀螺仪角速度包含恒定偏差 0.5，模拟真实器件的零偏，这正是积分漂移的根源
2. gyro_int += gyro_rate * dt一个“天真”的纯积分：角度越积越偏，用来作为对照组展示不滤波的后果
3. K = p_pred / (p_pred + R)卡尔曼增益：预测不确定度占总不确定度的比例，决定观测修正的力度
4. x_new = x_pred + K * (z - x_pred)状态更新：在预测基础上按 K 吸收观测残差，是卡尔曼滤波的核心一行
5. p_new = (1 - K) * p_pred信心压缩：观测注入后估计协方差减小，表示对状态更有把握
6. cf_angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * accel_angle互补滤波的一行核心：α 控制陀螺仪积分与加速度计修正的权重

常见误区

面试题