卡尔曼到底在算什么:从一维到它的变式以及工程版卡尔曼滤波算法C语言实现

把角速度走低通、加速度走卡尔曼那一回,结尾留了一句没算完的话:参数固定的时候,卡尔曼算出来的那个系数会收敛成一个定值。

这句话得算完,因为它后面跟着一个很实在的疑问------如果它最后就是一个固定系数的低通,那为什么还要用它?四行代码、两个参数、还要理解"估计误差",换来一个和一阶低通等价的东西,怎么算都不划算。

所以下面分三段:先把那个定值算出来,再看它凭什么还存在,最后讲怎么把参数调到你要的位置。

先把那句话算完

滤波器跑起来以后,四行里的量会各自走到一个稳定值。设稳定之后的增益是 K,那么"误差"这一项也不再变化:

text 复制代码
稳定时:p = (1 − K) × p + q        也就是 K × p = q
而增益:K = p ÷ (p + r)

把第一式里的 p = q ÷ K 代进第二式,整理一下就是一个关于 K 的一元二次方程:

text 复制代码
K² × r + K × q − q = 0

解出来:

text 复制代码
K = ( −q + √(q² + 4rq) ) ÷ (2r)

代进上一回用的那组参数 q = 0.001、r = 0.543:

text 复制代码
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:

c 复制代码
/* 数据不可信的时候把 r 调大,滤波器就更信自己的历史 */
void Kalman1D_SetR(Kalman1D *kf, float r)
{
    if (r <= 0.0f)
    {
        r = 0.001f;                 /* 参数边界:r 不能是 0 或负数 */
    }

    kf->r = r;
}

这样同一个滤波器,在数据好的时候跟得紧,在数据差的时候稳得住------这是固定系数的低通做不到的,它只有一个档。

理由二:多一个状态,能估一个没有测到的量

这是它真正值钱的地方。

一维卡尔曼的状态就一个:"我要平滑的那个量"。它能做的只有平滑。但很多问题的关键恰恰是有一个量你根本测不到,却能从别的量里推出来。

把陀螺仪和加速度计放在一起看就是这种问题。陀螺仪测的是角速度,要变成角度得积分;而它带着零偏,积分一小时能偏出去一大圈------讲零偏那一回已经算过一遍。加速度计能给出"哪边是下",能定出俯仰和横滚,但它抖,而且一提速就把运动当成重力。

关键在于:陀螺仪的零偏是可以估的。 它不会自己跳出来说话,但它的痕迹在数据里------角度积分起来总是往一个方向偏,这个"偏"的速度就是零偏。

所以把状态从一个变成两个:

text 复制代码
状态 1:角度     ------ 下一拍的角度 = 这一拍角度 + (角速度 − 零偏) × dt
状态 2:零偏     ------ 下一拍的零偏 = 这一拍的零偏(它变得很慢,就当成不变)
观测:加速度算出来的那个角度

两个状态、一个观测,卡尔曼照样能跑:它一边积分角速度,一边看着"积分出来的角度"和"加速度说出来的角度"差多少,把这个差按比例分成两份------一份用来修角度,一份用来修零偏。

c 复制代码
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 秒之后:

text 复制代码
只用陀螺仪积分      角度 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:

text 复制代码
r = q × (1 − K) ÷ K²

q 保持 0.001,想要稳态系数 0.1:

text 复制代码
r = 0.001 × 0.9 ÷ 0.01 = 0.09

代回去验证,稳态增益正好是 0.1。这条式子的意义是:调参不再是试数字,而是先回答"我允许它慢多少"。 固定的低通里,系数 0.1 大约 22 拍到 90%、0.042 大约 54 拍;卡尔曼跑起来要 56 拍------它按冷启动时的等效系数算,比同系数的低通稍慢一点。这些数都能算出来,也能在屏幕上一拍拍数出来。

代码里给两个小函数,正反两个方向都能算:

c 复制代码
#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 是测量噪声的方差。它有经验成分,但起点可以量:

text 复制代码
让被测对象保持不动,连续采 100 拍原始数据
算出这批数据的方差(或者干脆取"最大值减最小值"的一半再平方)
把这个数量级当成 r 的起点

比如静止时那个加速度轴在 ±0.02 g 之间晃,方差的量级就是 10⁻⁴ 那一档,r 就从这附近开始试。先量噪声,再定 R,比凭感觉填一个 0.5 强得多。

四、三组参数对照,以及把参数改坏的边界

拿同一组数据(加速度那一路),依次跑三组:

c 复制代码
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(屏幕上显示成一串没意义的数,比较还会全部为假)。

工程版的做法是在初始化时把参数卡到正数,更新之后检查一次结果:

c 复制代码
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 附近。

c 复制代码
/* 每拍调 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 变成负数------方差为负,物理上不可能------增益的符号就翻了,角度会朝相反方向修,而且越修越离谱。

判据不长:

c 复制代码
if ((kf->p00 < 0.0f) || (fabsf(kf->p01 - kf->p10) > 1e-6f))
{
    /* 记一次:协方差已经不对称,或者已经不合法了 */
}

便宜的一条修法是每拍结束把它们拉回平均:

c 复制代码
kf->p01 = 0.5f * (kf->p01 + kf->p10);
kf->p10 = kf->p01;

稳的一条是换一种更新写法,叫 Joseph 形式。一维的时候右边只剩两项:

text 复制代码
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。

c 复制代码
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>)。

c 复制代码
/* ===== 配置:这几个数是这台车跑出来的,换车要重测 ===== */
#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 毫秒的采样节拍里:

c 复制代码
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 从原始数据的方差起,周期一改必须重算,最后拿新息的均值验一遍。老边界还在:方向接反、丢脉冲、机械松动,它都看不见。

最后蜘蛛说

看不懂?没事,会用就行,看懂了也照样试着调参,仿真完美不等于工程完美

相关推荐
希和20274 小时前
代码讲解:STM32F103C8T6 驱动 HC-SR04 超声波测距
stm32·单片机·嵌入式硬件
编程小白菜1235 小时前
STM32L0 ADC串扰揭秘:充电电压如何“伪造”压力数据
stm32·单片机·嵌入式硬件
Zw-awa16 小时前
两路信号,两种滤波:角速度走低通滤波,加速度走卡尔曼滤波
stm32
橘为己有44119 小时前
STM32:USART 串口(2)
stm32·单片机·嵌入式硬件
Zw-awa2 天前
滤波是什么:限幅、滑动平均、中位值、一阶低通怎么选?嵌入式传感器滤波算法原理与C源码
stm32
Zw-awa3 天前
陀螺仪数据为什么看起来总在动:零偏、噪声和零漂
stm32
Zw-awa4 天前
什么是IMU:两颗传感器、一件别人替代不了的事,MPU6050、IMU660RA、JY901S等模块有何区别
stm32
Zw-awa5 天前
MPU6050:从原始加速度到姿态变化
stm32
少司府5 天前
嵌入式入门篇(二):初识STM32F103ZET6
linux·python·stm32·单片机·嵌入式硬件·mcu·嵌入式软件