扩展卡尔曼滤波 (EKF)

0.0 铺垫知识：从小尺子到宇宙的距离 (勾股定理)

在列出 GPS 方程之前，先搞懂数学里最基础的“算距离”，直接看图：

• 一维（直线）：想象一把直尺，点 A 和点 B 的距离就是坐标相减。为了不分大小（消掉负号），直接平方： 距离² = (x₂ - x₁)²
• 二维（桌面）：在桌面上找两点的距离，就是画个直角三角形。根据勾股定理，斜边² = 两条直角边²的和： 距离² = (x₂ - x₁)² + (y₂ - y₁)²
• 三维（空间）：在天上飞的三维宇宙里，加上高度 Z 轴，其实就是把长方体的对角线连起来。你可以看到，底面的“二维平面距离”加上 Z 轴差，又形成了一个新的直角三角形！规律依然极其简单粗暴： 距离² = (二维底面斜边)² + (Z 差)² = (x₂ - x₁)² + (y₂ - y₁)² + (z₂ - z₁)²

0.1 前置知识：宇宙级尺规作图 (GPS定位原理)

你有没有想过，GPS 是怎么知道你在哪的？本质上，这就是初中学的“圆规画圆求交点”。

卫星发送信号到你手机，光速 c 乘以时间差 (t_i - t)，就是距离（半径）。通过 4 颗卫星刚好能列出 4 元方程组，求解你接收机的三维坐标 (x, y, z) 和接收机钟差 t：

• 👉 t₁, t₂, t₃, t₄：卫星发射信号的时间。由于天上的卫星搭载了极其精准的原子钟，这四个时间是绝对同步且准确的。
• 👉 t：你的手机/接收机的未知时间。因为接收机只有普通的石英钟，时间并不准。所以我们需要第 4 颗卫星，把这个未知的 t（也就是接收机钟差）连同你的三维坐标 (x, y, z) 一起求解出来！

注：更高阶的厘米级定位技术 RTK（载波相位差分）将在独立章节展开，这里我们只把 GPS 当作一个会给你返回带噪声 $(x, y)$ 坐标的黑盒传感器。

0.2 前置知识：切割时间的魔法 (Odom 积分)

没有 GPS 的时候怎么走？靠轮速计 (测线速度 v) 和 IMU (测横摆角速度 ω)，把每一瞬间的微小移动加起来，这叫里程计 (Odometry)。

假设我们把时间切成无数个极短的片段 dt（比如 0.1 秒）。把这些微小的坐标变化，沿着轨迹一步一步累加起来（其实就是微积分的黎曼和），这就是你的航位推算轨迹。

1. 误差与方差：先量出“有多不准”

在 EKF 中，我们不再相信绝对精确的数字。所有的传感器都有噪声，我们需要用统计学来描述它们。

什么是误差 (Error) 与 方差 (Variance)？

假设轮速计的真实速度是 10 m/s。我们连续测了 3 次，得到了三个有偏差的读数。这中间的差值就是误差 (Error)：

• 测量 1: 10.2 ➜ 误差 = +0.2
• 测量 2: 9.9 ➜ 误差 = -0.1
• 测量 3: 9.9 ➜ 误差 = -0.1

注意看！如果我们直接算平均误差：(+0.2 - 0.1 - 0.1) / 3 = 0！正负完全抵消了，这会给我们一种“传感器极其完美”的致命错觉（实际上数据一直在乱跳）。

为了防止正负号互相抵消，数学家想出了绝妙的办法：把每次的误差都平方（负数平方也变正），加起来，再除以测量次数。这就是方差 (Variance, 记为 σ²) 的严格数学定义！

在我们的例子里：σ² = (0.2² + (-0.1)² + (-0.1)²) / 3 = (0.04 + 0.01 + 0.01) / 3 = 0.02。真正的波动被精确地计算出来了！

3 次看不出名堂，那就测 10000 次

