卡尔曼滤波

卡尔曼滤波（Kalman Filter）

关键词：状态估计、传感器融合、噪声抑制、MCU、STM32、Arduino 适用场景：ADC 采样抖动、陀螺仪/加速度计融合、温度/压力测量、GPS 定位、姿态解算

---

0. 为什么单片机上要用卡尔曼滤波？

单片机读取传感器时，几乎一定会遇到这些问题：

• 噪声：ADC 量化误差、电源纹波、EMI 干扰，让读数值上下抖动。
• 多传感器矛盾：陀螺仪积分会漂移，加速度计在运动时会乱，单独用哪个都不准。
• 实时性要求：不能像 PC 那样离线批量平滑，必须”每来一个采样就出一个数”。

卡尔曼滤波的优势恰好命中这些痛点：

1. 递推式（recursive）：只需保存上一次的”状态估计”和”协方差”，内存占用极小，适合 RAM 只有几 KB 的 MCU。
2. 最优估计：在高斯白噪声假设下，它是最小均方误差（MMSE）意义下的最优线性估计器。
3. 可融合多源：同一个框架里同时吃掉多个传感器，自动按”谁更可信”分配权重。

一句话直觉：卡尔曼滤波就是”预测值 + 实测值”按可信度加权融合，且权重会自己随噪声动态调整。

---

1. 核心思想（不看公式也能懂）

卡尔曼滤波每步做两件事：

1. 预测（Predict）：根据上一刻的状态 + 系统模型，猜这一刻的状态，并承认”猜的也有误差”。
2. 更新（Update / Correct）：来了一个真实测量值，把”猜测”和”测量”加权合并。谁的误差小（更可信），谁的权重就大。

权重大小由 卡尔曼增益 K（Kalman Gain） 决定：

• K 接近 1 → 更相信测量（测量很干净）
• K 接近 0 → 更相信预测（测量噪声很大）

而且 K 不是手动设的，是算法根据测量噪声 R 和预测不确定度 P 自动算出来的。这就是它比固定系数的滑动平均/低通滤波聪明的地方。

---

2. 数学框架（标准线性 KF）

状态方程与观测方程（离散时间）：

x_k = A·x_{k-1} + B·u_{k-1} + w_{k-1}      (状态转移，w ~ N(0,Q))
z_k = H·x_k + v_k                          (观测，v ~ N(0,R))

• x：系统状态向量（我们要估计的东西）
• z：测量向量（传感器读数）
• A：状态转移矩阵
• H：观测矩阵（状态到测量的映射）
• Q：过程噪声协方差（系统模型的不确定性）
• R：测量噪声协方差（传感器噪声，通常好测）
• P：估计误差协方差（我们对当前估计有多不自信）

五个核心方程

预测阶段

x_pred = A · x      (无控制输入时)
P_pred = A · P · Aᵀ + Q

更新阶段

K     = P_pred · Hᵀ · (H · P_pred · Hᵀ + R)⁻¹     (卡尔曼增益)
x     = x_pred + K · (z - H · x_pred)             (修正估计)
P     = (I - K · H) · P_pred                      (更新误差协方差)

对单片机来说，最常用的是 标量（1 维）版本，此时所有矩阵退化为标量，运算只有加减乘除。

---

3. 一维卡尔曼滤波（单片机最常写，也最够用）

3.1 推导（标量版）

假设要估计一个缓慢变化的量 x（比如温度、电池电压），模型：

x_k = x_{k-1} + w      (真实值基本不变，w 是过程噪声)
z_k = x_k + v          (测量值 = 真值 + 观测噪声)

标量递推：

// 预测
x_pred = x;          // 假设基本不变
P_pred = P + Q;      // 不确定度随模型噪声增大

// 更新
K   = P_pred / (P_pred + R);   // 卡尔曼增益
x   = x_pred + K * (z - x_pred);
P   = (1 - K) * P_pred;

注意 R 和 Q 都是方差（噪声的标准差平方），不是标准差本身。

3.2 整定直觉

