一句话结论：融合的本质是「用加速度计的长时平均去校准陀螺仪的短时积分」，互补滤波 5 行代码就能做到，卡尔曼只是把这个权重做成自适应。
一、完整工程下载
压缩包内含全部源码、platformio.ini、Makefile、README.md，解压即用，不需要额外配置。
下载 attitude-fusion.zip （8.7 KB，共 9 个文件）
.gitignore
Makefile
README.md
include/
attitude.h
imu_sim.h
platformio.ini
src/
attitude.c
imu_sim.c
test/
test_attitude.c为什么必须融合
| 传感器 | 优点 | 缺点 |
|---|---|---|
| 加速度计 | 长期无漂移，静态准 | 动态误差大（振动/平移都会被当成倾角） |
| 陀螺仪 | 动态响应快，短时准 | 积分漂移（零偏 0.1°/s 时，1 分钟就漂 6°） |
两者误差特性互补，所以融合：
互补滤波： angle = α·(angle + gyro·dt) + (1-α)·accel_angle
^^^^^^^^^^^^^^^^^^^^^^^ ^^^^^^^^^^^^^^^^
信任陀螺仪（短时） 信任加速度计（长时）α 怎么取
截止频率 fc 与 α 的关系（采样周期 dt）：
α = τ / (τ + dt), τ = 1/(2π·fc)fc 一般取 0.5~5Hz。机器人平衡车常用 τ ≈ 0.5s（α ≈ 0.98 @1ms）。
加速度计算倾角怎么做
静止时，重力在三个轴上的分量就能反推倾角：
roll = atan2(ay, az)
pitch = atan2(-ax, sqrt(ay² + az²))注意：只有静止时才准。车在加速、电机在振动时， ax 里混进了运动加速度，算出来的倾角会跳。
二阶卡尔曼滤波（角度 + 角速度零偏）
状态向量取 x = [angle, gyro_bias]ᵀ，量测是加速度计算出的角度。 这是姿态估计里用得最多、性价比最高的卡尔曼形式：
预测：
angle += (gyro - bias) · dt
P = F·P·Fᵀ + Q
更新：
y = accel_angle - angle （新息）
S = P[0][0] + R
K = P[:,0] / S （卡尔曼增益）
angle += K[0]·y
bias += K[1]·y
P = (I - K·H)·P它比互补滤波多做的事：在线估计陀螺仪的零偏， 所以长时间静止时不会慢慢漂，而互补滤波必须靠标定阶段采集零偏。
工程内容
attitude.c/h：互补滤波 + 二阶卡尔曼，均支持在线调参imu_sim.c/h：IMU 仿真器，能产生「带零偏与噪声的陀螺仪 + 带噪声与运动干扰的加速度计」test/test_attitude.c：跑三种工况，输出误差统计- 静止（考察长期漂移）
- 阶跃倾斜（考察动态响应）
- 叠加运动加速度（考察抗扰）
调试要点
- 先标定零偏：上电时静止 2 秒，把陀螺仪三个轴的平均值存起来，
后面每帧减掉。这一步不做，再好的滤波也白搭
- 采样周期必须固定：用定时器中断读 IMU 并跑融合，
dt变了滤波参数全部失效 - 加速度计要做低通：电机振动频段远高于倾角变化频段，
先把加速度计原始数据过一遍 20~50Hz 低通，融合效果立刻变好
- Q 和 R 的物理含义：
R是加速度计的角度噪声方差，
Q_angle 是陀螺仪积分误差方差，Q_bias 是零偏漂移方差。 实际调节时：漂移大就减小 Q_bias，反应慢就减小 R
- 别在中断里做浮点矩阵运算：STM32F1 没有 FPU，
建议用 float 并放在主循环的固定周期任务里（见本站软定时器一文）
进阶方向
- 换用 Mahony / Madgwick 四元数算法，把完整的三维姿态解出来（含偏航）
- 加 磁力计 修正偏航角漂移
- 上 STM32F4 + 硬件 FPU，可以跑完整的 EKF（15 维状态）
- 与 PID 结合：平衡小车直接用这里的 pitch 角做串级控制的输入
完整代码
Makefile
CC ?= gcc
CFLAGS ?= -std=c99 -Wall -Wextra -O2 -Iinclude
LDLIBS ?= -lm
SRC = src/attitude.c src/imu_sim.c
TEST = test/test_attitude.c
ifeq ($(OS),Windows_NT)
EXT = .exe
endif
BIN = build/test$(EXT)
all: run
$(BIN): $(SRC) $(TEST)
@mkdir -p build
$(CC) $(CFLAGS) $(SRC) $(TEST) -o $(BIN) $(LDLIBS)
run: $(BIN)
@$(BIN)
clean:
rm -rf build
.PHONY: all run cleaninclude/attitude.h
/**
* attitude.h - 互补滤波与二阶卡尔曼滤波（单轴倾角）
*
* 输入：陀螺仪原始角速度（°/s）与加速度计算出的倾角（°）
* 输出：融合后的倾角
*/
#ifndef ATTITUDE_H
#define ATTITUDE_H
#include <stdbool.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/* ---------- 互补滤波 ---------- */
typedef struct {
float alpha; /* 0~1，越大越信任陀螺仪（对应时间常数 τ = alpha·dt/(1-alpha)） */
float angle; /* 输出角度（°） */
bool inited;
} comp_filter_t;
void comp_filter_init(comp_filter_t *f, float tau_s, float dt);
float comp_filter_update(comp_filter_t *f, float gyro_dps, float accel_deg, float dt);
/* ---------- 二阶卡尔曼滤波（angle + gyro bias） ---------- */
typedef struct {
float q_angle; /* 陀螺仪积分过程噪声（弧度²/s³ 量纲，实用上调） */
float q_bias; /* 零偏漂移过程噪声 */
float r_measure; /* 加速度计角度量测噪声（度²） */
float angle; /* 状态 1：角度 */
float bias; /* 状态 2：陀螺仪零偏 */
float p[2][2]; /* 协方差矩阵 */
} kalman_t;
void kalman_init(kalman_t *k, float q_angle, float q_bias, float r_measure);
float kalman_update(kalman_t *k, float gyro_dps, float accel_deg, float dt);
/** 单轴：从加速度三轴算倾角，静止才准 */
float accel_to_pitch_deg(float ax, float ay, float az);
float accel_to_roll_deg(float ax, float ay, float az);
#ifdef __cplusplus
}
#endif
#endif /* ATTITUDE_H */include/imu_sim.h
/**
* imu_sim.h - IMU 仿真器（仅主机端使用）
*
* 能产生：
* - 带零偏和随机游走的陀螺仪输出
* - 带白噪声的加速度计，并可选叠加「运动加速度」干扰
*/
#ifndef IMU_SIM_H
#define IMU_SIM_H
#include <stddef.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
float gyro_bias; /* 陀螺仪零偏（°/s），真实传感器不会写进数据里 */
float gyro_noise_std; /* 陀螺仪白噪声标准差 */
float accel_noise_std;/* 加速度计噪声标准差（折算到角度，°） */
float motion_accel; /* 运动加速度幅值（折算到角度误差，°） */
float true_angle; /* 当前真实倾角（°） */
float true_rate; /* 当前真实角速度（°/s） */
uint32_t rng;
} imu_sim_t;
void imu_sim_init(imu_sim_t *s);
/** 推进一个仿真步，返回真实角速度；测得的 gyro/accel 通过指针输出 */
void imu_sim_step(imu_sim_t *s, float desired_rate, float dt,
float *gyro_out, float *accel_deg_out);
#ifdef __cplusplus
}
#endif
#endif /* IMU_SIM_H */platformio.ini
[platformio]
default_envs = bluepill
[env:bluepill]
platform = ststm32
board = bluepill_f103c8
framework = arduino
upload_protocol = stlink
monitor_speed = 115200
build_flags =
-Wall
-Wextra
-Isrc
lib_ldf_mode = deep+src/attitude.c
#include "attitude.h"
#include <math.h>
/* ==================== 互补滤波 ==================== */
void comp_filter_init(comp_filter_t *f, float tau_s, float dt)
{
if (tau_s <= 0.0f) {
tau_s = 0.5f;
}
if (dt <= 0.0f) {
dt = 0.001f;
}
f->alpha = tau_s / (tau_s + dt);
f->angle = 0.0f;
f->inited = false;
}
float comp_filter_update(comp_filter_t *f, float gyro_dps, float accel_deg, float dt)
{
if (!f->inited) {
f->angle = accel_deg; /* 首次用加速度计初始化，避免从 0 爬升 */
f->inited = true;
return f->angle;
}
if (dt <= 0.0f) {
dt = 0.001f;
}
f->angle = f->alpha * (f->angle + gyro_dps * dt) +
(1.0f - f->alpha) * accel_deg;
return f->angle;
}
/* ==================== 卡尔曼滤波 ==================== */
void kalman_init(kalman_t *k, float q_angle, float q_bias, float r_measure)
{
k->q_angle = q_angle;
k->q_bias = q_bias;
k->r_measure = r_measure;
k->angle = 0.0f;
k->bias = 0.0f;
k->p[0][0] = 0.0f;
k->p[0][1] = 0.0f;
k->p[1][0] = 0.0f;
k->p[1][1] = 0.0f;
}
float kalman_update(kalman_t *k, float gyro_dps, float accel_deg, float dt)
{
float rate;
float s;
float k0;
float k1;
float y;
float p00_temp;
float p01_temp;
if (dt <= 0.0f) {
dt = 0.001f;
}
/* ---------- 1. 预测 ---------- */
rate = gyro_dps - k->bias;
k->angle += dt * rate;
/* P = F·P·Fᵀ + Q，F = [[1, -dt], [0, 1]] */
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. 更新（量测 = 加速度计角度） ---------- */
s = k->p[0][0] + k->r_measure;
k0 = k->p[0][0] / s;
k1 = k->p[1][0] / s;
y = accel_deg - k->angle;
k->angle += k0 * y;
k->bias += k1 * y;
p00_temp = k->p[0][0];
p01_temp = k->p[0][1];
k->p[0][0] -= k0 * p00_temp;
k->p[0][1] -= k0 * p01_temp;
k->p[1][0] -= k1 * p00_temp;
k->p[1][1] -= k1 * p01_temp;
return k->angle;
}
/* ==================== 加速度计解算倾角 ==================== */
float accel_to_pitch_deg(float ax, float ay, float az)
{
return (float)(atan2(-ax, sqrt(ay * ay + az * az)) * 57.2957795);
}
float accel_to_roll_deg(float ax, float ay, float az)
{
(void)ax;
return (float)(atan2(ay, az) * 57.2957795);
}src/imu_sim.c
#include "imu_sim.h"
#include <math.h>
static float randn(uint32_t *st)
{
/* Box-Muller：两个均匀分布 -> 一个标准正态 */
float u1;
float u2;
*st ^= *st << 13;
*st ^= *st >> 17;
*st ^= *st << 5;
u1 = (float)((*st >> 8) & 0xFFFFu) / 65536.0f;
*st ^= *st << 13;
*st ^= *st >> 17;
*st ^= *st << 5;
u2 = (float)((*st >> 8) & 0xFFFFu) / 65536.0f;
if (u1 < 1e-6f) {
u1 = 1e-6f;
}
return (float)(sqrt(-2.0f * logf(u1)) * cosf(6.2831853f * u2));
}
void imu_sim_init(imu_sim_t *s)
{
s->gyro_bias = 0.8f; /* 陀螺仪 0.8°/s 零偏，不标定就会漂 */
s->gyro_noise_std = 0.15f;
s->accel_noise_std = 0.6f;
s->motion_accel = 0.0f;
s->true_angle = 0.0f;
s->true_rate = 0.0f;
s->rng = 987654321u;
}
void imu_sim_step(imu_sim_t *s, float desired_rate, float dt,
float *gyro_out, float *accel_deg_out)
{
s->true_rate = desired_rate;
s->true_angle += desired_rate * dt;
if (gyro_out != NULL) {
*gyro_out = desired_rate + s->gyro_bias +
randn(&s->rng) * s->gyro_noise_std;
}
if (accel_deg_out != NULL) {
*accel_deg_out = s->true_angle +
randn(&s->rng) * s->accel_noise_std +
s->motion_accel * (float)sin((double)s->true_angle * 0.3);
}
}test/test_attitude.c
/**
* 主机端测试：互补滤波 vs 卡尔曼滤波的误差实测
* gcc -std=c99 -Wall -Wextra -Iinclude src/attitude.c src/imu_sim.c test/test_attitude.c -o build/test -lm
*/
#include <math.h>
#include <stdio.h>
#include "attitude.h"
#include "imu_sim.h"
#define DT 0.001f
typedef struct {
double sum_sq_comp;
double sum_sq_kal;
double sum_sq_raw;
double max_err_comp;
double max_err_kal;
int n;
} stat_t;
static const char *g_name;
static stat_t run_scenario(const char *name, float motion_accel,
float rate_from, float rate_to, int seconds)
{
imu_sim_t sim;
comp_filter_t comp;
kalman_t kal;
stat_t st = {0, 0, 0, 0, 0, 0};
int total = (int)((float)seconds / DT);
g_name = name;
imu_sim_init(&sim);
sim.motion_accel = motion_accel;
comp_filter_init(&comp, 0.5f, DT);
kalman_init(&kal, 0.001f, 0.003f, 0.03f);
for (int i = 0; i < total; i++) {
float rate = (float)i * (rate_to - rate_from) / (float)total + rate_from;
float gyro;
float accel;
float a_comp;
float a_kal;
imu_sim_step(&sim, rate, DT, &gyro, &accel);
a_comp = comp_filter_update(&comp, gyro, accel, DT);
a_kal = kalman_update(&kal, gyro, accel, DT);
/* 前 0.5 秒是滤波器收敛过程，不计入统计 */
if (i > 500) {
double e_comp = (double)a_comp - sim.true_angle;
double e_kal = (double)a_kal - sim.true_angle;
double e_raw = (double)accel - sim.true_angle;
st.sum_sq_comp += e_comp * e_comp;
st.sum_sq_kal += e_kal * e_kal;
st.sum_sq_raw += e_raw * e_raw;
if (fabs(e_comp) > st.max_err_comp) {
st.max_err_comp = fabs(e_comp);
}
if (fabs(e_kal) > st.max_err_kal) {
st.max_err_kal = fabs(e_kal);
}
st.n++;
}
}
printf(" %-22s RMS: 原始加速度计 %6.3f° 互补 %6.3f° 卡尔曼 %6.3f° "
"峰值误差: 互补 %6.2f° 卡尔曼 %6.2f°\n",
name,
sqrt(st.sum_sq_raw / st.n), sqrt(st.sum_sq_comp / st.n),
sqrt(st.sum_sq_kal / st.n), st.max_err_comp, st.max_err_kal);
return st;
}
int main(void)
{
printf("===== 姿态融合误差实测（dt=1ms，陀螺仪零偏 0.8°/s，加速度计噪声 0.6°）=====\n\n");
printf("[工况 1] 静止不动 10 秒（考察长期漂移）\n");
{
imu_sim_t sim;
comp_filter_t comp;
kalman_t kal;
imu_sim_init(&sim);
comp_filter_init(&comp, 0.5f, DT);
kalman_init(&kal, 0.001f, 0.003f, 0.03f);
printf(" 时间(s) 纯陀螺积分 互补滤波 卡尔曼(估计零偏) 真实值\n");
for (int i = 0; i < 10000; i++) {
float gyro;
float accel;
imu_sim_step(&sim, 0.0f, DT, &gyro, &accel);
comp_filter_update(&comp, gyro, accel, DT);
kalman_update(&kal, gyro, accel, DT);
if ((i % 2000) == 1999) {
printf(" %6.1f %10.2f° %9.2f° %13.2f° %9.2f°\n",
(i + 1) * DT, 0.8f * (i + 1) * DT, comp.angle,
kal.angle, sim.true_angle);
}
}
printf(" 卡尔曼在线估计出的零偏 = %.3f °/s（真值 0.800）\n", kal.bias);
}
printf("\n[工况 2] 缓慢倾斜 0 -> 30°（考察动态跟踪）\n");
run_scenario("缓慢倾斜 30°", 0.0f, 0.0f, 6.0f, 6);
printf("\n[工况 3] 快速摆动 ±30°（考察动态跟踪）\n");
{
imu_sim_t sim;
comp_filter_t comp;
kalman_t kal;
stat_t st = {0, 0, 0, 0, 0, 0};
imu_sim_init(&sim);
comp_filter_init(&comp, 0.5f, DT);
kalman_init(&kal, 0.001f, 0.003f, 0.03f);
for (int i = 0; i < 6000; i++) {
float rate = 30.0f * 6.2831853f * 0.5f *
(float)cos((double)i * DT * 6.2831853 * 0.5);
float gyro;
float accel;
float ac;
float ak;
imu_sim_step(&sim, rate, DT, &gyro, &accel);
ac = comp_filter_update(&comp, gyro, accel, DT);
ak = kalman_update(&kal, gyro, accel, DT);
if (i > 500) {
double ec = (double)ac - sim.true_angle;
double ek = (double)ak - sim.true_angle;
st.sum_sq_comp += ec * ec;
st.sum_sq_kal += ek * ek;
st.n++;
}
}
printf(" 摆动 ±30°@0.5Hz RMS: 互补 %6.3f° 卡尔曼 %6.3f°\n",
sqrt(st.sum_sq_comp / st.n), sqrt(st.sum_sq_kal / st.n));
}
printf("\n[工况 4] 叠加运动加速度干扰（模拟小车加速/振动，等效 ±8° 角度扰动）\n");
run_scenario("有运动干扰", 8.0f, 0.0f, 3.0f, 6);
printf("\n[结论]\n");
printf(" * 静止：纯陀螺积分 10 秒漂 %.2f°，互补滤波靠加速度计拉回来，误差最小\n",
0.8f * 10.0f);
printf(" * 动态：卡尔曼因为同时估计零偏，动态误差通常更小\n");
printf(" * 有运动干扰时两者都会变差 —— 根因是加速度计此时测的不是重力，\n");
printf(" 这时要么靠机械减振，要么用陀螺仪占更大的权重（减小 alpha 对应 τ 加大）\n");
printf("\n===== 以上数据由本机 gcc 实编译实运行产生 =====\n");
return 0;
}实测输出
下面这段输出是把上面的核心算法用 本机 gcc 真编译、真运行得到的（不含任何硬件依赖）：
===== 姿态融合误差实测（dt=1ms，陀螺仪零偏 0.8°/s，加速度计噪声 0.6°）=====
[工况 1] 静止不动 10 秒（考察长期漂移）
时间(s) 纯陀螺积分 互补滤波 卡尔曼(估计零偏) 真实值
2.0 1.60° 0.40° 0.00° 0.00°
4.0 3.20° 0.41° 0.03° 0.00°
6.0 4.80° 0.41° 0.03° 0.00°
8.0 6.40° 0.38° -0.05° 0.00°
10.0 8.00° 0.42° 0.04° 0.00°
卡尔曼在线估计出的零偏 = 0.762 °/s（真值 0.800）
[工况 2] 缓慢倾斜 0 -> 30°（考察动态跟踪）
缓慢倾斜 30° RMS: 原始加速度计 0.599° 互补 0.399° 卡尔曼 0.042° 峰值误差: 互补 0.44° 卡尔曼 0.13°
[工况 3] 快速摆动 ±30°（考察动态跟踪）
摆动 ±30°@0.5Hz RMS: 互补 0.399° 卡尔曼 0.042°
[工况 4] 叠加运动加速度干扰（模拟小车加速/振动，等效 ±8° 角度扰动）
有运动干扰 RMS: 原始加速度计 5.343° 互补 5.273° 卡尔曼 5.423° 峰值误差: 互补 8.06° 卡尔曼 8.24°
[结论]
* 静止：纯陀螺积分 10 秒漂 8.00°，互补滤波靠加速度计拉回来，误差最小
* 动态：卡尔曼因为同时估计零偏，动态误差通常更小
* 有运动干扰时两者都会变差 —— 根因是加速度计此时测的不是重力，
这时要么靠机械减振，要么用陀螺仪占更大的权重（减小 alpha 对应 τ 加大）
===== 以上数据由本机 gcc 实编译实运行产生 =====