卡尔曼到底在算什么:从一维到它的变式以及工程版卡尔曼滤波算法C语言实现
把角速度走低通、加速度走卡尔曼那一回,结尾留了一句没算完的话:参数固定的时候,卡尔曼算出来的那个系数会收敛成一个定值。
这句话得算完,因为它后面跟着一个很实在的疑问——如果它最后就是一个固定系数的低通,那为什么还要用它?四行代码、两个参数、还要理解“估计误差”,换来一个和一阶低通等价的东西,怎么算都不划算。
所以下面分三段:先把那个定值算出来,再看它凭什么还存在,最后讲怎么把参数调到你要的位置。
先把那句话算完
滤波器跑起来以后,四行里的量会各自走到一个稳定值。设稳定之后的增益是 K,那么“误差”这一项也不再变化:
稳定时:p = (1 − K) × p + q 也就是 K × p = q
而增益:K = p ÷ (p + r)
把第一式里的 p = q ÷ K 代进第二式,整理一下就是一个关于 K 的一元二次方程:
K² × r + K × q − q = 0
解出来:
K = ( −q + √(q² + 4rq) ) ÷ (2r)
代进上一回用的那组参数 q = 0.001、r = 0.543:
K ≈ 0.042
0.042 和一阶低通里的 alpha 是同一个东西。 也就是说,这组参数下的卡尔曼,跑到稳定以后,和一条系数 0.042 的低通在做同一件事。上一回量到的那个数也对得上:上电追到 90%,卡尔曼要 56 拍,而一个固定 alpha = 0.042 的低通要 54 拍。
r 和 q 谁大谁小决定这个系数,而且不是等比例,是开根号(q 固定为 0.001):
r |
稳态增益 K |
相当于一个多大的 alpha |
|---|---|---|
| 0.0543 | 0.127 | 0.13 |
| 0.543 | 0.042 | 0.04 |
| 5.43 | 0.0135 | 0.014 |
r 放大十倍,系数大约降到三分之一。想让系数减半,r 得翻四倍——“把 R 调大更稳”这句话的收益是开根号的,这一点在调参的时候一定要记住,不然会一直加大 R 却发现没什么变化。
到这里,疑问就摆到桌面上了:既然是这样,为什么不用低通?
那它凭什么还存在
三个理由,第二个是主要的。
理由一:参数可以随时间变
低通的系数是写死的,一个数管到底。卡尔曼的系数是从 q、r 算出来的,所以只要这两个数变,系数就跟着变。
典型场合是“数据的可信度并不是一直一样”:传感器刚上电时不稳、转速低时编码器测速不准、温度低时器件噪声变大。这时候可以在运行中改 r:
/* 数据不可信的时候把 r 调大,滤波器就更信自己的历史 */
void Kalman1D_SetR(Kalman1D *kf, float r)
{
if (r <= 0.0f)
{
r = 0.001f; /* 参数边界:r 不能是 0 或负数 */
}
kf->r = r;
}
这样同一个滤波器,在数据好的时候跟得紧,在数据差的时候稳得住——这是固定系数的低通做不到的,它只有一个档。
理由二:多一个状态,能估一个没有测到的量
这是它真正值钱的地方。
一维卡尔曼的状态就一个:“我要平滑的那个量”。它能做的只有平滑。但很多问题的关键恰恰是有一个量你根本测不到,却能从别的量里推出来。
把陀螺仪和加速度计放在一起看就是这种问题。陀螺仪测的是角速度,要变成角度得积分;而它带着零偏,积分一小时能偏出去一大圈——讲零偏那一回已经算过一遍。加速度计能给出“哪边是下”,能定出俯仰和横滚,但它抖,而且一提速就把运动当成重力。
关键在于:陀螺仪的零偏是可以估的。 它不会自己跳出来说话,但它的痕迹在数据里——角度积分起来总是往一个方向偏,这个“偏”的速度就是零偏。
所以把状态从一个变成两个:
状态 1:角度 —— 下一拍的角度 = 这一拍角度 + (角速度 − 零偏) × dt
状态 2:零偏 —— 下一拍的零偏 = 这一拍的零偏(它变得很慢,就当成不变)
观测:加速度算出来的那个角度
两个状态、一个观测,卡尔曼照样能跑:它一边积分角速度,一边看着“积分出来的角度”和“加速度说出来的角度”差多少,把这个差按比例分成两份——一份用来修角度,一份用来修零偏。
typedef struct
{
float angle; /* 状态 1:角度(度) */
float bias; /* 状态 2:陀螺仪零偏(度每秒) */
float p00;
float p01;
float p10;
float p11; /* 协方差,手写展开的 2×2 */
float q_angle; /* 角度这一侧的模型噪声 */
float q_bias; /* 零偏这一侧的模型噪声 */
float r_measure; /* 加速度那一路的测量噪声 */
} Kalman2D;
void Kalman2D_Init(Kalman2D *kf, float q_angle, float q_bias, float r_measure)
{
kf->angle = 0.0f;
kf->bias = 0.0f;
kf->p00 = 0.0f;
kf->p01 = 0.0f;
kf->p10 = 0.0f;
kf->p11 = 0.0f;
kf->q_angle = q_angle;
kf->q_bias = q_bias;
kf->r_measure = r_measure;
}
/* 每拍调一次:gyro_rate 是角速度(度每秒),accel_angle 是加速度算出的角度,dt_s 是这一拍的时间 */
float Kalman2D_Update(Kalman2D *kf, float gyro_rate, float accel_angle, float dt_s)
{
float s;
float k0;
float k1;
float y;
float p00_old;
float p01_old;
/* 一、预测:状态往前推一步,协方差也跟着涨 */
kf->angle += (gyro_rate - kf->bias) * dt_s;
kf->p00 += (dt_s * ((dt_s * kf->p11) - kf->p01 - kf->p10 + kf->q_angle));
kf->p01 -= dt_s * kf->p11;
kf->p10 -= dt_s * kf->p11;
kf->p11 += kf->q_bias * dt_s;
/* 二、算增益:两个状态各自该信测量多少 */
s = kf->p00 + kf->r_measure;
k0 = kf->p00 / s;
k1 = kf->p10 / s;
/* 三、更新:把“加速度说出来的角度”和“积分出来的角度”之差分给两个状态 */
y = accel_angle - kf->angle;
kf->angle += k0 * y;
kf->bias += k1 * y;
/* 四、修完误差,下一拍接着用。这一步要用更新之前的 p00、p01,
所以先存一份——否则上一行刚改小的数会被下一行拿去用 */
p00_old = kf->p00;
p01_old = kf->p01;
kf->p00 = p00_old - (k0 * p00_old);
kf->p01 = p01_old - (k0 * p01_old);
kf->p10 = kf->p10 - (k1 * p00_old);
kf->p11 = kf->p11 - (k1 * p01_old);
return kf->angle;
}
那几行 p 看着吓人,但每一步都在干同一件事:先把不确定度按模型推一步,再按测量收回一点。 一维的时候它们退化成上一回里那两个数(p_last 和 p_now)。
两个状态各用一套参数:q_angle 管“角度这一拍可能变多少”,q_bias 管“零偏这一拍可能变多少”(它变得很慢,所以给一个很小的数),r_measure 还是测量的噪声。
跑起来会看到什么?用一组造出来的数据试:真值每秒转 5 度,陀螺仪带 0.5 度每秒的零偏,加速度算出来的角度叠着抖动。12 秒之后:
只用陀螺仪积分 角度 66.0 度(真值 60.0 度,误差 +6.0 度)
两状态卡尔曼 角度 59.95 度(误差 −0.05 度),零偏估计 0.55(真值 0.50)
零偏估计是一路慢慢爬上来的,中间还会在真值附近晃(第 2 秒估到 0.38,第 4 秒又回到 0.31,第 8 秒才到 0.58)。它不是一步到位,也不需要一步到位——重要的是角度那一侧的误差整段都压在 0.3 度以内,而纯积分已经在往外跑 6 度了。
这就是低通做不到的事。 低通只能平滑它拿到的那一个数;它没有“零偏”这个状态,也就没有地方安放这个估计。想让它替你估一个没测到的量,得给它一个状态。
理由三:多个来源、不同速率
同一个量往往有好几个来源:加速度计和陀螺仪、编码器和视觉、卫星和惯性。它们的更新率不一样、可信度不一样、失效的场合也不一样。
卡尔曼给的是一套“按可信度加权”的写法:谁的不确定度小,谁的话就更算数;某个来源暂时不可信,把它的 r 调大就行,不用改整个结构。换来源、加来源,都不需要重写滤波器。
手册里对 DMP 有一段话,把这件事讲得很清楚:运动处理算法通常要跑在很高的速率上,往往 200 赫兹左右,才能给出低延迟的结果——哪怕应用界面 5 赫兹才更新一次,内部该跑 200 赫兹还是得跑。这就是“内部速率和输出速率可以分开”。卡尔曼做的是同一件事:状态按传感器来的速率更新,界面按自己的节奏去取最新值,两者不必相等。
再说一句:非线性的时候
上面的模型都是线性的——“下一拍的角度 = 这一拍角度 + 角速度 × 时间”,是一条直线关系。可真实的姿态不是:倾斜到一定程度,角速度和角度变化之间的关系里会出现三角函数;两个状态相乘的情况也会出现。
一旦模型变成非线性的,协方差就不能再靠几个乘加直接往前推了。做法是先在当前工作点附近把关系近似成直线,用一阶偏导(也就是雅可比)替代原来的常数系数,这就是扩展卡尔曼滤波(EKF)。它还是那套预测、算增益、更新、修误差,只是把不确定度往前传的算法换了。
一维的场合,雅可比退化成一个 1,所以你看到的那四行里一个偏导都没有。
| 你手里的问题 | 需要什么 |
|---|---|
| 一个量有噪声,只是想平滑它 | 一阶低通就够,卡尔曼只是把系数算得更讲究 |
| 数据可信度会变 | 卡尔曼,让 q 或 r 随时间变 |
| 有一个量测不到,但能从别的量推出来 | 多状态的卡尔曼 |
| 多个来源、不同更新率 | 多状态的卡尔曼,配合各自的 r |
| 模型里出现三角函数、乘法 | 扩展卡尔曼(EKF) |
怎么调参
参数只有两个(多状态时多几个),但它们的量纲不直观,所以给几个能落地的办法。
一、反推法:先定“要多慢”,再反推 r
上面那个稳态公式是可以倒过来用的。想让它稳定后的系数是 K,就解出对应的 r:
r = q × (1 − K) ÷ K²
q 保持 0.001,想要稳态系数 0.1:
r = 0.001 × 0.9 ÷ 0.01 = 0.09
代回去验证,稳态增益正好是 0.1。这条式子的意义是:调参不再是试数字,而是先回答“我允许它慢多少”。 固定的低通里,系数 0.1 大约 22 拍到 90%、0.042 大约 54 拍;卡尔曼跑起来要 56 拍——它按冷启动时的等效系数算,比同系数的低通稍慢一点。这些数都能算出来,也能在屏幕上一拍拍数出来。
代码里给两个小函数,正反两个方向都能算:
#include <math.h>
/* 给定 q、r,稳态增益是多少(解 K²r + Kq − q = 0,取正根) */
float Kalman1D_SteadyGain(float q, float r)
{
return (-q + sqrtf((q * q) + (4.0f * r * q))) / (2.0f * r);
}
/* 想要稳态增益是多少,反推 r。target_gain 必须落在 0 和 1 之间 */
float Kalman1D_GainToR(float q, float target_gain)
{
if ((q <= 0.0f) || (target_gain <= 0.0f) || (target_gain >= 1.0f))
{
return 0.0f; /* 参数不合理:返回 0,让调用方别用这个结果 */
}
return (q * (1.0f - target_gain)) / (target_gain * target_gain);
}
二、q 和采样周期绑在一起
q 的含义是“这一拍模型可能变多少”。它天生带着采样周期——周期一变,这个“一拍”就变了。量纲上有两种写法:按“每一拍”直接给一个方差,或者按“每秒”给一个噪声密度、用的时候乘 dt。这篇前半按前者;后面那份工程代码按后者,差一个 dt,换算的时候别混。
同样一段物理过程,采样周期从 10 毫秒变成 20 毫秒,每一拍之间的变化就翻倍——q 不变,滤波器就会显得比原来“迟钝”。反过来把周期改小,q 不变会让它更信新数据。
所以:周期一改,参数就要重算。 粗算的办法是按时间比例缩放 q,再在曲线上确认;不要指望一组数字在所有周期下都合适。这也是为什么“从别人那里抄一组 Q、R”基本没用——那一组数是为别人的采样周期和别人的信号准备的。
还有一层:dt_s 未必每拍都一样。读数据的节拍被别的任务拖长一点(刷屏、串口打印、擦 Flash),这一拍可能是 25 毫秒而不是 10 毫秒,可 q 还是按 10 毫秒一拍算的。单次偏差看不出来,连续拖几次,滤波器就会以为模型比实际更稳,跟不上真实的变化。
处理办法两条:把读锁死在定时器中断里,让 dt_s 真的是常数;或者用实测的 dt_s 去缩放 q——q 是按时间比例长大的,一拍长了一倍,它也按比例放大。代码里 dt_s 是从外面传进来的,传进去的是实测差值还是写死的常数,结果完全是两回事。
三、r 的起点从原始数据的抖动里估
r 是测量噪声的方差。它有经验成分,但起点可以量:
让被测对象保持不动,连续采 100 拍原始数据
算出这批数据的方差(或者干脆取“最大值减最小值”的一半再平方)
把这个数量级当成 r 的起点
比如静止时那个加速度轴在 ±0.02 g 之间晃,方差的量级就是 10⁻⁴ 那一档,r 就从这附近开始试。先量噪声,再定 R,比凭感觉填一个 0.5 强得多。
四、三组参数对照,以及把参数改坏的边界
拿同一组数据(加速度那一路),依次跑三组:
Kalman1D_Init(&kf, 0.001f, 5.43f, 0.02f); /* 稳,但慢:稳态系数约 0.014 */
Kalman1D_Init(&kf, 0.001f, 0.543f, 0.02f); /* 折中:稳态系数约 0.042 */
Kalman1D_Init(&kf, 0.001f, 0.0543f, 0.02f); /* 快,但噪声会进来:约 0.127 */
三组的差别在阶跃那一下和抖动段上最明显:越稳的那条爬得越慢,越快的那条毛刺越多。
然后故意把参数写坏,把边界摸清楚:
r = 0:增益算出来是 1,滤波器等于不滤波,全盘相信测量;q = 0:误差项只减不增,跑一阵以后增益趋近于 0,输出就“冻”在那儿;- 两个都是负数或零:除零会让输出变成
NaN(屏幕上显示成一串没意义的数,比较还会全部为假)。
工程版的做法是在初始化时把参数卡到正数,更新之后检查一次结果:
void Kalman1D_InitGuarded(Kalman1D *kf, float q, float r, float p_start)
{
if (q <= 0.0f)
{
q = 0.001f;
}
if (r <= 0.0f)
{
r = 0.001f;
}
Kalman1D_Init(kf, q, r, p_start);
}
/* 更新之后检查一次:结果不是有限数就退回当前测量值 */
float Kalman1D_SafeUpdate(Kalman1D *kf, float input)
{
float value = Kalman1D_Update(kf, input);
if (!isfinite(value)) /* NaN、正无穷、负无穷,一律拦下 */
{
kf->out = input;
kf->p_last = kf->q;
return input;
}
return value;
}
这里用 isfinite(),<math.h> 里就有。常见的写法是 value != value(自己不等于自己才是 NaN),但它只认 NaN:一旦溢出成正负无穷,那个判断是假的,会漏过去——而溢出在浮点里比 NaN 更容易发生。至于参数在运行时被外部改坏了,Kalman1D_SetR 里那一层判断已经挡住负数了。
五、怎么知道参数调对了:看新息,不看曲线
两条曲线叠在一起,很难看出 q、r 给得对不对——都“看起来挺平滑”。有个能算的判据:新息,也就是这一拍的测量减上一拍的预测。
如果滤波器是自洽的,新息应该是一个均值为 0、方差等于“预测误差加测量噪声”(p_last + q + r)的随机量。把每一拍的 新息² ÷ 这个方差 算出来,攒一批取平均,就是归一化新息平方,它应该落在 1 附近。
/* 每拍调 Kalman1D_Update() 之前算一次;攒 50 到 100 拍取平均 */
static float Kalman1D_Nis(const Kalman1D *kf, float input)
{
float y = input - kf->out; /* 新息:测量减预测 */
float s = kf->p_last + kf->q + kf->r; /* 新息本该有的方差 */
return (y * y) / s; /* 这一拍的 NIS */
}
均值持续大于 2,说明滤波器把新息的方差估小了——r 给小了、q 给小了,或者模型压根没描述住真实的变化,都会往这个方向走;持续小于 0.5,说明方差估大了,它反应得比需要的更慢。这比盯着曲线猜要硬得多:曲线可以靠眼睛自圆其说,这个数字不行。
一句提醒:这个判据只检查“参数和实际抖动配不配”,查不出模型写错了。模型错的时候,新息会被滤波器悄悄吸收掉,均值反而可能很漂亮。
再往上:融合与组合导航
两个状态已经是“多传感器融合”的雏形:一个来源是陀螺仪(经过模型积分),另一个来源是加速度计。它们的可信度不同、更新率也可以不同,融合的结果比任何单一来源都稳。
把这个思路放大,就是两条常见的路:
- 多传感器融合:编码器加视觉、雷达加相机、卫星加惯性。每个来源给一份估计和一份不确定度,谁的不确定度小,谁就更算数;某个来源失真了,把它的
r调大就能把它边缘化。加一个来源要补的是测量矩阵、噪声和更新时机,框架本身不用推倒重来。 - 组合导航:卫星定位(GNSS)长期不漂,但更新慢、被遮挡就断;惯性器件短期很稳,但会漂。两者合起来,卫星补上惯性缺的那条基准线,惯性填上卫星两次更新之间的空档——讲 IMU 是什么那一回里那段“互相修正”,落到代码里就是这套加权。而且这个补偿是双向的:有了绝对位置做参照,惯性的零偏能被反过来估出来,正好就是这里“零偏当第二个状态”那件事的放大版。
- 芯片自己就做了一部分:这颗 MPU6050 片内有个 DMP,可以跑融合。这里要说得准确一点——DMP 本身只是一台小处理器,具体跑什么由厂商的运动驱动决定;手册里“运动处理跑 200 赫兹”说的是它的内部节拍。辅助 I2C 有五路从机通道,但能力不一样:
SLV0到SLV3可以一起组成一组、随每次采样打包进 FIFO,SLV4只做单次读写、不进 FIFO——所以“一次挂 4 个”说的是能一起打包的那四路。磁力计挂在这条总线上,融合结果由 DMP 算好再给单片机。同一件事有两条路:要么像前面这样自己写四行,要么把一部分交给 DMP、单片机只取结果。
接回控制环
同一套东西可以原样接到速度环上:编码器算出实际转速,先过一遍滤波,PID 再拿估计值和目标值比。噪声大的时候这一层很值钱——一个 192 的错读数直接进 PID,下一拍就会先把电机压下去再补回来,而滤波后的估计不会跳。
但两件事要记住:
- 参数要按控制周期、测速窗口和电机响应重新定,别把这里的数字直接搬到一个 10 毫秒的环里;
- 滤波修不了方向接反、编码器丢脉冲、机械松动。这些不在信号里,滤波器看不见它们。
三处最容易埋雷的地方
上面那些调法是从曲线上看出来的。工程上还有三件事,坏了的时候曲线不一定难看,但数会一点一点变坏。
一、协方差会失去对称,最后连正负都错
两个状态的协方差是一个 2×2 的对称矩阵:p01 和 p10 本该永远相等。但更新那四行里它们是分开算的,浮点每拍都会丢一点点对称性。按 50 赫兹跑半小时就是九万拍,误差攒起来,p01 和 p10 开始往两个方向走。走到某一拍 p00 变成负数——方差为负,物理上不可能——增益的符号就翻了,角度会朝相反方向修,而且越修越离谱。
判据不长:
if ((kf->p00 < 0.0f) || (fabsf(kf->p01 - kf->p10) > 1e-6f))
{
/* 记一次:协方差已经不对称,或者已经不合法了 */
}
便宜的一条修法是每拍结束把它们拉回平均:
kf->p01 = 0.5f * (kf->p01 + kf->p10);
kf->p10 = kf->p01;
稳的一条是换一种更新写法,叫 Joseph 形式。一维的时候右边只剩两项:
P新 = (1 − K)² · P旧 + K² · R
两项都是平方,都不小于零,加起来自然也不小于零,所以不管怎么跑,方差都不会变负。(原来那行 P新 = (1 − K) · P旧 在 K 略微超过 1 的时候会直接翻负号——而 K 在浮点里是真的可能略微超过 1。)二维展开成四行会难看一些,但道理一样;小车这种规模,先用上面那个拉回平均的办法通常就够了。
二、q 不是可以照抄的数
q 容易被当成一个固定的“模型有多不靠谱”的系数。它有量纲:角度方差的单位是度²,而它描述的是“一次预测之后,多出来多少度²的不确定”。采样周期一变,这个“一次”就变了。
同一套连续噪声,周期从 10 毫秒改成 20 毫秒,q 要跟着翻倍——一次预测跨了两倍的时间,模型误差按时间累积。周期减半,q 也减半。这是量纲决定的:改了周期却不改 q,等于偷偷改了“该信模型多少”,而屏幕上只表现为“响应变快了”或者“变粘了”,看不出根因。
正规的做法是先量连续噪声密度——陀螺的角度随机游走,单位是度每根号小时——再乘 dt 换成每一拍的方差。0.001 可以当起点,但它只对某一个 dt 成立。
三、p_last 和 r 都要有个来源
p_last 的初值(p_start)落在两个极端之间:太小,前几秒滤波器死信陀螺,加速度怎么喊都不理,角度会一直按陀螺漂;太大,第一帧测量就被当成真理,角度先跳一下再慢慢收回来。合理的取法是按“开机时我有多不确定”来定——初始角度真的不知道,就给个几十度的方差,让它先信测量。
r 可以直接量:静止放着采一千个样本,算方差。但要留意它只代表瞬时抖动;真正让角度越走越远的那部分,也就是零偏不稳定和角度随机游走,要用 Allan 方差这一类办法单独量,量出来的是另外一组数。拿单次方差当全部噪声,r 会偏小,滤波器就过于相信测量,上一节那个 NIS 会稳定地大于 2。
再往里走:专业写法长什么样
上面那些够把小车跑起来了。再往里走,有几样东西是行业里的标准动作。现在看不懂没关系——先知道它们各自叫什么、解决的是哪一类问题,等哪天发现自己卡在同一个地方第三次,就该回来查这几个名字了。
一、平方根滤波:不传协方差,传它的“根”
标准卡尔曼每拍都要更新 P,而 P 里的元素能差好几个数量级:角度的方差在 1e-8 量级,零偏的方差在 1e-6,交叉项还要更小。拿 float 存这种矩阵,小项在大项参与的那次加法里会被直接舍掉,P 就慢慢不再是那个“对称正定”的东西了。
专业做法根本不传 P,而是传它的因子。最常用的是 P = U·D·Uᵀ(U 单位上三角,D 对角非负),每拍只更新 U 和 D,再由它们算增益。数学上和标准卡尔曼完全等价,但 P 永远正定,而且精度要求能降一半——同样的精度,位数可以少用几位。这套叫 UD 分解,航空惯导里的标准写法,现在不少飞控的姿态估计还是这么写的。
二、误差状态:滤的是“差多少”,不是“是多少”
姿态用四元数的时候,不能直接把它当滤波状态:四个分量只有三个自由度,第四个被“长度等于 1”钉住,而卡尔曼假设状态可以自由加减。硬滤的结果是每拍归一化一次,归一化又会把协方差该有的形状搅乱。
专业写法绕开这一层:状态里放的是“误差角”——三个小量,可以自由加减;滤波跑完,再把误差注回四元数。名字叫误差状态卡尔曼(ESKF),姿态估计的标准答案。它顺手解决的另一个问题:横滚接近 90 度时欧拉角会撞上万向节死锁,在误差状态形式下这件事根本不出现。
三、新息门控:先判这一拍能不能信
上一节那个 NIS 是攒一批看平均,工程上还有逐拍版:每一拍算 y² / s,超过阈值就认定这个测量是野值,直接丢掉这一拍,输出仍取预测值。阈值查卡方分布——一个自由度下,95% 是 3.84,99.9% 是 10.8。
float y = accel_angle - kf->angle;
float s = kf->p00 + kf->r_measure;
if ((y * y) > (KALMAN_GATE * s)) /* KALMAN_GATE 取 10.8,约 99.9% */
{
/* 这一拍判为野值:不更新,输出还是预测值 */
}
案例:小车撞一下桌腿,加速度计瞬间报出一个 200 度的假倾角。没有门控,滤波器被这一拍拽走,然后要好几百毫秒才爬回来,屏幕上的角度先跳一下再慢慢回正;有门控,这一拍压根进不去。
四、自适应 q:模型变了,q 也得跟着变
前面说 q 要跟 dt 走。还有一层:同一个 dt 下,模型本身也可能变——负载加重、温度升高、电池电压掉了,陀螺的噪声和零偏都会变,而 q 还钉在昨天的数上,滤波器就会“太自信”。
两条常见做法。一是渐消记忆:每拍把 P 乘一个略大于 1 的因子(1.01 就够),等效于“每隔一会儿忘掉一点历史”,模型悄悄变了也不至于跟不上。二是在线调 q:看新息,新息持续偏大就把 q 抬上去,让它更信测量;偏小就压回来。前者叫渐消因子,后者叫自适应卡尔曼,组合导航里几乎是标配。
案例:车跑直线时陀螺很稳,q 该小;一过减速带,车体振动让读数抖起来,新息突然变大。定死的 q 在这时候反应迟钝,自适应 q 把增益推高,收住的时间从几百毫秒缩到几十毫秒。
五、序列标量更新:一个一个喂,不攒矩阵
测量是一维的时候,卡尔曼退化成标量除法,连矩阵逆都不用求。有多个测量时,标准写法把它们堆成向量、求一次 (H·P·Hᵀ + R) 的逆;专业写法是拆成一个一个标量依次更新——结果在数学上完全一样,但每次只除一个数,计算量小,数值也更稳。
飞控里“陀螺先更新、加速度再更新、磁力计最后更新”就是这个意思。九轴融合一拍的测量是 9 维,求 9×9 的逆在单片机上很贵;拆成 9 次标量更新,只有 9 次除法。
这几样现在记不住不要紧。给一个判断标准:同一个问题你卡到第三次,就该去查它的名字了——调参调到第三次还调不好,多半说明这一层缺了东西,跟手气无关。真到那时候再翻回来,会发现它们并不难,难的是先知道它们存在。
一份能直接用的实现:工程版和教科书差在哪
上面讲的都是“为什么”。真拿到车上跑,会在七个地方和教科书那版分开——每一处都是被现场逼出来的,不是口味问题。下面这份是完整的,能直接编译(sqrtf、atan2f 记得带上 <math.h>)。
/* ===== 配置:这几个数是这台车跑出来的,换车要重测 ===== */
#define KF_GATE_CHI2 10.8f /* 卡方检验阈值,1 自由度 99.9% */
#define KF_R_INFLATE 36.0f /* 测量不可信时 r 放大的倍数 */
#define KF_ACC_TOL_G 0.15f /* 加速度模长偏离 1g 超过这么多,就认为不可信 */
#define KF_P_MAX 1.0e4f /* 协方差上限,发散了也要能自己走回来 */
#define KF_P_MIN 1.0e-6f /* 协方差下限,不能让它到零 */
#define KF_BIAS_MAX_DPS 20.0f /* 手册给的初始零偏保证值就是 ±20 度每秒 */
typedef struct
{
float angle; /* 状态一:角度,度 */
float bias; /* 状态二:陀螺零偏,度每秒 */
float p00, p01, p10, p11;
float q_angle; /* 陀螺积分带来的角度噪声,度^2 每秒 */
float q_bias; /* 零偏自己的游走,(度每秒)^2 每秒 */
float r_measure; /* 加速度计测出来的角度,方差,度^2 */
uint32_t reject_cnt; /* 连续被门控丢掉的拍数,调参时盯这个 */
} KalmanAttitude;
void KalmanAttitude_Init(KalmanAttitude *kf)
{
kf->angle = 0.0f;
kf->bias = 0.0f;
kf->p00 = 30.0f * 30.0f; /* 开机时角度有多不确定:给个大的,先信测量 */
kf->p01 = 0.0f;
kf->p10 = 0.0f;
kf->p11 = 5.0f * 5.0f; /* 零偏的不确定 */
kf->q_angle = 0.0005f;
kf->q_bias = 0.0001f;
kf->r_measure = 0.5f * 0.5f;
kf->reject_cnt = 0U;
}
void KalmanAttitude_Predict(KalmanAttitude *kf, float gyro_rate_dps, float dt_s)
{
kf->angle += (gyro_rate_dps - kf->bias) * dt_s;
/* 两行里藏着 q 和 dt 的关系:q 的单位是“每秒”,乘上 dt 才是这一拍 */
kf->p00 += dt_s * (dt_s * kf->p11 - kf->p01 - kf->p10 + kf->q_angle);
kf->p01 -= dt_s * kf->p11;
kf->p10 -= dt_s * kf->p11;
kf->p11 += kf->q_bias * dt_s;
}
/* 返回 1:这一拍用上了;0:被判成野值,没更新 */
uint8_t KalmanAttitude_Update(KalmanAttitude *kf,
float measured_angle,
float accel_norm_g)
{
float r;
float s;
float y;
float k0, k1;
float p00_old, p01_old;
/* 一、先问这个测量本身可不可信:加速度模长离开 1g 越远,角度越不准 */
r = kf->r_measure;
{
float dev = accel_norm_g - 1.0f;
if (dev < 0.0f) { dev = -dev; }
if (dev > KF_ACC_TOL_G) { r *= KF_R_INFLATE; }
}
/* 二、门控:新息平方比超过卡方阈值,这一拍直接丢 */
s = kf->p00 + r;
y = measured_angle - kf->angle;
/* 角度是环形量:179 度和 −179 度只差 2 度,不是 358 度 */
while (y > 180.0f) { y -= 360.0f; }
while (y < -180.0f) { y += 360.0f; }
if ((y * y) > (KF_GATE_CHI2 * s))
{
if (kf->reject_cnt < 0xFFFFFFFFU) { kf->reject_cnt++; }
return 0U;
}
kf->reject_cnt = 0U;
/* 三、增益,更新两个状态 */
k0 = kf->p00 / s;
k1 = kf->p10 / s;
kf->angle += k0 * y;
kf->bias += k1 * y;
if (kf->angle > 180.0f) { kf->angle -= 360.0f; }
if (kf->angle < -180.0f) { kf->angle += 360.0f; }
/* 四、协方差更新:先存旧值,再对称化 */
p00_old = kf->p00;
p01_old = kf->p01;
kf->p00 = p00_old - (k0 * p00_old);
kf->p01 = p01_old - (k0 * p01_old);
kf->p10 = kf->p10 - (k1 * p00_old);
kf->p11 = kf->p11 - (k1 * p01_old);
kf->p01 = 0.5f * (kf->p01 + kf->p10);
kf->p10 = kf->p01;
/* 五、限幅:零偏跑出手册范围说明模型错了,协方差发散也要能收回来 */
if (kf->bias > KF_BIAS_MAX_DPS) { kf->bias = KF_BIAS_MAX_DPS; }
if (kf->bias < -KF_BIAS_MAX_DPS) { kf->bias = -KF_BIAS_MAX_DPS; }
if (kf->p00 < KF_P_MIN) { kf->p00 = KF_P_MIN; }
if (kf->p11 < KF_P_MIN) { kf->p11 = KF_P_MIN; }
if (kf->p00 > KF_P_MAX) { kf->p00 = KF_P_MAX; }
if (kf->p11 > KF_P_MAX) { kf->p11 = KF_P_MAX; }
return 1U;
}
调用就三步,放在 10 毫秒的采样节拍里:
static KalmanAttitude g_kf;
void Attitude_Task(const MPU6050_Data *data, float dt_s)
{
float ax = data->accel_x_g;
float ay = data->accel_y_g;
float az = data->accel_z_g;
float norm = sqrtf((ax * ax) + (ay * ay) + (az * az));
float measured = atan2f(ay, az) * 57.29578f; /* 横滚角,度 */
KalmanAttitude_Predict(&g_kf, data->gyro_x_dps, dt_s);
if (KalmanAttitude_Update(&g_kf, measured, norm) == 0U)
{
/* 这一拍是野值:可以打一行日志,也可以什么都不做 */
}
}
七处和教科书不一样的地方:
| 教科书那版 | 这里 | 为什么 |
|---|---|---|
r 是常数 |
按加速度模长偏离 1g 放大 36 倍 | 车一加速,加速度计量到的就不只是重力了 |
| 每一拍都信 | 新息超过卡方阈值就丢这一拍 | 撞一下桌腿、I2C 错一位,一个假测量能拽走好几百毫秒 |
p01、p10 分别算 |
每拍做一次对称化 | 浮点会跑偏;跑到 p00 变负,就再也回不来 |
| 协方差没有上限 | p00、p11 上下都夹住 |
真发散的时候,限幅是唯一的自愈机会 |
q 直接写常数 |
q 当“每秒”,乘上 dt 才是每拍 |
换采样周期不用重新调参数 |
| 零偏自由漂 | 夹在 ±20 度每秒 | 手册讲的初始零偏保证值就是 ±20,超出去就是模型错了 |
| 残差直接相减 | 先折回 ±180 度再算 | 角度是环形的,179 和 −179 只差 2 度,直接减会得到 −358 |
换到位置和速度,骨架不用改
这套东西的价值不在“角度”这个词上,在两个状态的关系上。把“角度、零偏”换成“位置、速度”:
- 预测:
位置 += 速度 * dt,速度不变; - 协方差:
p00 += dt*(p01 + p10 + dt*p11) + q_pos、p01 += dt*p11、p10 += dt*p11、p11 += q_vel; - 测量:
y = 测到的位置 - 预测的位置,取p00 + r。
注意这里的 dt 是正号。角度—零偏那个模型的 F 里 dt 前面是负的(零偏越大,预测出来的角度越偏),位置—速度是正的(速度越大,预测的位置越远);符号抄错,协方差就会朝反方向长。
其余五处——门控、自适应 r、对称化、限幅、q 乘 dt——一模一样(原来夹零偏,换到这边就是夹速度)。这就是自动驾驶里跟车、融合轮速和卫星定位的标准骨架(匀速模型)。它跟不上急刹和急加速,那要换成交互多模型(IMM),几条模型并行跑、按似然加权——那又是另外的事了......
最后橘猫说
卡尔曼的状态只有一个时,它跑稳之后和一条固定系数的低通做的是同一件事——q = 0.001、r = 0.543 解出 0.042,而且 r 与这个系数是开根号的关系。它的价值在固定系数的低通做不到的三件事:参数能随时间变、能多带一个测不到的状态(陀螺零偏就是这么估出来的)、能按各自的 r 融合多个来源;模型里出现三角函数或相乘,四步就推广成 EKF。调参别试数字:先定“我要它多慢”,用 r = q(1−K)/K² 反推,r 从原始数据的方差起,周期一改必须重算,最后拿新息的均值验一遍。老边界还在:方向接反、丢脉冲、机械松动,它都看不见。
最后蜘蛛说
看不懂?没事,会用就行,看懂了也照样试着调参,仿真完美不等于工程完美

卡尔曼滤波常被说得很神秘。可当它只估一个量的时候,结果为什么不比一条普通滤波更强——那它真正多出来的本事是什么?
浙公网安备 33010602011771号