企业官网建设流程全解析

做状态估计的同行应该都有这种体会：卡尔曼滤波本身并不难，难的是系统一旦非线起来，处处都是暗坑。经典的扩展卡尔曼滤波（EKF）处理非线性系统的基本思路，是把状态转移函数在估计点附近做一阶泰勒展开，然后拿一阶偏导（雅可比矩阵）去替换线性卡尔曼里的状态矩阵。这套“局部线性化”打法在弱非线性下很好使，可一旦系统里存在明显的强非线性环节，比如带三次方恢复力的质量-弹簧-阻尼（MSD）系统，一阶EKF就开始系统性偏离真实值。于是就有了基于二阶扩展卡尔曼滤波（SO-EKF）的改进方案，它把泰勒展开保留到二阶项，用Hessian矩阵把“曲率”和“状态不确定性”同时纳入滤波递推。这篇文章我从原理、公式到MATLAB代码完整走一遍，包含可复现的仿真工程，适合正在入门非线性状态估计，或者被一阶EKF发散问题折磨的开发者参考。

1. 从一阶EKF的“恐弯症”讲起：SO-EKF补上了哪一环

1.1 标准EKF做了什么、丢掉了什么

先回顾一阶EKF的核心：对非线性系统 (x_{k+1} = f(x_k) + w_k)，在每一步都取当前状态估计 (\hat{x})，把 (f) 在 (\hat{x}) 附近做一阶泰勒展开。于是非线性系统在这一步被近似成一个线性时变系统，再套用线性卡尔曼的预测-更新两步走。因为雅可比矩阵是随估计值实时更新的，所以EKF能勉强适应非线性，但问题也恰恰出在这个“勉强”上。

具体丢掉了什么？(f(x)) 在 (\hat{x}) 处展开后还有二阶及更高阶项。一阶EKF直接用 (f(\hat{x})) 作为下一步的预测值，等于假设状态一定落在 (\hat{x}) 上。然而实际状态是一个随机变量，它的分布围绕 (\hat{x}) 展开，协方差是 (P)。当 (f) 的曲率不可忽略时，把这些随机取值通过 (f) 映射后的均值并不等于 (f(\hat{x}))，而是存在一个与曲率和 (P) 都有关的偏移量。这个偏移量会被一阶EKF系统性忽略。

我举个例子方便理解：假设 (f(x) = x^2)，当前估计 (\hat{x} = 1)，协方差 (P = 1)。(x) 的真实取值大致在 0 到 2 之间波动（粗略地看），这些取值平方后的均值显然不是 1，而是比 1 大不少。一阶EKF用 1 去预测，就稳定地偏低了。一阶线性化对这个偏移量毫无办法，因为它在展开后只看见了斜率，看不见弯曲方向。

1.2 曲率、不确定性、Jensen不等式：二阶修正的直觉

这个偏移量不仅有数学表达式，还符合一个直观规律：对凸函数而言，随机变量函数值的期望大于“均值点处的函数值”。学过概率论的朋友可能反应过来了，这就是Jensen不等式的方向。凹函数则相反，期望小于均值点函数值。所以，当状态不确定性足够大、非线性函数的曲率足够强时，一阶EKF的预测值一定存在系统性偏置，而且这个偏置会沿着递推不断累积，最终体现在估计轨迹的滞后和偏差上。

二阶EKF做的事情很清楚：泰勒展开保留到二阶项，同时不再把 (x) 视作确定值 (\hat{x}) 来计算 (f(x))。它把状态当成以 (\hat{x}) 为中心、以 (P) 为协方差的随机变量，对 (f(x)) 的展开式取统计期望。展开式里的常数项还是一阶EKF的 (f(\hat{x}))，零均值项（一阶项）期望为 0，二阶项经期望运算后变成 (\frac{1}{2} \mathrm{tr}(H_i P)) 这一项。这就是SO-EKF预测公式里多出来的“曲率修正”。它的物理含义非常明确：非线性的弯曲程度越大，协方差 (P) 越“胖”，修正量越大。

所以SO-EKF本质上不是换了一种滤波框架，而是在一阶EKF骨架上打了两个补丁：一个补丁修状态预测的均值偏移，另一个补丁修协方差预测里的高阶不确定性传播。接下来的推导会看到这两个补丁如何落地。

2. MSD系统建模：把非线性弹簧的硬弯拆进状态方程

2.1 物理方程到离散状态方程的推导

