一阶卡尔曼滤波入门教程：从原理到单片机 C 代码实现
在单片机项目中，我们经常会读取传感器数据，比如温度、角度、电压、电流、距离、速度等。
但是实际传感器数据往往不是稳定的，而是会不断抖动。
比如真实温度可能是 25℃，但传感器读出来可能是：
text24.8、25.2、25.1、24.9、25.3、24.7如果直接使用这些数据，控制系统就容易抖动。
所以我们需要一种滤波算法，让数据变得更平滑。
本文要讲的就是一种非常经典、非常实用的滤波算法：一阶卡尔曼滤波。
一、什么是卡尔曼滤波？
卡尔曼滤波是一种用于估计真实值的算法。
它的核心思想是：
text
不要完全相信传感器测量值，
也不要完全相信上一次估计值，
而是根据两者的可信程度，计算出一个更合理的新估计值。通俗来说：
卡尔曼滤波就是在"预测值"和"测量值"之间做一个聪明的加权平均。
如果传感器噪声很大，就少相信传感器。
如果系统变化很快，就多相信传感器。
二、一阶卡尔曼滤波适合什么场景？
一阶卡尔曼滤波适合处理单个变量的滤波问题。
例如：
| 应用场景 | 滤波变量 |
|---|---|
| 温度采集 | 温度值 |
| 电池电压检测 | 电压值 |
| ADC 采样 | 模拟量 |
| 超声波测距 | 距离值 |
| 红外测距 | 距离值 |
| 电机速度检测 | 速度值 |
| IMU 角度简单滤波 | 角度值 |
| 压力传感器 | 压力值 |
例如：
text
输入：带噪声的传感器数据
输出：更加平滑、可信的估计值三、为什么叫"一阶"卡尔曼滤波？
一阶卡尔曼滤波只估计一个状态变量。
例如只估计温度：
text
x = 温度或者只估计电压：
text
x = 电压或者只估计距离：
text
x = 距离它不像多维卡尔曼滤波那样需要矩阵运算，所以非常适合单片机入门使用。
四、一阶卡尔曼滤波的核心变量
一阶卡尔曼滤波主要有几个变量：
| 变量 | 含义 |
|---|---|
| x | 当前估计值 |
| z | 当前测量值 |
| p | 估计误差协方差 |
| q | 过程噪声 |
| r | 测量噪声 |
| k | 卡尔曼增益 |
看起来名字比较专业，但可以简单理解：
| 变量 | 通俗理解 |
|---|---|
| x | 滤波后的结果 |
| z | 传感器读到的原始数据 |
| p | 当前估计值的不确定程度 |
| q | 系统本身变化的不确定程度 |
| r | 传感器测量的不确定程度 |
| k | 本次有多相信传感器数据 |
五、最核心的公式
一阶卡尔曼滤波的公式如下：
text
预测估计值：
x = x
预测误差：
p = p + q
计算卡尔曼增益：
k = p / (p + r)
更新估计值：
x = x + k * (z - x)
更新误差：
p = (1 - k) * p其中最关键的是这一句：
text
x = x + k * (z - x)它的意思是：
text
新的估计值 = 旧的估计值 + 卡尔曼增益 × 测量误差其中：
text
z - x表示当前测量值和估计值之间的差距。
六、卡尔曼增益 k 怎么理解？
卡尔曼增益 k 决定了当前滤波结果更相信谁。
text
k 越大，越相信测量值
k 越小，越相信上一次估计值例如：
text
k = 0.8说明这次比较相信传感器测量值。
text
k = 0.1说明这次不太相信传感器，更相信之前的估计结果。
七、q 和 r 怎么理解？
一阶卡尔曼滤波中最重要的两个参数是：
text
q：过程噪声
r：测量噪声1. q 是什么？
q 表示系统本身变化的不确定程度。
可以理解为：
你认为真实值本身变化得快不快。
如果被测量的对象变化很快，比如电机速度、机器人姿态角，那么 q 可以稍微大一点。
如果被测量的对象变化很慢，比如室温、电池电压，那么 q 可以小一点。
2. r 是什么？
r 表示传感器测量噪声。
可以理解为：
你认为传感器数据有多不可信。
如果传感器噪声很大，r 就设置大一点。
如果传感器比较稳定，r 就设置小一点。
八、q 和 r 对滤波效果的影响
| 参数变化 | 效果 |
|---|---|
| q 增大 | 响应更快，但数据更容易抖 |
| q 减小 | 数据更平滑，但响应变慢 |
| r 增大 | 更不相信测量值，滤波更平滑 |
| r 减小 | 更相信测量值，响应更快 |
简单总结：
text
想要响应快：增大 q 或减小 r
想要更平滑：减小 q 或增大 r九、一阶卡尔曼滤波完整 C 语言代码
下面是一个非常适合单片机使用的一阶卡尔曼滤波代码。
1. 定义结构体
c
typedef struct
{
float x; // 当前估计值
float p; // 估计误差协方差
float q; // 过程噪声
float r; // 测量噪声
float k; // 卡尔曼增益
} KalmanFilter_t;2. 初始化函数
c
void Kalman_Init(KalmanFilter_t *kf, float init_x, float init_p, float q, float r)
{
kf->x = init_x;
kf->p = init_p;
kf->q = q;
kf->r = r;
kf->k = 0.0f;
}3. 更新函数
c
float Kalman_Update(KalmanFilter_t *kf, float measurement)
{
// 1. 预测误差协方差
kf->p = kf->p + kf->q;
// 2. 计算卡尔曼增益
kf->k = kf->p / (kf->p + kf->r);
// 3. 更新估计值
kf->x = kf->x + kf->k * (measurement - kf->x);
// 4. 更新估计误差协方差
kf->p = (1.0f - kf->k) * kf->p;
return kf->x;
}十、完整可移植代码
c
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
typedef struct
{
float x;
float p;
float q;
float r;
float k;
} KalmanFilter_t;
void Kalman_Init(KalmanFilter_t *kf, float init_x, float init_p, float q, float r);
float Kalman_Update(KalmanFilter_t *kf, float measurement);
#endif
c
#include "kalman_filter.h"
void Kalman_Init(KalmanFilter_t *kf, float init_x, float init_p, float q, float r)
{
kf->x = init_x;
kf->p = init_p;
kf->q = q;
kf->r = r;
kf->k = 0.0f;
}
float Kalman_Update(KalmanFilter_t *kf, float measurement)
{
kf->p = kf->p + kf->q;
kf->k = kf->p / (kf->p + kf->r);
kf->x = kf->x + kf->k * (measurement - kf->x);
kf->p = (1.0f - kf->k) * kf->p;
return kf->x;
}十一、使用示例：ADC 电压滤波
假设我们通过 ADC 采集电池电压。
原始电压值可能会抖动：
text
12.01V
12.08V
11.96V
12.04V
12.10V
11.98V使用卡尔曼滤波后，数据会更平滑。
1. 定义滤波器对象
c
KalmanFilter_t voltage_filter;2. 初始化滤波器
c
Kalman_Init(&voltage_filter, 12.0f, 1.0f, 0.01f, 0.5f);参数含义：
text
init_x = 12.0 初始估计值
init_p = 1.0 初始误差
q = 0.01 过程噪声
r = 0.5 测量噪声3. 周期调用滤波函数
c
void ADC_Task(void)
{
float voltage_raw;
float voltage_filter_value;
voltage_raw = ADC_GetVoltage();
voltage_filter_value = Kalman_Update(&voltage_filter, voltage_raw);
printf("raw: %.2f, filter: %.2f\r\n", voltage_raw, voltage_filter_value);
}十二、使用示例：电机速度滤波
编码器测速时，速度经常会跳动。
例如：
text
98
103
101
95
107
100可以使用一阶卡尔曼滤波让速度更平滑。
c
KalmanFilter_t speed_filter;
void Speed_Filter_Init(void)
{
Kalman_Init(&speed_filter, 0.0f, 1.0f, 0.1f, 2.0f);
}
float Get_Filtered_Speed(void)
{
float speed_raw;
float speed_filtered;
speed_raw = Encoder_GetSpeed();
speed_filtered = Kalman_Update(&speed_filter, speed_raw);
return speed_filtered;
}如果速度响应太慢，可以适当增大 q 或减小 r。
十三、使用示例：温度传感器滤波
温度变化一般比较慢，所以可以让滤波更平滑。
c
KalmanFilter_t temp_filter;
void Temp_Filter_Init(void)
{
Kalman_Init(&temp_filter, 25.0f, 1.0f, 0.001f, 0.1f);
}
float Get_Filtered_Temperature(void)
{
float temp_raw;
float temp_filtered;
temp_raw = Sensor_GetTemperature();
temp_filtered = Kalman_Update(&temp_filter, temp_raw);
return temp_filtered;
}温度场景中：
text
q 可以小一点
r 根据传感器噪声设置十四、参数怎么调？
一阶卡尔曼滤波最常调的是 q 和 r。
1. 数据太抖怎么办？
如果滤波后数据还是很抖，可以：
text
减小 q
增大 r例如：
c
Kalman_Init(&filter, 0.0f, 1.0f, 0.001f, 1.0f);2. 数据响应太慢怎么办？
如果真实值已经变化了，但滤波结果跟得很慢，可以：
text
增大 q
减小 r例如：
c
Kalman_Init(&filter, 0.0f, 1.0f, 0.1f, 0.1f);3. 常见参数参考
| 场景 | q 推荐值 | r 推荐值 |
|---|---|---|
| 温度滤波 | 0.001 ~ 0.01 | 0.1 ~ 1 |
| 电压滤波 | 0.001 ~ 0.05 | 0.1 ~ 2 |
| 距离滤波 | 0.01 ~ 0.1 | 1 ~ 10 |
| 电机速度滤波 | 0.05 ~ 0.5 | 1 ~ 10 |
| 角度简单滤波 | 0.001 ~ 0.1 | 0.1 ~ 5 |
注意：
这些只是入门参考值，实际项目中需要根据传感器噪声和系统响应速度进行调整。
十五、一阶卡尔曼滤波和一阶低通滤波的区别
一阶低通滤波常见公式是：
c
y = alpha * x + (1.0f - alpha) * y;它的思想是固定比例平滑。
而卡尔曼滤波会根据 p、q、r 动态计算卡尔曼增益 k。
| 对比项 | 一阶低通滤波 | 一阶卡尔曼滤波 |
|---|---|---|
| 参数 | alpha | q、r |
| 核心思想 | 固定比例平滑 | 根据不确定度动态调整 |
| 实现难度 | 很简单 | 稍复杂 |
| 响应能力 | 固定 | 可动态变化 |
| 单片机适用性 | 很适合 | 也很适合 |
| 调参难度 | 简单 | 中等 |
简单理解：
text
低通滤波：固定相信多少新数据
卡尔曼滤波：动态判断该相信多少新数据十六、卡尔曼滤波的优点
text
1. 滤波效果比较平滑
2. 能根据噪声情况动态调整
3. 适合传感器数据处理
4. 运算量不大，适合单片机
5. 参数含义比较清晰十七、卡尔曼滤波的缺点
text
1. 参数 q 和 r 需要调试
2. 初学者不太容易理解 p、q、r
3. 模型假设过于简单时效果有限
4. 如果传感器数据异常跳变，也需要额外处理十八、实际工程中的注意事项
1. 初始值 init_x 要合理
如果你滤波的是温度，真实温度大概是 25℃，可以设置：
c
init_x = 25.0f;如果不知道初始值，可以用第一次测量值作为初始值。
c
float first_value = Sensor_Read();
Kalman_Init(&filter, first_value, 1.0f, 0.01f, 0.5f);2. q 和 r 不要乱设成 0
不建议这样设置：
c
q = 0;
r = 0;尤其是 r 不能为 0，否则计算：
c
k = p / (p + r);可能出现异常情况。
3. 需要配合异常值处理
如果传感器突然读到一个明显错误的数据，比如：
text
正常距离：100cm
突然测到：9999cm这时候不应该直接交给卡尔曼滤波。
可以先做异常判断：
c
if (measurement > 500.0f)
{
return last_value;
}4. 滤波不是越平滑越好
滤波越平滑，通常响应越慢。
如果是温度、电压这种慢变量，可以追求平滑。
如果是电机速度、姿态角这种快变量，就不能滤得太慢，否则控制会滞后。
十九、一个带异常值判断的版本
c
float Kalman_Update_WithLimit(KalmanFilter_t *kf, float measurement, float max_jump)
{
float diff;
diff = measurement - kf->x;
if (diff > max_jump || diff < -max_jump)
{
measurement = kf->x;
}
kf->p = kf->p + kf->q;
kf->k = kf->p / (kf->p + kf->r);
kf->x = kf->x + kf->k * (measurement - kf->x);
kf->p = (1.0f - kf->k) * kf->p;
return kf->x;
}使用示例：
c
filtered_value = Kalman_Update_WithLimit(&filter, raw_value, 20.0f);表示如果本次测量值相对于当前估计值跳变超过 20，就认为它可能是异常值。
二十、适合初学者记忆的版本
如果刚开始学，只需要记住下面这几句话：
text
x 是滤波后的值
z 是传感器测量值
q 越大，响应越快
r 越大，滤波越平滑
k 越大，越相信测量值
k 越小，越相信历史估计值核心代码只有几行：
c
p = p + q;
k = p / (p + r);
x = x + k * (z - x);
p = (1 - k) * p;二十一、总结
一阶卡尔曼滤波非常适合单片机中的传感器数据处理。
它的核心思想是：
text
在预测值和测量值之间，根据可信程度动态取一个更合理的估计值。它适合处理：
text
温度
电压
ADC
距离
速度
角度
压力
电流在实际项目中，只要掌握：
text
x：估计值
z：测量值
p：估计误差
q：过程噪声
r：测量噪声
k：卡尔曼增益就可以很快把一阶卡尔曼滤波应用到单片机项目中。
最后记住调参口诀：
text
想更平滑：减小 q，增大 r
想响应更快：增大 q，减小 r二十二、最终推荐模板
c
typedef struct
{
float x;
float p;
float q;
float r;
float k;
} KalmanFilter_t;
void Kalman_Init(KalmanFilter_t *kf, float init_x, float init_p, float q, float r)
{
kf->x = init_x;
kf->p = init_p;
kf->q = q;
kf->r = r;
kf->k = 0.0f;
}
float Kalman_Update(KalmanFilter_t *kf, float measurement)
{
kf->p = kf->p + kf->q;
kf->k = kf->p / (kf->p + kf->r);
kf->x = kf->x + kf->k * (measurement - kf->x);
kf->p = (1.0f - kf->k) * kf->p;
return kf->x;
}调用示例：
c
KalmanFilter_t filter;
Kalman_Init(&filter, 0.0f, 1.0f, 0.01f, 0.5f);
while (1)
{
float raw_value;
float filter_value;
raw_value = Sensor_Read();
filter_value = Kalman_Update(&filter, raw_value);
printf("raw = %.2f, filter = %.2f\r\n", raw_value, filter_value);
}结语
对于单片机开发来说，一阶卡尔曼滤波是一个非常值得掌握的小算法。
它不像完整卡尔曼滤波那样需要大量矩阵运算，但已经能解决很多传感器抖动问题。
如果你正在做：
text
ADC 采样
电池电压检测
电机速度采集
超声波测距
IMU 角度处理
温度检测都可以尝试加入一阶卡尔曼滤波，让数据更加稳定，系统表现更加可靠。