第 1 步看到了:相邻两帧直接算速度,一帧量偏 0.25 米,速度就跳到 −7.5。卡尔曼滤波每一帧做两件事:先按上一帧的速度猜前车现在在哪,再看雷达量到哪,两边按“谁更有把握”折中。
我 20 m/s、前车 18 m/s,前车车尾从 20 米开始,每秒近 2 米。图是相对视角:把我的车固定在左边,看前车怎么一点点靠近。数字和 examples/a_stack/a6_diff_speed.py 是同一组。
4.0 秒这一帧雷达量偏了(量到 11.75 米,真实 12.00 米),第 1 步的 diff_speed 在这里算出 −7.5。下面是一条“离前车多远”的数轴,钟形曲线表示“大概在哪、有多没把握”:钟越窄,越有把握。
| 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 就大,多信量 |
上面是 KalmanCV 里要写的 7 行,亮的那行就是这一步;下面用这一帧的真实数字把它算一遍。看懂一行再点“下一步”。