参数	含义	调大 / 调小的影响
R（测量噪声方差）	传感器噪声有多大	R 大 → 不信测量 → 滤波平滑但滞后；R 小 → 信测量 → 跟随快但抖
Q（过程噪声方差）	真值变化有多剧烈	Q 大 → 认为真值在动 → 跟得快；Q 小 → 认为稳定 → 更平滑
P（初值）	初始不确定度	给个大一点的值（如 1.0）帮助快速收敛，稳态后被算法接管

经验公式：R 可用实测方差估计 —— 让传感器静止，采集 N 个样本，算方差 R ≈ var(z)。Q 一般取 R 的 1/100 ~ 1/1000，再根据响应速度微调。

3.3 C 语言实现（无浮点库也能跑，定点友好）

/* kalman_1d.h */
#ifndef KALMAN_1D_H
#define KALMAN_1D_H
 
typedef struct {
    float x;   // 当前估计值
    float P;   // 估计误差协方差
    float Q;   // 过程噪声方差
    float R;   // 测量噪声方差
} Kalman1D;
 
void kalman1d_init(Kalman1D *k, float init_x, float Q, float R) {
    k->x = init_x;
    k->P = 1.0f;     // 初始不确定度设大点
    k->Q = Q;
    k->R = R;
}
 
float kalman1d_update(Kalman1D *k, float z) {
    // 预测
    float P_pred = k->P + k->Q;
 
    // 更新
    float K = P_pred / (P_pred + k->R);
    k->x = k->x + K * (z - k->x);
    k->P = (1.0f - K) * P_pred;
 
    return k->x;
}
 
#endif

用法示例（ADC 温度采样）：

Kalman1D temp_kf;
kalman1d_init(&temp_kf, 25.0f, 0.001f, 0.5f);  // Q 小(稳), R 大(噪声大)
 
float raw = read_adc_voltage();                // 每 10ms 采样一次
float filtered = kalman1d_update(&temp_kf, raw);

3.4 定点（整数）实现要点（资源极紧的 MCU）

浮点占 ROM/RAM 且慢。可用 Q 格式定点数：

• 把 x, P, Q, R 存为 int32_t，统一放大 2^12（Q12）倍。
• K = P_pred * (1<<12) / (P_pred + R) 用整数除法，注意防溢出。
• 乘除用移位近似：x += (K * (z - x)) >> 12。
• 实测：8 位 AVR（Arduino Uno）上定点 KF 比浮点版本快 3~5 倍。

---

4. 二维卡尔曼滤波（陀螺仪 + 加速度计 角度融合）

单片机姿态估计经典问题：陀螺仪积分得角度但会漂，加速度计得角度但有振动噪声。用 2 维 KF 融合。

状态 x = [角度, 角速度偏差]ᵀ，模型：

x_k = [1  -dt] · x_{k-1} + w
       [0   1 ]
z_k = [1   0 ] · x_k + v          // 测量只给角度(加速度计算的角度)

C 实现（用 2x2 矩阵，手写展开，避免引入矩阵库）：

/* 角度融合：状态 [angle, bias] */
typedef struct {
    float angle;   // 估计角度
    float bias;    // 陀螺仪零偏
    float P[2][2]; // 2x2 协方差
    float Q_angle, Q_bias, R_measure;
} KalmanAngle;
 
void kalman_angle_init(KalmanAngle *k, float Qa, float Qb, float Rm) {
    k->angle = 0; k->bias = 0;
    k->P[0][0] = 0; k->P[0][1] = 0;
    k->P[1][0] = 0; k->P[1][1] = 0;
    k->Q_angle = Qa; k->Q_bias = Qb; k->R_measure = Rm;
}
 