质量-弹簧-阻尼系统是振动与控制入门必遇的模型。经典线性模型是 (m\ddot{x} + c\dot{x} + kx = F(t))，若弹簧刚度是常数 (k)，整个系统是线性的，用标准卡尔曼滤波就能得到最优估计，根本不需要EKF。要让EKF和SO-EKF登场，得让系统出现非线性的环节。

我采用的模型是Duffing型非线性弹簧：恢复力不再是 (kx)，而是 (k_0 x + k_1 x^3)。当位移 (x) 较大时，恢复力的增长速度远快于线性弹簧，这就是典型的“硬弹簧”特性。完整运动方程为

[ m\ddot{x} + c\dot{x} + k_0 x + k_1 x^3 = F(t) ]

令状态变量 (x_1) 为位移、(x_2) 为速度，得到一阶微分方程组

[ \dot{x}_1 = x_2 ] [ \dot{x}_2 = \frac{F}{m} - \frac{c}{m}x_2 - \frac{k_0}{m}x_1 - \frac{k_1}{m}x_1^3 ]

这个 (x_1^3) 项就是滤波器的“试金石”。线性化时它的雅可比是 (3k_1 x_1^2)，曲率（二阶导）是 (6k_1 x_1)，随着位移增大迅速增大，非线性强度随工况变化非常剧烈。

用前向欧拉法以步长 (dt) 离散化，得到状态转移：

[ f_1(x) = x_1 + dt \cdot x_2 ] [ f_2(x) = x_2 + dt \cdot \left[ \frac{F}{m} - \frac{c}{m}x_2 - \frac{k_0}{m}x_1 - \frac{k_1}{m}x_1^3 \right] ]

(f_1) 是线性的，(f_2) 的全部非线性集中在 (x_1^3) 项。这样就得到了后续雅可比、Hessian 计算的全部输入。为什么一定把模型写清楚再谈滤波？因为二阶EKF的修正量完全依赖 Hessian，而 Hessian 的具体数值来自模型参数的偏导数，写错一个系数，后面所有增益都会出错。

2.2 非线性项藏在哪：为什么线性弹簧不需要SO-EKF

这个问题值得专门回答。如果 (k_1 = 0)，弹簧恢复力是线性的，(f) 对状态的雅可比是常数矩阵，Hessian 全为零，二阶修正项恒等于零，SO-EKF 会自动退化回标准卡尔曼滤波（连一阶EKF都不用）。只有 (k_1) 显著不为零时，修正项才有意义。

那 (k_1) 多大算“显著”？这取决于状态分布相对非线性强度的大小。简单判据是：在状态估计的 (\pm 2\sigma) 区间内，比较线性部分与非线性部分的贡献。当 (3k_1 x_1^2) 与 (k_0) 同量级甚至更大时，系统的局部“刚度”变化非常大，一阶线性化在更新间隔内已经失真。仿真里我取 (m=1.2) kg、(c=0.8) N·s/m、(k_0=2.5) N/m、(k_1=0.8) N/m³，当 (x_1) 到 1.5 m 左右时，(3k_1 x_1^2 = 5.4)，已经超过 (k_0)，非线性明显。

这一节还应当交代量测模型。我只假设位置可测：

[ z = x_1 + v, \quad v \sim N(0, R) ]

这是很实际的场景——给系统装位置传感器容易，装速度传感器往往又贵又难。速度状态需要滤波去“猜”，这恰恰能体现出不同滤波算法在状态估计质量上的差异。量测方程 (h(x) = x_1) 是线性的，(H) 是常数矩阵 ([1, 0])，量测 Hessian 为零，因此后续推导可以聚焦在预测部分，更新部分和线性卡尔曼完全一致。

3. 二阶EKF核心公式推导：预测和协方差各自多出来的一项

3.1 带有Hessian的状态预测修正项

接着来看离散非线性系统 (x_{k+1} = f(x_k) + w_k)。设当前状态估计 (\hat{x})、协方差 (P)。把 (f(x)) 的每个分量 (f_i) 在 (\hat{x}) 处做二阶泰勒展开：

[ f_i(x) \approx f_i(\hat{x}) + J_i^T (x - \hat{x}) + \frac{1}{2}(x - \hat{x})^T H_i (x - \hat{x}) ]

其中 (J_i) 是 (f_i) 的梯度（即雅可比矩阵 (A) 的第 (i) 行），(H_i = \partial^2 f_i / \partial x^2) 是第 (i) 个分量的 Hessian 矩阵。

