第 5 篇：卡尔曼滤波（下）------50 行 C 代码 + MPU6050 实战
上篇把原理讲透了。这篇直接上代码------一维卡尔曼、二维卡尔曼、MPU6050 角度估计，全都有。
1. 一维卡尔曼（不到 50 行）
c
// kalman1d.h
typedef struct {
float x; // 状态估计值
float p; // 估计误差协方差
float q; // 过程噪声协方差
float r; // 测量噪声协方差
} kalman1d_t;
void kalman1d_init(kalman1d_t *kf, float q, float r, float init_x);
float kalman1d_update(kalman1d_t *kf, float measurement);
c
// kalman1d.c
#include "kalman1d.h"
void kalman1d_init(kalman1d_t *kf, float q, float r, float init_x) {
kf->x = init_x;
kf->p = 1.0f; // 初始不确定性，设为 1（会快速收敛）
kf->q = q; // 过程噪声
kf->r = r; // 测量噪声
}
float kalman1d_update(kalman1d_t *kf, float z) {
// === 预测 ===
// x = x（一维无控制输入，状态不变）
kf->p = kf->p + kf->q; // P⁻ = P + Q
// === 更新 ===
float k = kf->p / (kf->p + kf->r); // K = P⁻/(P⁻+R)
kf->x = kf->x + k * (z - kf->x); // x̂ = x̂⁻ + K(z - x̂⁻)
kf->p = (1.0f - k) * kf->p; // P = (1-K)P⁻
return kf->x;
}使用示例------超声波测距：
c
kalman1d_t dist_kf;
void setup(void) {
// R = 测距噪声方差。超声波测距精度约 ±2cm，方差 ≈ 4
// Q = 目标不会瞬间移动，设小一点
kalman1d_init(&dist_kf, 0.01f, 4.0f, 100.0f);
}
void loop(void) {
float raw_dist = ultrasonic_read_cm(); // 超声波原始距离
float opt_dist = kalman1d_update(&dist_kf, raw_dist);
printf("raw=%.1fcm, kalman=%.1fcm\r\n", raw_dist, opt_dist);
delay_ms(50);
}2. 二维卡尔曼------同时估计位置和速度
很多场景下，我们不仅想知道"现在在哪"，还想知道"现在多快"。
状态向量 x = 位置, 速度ᵀ
c
// kalman2d.h
typedef struct {
float x[2]; // [位置, 速度]
float p[2][2]; // 2×2 协方差矩阵
float q[2][2]; // 过程噪声
float r; // 测量噪声（只测位置）
float dt; // 采样间隔
} kalman2d_t;
void kalman2d_init(kalman2d_t *kf, float dt, float q_pos, float q_vel, float r);
float kalman2d_update(kalman2d_t *kf, float measurement);
void kalman2d_get_state(kalman2d_t *kf, float *pos, float *vel);
c
// kalman2d.c
#include "kalman2d.h"
#include <string.h>
void kalman2d_init(kalman2d_t *kf, float dt, float q_pos, float q_vel, float r) {
kf->dt = dt;
kf->r = r;
memset(kf->x, 0, sizeof(kf->x));
memset(kf->p, 0, sizeof(kf->p));
kf->p[0][0] = 1.0f; kf->p[1][1] = 1.0f; // 初始协方差
kf->q[0][0] = q_pos; kf->q[1][1] = q_vel; // 对角噪声阵
}
float kalman2d_update(kalman2d_t *kf, float z) {
float dt = kf->dt;
// === 预测 ===
// x⁻[0] = x[0] + x[1]*dt → 位置 = 位置 + 速度×时间
// x⁻[1] = x[1] → 速度不变（匀速模型）
float x_pred[2];
x_pred[0] = kf->x[0] + kf->x[1] * dt;
x_pred[1] = kf->x[1];
// P⁻ = A·P·Aᵀ + Q
// A = [[1, dt], [0, 1]]
float p_pred[2][2];
p_pred[0][0] = kf->p[0][0] + 2*dt*kf->p[0][1] + dt*dt*kf->p[1][1] + kf->q[0][0];
p_pred[0][1] = kf->p[0][1] + dt*kf->p[1][1];
p_pred[1][0] = p_pred[0][1];
p_pred[1][1] = kf->p[1][1] + kf->q[1][1];
// === 更新（只测量位置 H=[1,0]）===
float s = p_pred[0][0] + kf->r; // 新息协方差
float k0 = p_pred[0][0] / s; // 卡尔曼增益 K[0]
float k1 = p_pred[1][0] / s; // 卡尔曼增益 K[1]
float y = z - x_pred[0]; // 新息 = 测量值 - 预测值
kf->x[0] = x_pred[0] + k0 * y; // 更新位置
kf->x[1] = x_pred[1] + k1 * y; // 更新速度
kf->p[0][0] = (1 - k0) * p_pred[0][0];
kf->p[0][1] = (1 - k0) * p_pred[0][1];
kf->p[1][0] = p_pred[1][0] - k1 * p_pred[0][0];
kf->p[1][1] = p_pred[1][1] - k1 * p_pred[0][1];
return kf->x[0];
}
void kalman2d_get_state(kalman2d_t *kf, float *pos, float *vel) {
*pos = kf->x[0];
*vel = kf->x[1];
}3. 实战：MPU6050 角度卡尔曼滤波
用二维卡尔曼估计俯仰角------同时得到角度和角速度。
c
kalman2d_t angle_kf;
void mpu6050_angle_init(void) {
// dt = 5ms (200Hz 采样)
// q_pos = 0.001 (角度过程噪声小)
// q_vel = 0.003 (角速度噪声稍大)
// r = 0.03 (加速度计推算角度的噪声，实际测出的方差)
kalman2d_init(&angle_kf, 0.005f, 0.001f, 0.003f, 0.03f);
}
float mpu6050_get_angle(void) {
float ax, ay, az, gx, gy, gz;
mpu6050_read_all(&ax, &ay, &az, &gx, &gy, &gz);
// 加速度计推算的角度作为"测量值"
float accel_angle = atan2f(ay, az) * 180.0f / 3.14159f;
// 卡尔曼融合
float angle = kalman2d_update(&angle_kf, accel_angle);
// 注意：这里把角速度信息用在了预测模型里（匀速模型）
// 更精确的做法是把 gyro 作为控制输入 u[k] 放进预测方程
return angle;
}效果对比：
makefile
纯加速度计: ±3° 晃动（高频振动）
纯陀螺仪: 持续漂移（1分钟漂5°）
互补滤波: ±1°
卡尔曼: ±0.5°4. 卡尔曼滤波的坑
坑 1：Q 和 R 的初始值选错导致发散 → 解决办法：先用传感器数据手册算 R，Q 从 0.001 开始调
坑 2：协方差矩阵不对称导致数值不稳定 → 解决办法：强制对称 P[0][1] = P[1][0] = (P[0][1]+P[1][0])/2
坑 3：非线性系统用线性卡尔曼 → 线性卡尔曼假设 A·x+B·u，如果你的系统不是线性的（比如四旋翼姿态），需要用扩展卡尔曼(EKF)
下一篇：互补滤波------陀螺仪+加速度计的最佳拍档，比卡尔曼更简单实用