EKF工程实践：雅可比矩阵、协方差传播与NIS一致性检验

简介：这份压缩包聚焦9维IMU姿态跟踪与多传感器融合，包含扩展卡尔曼滤波（EKF）以及EKF-IMU、ESKF-IMU两类算法的实现，并对应9维IMU集成处理器姿态跟踪中的双阶段卡尔曼滤波论文思路。内容面向机器人、导航定位、可穿戴设备与惯性导航方向的学习者，可用于对照论文理解状态建模、观测更新、姿态解算与误差状态滤波流程，并梳理滤波初始化、预测校正、协方差更新和姿态输出等关键环节。压缩包约2.23MB，文件总数与主要文件类型明细暂无数据，下载后可按目录结构查看代码组织。已有441人学习，说明该主题在惯性姿态估计场景中具有一定关注度。读者可重点比较EKF与ESKF两条IMU处理路径，理解双阶段滤波在陀螺仪、加速度计与磁力计融合中的差异，并据此搭建复现、验证和排错思路。

1. 线性假设失效后，EKF 补的是什么

扩展卡尔曼滤波（EKF）从根本上说并不直接解决非线性问题。它做的是在每个时间步把非线性函数在当前估计点附近展开为一阶近似直线，再套用线性卡尔曼滤波的闭式解。这个说法解释了为什么一个“近似”的滤波器能在组合导航、目标跟踪、位姿估计等场合稳定使用多年。真正决定它能否正常工作的，是雅可比矩阵在哪个点上求值、协方差被近似掉了多少、过程噪声矩阵 Q 到底在补偿哪部分偏差。搞懂这三件事，调试 EKF 就会从依赖经验变成可验证的工程。

2. 一阶泰勒展开：雅可比矩阵为什么是 EKF 的全部秘密

2.1 从非线性函数到局部线性模型

线性卡尔曼滤波的推导可以归结为两个假设：状态转移和观测都是线性的，所有噪声和状态都服从高斯分布。高斯分布经过线性变换后仍是高斯分布，因此整条推理链只需要均值和协方差两个量就能完整描述概率分布。滤波器的每一次预测和更新，本质上是在做高斯分布的加法、乘法和线性映射。

现实系统里这两条假设几乎都不成立。测距是平方根运算，方位角是反正切，姿态更新是旋转矩阵连乘，位姿复合是流形上的运算。这些函数一旦套进线性卡尔曼滤波的框架，高斯假设立刻失效：一个高斯分布经过非线性变换后，分布形状可能偏移，可能形成双峰，也可能在某个方向上被拉长成弯曲的形状。

EKF 的处理方式是：在每个时刻，围绕当前估计点 $\hat{x}$ 对非线性函数做一阶泰勒展开，并把二次及以上的项丢掉。

$$ f(x) \approx f(\hat{x}) + F,(x - \hat{x}), \qquad F = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}} $$

这个操作相当于用一个超平面去贴近一张曲面，贴近点就是当前估计。距离越远，误差越大。丢掉二阶项之后，原本非高斯的分布被强行恢复为高斯，EKF 输出的协方差描述的是一个被扭曲过的分布。这正是“有偏”的来源，也是 EKF 只能称为“近似最优”的原因。

这里有一个很容易被忽略的细节：线性化所依据的点是“估计值”，而不是“真值”，因为真值永远未知。因此 $F$ 和 $H$ 每一帧都在变化，它们是随状态变化的矩阵，而不是常数矩阵。很多初次实现 EKF 的代码把雅可比算成常量矩阵，在小误差范围内似乎还能运行，一旦状态变化稍微剧烈，滤波器就会发散。

常见的实现思路是：把非线性函数就地线性化，再借用线性最优增益公式。这种“就地”两个字是 EKF 全部价值的来源，也是它全部问题的来源。

2.2 雅可比矩阵的演练与验证方式

用一个最小的例子把推导走一遍。状态取四维 $[p_x, p_y, v_x, v_y]$，观测只有雷达的距离和方位：

$$ h(x) = \begin{bmatrix} \sqrt{p_x^2 + p_y^2} \ \operatorname{atan2}(p_y, p_x) \end{bmatrix} $$

对 $h$ 求偏导，得到 $2 \times 4$ 的观测雅可比 $H$。距离对位置的两个偏导分别是 $p_x/r$ 和 $p_y/r$，它们正好是单位视线向量的两个分量；方位角的两个偏导是 $-p_y/r^2$ 和 $p_x/r^2$。速度分量对应的两列为零，因为观测与速度无关。这种几何解释很有用：它能帮你判断符号是否写反，因为单位视线向量一定指向目标。

