10b 卡尔曼滤波五矩阵数值举例——把 F / P / Q / R / H / K 跑一遍¶
本篇是 10 卡尔曼滤波基础 §八「五矩阵物理意义」的数值版：§八讲"每个矩阵是什么"，本篇用一个最小的一维例子，把五个矩阵代成具体的数字，一步步算给你看。建议先读 10 篇，再来看本篇。
本篇为 10 篇的外链扩展，不进主目录导航——想从体系主线走，请回 10 篇。
约定：本文用标准观测约定 \(H=[1,0]\)、新息 \(r = z_{\text{meas}} - H\hat x^-\)（观测 − 预测），因此增益 \(K\) 为正数。很多资料（包括 deepseek 的讲解）用 \(H=[-1,0]\)、\(r = \hat x^- - z_{\text{meas}}\)，此时 \(K\) 是负数——两者完全等价，只是新息定义方向相反，详见文末「关于 H 符号」一节。
0. 为什么值得专门跑一遍数值¶
读 10 篇你会记住"F 是误差怎么演化、Q 是每步多不可信、R 是量测多准、P 是账本、K 是投票权比"。但数字到底长什么样、它们之间怎么相乘，只有亲手算一遍才踏实。下面用最朴素的场景。
（顺带一句：惯导为什么不用"全量状态直接 EKF"而用误差态 ESKF——姿态不是向量、不能硬加，协方差量级悬殊会数值畸变，四元数还有单位范数约束——详见 11 EKF 与 ESKF。本文把"状态"当成一个普通的误差态来看，不影响下面的矩阵计算。）
1. 问题设定：一维匀速运动¶
物体沿直线匀速运动，状态取位置 \(p\) 与速度 \(v\)：
- 连续模型：\(\dot p = v\)，\(\dot v = w\)（\(w\) 是加速度白噪声，方差谱密度记为 \(q\)）。
- 因此连续系数矩阵——F / G 来自物理方程本身，不是积分算出来的（这一点见 02 微分方程与状态空间离散化 关于"F/G 从哪来"的岔路口说明）：
$\(F = \begin{bmatrix}0&1\\0&0\end{bmatrix},\qquad G = \begin{bmatrix}0\\1\end{bmatrix}\)$
- 离散化（\(\Delta t = 1\ \text{s}\)，匀速白加速模型有闭式解）：
$\(\Phi = e^{F\Delta t} = I + F\Delta t = \begin{bmatrix}1&1\\0&1\end{bmatrix},\qquad \Gamma = G\Delta t = \begin{bmatrix}0\\1\end{bmatrix}\)$
离散化这一步怎么从连续 \(F,G\) 得到 \(\Phi,\Gamma,Q_d\)（矩阵指数与积分的完整推导）：见 02 微分方程与状态空间离散化 §四「连续 → 离散：\(F_d\) 与 \(Q_d\)」；该页 §三 还讲了矩阵指数 \(e^{F\Delta t}\) 的多种算法与数值例子。本篇只负责"把结果代成数字"，推导请回那篇。
- 离散过程噪声（矩阵指数积分的闭式结果）：
$\(Q_d = \int_0^{\Delta t} e^{F(\Delta t-\tau)}\,G\,q\,G^\top\,e^{F^\top(\Delta t-\tau)}\,d\tau = q\begin{bmatrix}\Delta t^3/3 & \Delta t^2/2\\[4pt] \Delta t^2/2 & \Delta t\end{bmatrix}\)$
代入 \(\Delta t=1,\ q=0.1\)：
$\(Q_d = 0.1\begin{bmatrix}1/3 & 1/2\\ 1/2 & 1\end{bmatrix} = \begin{bmatrix}0.0333 & 0.05\\ 0.05 & 0.10\end{bmatrix}\)$
F 在这里的角色：\(\Phi = I + F\Delta t\) 的第 (1,2) 元 = 1，就是"位置随速度走一步"；F 其余为 0 表示"匀速假设下速度不被状态本身改变"。\(Q_d\) 的 (2,2) 元最大（0.10），因为加速度噪声直接注入速度。
2. 初值 \(P_0\) 与它的含义¶
对角元 = 各状态初值方差：位置标准差 \(\sqrt{100}=10\ \text{m}\)，速度标准差 \(\sqrt{1}=1\ \text{m/s}\)。非对角为 0，表示初始认为位置、速度不相关。
3. 预测步（第 1 步）¶
初始估计 \(\hat x_0 = [0, 0]^\top\)（我们猜物体在原点、静止）。先按模型推一步：
协方差预测：
注意 \(P^-\) 的 (1,1) 元从 100 涨到 101.03——预测让不确定性膨胀，多出来的 1.03 正是 \(Q_d\) 在位置方向的注入。这正是 10 篇 §8.3「时间更新 P 单调膨胀」的数值体现。
4. 量测步（第 1 步）¶
假设这一时刻 GNSS 给了一个位置观测 \(z_{\text{meas}} = 2\)（单位 m）。
- 观测矩阵（只观测位置）：\(H = [1, 0]\)。
- 量测噪声方差：\(R = 1\)（即 GNSS 位置标准差 1 m）。
新息（观测 − 预测）：
新息协方差：
增益（"信谁"的权重）：
状态修正：
协方差修正（Joseph 形式，此处 \((I-KH)P^-\) 与之等价）：
读这几个数字：
- \(K\) 的位置分量 \(0.99 \approx 1\)：因为预测位置方差（101）≫ 量测方差（1），滤波器几乎全信 GNSS → 修正后位置 1.98 紧贴观测 2。
- \(K\) 的速度分量 \(0.01 \approx 0\)：量测不含速度信息，速度几乎不被这次更新拉动（0 → 0.021）。但它已从"纯预测"的 0 变成 0.021——这是位置耦合 + 运动模型带来的"间接估计"（10 篇 §五观察②）。
- \(P\) 的位置对角从 101 骤降到 0.99：观测把位置不确定性狠狠压小。速度对角反而从 1 升到 1.089——位置被锁定后，速度的不确定性相对"显形"了（不确定度的代价转移），后续步会再降下来。
5. 再来一步，看趋势¶
第 2 步：再预测一次（\(\hat x^- = \Phi \hat x = [2.0010,\ 0.0206]^\top\)），再给一个观测 \(z_2 = 2.5\)。
位置方差轨迹：100 → 0.99 → 0.68，单调收缩并趋于与 \(R\) 平衡的稳态；速度方差 1 → 1.089 → 0.767，先略升后降。这就是 10 篇 §五「③ 协方差单调收缩」与「④ 误差落在 3σ 带内」在 2×2 上的具体模样。
想跑满 50 步看收敛曲线？把上面的递推写成循环即可——10 篇 §五的演示图（位置/速度/协方差/误差带四子图）正是这个一维模型跑了 50 步的结果。
6. 关于 H 符号（与 deepseek 讲解对照）¶
deepseek 的讲解用 \(H=[-1,0]\)、新息定义为 \(r = \hat x^- - z_{\text{meas}}\)（预测 − 观测）。代入本文数字会得到：
结果完全一样——只是 \(K\) 和 \(r\) 同时变号。所以两种约定等价，选哪种都行：
- 本文约定 \(H=[1,0]\)、\(r = z - H\hat x^-\)：增益为正，直觉上"新息越大、拉得越多"，更直白。
- deepseek 约定 \(H=[-1,0]\)：把"预测减观测"当成新息，增益为负，第一次见容易觉得"负号怪"。
工程代码里以你用的库为准（PSINS / PX4 / 本项目 ins_eskf_15d.c 各自有固定约定），关键是新息、H、K 三者的符号自洽。
7. 回头看五个矩阵¶
| 矩阵 | 本文里的具体数字 | 扮演的角色 |
|---|---|---|
| \(F\)（→\(\Phi\)） | \(\Phi=[[1,1],[0,1]]\) | "状态怎么往前走"：位置随速度走 |
| \(Q_d\) | \([[0.033,0.05],[0.05,0.10]]\) | "每步往前走时，模型多不可信"：加速度噪声注入 |
| \(P_0\) | \(\operatorname{diag}(100,1)\) | "一开始有多无知" |
| \(P^-\) | \([[101.03,1.05],[1.05,1.10]]\) | 预测后"中间账本"，比 \(P_0\) 膨胀 |
| \(H\) | \([1,0]\) | "量测能读到状态的哪一部分"（这里只读到位置） |
| \(R\) | \(1\) | "量测本身多准"（GNSS 位置 1 m 标准差） |
| \(S\) | \(102.03\) | 新息的不确定性 = 预测不确定 + 量测不确定 |
| \(K\) | \([[0.99],[0.01]]\) | "信谁"：位置几乎全信量测，速度几乎不信 |
| \(P\) | \([[0.99,0.01],[0.01,1.09]]\) | 更新后账本，比 \(P^-\) 收缩 |
回主线路：10 卡尔曼滤波基础 —— 五矩阵的物理意义、从传感器手册推 \(Q/R\)、与本项目 ESKF 15 态的关系。
系列首页：惯性导航与惯导解算 · 自学科普系列