把车架空、轮子空转，真实速度死死锁在 10 m/s。让电脑连着轮速计一直读，每读一次，就在数轴上点一个点——点会往哪儿堆？

没有人安排过这个形状。我们只是让它一直读，点自己堆成了这样：中间最密，往两边越来越稀，左右还基本对称。

跑到最后看那个 σ²——它稳稳停在 0.02 附近，正是刚才 3 次测量手算出来的那个数。这条堆出来的曲线有名字：正态分布，俗称钟形曲线。

而方差 σ²，就是这口钟的胖瘦——除了胖瘦，它没有别的花样可玩：

神奇的地方：换个东西量，还是这口钟

量什么	背后由什么一点一点凑出来	形状
轮速计的一次读数	齿轮间隙 + 路面颗粒 + 轮胎形变 + 计时抖动 + ……	钟形
IMU 的一次角度	温度漂移 + 电路噪声 + 机械振动 + ……	钟形
一个班同学的身高	几百个基因 + 营养 + 睡眠 + 运动 + ……	钟形
一炉螺丝的直径	刀具磨损 + 材料软硬 + 机床振动 + ……	钟形

看第二列——它们的共同点是：结果都由一大堆微小原因「加」起来。这就是钟形的来路。

为什么？数一数就明白了

先把原因砍到只剩 5 个，每个各让读数偏 +0.02 或 −0.02。一共 2×2×2×2×2 = 32 种凑法，一种一种数过去：

只有 5 个原因，就已经堆出钟的雏形了。真实的轮速计有几百上千个原因，柱子密到看不见缝，就成了本节开头那条光滑曲线。

身高也一样：想长到 2 米，几百个基因得全部往高里投票——路太少了，所以极高极矮的人罕见，中间的人挤成一堆。

这条定律，就是后面所有推导的地基：形状已经被大自然定死了，描述一个不确定的量，我们只剩两件事要交代——中心在哪，和钟有多胖（方差）。

EKF 从头到尾搬来搬去的，就只有这两样。等状态变成 x、y、θ 三个数，「中心」变成三个数，「胖瘦」就得升级成一张表——那就是后面要请出来的 P 矩阵。

2. 闭眼走三步：误差怎么滚雪球

走一步，只发生两件事：老误差被搬运——车头歪着，θ 的误差会漏进 x、y；这一步的读数又各错一份——轮速计的距离、IMU 的角度。

先写出“误差怎么搬运”

让小车朝正东（θ = 0）每步走 1 米。现在 IMU 错了一点点，车头偏了 δθ——直接看它走到哪：

两笔都记下来了。但它俩根本不是一个量级——代入真数字看一眼：

车头偏了 δθ	y 多走 sin(δθ)	x 少走 1−cos(δθ)	差多少倍
0.1 弧度（5.7°）	0.0998 米 ≈ 10 厘米	0.0050 米 = 半厘米	20 倍
0.01 弧度（0.57°）	0.0100 米 = 1 厘米	0.00005 米 = 0.05 毫米	200 倍

看最后一列：δθ 小 10 倍，横向那份跟着小 10 倍，沿路那份却小了 100 倍。因为 sin(δθ) ≈ δθ，而 1−cos(δθ) ≈ δθ²/2 —— 一个是误差本身，一个是误差的平方。0.05 毫米的账我们不记了。

三行搬运规则就这么读出来了（δ 读作“误差”）：

误差只有两个来源——轮速计量的“走了多远”，IMU 量的“转了多少度”。看图：

传感器一直不准——每读一次就错一份，这次错多少跟上次没关系，所以每走一步都得往账上再记一笔 Q。起点上账目全是 0。开算：

第 1 步：老账全是 0，没什么可搬，轮速计和 IMU 这一次读数各错了一份：

• Var(x) = 0.01 (轮速计)　Var(y) = 0　Var(θ) = 0.01 (IMU)
• ← 横向是 0：没有传感器量它，θ 也才刚出错，还没来得及把 y 带偏。

第 2 步：老账不为 0 了，θ 的误差开始往 y 里漏。一个一个算。