手推偏导是 EKF 实现里最容易出错的一步，一个负号写错，滤波器不会报错，只会缓慢地往错误方向收敛。比较可靠的做法是用数值差分做对拍：

import numpy as np def h(x): """观测函数：距离 + 方位""" px, py = x[0], x[1] return np.array([np.hypot(px, py), np.arctan2(py, px)]) def H_analytic(x): """解析雅可比，2x4""" px, py = x[0], x[1] r2 = px * px + py * py r = np.sqrt(r2) return np.array([[px / r, py / r, 0.0, 0.0], [-py / r2, px / r2, 0.0, 0.0]]) def H_numeric(x, eps=1e-6): """中心差分，用来对拍解析雅可比""" n = x.shape[0] out = np.zeros((2, n)) for i in range(n): d = np.zeros(n) d[i] = eps out[:, i] = (h(x + d) - h(x - d)) / (2.0 * eps) return out x = np.array([3.0, 4.0, 0.5, 0.2]) err = np.abs(H_analytic(x) - H_numeric(x)).max() print(err) # 正常应在 1e-8 量级；大于 1e-4 说明偏导推错了

eps取 $10^{-6}$ 是中心差分的常规选择，精度是 $O(\varepsilon^2)$，再小会被浮点误差反过来掩盖。这段对拍的真正价值在于它在一帧之内就能暴露出符号错误和参考系错误。方位角那一行的差分要特别注意：如果 $x$ 的取值让atan2跨过 $\pm\pi$ 边界，数值差分会算出接近 $2\pi$ 的差值，对拍结果会异常大，但错误出在差分本身，而不是解析解。

提示：手推完雅可比之后，先用随机 generated 的几组状态做数值对拍，再接入滤波器。这一步花的时间远比事后查发散便宜。

2.3 线性化点：先验、后验与迭代重线性化

EKF 在两个地方需要雅可比，两处用的线性化点不同。预测步用的是上一步的后验估计 $\hat{x}{k-1|k-1}$，因为那是已知的、同时也是最接近真实状态的可用值。观测更新步用的是本步的先验 $\hat{x}{k|k-1}$，因为此时还没有后验，只能在先验上求 $H$。

这个顺序带来了一个隐藏限制：观测雅可比在预测点上求值，如果预测点本身离真值很远，$H$ 就没有参考价值。初始几帧、状态发生突变、传感器突然收到大范围观测时，EKF 都会遇到这个问题，表现出收敛缓慢、输出跳变，严重时协方差更新方向相反，直接发散。

迭代扩展卡尔曼滤波（IEKF）就是针对这个缺陷提出的：在先验点上线性化，做一次更新得到后验，再用后验重新线性化，再更新，循环若干次直到状态增量足够小。从优化视角看，这等价于对最大后验目标函数做高斯牛顿迭代，每一次重线性化都在降低线性化误差。代价是每帧的计算量成倍增加，实时性要求高、非线性温和的场景没有必要采用。

3. 协方差怎么传播：P 矩阵在非线性下的近似代价

3.1 P 的传播与 Q 的真实角色

线性情形下，协方差传播公式 $P^- = F P F^{\mathsf{T}} + Q$ 是精确成立的，它来自高斯分布经过线性变换后的方差公式。非线性情形下，这个式子变成了一阶近似：真正的协方差还有一项与函数二阶导（Hessian）相关的修正，量级正比于 $\lVert P \rVert$。协方差越大，二阶项越不能忽略，这也是为什么 EKF 在“不确定性很大”的时候特别不可靠。

$Q$ 的含义经常被误解。教科书把它叫“过程噪声协方差”，于是很多人把它当成传感器或执行机构的随机噪声去标定。实际工程里，$Q$ 很大一部分在补偿的是“模型没有建模到的部分”：没考虑的风阻，没建模的轮胎滑移，被简化掉的姿态动力学。$Q$ 设得太小，滤波器会过度相信预测，新观测的信息被增益 $K$ 压缩掉，输出的协方差也偏小，形成“过度自信”，一旦真实状态偏离模型，滤波器会拒绝相信观测，最终发散。$Q$ 设得太大，滤波器会过度相信观测，输出抖动、滞后减小但噪声被放大。

常见的实现思路是给 $Q$ 按物理量纲分解：位置通道上给与加速度扰动相匹配的量级，速度通道上给与力扰动相匹配的量级，再乘上离散化时间。这样即使数值要调，也是在一个有物理意义的区间里调，而不是全局乱试。