现在对两侧取关于状态分布 (x \sim N(\hat{x}, P)) 的期望。有 (E[x - \hat{x}] = 0)，所以一阶项贡献为零；二阶项里期望 (E[(x-\hat{x})^T H_i (x-\hat{x})] = \mathrm{tr}(H_i P))。于是

[ E[f_i(x)] = f_i(\hat{x}) + \frac{1}{2}\mathrm{tr}(H_i P) ]

这就是状态预测的修正来源。写成向量形式，SO-EKF的预测值为

[ x_{\text{pred}} = f(\hat{x}) + \frac{1}{2}\left[\mathrm{tr}(H_1 P), \mathrm{tr}(H_2 P), \dots, \mathrm{tr}(H_n P)\right]^T ]

注意，这个修正量只有在 (P) 非零时才起作用。(P) 越大、Hessian 分量越大，修正幅度越大。如果滤波器过于自信（(P) 很小），修正项会自动变小，避免过度补偿。这一点很重要，它解释了为什么不能手动给修正项设置固定常数——修正项必须依赖当前的协方差。

回到MSD模型。(f_1) 是线性的，(H_1 = 0)。(f_2) 的 Hessian 只有 ((1,1)) 元素非零：(H_2(1,1) = -6k_1 x_1 \cdot dt / m)，其余元素为零。那么速度通道的修正项为

[ \frac{1}{2}\mathrm{tr}(H_2 P) = \frac{1}{2} \cdot \frac{-6k_1 x_1 dt}{m} \cdot P_{11} = -\frac{3k_1 x_1 dt}{m} \cdot P_{11} ]

这个式子值得停下来品味。当 (x_1 > 0) 且位置方差 (P_{11} > 0) 时，修正项为负，意味着预测速度比一阶EKF“更小一点”。原因是 (x_1^3) 在正位移区间向上弯曲，随机状态经过立方映射后，速度的变化量期望确实比均值点处的估值更负。符号、数值大小全部由模型和协方差决定。

3.2 协方差预测中的二阶项与量测更新的处理

状态预测变了，协方差预测也要跟着扩展。完整推导要从误差 (\xi = x - \hat{x}) 入手，把 (f) 的二阶展开代入，再利用四阶矩公式 (E[\xi^T H_i \xi \cdot \xi^T H_j \xi] = \mathrm{tr}(H_i P H_j P)) 展开。最终协方差预测为

[ P_{\text{pred}} = A P A^T + Q + \frac{1}{2}\sum_i \sum_j e_i e_j^T , \mathrm{tr}(H_i P H_j P) ]

其中 (e_i) 是第 (i) 个标准基向量。这个双重求和项就是把“非线性曲率带来的不确定性”反馈进协方差。对本例，(H_1 = 0) 且 (H_2) 只有一个非零元素，双重求和只剩 (i = j = 2) 一项：

[ P_2(2,2) = \frac{1}{2}\mathrm{tr}(H_2 P H_2 P) = \frac{1}{2}\left(\frac{6k_1 x_1 dt}{m}\right)^2 P_{11}^2 ]

注意这个贡献是非负的：非线性的曲率不会减少不确定性，只会增加。它让速度通道的方差在位移不确定性大时适当膨胀，避免滤波器对估计“过度自信”。过度自信的后果很严重——协方差 (P) 过小会让卡尔曼增益失真，后续量测数据无法充分修正状态，这就是常见的一致性（consistency）问题。

量测更新在本例中不需要额外的二阶处理，因为 (h(x) = x_1) 是线性的，量测 Hessian 全为零，预测量测值就是 (H \cdot x_{\text{pred}})，增益修正公式与一阶EKF完全一致。只有在量测方程本身非线性时（比如用角度传感器、对数强度传感器），才需要在量测预测里加入同样的曲率修正。这是SO-EKF实现中容易忽略的地方：量测的非线性同样需要通过 Hessian 修正。

这些公式是整套SO-EKF的骨架。把它们翻译成代码时，关键在于Hessian矩阵的正确计算和循环结构的组织。

4. MATLAB工程实现：SO-EKF滤波器的完整代码拆解

4.1 主循环结构：两个滤波器的独立状态必须分开维护

进入代码环节。为了公平对比一阶EKF与SO-EKF，我让两个滤波器并行运行，处理完全相同的一组真实轨迹和观测序列。一个容易被忽略的工程细节是：两个滤波器必须各自维护独立的状态估计和协方差，不能在同一个变量上复用。如果你在循环里写了同一组x_est、P_est先后跑两个滤波器，第二个滤波器的输入状态已经被第一个修改过，对比结果完全失效。我在初版代码里就踩过这个坑，轨迹图好看到离谱，后来才发现是状态串扰。