x 和 θ 最省事，各自再记一份这次读数的误差：Var(x) = 0.01 + 0.01 = 0.02（轮速计），Var(θ) = 0.02（IMU）。

y 麻烦，因为 新 δy = δy + δθ，两个误差加在了一起。方差是“误差平方的平均”，那就把它平方——用初中的完全平方公式：

头尾两项都认识。但中间那个「平均(δy·δθ)」是全新的——它不是任何一个量的方差。它是什么？

这个新冒出来的东西，就是“协方差”

平均(δy·δθ) 不是谁的方差——它量的是两个误差会不会一起动。y 和 θ 为什么会一起动？因为它们本来就是连体的。看图：

假设车头本该朝正东（理想轨迹），结果传感器有一点点误差，车头偏北了。当车子往前开时，你看上面的图：

X 轴方向没走到位（产生了负误差），而 Y 轴方向凭空多走了一段（产生了正误差）！

也就是说，只要 θ 错了，X 和 Y 的误差就产生了联动关系。数学家如何计算这种联动？很简单，把每次的 X 误差和 Y 误差相乘，然后求平均：

在图里的这一步：X 是负误差，Y 是正误差。负数乘正数等于负数。这就意味着，算出来的协方差是一个负值。它在数学上精确地表达了：“只要 X 偏小，Y 就必定偏大”这种死死绑定的物理现象！

图解黄色的“协方差椭圆”：为什么误差散点有的是正圆，有的是椭圆？我们直接用两组硬核数据算一下：

• 场景 1：X 和 Y 误差互相独立 (散点形成正圆)
  测了 4 次。X 误差是 [+1, -1, -1, +1]，Y 误差是 [+1, +1, -1, -1]。
  两者各自瞎跳，毫无规律。我们代入公式算一下协方差：
  Cov = ( (1×1) + (-1×1) + (-1×-1) + (1×-1) ) / 4 = (1 - 1 + 1 - 1) / 4 = 0
  👉 物理意义：协方差 = 0，说明毫无联动关系。散点在各个方向均匀分布，误差边界就是一个正圆。
• 场景 2：X 和 Y 产生强联动 (散点形成倾斜椭圆)
  回到我们上面的图。因为车头偏向，导致只要 X 没走到位 (负)，Y 就一定凭空多走 (正)。
  X 误差是 [-1, -2, 1, 2]，Y 误差是 [2, 4, -2, -4]（严格负相关）。
  Cov = ( (-1×2) + (-2×4) + (1×-2) + (2×-4) ) / 4 = (-2 - 8 - 2 - 8) / 4 = -5
  👉 物理意义：协方差 = -5。这个巨大的负数，意味着散点绝大部分都聚集在第二和第四象限（左上和右下）。所以在二维图里，原本的正圆被死死拉扯成了一个倾斜的椭圆（图中黄色的形状）。

回到第 2 步。此刻 y 和 θ 之间还没建立任何关系，所以 Cov(y,θ) = 0，代进去：

但这一步走完，情况就变了——新 δy 里塞进了一份 δθ，它俩从此绑上了：

← 协方差从 0 凭空长了出来。记住这个数，下一步它就要发威。

第 3 步：这下那个 2 × Cov(y,θ) 真的咬人了——因为 Cov(y,θ) 不再是 0：

牵连也继续变粗：Cov(y,θ) = 0.01 + 0.02 = 0.03。而老实的 x 还是慢吞吞：Var(x) = 0.03。

把三步的误差，画到轨迹上

方差的单位是“米²”，画不到地上。开回根号才是米（第 1 节：方差是“平均错多少”的平方）——开出来的这个数，就是误差棒的长度。三步的方差，逐个开根号：

（0.14 × 0.14 = 0.0196 ≈ 0.02、0.17 × 0.17 = 0.0289 ≈ 0.03、0.22 × 0.22 = 0.0484 ≈ 0.05——反乘回去就对上了。）

把这几个长度，直接画到轨迹上：