3.2 观测更新：S、K 与马氏距离

观测更新的三个核心量是新息 $\nu = z - h(\hat{x}^-)$、新息协方差 $S = H P^- H^{\mathsf{T}} + R$、增益 $K = P^- H^{\mathsf{T}} S^{-1}$。$K$ 的几何意义很清楚：它是在“相信预测”与“相信观测”之间按协方差做的加权。$S$ 越小，说明这个观测方向上预测的不确定性相对观测噪声更小，增益越大；反过来，$R$ 大则 $K$ 小。

$S$ 还有一个被低估的用途：一致性检验。归一化新息平方 $\text{NIS} = \nu^{\mathsf{T}} S^{-1} \nu$ 在滤波器一致时应该服从自由度等于观测维数的卡方分布。这条性质比任何曲线观察都客观，它能把“滤波器是不是过度自信”变成一个可以画出来的统计量。

R的标定相对容易：把设备静止放置，采集一段观测，算样本协方差即可。难点在于角度通道，静止时方位角读数在真值附近抖动，直接算方差没问题，但如果真值恰好落在 $\pm\pi$ 附近，样本会横跨边界，方差会被算成接近 $\pi^2$。处理方法是先把角度展开到连续值再统计。

3.3 数值稳定性：约瑟夫形式与正定性

朴素更新式 $P = (I - K H) P^-$ 在数学上等价于精确的贝叶斯更新，但在浮点运算里会出问题：$I - K H$ 不是正交矩阵，乘积会让 $P$ 逐渐失去对称性，长时间运行后可能出现负特征值，滤波器要么输出 NaN，要么在某个方向上“负方差”，行为完全失控。

约瑟夫形式（Joseph form）解决的就是这个：

$$ P = (I - K H) P^- (I - K H)^{\mathsf{T}} + K R K^{\mathsf{T}} $$

def update(P, H, R, K): """约瑟夫形式协方差更新，对任意 K 都保持半正定""" n = P.shape[0] I = np.eye(n) A = I - K @ H # 对称化：抵消浮点累积造成的微小不对称 P_new = A @ P @ A.T + K @ R @ K.T return 0.5 * (P_new + P_new.T)

这段代码有两个值得说明的地方。第一，$A P A^{\mathsf{T}} + K R K^{\mathsf{T}}$ 的结构保证了两项都是半正定的，只要 $P$ 和 $R$ 半正定，结果就半正定，与 $K$ 是否取最优值无关。第二，末尾的对称化不是形式主义：矩阵乘法本身会引入 $10^{-16}$ 量级的非对称，几百帧累积下来，协方差求逆时会出现方向性偏差。

注意：约瑟夫形式只解决数值层面的正定性，不解决线性化误差导致的理论不一致。滤波器显示协方差很小、实际误差很大，属于一致性问题，需要靠 NIS 检验发现。

4. 从公式到可跑代码：一份 EKF 的骨架实现

4.1 预测步：f 与 F 的分工

把二维匀速模型配雷达观测作为骨架。状态 $[p_x, p_y, v_x, v_y]$，采样周期 $dt$，过程噪声通过加速度扰动 $q$ 注入。