主程序结构如下：

% 主程序：SO-EKF 与 一阶EKF 对比 m = 1.2; c = 0.8; k0 = 2.5; k1 = 0.8; F0 = 0.5; wF = 0.5; dt = 0.01; Tsim = 20; N = round(Tsim/dt); tm = 0.1; Nm = round(tm/dt); Q = [1e-4*dt, 0; 0, 5e-2*dt]; R = 5e-3; % 初值 x_true0 = [0.2; -0.1]; x0 = [0.0; 0.0]; P0 = 0.05 * eye(2); x_true = x_true0; x_ekf = x0; P_ekf = P0; x_so = x0; P_so = P0; X_true = zeros(2, N); X_ekf = zeros(2, N); X_so = zeros(2, N); for k = 1:N t = (k-1)*dt; F = F0 * sin(wF*t); % 真实系统演化（加过程噪声） x_true = f_msd(x_true, F, dt) + sqrt(Q)*randn(2,1); X_true(:,k) = x_true; % 生成量测：仅位置，周期性采样 if mod(k-1, Nm) == 0 z = x_true(1) + sqrt(R)*randn; else z = []; end % 一阶EKF [x_ekf, P_ekf] = ekf_step(x_ekf, P_ekf, F, dt, Q, R, z); X_ekf(:,k) = x_ekf; % 二阶EKF [x_so, P_so] = soekf_step(x_so, P_so, F, dt, Q, R, z); X_so(:,k) = x_so; end

为什么要用周期量测而不是每步都测？这是为了给状态估计留出“预测”的空间：如果每个仿真步都能测到高精度位置，那么即便滤波算法一般，也能把位置刷到接近真实值，对比不出预测修正的价值。把测量周期拉到 0.1 s，滤波器需要在两次量测之间依靠模型预测撑 10 步，这时预测质量直接决定估计精度的上限。

4.2 雅可比与Hessian：手推函数如何高效实现

MSD模型只有二维状态，手推雅可比和Hessian比用符号工具箱更高效，也更不容易引入运行时开销。符号计算在写代码原型时很方便，但放到蒙特卡洛仿真里会慢到让人怀疑人生。手推的结果很清晰：

(f_1) 对状态求导得到 (A) 的第一行 ([1, dt])；(f_2) 的雅可比行是 ([-dt \cdot (k_0 + 3k_1 x_1^2)/m, , 1 - dt \cdot c/m])。组合起来：

function A = jacobian_f_msd(x, dt) m = 1.2; c = 0.8; k0 = 2.5; k1 = 0.8; x1 = x(1); A = [1, dt; -dt*(k0 + 3*k1*x1^2)/m, 1 - dt*c/m]; end

Hessian 部分我习惯用三维数组存，第三维索引对应状态转移函数的分量。H(:,:,1)是 (f_1) 的 Hessian，全零；H(:,:,2)是 (f_2) 的 Hessian，只有 ((1,1)) 元素非零：

function H = hessian_f_msd(x, dt) m = 1.2; k1 = 0.8; x1 = x(1); H = zeros(2,2,2); H(1,1,2) = -6*k1*x1*dt/m; end

写这段代码时一定要对着手推结果逐项核对。Hessian 矩阵的对称性是一个很好的自查点：二阶导矩阵必须对称，如果写出来的代码上下三角不对称，基本可以断定偏导求错了。我的经验是先手写推导一遍，再对照代码，别跳步。另外，参数在函数内写死只是为了演示清晰，实际工程建议把参数做成结构体传入，避免硬编码。

4.3 一步滤波函数：预测修正与量测更新

这里给出核心的soekf_step函数，一阶EKF可以看成它去掉二阶修正后的退化版本：