（绿棒 = 沿路误差 ±√Var(x)，橙棒 = 横向误差 ±√Var(y)，都贴着车头方向画；椭圆与误差棒严格按手算数字绘制）

对着图看这三行，一切都对上了：

• 👉 沿路方向（x）：方差 0.01→0.02→0.03，半径 0.10→0.14→0.17。老老实实线性长——它只吃轮速计那一份误差，跟 θ 无关。
• 👉 横向（y）：方差 0→0.01→0.05，半径 0→0.10→0.22。从零开始，越长越快——它没有自己的传感器，每一分不确定都是 IMU yaw 漏过来的。
• 👉 y–θ 牵连：0→0.01→0.03，凭空长出来、越绑越死。它不直接画在图上，却是横向爆炸的幕后推手：每一步 y 都要多吃一份 2 × Cov(y,θ)。

为什么图上的椭圆是斜的？因为误差棒贴着车头方向画——路一弯，车头在转，椭圆自然跟着转。换个坐标系记它，形状一点不变：

同一团误差，两种记法而已。

3. 6 个数装不下了：P 矩阵登场

清点：我们一路上其实在追 6 个数

回看刚才三步，每一步要更新的就是这些：

• 👉 三个“自己有多散”（方差）：Var(x)、Var(y)、Var(θ)
• 👉 三对“互相怎么绑”（协方差）：Cov(x,y)、Cov(x,θ)、Cov(y,θ)

一共 6 个数——缺一不可，也就够了。可要是把它们排成一行清单，马上会撞上一个麻烦。

停一下：那个碍眼的“2×”，把方阵逼出来了

手算是能算，但你有没有觉得哪里别扭？盯住这一项：

这个 2 是哪来的？回头看完全平方：中间项是 δy·δθ 和 δθ·δy——同一个数，占了两个位置，所以要数两遍。

麻烦在这儿：6 个数排成一行清单，Cov(y,θ) 只占一个格子，你就得手动记住“用到它要乘 2”。状态一多，这种特例必写错。

6 个数，摊进 3×3 = 9 个格子，对角线放方差，其余对称地放牵连。这才是 EKF 里那个 P 矩阵——它多出来的 3 个格子不是浪费，是拿冗余换掉了特例：

对称不是为了好看：“y 牵连 θ”和“θ 牵连 y”本就是同一个数，它必须被数两次，所以必须被摆两次。方阵是这个需求的结果。

归纳：这 6 条规则，其实是同一条

现在把刚才手算时用到的规则，一条条摆出来——每个格子一条：

六条长得完全不一样，有的带 2×，有的干脆只有一项。但只要看它们是怎么来的，就会发现全是同一个动作：

不信随手验两条（配方就是那三行搬运规则）：

和表里那两条一字不差。也就是说：6 条规则不是 6 个知识点，是同一个动作跑了 6 遍。

而「新 δi 的配方」是什么？就是搬运规则的第 i 行。

插一句：这三行系数，凭什么叫「雅可比」

回想这三个系数是怎么问出来的——「θ 错一点点，输出跟着错多少」。这就是斜率。

为什么非得用斜率？因为运动方程里坐着 sin、cos，是弯的；弯的东西没有固定的换算倍数。但只要误差很小，就能拿当前这一点的切线斜率当换算比例：

表里那个 1 和 0，就是这么来的：

把所有「哪个输入 → 哪个输出」的斜率排成一张方表，数学上就叫雅可比矩阵——说白了就是一整张误差换算率表。它随当下的 θ、d 变，所以每走一步都要重算一次。

这三行系数摊成方阵，就是我们的 F：

把这个动作对 9 个格子统一写出来，紧凑记法正是 F P Fᵀ；再加上这一步的 Q——6 条规则合订成了一行：

为什么左边乘 F，右边还要再乘一个 Fᵀ？

因为 P 的每一格，装的都是两个误差的乘积——比如 Cov(y,θ) = 平均( δy × δθ )。而搬运规则一次只能搬一个误差。两个误差，就得搬两次：