/* rate: 陀螺仪角速度(rad/s); angle_meas: 加速度计算出的角度; dt: 采样周期 */
float kalman_angle_update(KalmanAngle *k, float rate, float angle_meas, float dt) {
    // 1) 预测
    k->angle += dt * (rate - k->bias);
    k->P[0][0] += dt * (dt*k->P[1][1] - k->P[0][1] - k->P[1][0] + k->Q_angle);
    k->P[0][1] -= dt * k->P[1][1];
    k->P[1][0] -= dt * k->P[1][1];
    k->P[1][1] += k->Q_bias * dt;
 
    // 2) 更新（测量 = 角度）
    float S = k->P[0][0] + k->R_measure;       // 残差协方差
    float K0 = k->P[0][0] / S;
    float K1 = k->P[1][0] / S;                 // 卡尔曼增益
    float y  = angle_meas - k->angle;          // 残差
    k->angle += K0 * y;
    k->bias  += K1 * y;
    float P00 = k->P[0][0], P01 = k->P[0][1];
    k->P[0][0] -= K0 * P00;  k->P[0][1] -= K0 * P01;
    k->P[1][0] -= K1 * P00;  k->P[1][1] -= K1 * P01;
 
    return k->angle;
}

这就是著名的 “Kalman filter for tilt” 经典写法，被无数平衡车/四轴项目使用。比起互补滤波，它无需手调时间常数 α，且对陀螺仪零偏有在线估计（bias 状态）能力。

---

5. 和其他滤波器的对比（单片机选型）

滤波器	内存	CPU	多传感器	滞后	说明
滑动平均	极小	极小	否	中	简单但群延迟大，对脉冲无效
一阶低通 IIR	极小	极小	否	可调	y += a*(x-y)，截止频率固定
互补滤波	小	小	是(2路)	小	手调 α，平衡车常用
卡尔曼滤波	小	中	是(多路)	小	自动调权，最优，但需建模型
扩展 KF (EKF)	中	高	是	小	非线性系统（如 GPS+IMU），算力要求高

选型建议：

• 单传感器慢变信号（温度、电压）→ 一维 KF（§3）或一阶低通。
• 两传感器互补（陀螺+加计）→ 二维 KF（§4）或互补滤波。
• 强非线性/高精度定位 → EKF/UKF，需 F4/F7/H7 级别 MCU。

---

6. 单片机落地常见坑

1. R/Q 用错单位：KF 里 Q、R 都是方差。很多人把标准差直接填进去，导致收敛奇慢或震荡。先算 var() 再填。
2. dt 不固定：KF 假设采样周期恒定。用定时器中断固定周期，或在 update 里传真实 dt（如 §4）。不要用 delay() 凑周期，会引入抖动。
3. 数值发散：P 矩阵各元素可能因舍入变成负或 NaN（尤其定点）。定期做 if (P < 0) P = 1e-3; 保护，或加对称化。
4. 一上来就上多维：先用一维 KF 跑通 ADC，再升级。多维 KF 的矩阵写错极难 debug。
5. 不校准传感器：陀螺仪零偏、ADC 增益误差先标掉，再让 KF 处理随机噪声，分工明确。
6. 认为 KF 万能：若噪声不是近似高斯、或存在未建模动态（如剧烈冲击），KF 会大幅滞后。此时考虑 EKF、粒子滤波或加异常值剔除（先卡掉突变再进 KF）。

---

7. 一个完整的 STM32 风格示例（伪代码流程）

// 主循环 / 定时器中断中，固定周期 100Hz
void TIM_IRQHandler(void) {
    float dt = 0.01f;
 
    float accel_angle = atan2(read_acc_y(), read_acc_z()); // 加速度计角度
    float gyro_rate   = read_gyro_x() * DEG2RAD;           // 陀螺仪角速度
 
    float angle = kalman_angle_update(&kf, gyro_rate, accel_angle, dt);
 
    // angle 即为融合后的稳定角度，可直接用于控制/显示
    motor_pid_setpoint(angle);
}

---

8. 速查清单（Cheat Sheet）

• 状态 x 选对了吗？要估计什么就放什么（含零偏等）
• R 用实测方差估计（传感器静止采集算 var）
• Q 先取 R/100，看响应再调
• P 初值给 1.0 左右，不用纠结
• 采样周期 dt 固定（定时器中断）
• 先做一维验证，再上多维
• 定点前先浮点跑通，再移植

---