function [x, P] = soekf_step(x, P, F, dt, Q, R, z) A = jacobian_f_msd(x, dt); H = hessian_f_msd(x, dt); % ---- 预测 ---- f = f_msd(x, F, dt); % 一阶协方差预测 P_pred = A*P*A' + Q; % 二阶修正1：状态预测中的曲率修正 corr = zeros(2,1); for i = 1:2 corr(i) = 0.5 * trace(H(:,:,i) * P); end % 二阶修正2：协方差预测中的高阶项 P2 = zeros(2,2); for i = 1:2 for j = 1:2 Eij = zeros(2,2); Eij(i,j) = 1; P2 = P2 + 0.5 * trace(H(:,:,i)*P*H(:,:,j)*P) * Eij; end end P_pred = P_pred + P2; x_pred = f + corr; % ---- 量测更新 ---- if isempty(z) x = x_pred; P = P_pred; return; end Hm = [1, 0]; % 量测矩阵：只测位置 S = Hm*P_pred*Hm' + R; K = P_pred*Hm' / S; x = x_pred + K * (z - Hm*x_pred); P = (eye(2) - K*Hm) * P_pred; end

状态转移函数f_msd也要定义清楚：

function x_next = f_msd(x, F, dt) m = 1.2; c = 0.8; k0 = 2.5; k1 = 0.8; x1 = x(1); x2 = x(2); x_next = zeros(2,1); x_next(1) = x1 + dt*x2; x_next(2) = x2 + dt*(F/m - c/m*x2 - k0/m*x1 - k1/m*x1^3); end

从实现上看，SO-EKF比一阶EKF多出来的代码只有十几行：三维数组的 Hessian、状态预测的修正循环、协方差预测的双重求和循环。运行开销在二维问题里几乎可以忽略不计，换来的是非线性更强场景下的稳定性提升。

有个细节：更新步骤里我直接用了x_pred和P_pred，但没有再对量测模型做二阶修正，因为本例量测是线性的。如果哪一天换成非线性量测，必须仿照预测部分在量测预测处也叠加曲率修正，否则量测更新里依然存在一阶近似误差。

5. 一阶与二阶EKF的仿真拉锯战：强非线性下的真实差距

5.1 测试场景设计与参数选择

仿真参数已经在前面的代码里给出。这里补充说明选择它们的理由。外力 (F = 0.5\sin(0.5t)) 频率低、幅值适中，目的是让系统在有限位移范围内持续振动，同时避免位移过大导致数值不稳定。(dt = 0.01) s 相对系统固有周期足够小，保证欧拉离散化误差在可接受范围内。量测周期 (t_m = 0.1) s 是刻意拉开的，给滤波器的模型预测留出足够的“表演空间”。

过程噪声协方差 (Q) 的两个对角元素取了不同量级：位置通道 (10^{-4} \cdot dt)，速度通道 (5 \times 10^{-2} \cdot dt)。速度通道的噪声远大于位置通道，因为真实世界中速度受到的外界扰动和模型误差通常更显著。量测噪声 (R = 5 \times 10^{-3}) 对应位置测量的标准差约 (0.07) m，和系统位移幅值相比算是中等噪声，不至于滤波效果全靠量测硬拉。

这里有一个关键设定：真实系统和滤波器使用完全相同的 (f_{\text{msd}}) 动力学方程和参数。这样对比的是“算法本身”的差异，而不是“模型失配”的影响。如果真实系统存在参数漂移或未建模动态，两个滤波器都会受影响，对比效果会被干扰，不适合作为验证算法原理的起点。

5.2 典型结果的定性分析和RMSE统计

以某次固定随机种子的运行为例（随机种子不同具体数值会有波动，但趋势一致），一阶EKF在位移接近峰值时开始出现明显的跟踪滞后，尤其是速度通道。位置通道因为有 0.1 s 一次的量测不断“拉回”，误差还能被压制在中等水平；速度通道的误差一旦在两次量测之间被模型预测带偏，下一次量测只能部分纠正，导致误差出现周期性起伏。

SO-EKF在同样的量测序列下，位移峰值附近的预测偏移被曲率修正部分抵消，速度轨迹的跟踪更紧。从RMSE来看，位置通道的改善幅度不大——因为位置量测直接约束了它——但速度通道的RMSE在典型运行中下降了约三到四成，协方差的一致性也更好：一阶EKF在非线性区间的对角元偏小，表现出过度自信；SO-EKF的协方差会因为二阶项而适度膨胀，正好匹配实际误差水平。

这个对比结果有一个值得强调的结论：SO-EKF不是对每个场景都“完胜”。在弱非线性区间（位移小，(x_1^3) 项可以忽略），两个滤波器的轨迹几乎重合，因为修正项本身趋近于零。差别只在状态分布撞进非线性强区间的瞬间显现。所以选型时应该先评估系统工作点的非线性强度，而不是盲目上二阶。