import numpy as np def f_predict(x, dt): """状态转移：匀速模型""" px, py, vx, vy = x return np.array([px + vx * dt, py + vy * dt, vx, vy]) def F_jacobian(dt): """匀速模型的雅可比是常数矩阵，这是特例，不是通例""" return np.array([[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt ], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) def Q_matrix(dt, q): """把加速度扰动 q 映射到状态空间，位置项含 dt^3/3""" dt2 = dt * dt dt3 = dt2 * dt return q * np.array([[dt3 / 3, 0, dt2 / 2, 0 ], [0, dt3 / 3, 0, dt2 / 2], [dt2 / 2, 0, dt, 0 ], [0, dt2 / 2, 0, dt ]])

Q_matrix里位置项是 $dt^3/3$、交叉项是 $dt^2/2$，这个系数来自连续白噪声加速度模型对时间积分后的协方差，不是随便填的。如果用 $q \cdot I$ 这种简化形式，位置和速度通道的噪声比例就没法分别调节，dt变化时滤波器行为也会改变。q的量纲是加速度方差，物理意义明确，调起来有参照。

匀速模型的 $F$ 是常数矩阵，这只是因为模型本身线性。一旦换成转弯模型、自行车模型或带姿态的状态，$F$ 就会依赖当前状态，必须在每帧重新计算。

4.2 更新步：新息、增益与角度归一化

def ekf_update(x_pred, P_pred, z, R): """z = [距离, 方位]，方位必须与预测方位做角度差归一化""" px, py = x_pred[0], x_pred[1] r = np.hypot(px, py) r2 = r * r # 观测预测 z_pred = np.array([r, np.arctan2(py, px)]) # 新息：角度通道必须先 wrap 到 [-pi, pi]，否则 +pi 与 -pi 的差是 2*pi nu = z - z_pred nu[1] = (nu[1] + np.pi) % (2.0 * np.pi) - np.pi # 观测雅可比 H = np.array([[px / r, py / r, 0.0, 0.0], [-py / r2, px / r2, 0.0, 0.0]]) S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) x_new = x_pred + K @ nu # 状态里的角度也要归一化，避免累积超过 [-pi, pi] x_new[1] = (x_new[1] + np.pi) % (2.0 * np.pi) - np.pi I = np.eye(4) A = I - K @ H P_new = A @ P_pred @ A.T + K @ R @ K.T P_new = 0.5 * (P_new + P_new.T) return x_new, P_new, nu, S

nu[1]那一行是整个更新步里最关键的一行。距离新息可以直接相减，角度新息不行：观测方位是 $-179°$、预测方位是 $179°$ 时，直接相减得到 $-358°$，滤波器会认为误差接近一整圈，然后用一个巨大的增益把状态推向错误方向。归一化之后差值变成 $+2°$，才是真实的角度偏差。这个 bug 的隐蔽性在于它只在目标靠近 $\pm\pi$ 边界时触发，跑直线轨迹的仿真里根本复现不出来。

np.linalg.inv(S)在小维度上没问题，观测维度升到十几维时应该改用 Cholesky 分解求解 $K = P H^{\mathsf{T}} S^{-1}$，既快又稳，还能顺便拿到 $S$ 的正定性检查。

4.3 用 NEES 与 NIS 检验滤波器是否“说真话”

调参最怕的是看着轨迹曲线觉得“还行”。曲线能看出滞后和抖动，看不出滤波器是不是在骗自己。一致性检验提供的是统计层面的判据：滤波器报告的协方差，是否真实反映了误差分布。

状态误差平方的马氏范数 $\text{NEES} = (x - \hat{x})^{\mathsf{T}} P^{-1} (x - \hat{x})$ 在一致时应服从自由度等于状态维数的卡方分布；$\text{NIS} = \nu^{\mathsf{T}} S^{-1} \nu$ 服从自由度等于观测维数的卡方分布。仿真里真值已知，NEES 可以直接算；实车上真值未知，只能用 NIS。

from scipy.stats import chi2 def nees_ok(nees_series, n): """Monte Carlo 平均后的 NEES 落在双侧 95% 区间内即认为一致""" mean_nees = np.mean(nees_series) lo = chi2.ppf(0.025, n * len(nees_series)) / len(nees_series) hi = chi2.ppf(0.975, n * len(nees_series)) / len(nees_series) return lo, mean_nees, hi

判断标准很直白：平均 NEES 显著大于上界，说明滤波器过度自信，报告的 $P$ 比实际误差小，需要增大 $Q$ 或 $R$；显著小于下界，说明滤波器过度保守，协方差虚高，跟踪会显得迟钝。这条判据比反复试参数可靠得多，也是把“调参”变成“诊断”的转折点。

5. 避坑与排查：EKF 发散的五个典型现场

5.1 现象：航向角在 $\pm\pi$ 附近突然跳变，滤波器崩掉

原因几乎一定是新息没有做角度归一化。角度是周期量，$+179°$ 和 $-179°$ 在物理上只差 $2°$，在数值上差了 $358°$。只要轨迹或目标穿越这条边界，直接相减得到的新息就会被放大成一个大误差，增益随之被推到接近 1，状态被强行拉向错误一侧。

解决方式是所有涉及角度的减法都包一层归一化：(a - b + pi) % (2*pi) - pi。同一套规则要贯穿新息计算、状态更新后的角度修正、以及 $R$ 矩阵的角度通道标定。血泪经验是这条规则必须写成工具函数全局复用，而不是在每个调用点手写一遍。

5.2 现象：观测更新后状态往反方向走，越更新越偏

原因是观测雅可比的符号或者参考系约定与观测模型不一致。典型场景是方位角定义为“从 x 轴逆时针”还是“从 y 轴顺时针”，以及旋转矩阵是主动旋转还是被动旋转。这两套约定在单看公式时都自洽，混用之后偏导符号就反了。

排查方法是用数值差分对拍，对同一个状态点比较解析雅可比和差分结果，误差超过 $10^{-4}$ 就说明推导有问题。更彻底的做法是先用一个已知真值、无噪声的仿真跑单帧更新，看估计值是否向真值靠近；如果一帧之后就偏离，问题一定在雅可比而不是在噪声参数。

5.3 现象：轨迹平滑但严重滞后，或者抖动剧烈

这是 $Q$ 和 $R$ 的比例失衡。$Q$ 相对 $R$ 太小，滤波器过度相信模型，观测被当成噪声压掉，输出表现为滞后；$Q$ 相对 $R$ 太大，滤波器追着观测跑，输出表现为抖动。

关键认识是这两个矩阵只有相对比例重要，同时放大或缩小不改变增益。所以正确的调参顺序是先按物理量纲把 $R$ 从静止数据标出来定死，再调 $Q$。如果反过来先调 $R$，就会陷入两个参数互相抵消的死循环。判据用 NIS：平均 NIS 高于卡方上界就增大 $Q$，低于下界就减小 $Q$。

5.4 现象：跑了几千帧后 P 出现负特征值，输出 NaN

协方差在浮点运算中失去正定性。常见诱因有三个：用朴素形式做更新、用单精度浮点、状态量纲差异过大导致矩阵条件数很差。

解决分三层。第一层换成约瑟夫形式更新，并且每次更新后做对称化。第二层做量纲归一化，把位置用米、速度用米每秒统一到同一数量级，避免 $10^6$ 和 $10^{-6}$ 出现在同一个矩阵里。第三层如果还不行，改用平方根滤波，直接传播协方差的 Cholesky 因子，从结构上保证正定性，代价是实现复杂度上升一档。

5.5 现象：初始几帧怎么都不收敛，或者大范围观测时突然发散

线性化点离真值太远，一阶泰勒展开的截断误差已经大到让 $H$ 失去意义。这种情形在初始状态猜得很差、或者观测突然跳到大范围内时最明显。

处理方式按成本从低到高排：给一个更好的初值，并且把初始协方差 $P_0$ 设得足够大，让滤波器在早期愿意相信观测；对前若干帧做迭代重线性化，用更新后的估计重新计算 $H$；如果非线性程度本身就很高（大视场方位观测、强机动目标），那就该考虑换滤波器而不是继续调参。

6. 设计取舍：为什么明知有偏还要用 EKF

6.1 三种替代方案各自的代价

EKF 的一阶近似有明确缺陷，学术界也早就给出了更好的方案，但工程上 EKF 依然是主力，原因在于代价结构。

UKF 用一组确定性采样点绕过雅可比，能捕获二阶项，实现上不需要手推偏导，这一点在大状态维度下非常诱人。但 sigma 点数量是 $2n+1$，状态到二三十维时计算量就超过 EKF 了，而且它对协方差的 Cholesky 分解很敏感，$P$ 一旦不正定就直接崩，比 EKF 更娇气。

粒子滤波能表达多峰分布，代价是高维下需要指数级的粒子数。因子图优化能给全局最优解，代价是延迟和内存，适合离线或低频后端，不适合每毫秒都要出结果的场合。

6.2 我实际判断的顺序

接手一个估计问题时，判断顺序大致是这样：先看非线性程度。如果状态转移和观测在工作的取值范围内近似线性，或者虽然非线性但协方差很小（线性化误差正比于 $\lVert P \rVert$），EKF 就是性价比最高的选择，嵌入式上几千行代码就能跑，协方差显式可读，出问题能定位。

如果非线性明显（大角度旋转、大视场观测、强机动），先试着用迭代重线性化把精度补回来，成本比换 UKF 低。如果还是过不了 NEES 检验，再换 UKF。只有在分布本身是多峰的（数据关联不确定、遮挡导致观测断续）时，才值得上粒子滤波或多假设方法。

有一条判断标准我一直在用：能不能用 NIS 把滤波器的一致性量化出来。能，就说明这套 EKF 是“可观测、可诊断”的，值得投入调参；不能，比如观测维度太低、或者数据里根本没有激励，那再怎么调都是在盲猜，不如先去改传感器配置或观测模型。

从那以后我每次接手一个 EKF 工程，第一件事不是调 Q 和 R，而是先跑一遍 NIS 一致性检验，把滤波器是不是在“骗自己”变成一条有上下界的曲线摆在面前，再去动参数。这个习惯让我少走了很多弯路，也希望帮到你。

本文还有配套的精品资源，点击获取