对上号了：这一格用的正是 F 第 2 行（管新δy）和 F 第 3 行（管新δθ），展开就是我们手算过的 Cov(y,θ) + Var(θ)。

那右边为什么写成 Fᵀ？因为矩阵乘法只认「行 × 列」这一种吃法：

动手：完全按矩阵规则，把第 3 步再算一遍

矩阵乘法只有一条规则：结果的第 i 行第 j 列 ＝ 左边第 i 行 × 右边第 j 列（对应位置相乘，再全加起来）。材料就三样：

第一拳：F × P　（搬第一个误差）

F 的第 2 行是 0 1 1，读作「新 δy ＝ 老 δy ＋ 老 δθ」。按规则，结果的第 2 行就是：

第 1 行的 1 0 0 和第 3 行的 0 0 1 更省事——原样抄 P 的对应行。三行拼起来：

第二拳：× Fᵀ　（第二个误差也得搬）

Fᵀ 的第 2 列，就是 F 的第 2 行立起来：(0, 1, 1)。算第 2 行第 2 列——也就是 Var(新y) 那一格：

九个格子照这么扫一遍：

第三拳：+ Q　（这一步新读的两份误差）

对一下答案

格子	第 2 节手算出来的	矩阵刚才算出来的
Var(x)	0.03	0.03	✓
Var(y)	0.05	0.05	✓
Var(θ)	0.03	0.03	✓
Cov(y,θ)	0.03	0.03	✓

这就是矩阵的魅力

	手算那 6 条规则	这一行公式
要写几条	6 条，条条长得不一样	1 行
那个 2×	得你自己记住	自动
状态从 3 个变 10 个	重推 55 条规则	一个字都不用改
换成无人机、机械臂	全部推倒重来	只换 F 和 Q 里的数

矩阵没有替你多算一分钱的账——它只是把「你必须记住的特例」变成了「格子自己会做的事」。

4. 融合的全部秘密：加权平均与增益 K

P 已经把「我有多没底」算清楚了。现在 GPS 来了，它也带着自己的方差。该听谁的？听几成？先把 x 一个数拎出来练透——后面只是把它武装到多维。

两个说法，一个位置

让车沿直线开一会儿，只看东西方向那个 x。同一时刻，手里有两个说法：

笨办法是取中间值 12。但 GPS 此刻明明更有底，答案理应偏向 GPS。偏多少？我们不猜——把所有掺法都试一遍，看哪种掺出来的方差最小。方差最小 = 最可信，这是第 1 节立下的规矩。

试出来的最优配方

设掺法为：听推算 w 成，听 GPS 1−w 成。合成 = w × 10 + (1−w) × 14。它的方差怎么算？两条规则，都来自“方差是误差的平方”：

规则一 · 缩放	误差乘以 w，方差乘以 w²　——错缩到一半，平方缩到四分之一
规则二 · 相加	两个互不相干的误差叠加，方差直接相加　——推算错和 GPS 错有时同向有时反向，会部分抵消，所以不是 σ 相加

纯加减乘除，把所有掺法列出来：

验算 w = 0.2 那一格：0.2²×4 + 0.8²×1 = 0.16 + 0.64 = 0.8 ✓

把每种掺法的方差全画出来，是一条碗形抛物线。所谓“最优估计”没有任何玄学——就是碗底。这张小表里藏着三个发现：

① 碗底在 w=0.2	听推算 2 成、听 GPS 8 成。不是全信 GPS——w=0 那格是 1.00，掺一点推算反而更好。
② 0.80 < 1.00	合成结果比最好的那位证人还准。融合永远不亏——这就是传感器融合存在的全部意义。
③ 0.2 = 1 ÷ (4+1)	最优权重根本不用逐个试，有公式：各听多少，跟自己的方差成反比。方差是你的“罪证面积”，面积越大，话语权越小。

换一种写法，K 就诞生了

