卡尔曼:猜一步,量一下

卡尔曼:猜一步,量一下

第 1 步看到了:相邻两帧直接算速度,一帧量偏 0.25 米,速度就跳到 −7.5。卡尔曼滤波每一帧做两件事:先按上一帧的速度猜前车现在在哪,再看雷达量到哪,两边按“谁更有把握”折中。

⓪ 真实场景:跟着前车,每 0.1 秒量一次

雷达这一帧量到的车尾(有 ±0.2 米的误差) 前车真实的车尾

我 20 m/s、前车 18 m/s,前车车尾从 20 米开始,每秒近 2 米。图是相对视角:把我的车固定在左边,看前车怎么一点点靠近。数字和 examples/a_stack/a6_diff_speed.py 是同一组。

① 一帧里的三步:就看 4.0 秒那一帧

4.0 秒这一帧雷达量偏了(量到 11.75 米,真实 12.00 米),第 1 步的 diff_speed 在这里算出 −7.5。下面是一条“离前车多远”的数轴,钟形曲线表示“大概在哪、有多没把握”:钟越窄,越有把握。

估计(上一帧 / 合出来的) 猜 量 真实 12.00 米(算法不知道)

d前车车尾离我多少米(要估计的第 1 个数)
v前车相对我的速度,m/s,负数 = 在靠近(要估计的第 2 个数,雷达量不到,只能推出来)
P有多没把握:钟的宽度。图上写的 ±0.12 是 √P(标准差),P 本身是它的平方
Q每猜一步多出来的没把握:0.1 秒里前车可能加速、减速,所以猜完钟会变宽
z这一帧雷达量到的距离
R雷达量距离的误差:0.2 米,R = 0.2² = 0.04
K这一帧信“量”多少(0~1)。猜的钟窄,K 就小,多信猜;量的钟窄,K 就大,多信量

② 七行代码,一行一行看:还是 4.0 秒那一帧

上面是 KalmanCV 里要写的 7 行,亮的那行就是这一步;下面用这一帧的真实数字把它算一遍。看懂一行再点“下一步”。