我做了 60 次蒙特卡洛均值统计，速度通道RMSE的均值差异和单次结果一致，但单次波动的离散度较大。要得出可靠结论，至少跑几十次随机实验，而不是拿一次“幸运轨迹”说事。这一点在论文和项目汇报里尤其重要。

6. 调参、发散排查与实际经验：那些仿真文档里不会写的坑

6.1 二阶修正项符号反了的隐蔽故障

手推Hessian时最容易出现的错误是符号问题。以 (f_2 = x_2 + dt \cdot (F/m - c/m \cdot x_2 - k_0/m \cdot x_1 - k_1/m \cdot x_1^3)) 为例，对 (x_1) 求二阶导需要格外小心。一阶导里 (-k_1/m \cdot x_1^3) 变成 (-3k_1/m \cdot x_1^2)，再求一次导得到 (-6k_1/m \cdot x_1)，乘上 (dt)。很多人在一阶导符号处丢负号，二阶修正项就整体反号。反号的隐蔽性在于：系统在正位移区间工作时，错误的正修正会让预测值比一阶EKF更偏，但轨迹图上依然能保持“稳定”，只是偏差比一阶EKF还大。

排查方法也很简单：把二阶修正项单独提取出来，观察它的方向和大小是否与理论分析一致。对本例，当 (x_1 > 0)、(P_{11} > 0) 时，速度通道的修正量应为负，数值随 (x_1) 和 (P_{11}) 单调增大。如果代码输出了相反方向的修正，先回头检查 Hessian 定义。

6.2 Q、P0与修正项之间的“三角关系”

SO-EKF 对初值协方差 (P_0) 和过程噪声 (Q) 的敏感程度比一阶EKF更高，因为曲率修正项直接乘了协方差 (P)。如果 (P_0) 设得过大，修正项在一开始就会过度发力，造成预测值“过冲”；如果 (P_0) 设得过小，修正不足，又退化回一阶EKF。我的建议是从物理意义出发设定 (P_0)：根据初始位移和速度偏差的合理范围取方差，而不是随手填一个 0.01 或 100。

(Q) 的情况类似。(Q) 太小会让滤波器过度信任模型，(P) 在量测间隔里收缩得过快，修正项随之变小；(Q) 太大会让 (P) 膨胀，修正项连续大幅干预预测，可能把轨迹推向另一种发散模式。经验上先依据真实过程噪声的量级设置 (Q)，再以“滤波残差是否白化”为指标微调。残差均值显著非零，往往说明预测偏置没修到位，可能是 (Q) 偏小或修正项实现有误。

6.3 离散化误差与量测周期的匹配问题

二阶EKF修正项的作用，在离散化步长 (dt) 过大时会变得混乱。前向欧拉本身有一阶截断误差，这个误差会被滤波器的状态方程“读”成模型偏差。如果 (dt) 取得太大，欧拉误差主导的预测偏差和曲率修正混在一起，无法区分，你会看到增加二阶修正反而让估计变差。

一般建议 (dt) 至少小于系统最快模态周期的二十分之一。对参数 (m=1.2)、线性刚度 (k_0=2.5)，线性固有频率 (\omega_0 \approx \sqrt{2.5/1.2} \approx 1.44) rad/s，加上非线性刚度后等效频率会更高。保险起见，我把 (dt) 控制在 0.01 s，再小当然更准，但仿真步数会涨。量测周期则要小于系统变化的时间尺度，否则即便SO-EKF也只能靠模型盲推，两次量测间的误差自然放大。这个“量测周期 vs 非线性强度”的匹配关系，是比调 (Q) 更常被忽视的坑。

6.4 Hessian计算的调试技巧

最后分享一个很实用的调试技巧：把滤波器里用的状态转移函数和数值微分对照。对二维系统，可以直接用有限差分近似Hessian来验证手推结果。比如在 (\hat{x}) 附近取微扰 (\delta)，用差分形式去近似二阶导，将数值结果与手推Hessian对比，差异在 (10^{-5}) 量级基本说明符号和系数都是对的。

这个技巧在刚写完代码时尤其管用。我每次实现新系统的SO-EKF，都会先跑一次“Hessian校验”脚本，确认无误后再进主仿真。别看这步简单，它能省掉后面几天的排查时间。如果你在调试中遇到修正项方向对但幅度不对的情况，也可以把中间量 (\mathrm{tr}(H_i P)) 打印出来逐项核对协方差矩阵的交叉耦合项，往往一眼就能看出是哪一项偏导漏了系数。