合成 = 0.2×10 + 0.8×14 = 13.2。同一个式子，可以改写成「以推算为基准，朝 GPS 挪」：

这个 K，就是卡尔曼增益。它的读法非常口语：「朝 GPS 挪几成」。分子是我自己的不确定——我越没底，挪得越狠：

推算的方差	K	行为
很大（心里没底）	→ 1	大步挪向 GPS
很小（很有底）	→ 0	GPS 只配微调
两边一样烂	= 0.5	各听一半

融合之后的新方差，也有一步到位的算法（结果和碗底 0.8 一致）：

每个说法画成一座可能性山丘：山顶 = 它报的数，山越宽 = 越没底。蓝色是融合结果——它永远站在两山之间、偏向瘦的那座，而且比两座山都瘦。把推算调烂，看 K 冲向 1；两边调成一样，K 停在 0.5。

较真卡：碗底为什么正好落在 K？——两座山相乘

K 不是枚举出来的巧合，是两座山丘相乘的必然。“同时满足两个说法”（既在推算附近、又在 GPS 附近），数学上就是两座钟形相乘——只有两处都高的地方才留得下。

钟形山丘的公式是 exp(−偏差² / 2σ²)：离峰越远越矮，而且按平方矮得飞快。两座相乘，指数相加：

− (x−10)² / (2×4) 　−　 (x−14)² / (2×1)

两条开口向下的抛物线相加，还是一条抛物线 → 乘积还是一座钟形。

这就是卡尔曼能一直跑下去的根：高斯 × 高斯 = 高斯。形状闭合，所以每一步只需拎着「峰、宽」两个数往下递推，永远不用记整条曲线。也正因如此，卡尔曼要求噪声是高斯的——换个形状，相乘就不闭合了。

新山峰落在哪？两条抛物线相加，顶点 = 按陡峭度加权的平均，陡峭度 = 1/方差（越尖越确定）：

新峰 = ( 10×(1/4) + 14×(1/1) ) ÷ ( 1/4 + 1/1 ) = 16.5 ÷ 1.25 = 13.2　← 和正文 K 加权一字不差
新宽 = 1/σ′² = 1/4 + 1/1 = 1.25 　→　 σ′² = 0.8　← 正是 (1−K)×4

“按 1/方差加权”翻回人话，正是碗底那一格。碗底不是拟合出来的——它是两个高斯信念相乘的顶点，一分不多一分不少。

这就是卡尔曼滤波

给两个说法起正式的名字：

从一个数到三个数：那 5 行是怎么长出来的

先看清楚 GPS 到底给了什么——2 个坐标，外加这 2 个数各自的方差；关于 θ，一个字都没有。而我们的状态有 3 个数。两边对不上，就是全部麻烦的根：

① R 怎么量：和量轮速计一个办法

第 1 节量轮速计是把车架空连读 10000 次。GPS 照抄：把车停在一个已知点上别动，连报 3000 次，看点往哪儿散。

边上那两条还是第 1 节那口钟。拿第 2 节的老规矩量这团点（误差平方的平均 = 方差，两个误差乘积的平均 = 协方差）：Var(x) ≈ 0.05、Var(y) ≈ 0.05、Cov(x,y) ≈ 0——R 就量出来了：

到这儿两边的方差都是量出来的：Q（轮速+IMU）第 1 节量的，R（GPS）刚量的。EKF 不靠调参，靠标定。另外注意——这团点里没有 θ 这一维，你根本量不出它，因为 GPS 没报。

② 先不用矩阵：一个数一个数地融合

材料：预测 [3.00, 0.20, 0.05]，P 用走完第 3 步那本账 [[0.03,0,0],[0,0.05,0.03],[0,0.03,0.03]]，GPS 报 [3.20, 0.10]。上一节那条一维公式够用了：

③ 摞起来：K 为什么必须是 3 行 2 列

先想清楚 K 的活儿——它把「GPS 的分歧」变成「状态的修正」：修正 = K × 分歧。两头各有几个数，现实已经钉死了：

吃进去几个数	GPS 一次报几个数，就有几个分歧 → 2　⇒　K 有 2 列
吐出来几个数	状态有几项，就得修几项 → 3　⇒　K 有 3 行

矩阵乘法的规矩正是这样数的：行数 = 吐出来几个数，列数 = 吃进去几个数。所以 K 只能是 3 行 2 列——多一格少一格都乘不通。摆出来长这样：

3×2 ＝ 6 个配对，每一对都得单独回答一次「摊多少」，一个都不能省。比如第 3 行第 2 列的 0.3：南北向那个分歧，要往 θ 上摊 0.3 份——GPS 明明没测 θ，θ 就是靠这一格被修的。

形状随两头变，规矩不变：

如果……	K 变成	为什么
GPS 换成也报朝向的组合导航（报 3 个数）	3 行 3 列	吃进去变 3 个
状态里再塞进速度（变成 4 项）	4 行 2 列	吐出来变 4 个
只收 GPS 的东西向坐标（报 1 个数）	3 行 1 列	吃进去只剩 1 个

三格的算法归成一句：K 的第 i 行第 j 列 ＝「状态第 i 项 和 GPS 第 j 个读数的牵连」÷「GPS 第 j 个读数那条线上的总方差」。

照这句话去 P 里取东西，分子要「每一项 与 x、y 的牵连」（挑列，得 3×2），分母要「x、y 自己那块 + R」（挑行又挑列，得 2×2）。两处是同一个动作：只留 GPS 看得见的那几项，θ 那行那列丢掉。

④ 把这个动作写成一张表，就是 H

H 回答的就一句话：如果车真在这个状态，GPS 应该报什么？

每格填什么	状态里这一项变 1，GPS 该报的数跟着变多少（这就是斜率）
我们这题	x 变 1 米 → 读数变 1；y 变 → 读数x 变 0；θ 转 → 读数变 0（原地转头，位置没动）　⇒　H = [[1,0,0],[0,1,0]]
用在哪	H x 翻译位置 ｜ H P Hᵀ 翻译不确定（才能和 R 比） ｜ P Hᵀ 反着翻，把分歧摊回状态
为什么它不随时间变	「读数 = x」是直线，斜率处处相同 → 常数。F 管的 cos(θ) 是弯的，斜率随 θ 变 → 每步重算。这就是 EKF 那个 E：弯的地方拿切线凑；我们这题只有 F 需要。

验一下：P Hᵀ = P 的前两列 = [[0.03,0],[0,0.05],[0,0.03]] ✓ 正是分子（θ 那行的 0.03 就是它搭顺风车的通道）；H P Hᵀ + R = [[0.03,0],[0,0.05]] + R = [[0.08,0],[0,0.10]] ✓ 正是分母。

⑤ 回头看：一维那条 K = P/(P+R)，其实是个简写

我们一直把它读成「我的方差 ÷ 总方差」。这个读法在一维碰巧成立，一到多维就接不上。它真正的模板是：

一维时被测量就是我自己，两处于是双双塌成 P——这才是它长成 P/(P+R) 的原因：

	一维	多维
被测量是什么	就是我自己 x	GPS 报的那 2 个数
分子 = 牵连	Cov(x,x) = P	P Hᵀ　(3×2)
分母 = 被测量的总方差	P + R	H P Hᵀ + R　(2×2)

那为什么分子只挂一个 Hᵀ，分母左右各挂一个 H？用第 3 节推 F P Fᵀ 时那条老规矩：一格里装着两个误差，哪个误差要翻译到 GPS 那边，就在哪边乘一次。

要算的	这一格里的两个误差	谁要翻译	写出来
分母（读数自己的方差）	读数误差 × 读数误差	两个都要	H P Hᵀ
分子（状态与读数的牵连）	状态误差 × 读数误差	只有右边那个（左边本来就是状态）	P Hᵀ

拿我们的数字对一遍，一维的味道立刻回来了：

⑥ 除法改成求逆，K 就拼出来了

分母成了 2×2 的表，没法直接除。一维时 a ÷ b = a × (1/b)，而 1/b 的定义是「乘上去等于 1」；矩阵版照抄：乘上去等于单位阵 I 的那个矩阵，记作 S⁻¹。对角阵各自取倒数：S⁻¹ = [[12.5,0],[0,10]]（验：0.08×12.5=1 ✓）。

顺手验一下尺寸，正好落在 ③ 数出来的 3×2 上：P Hᵀ = (3×3)(3×2) = 3×2；H P Hᵀ + R = (2×3)(3×3)(3×2) + (2×2) = 2×2，求逆还是 2×2；两者相乘 (3×2)(2×2) = 3×2 ✓

⑦ 挪过去，再把 P 缩掉

分歧写成 ν = z − H x（ν 是希腊字母 nu，读“纽”，就是“分歧”的代号——不是速度 v，单位是米），修正就是 x ← x + K ν；方差收缩把一维的 (1−K) 换成 (I − K H)（K 是 3×2、H 是 2×3，乘出来正好 3×3，和 I 同尺寸）：

	修正前	修正后	变化
x	3.00	3.075	Var 0.03 → 0.01875（−37.5%）
y	0.20	0.15	Var 0.05 → 0.025（−50%）
θ	0.05	0.02	Var 0.03 → 0.021（−30%）

一维 → 多维，逐字对照

一维	多维	改了什么
z − x	z − H x	先翻译成 GPS 该报什么
P + R	H P Hᵀ + R	P 也得翻译过去，才能和 R 相加
P ÷ (P+R)	P Hᵀ (H P Hᵀ + R)⁻¹	除法→求逆；Hᵀ 把分歧摊回状态
(1 − K) P	(I − K H) P	1 换成 I，K 换成 K H

没有一处是新知识——全是「一个数一个数地融合」，加上「只留 GPS 看得见的那几项」。课本上那 5 行，就是这么长出来的：

• 【预测 Predict】 闭眼推算
• 1. x_new = f(x, u) // 坐标累加
• 2. P_new = F P Fᵀ + Q // 老账被搬运放大，再加这一步新读的误差
• 【更新 Update】 睁眼修正
• 3. K = P Hᵀ (H P Hᵀ + R)⁻¹ // 就是 P/(P+R)，只是多维要求逆
• 4. x = x_new + K (z − H x_new) // 朝 GPS 挪 K 成
• 5. P = (I − K H) P_new // 就是 (1−K)×P，方差缩水

字母对照表：谁是谁，从哪来

GPS 的方差就是 R——它在这 5 行里只出现一次，就在第 3 行括号里的那个 R。全部字母摆一遍：

字母	是什么	哪来的	几行几列
x	状态：车在哪、车头朝哪　（速度不在里面）	轮速+IMU 一步步累加	3 个数
P	我自己有多没底（含牵连）	第 2、3 节一步步滚出来	3×3
Q	轮速计 + IMU 这一步各错的那份	用第 1 节那个办法，架空轮子测出来	3×3
F	状态 → 下一步状态 的斜率表	运动方程求斜率，每步重算	3×3
z	GPS 这次报的坐标	GPS 直接给	2 个数
R	GPS 自己的方差（它有多没底）	停车不动连报几千次量出来	2×2
H	状态 → GPS 读数 的斜率表	观测方程求斜率，GPS 是直的 → 常数	2×3
ν	GPS 和我的分歧	现算：z − H x	2 个数
K	朝 GPS 挪几成	现算，由 P 和 R 定	3×2

表里没有速度——轮速计读的 v 和 IMU 读的 w 是每步现读现用的输入，用完就扔，不占 P 的格子，也不参与 K。

整张表里只有两样东西是你事先要交代给滤波器的：Q（轮速+IMU 有多烂）和 R（GPS 有多烂）。而这两个都不是拍脑袋——都是拿传感器测出来的。剩下的字母，全是算出来的。