企业官网建设流程全解析

第一次用线性卡尔曼滤波做目标跟踪的时候，我踩过一个很典型的坑：目标明明在匀速直线运动，但一旦把雷达观测换成距离和方位角，滤波器就开始发疯，估计轨迹绕着真值来回震荡，甚至直接飞出屏幕。后来才反应过来，问题不在算法实现，而在“线性”这两个字——真实世界里几乎没有多少系统是严格线性的，雷达测的是斜距和方位角，卫星定轨里有与位置三次方成反比的摄动力，电池SOC估计中开路电压和SOC是一条带滞回的非线性曲线。这些事情，线性卡尔曼滤波（KF）根本接不住。

扩展卡尔曼滤波（Extended Kalman Filter，EKF）就是为这种情况准备的：它把非线性的状态方程和观测方程在当前工作点附近做一阶泰勒展开，得到雅可比矩阵，然后用标准的卡尔曼滤波框架继续递推。Matlab里实现EKF并不复杂，核心代码量可能比线性KF还少，但工程上的坑主要集中在雅可比矩阵、噪声协方差设置和角度归一化这些细节上。这篇文章我会把EKF的原理、Matlab实现、调试经验和选型边界一次性讲透，适合刚入门卡尔曼滤波、准备把EKF用到实际项目里的读者。

1. 为什么线性卡尔曼滤波在真实系统里撑不住

1.1 标准KF的黄金假设，现实世界很难满足

要理解EKF为什么存在，先得看标准KF假设了什么。卡尔曼滤波的五个公式里，状态预测和观测更新都依赖两个常数矩阵：状态转移矩阵F和观测矩阵H。F告诉你“上一时刻的状态怎么线性叠加到这一时刻”，H告诉你“当前状态怎么线性映射成观测量”。这意味着被估计的系统必须满足两个条件：状态演化是线性的，观测方程也是线性的。

放到工程里，这两个条件往往都很苛刻。一个最简单的反例就是测距测角传感器——雷达、激光雷达、声呐都这样。系统状态是笛卡尔坐标系下的位置和速度，观测却是极坐标系下的距离和方位角。距离和位置的关系是开根号，方位角和位置的关系是反正切，这哪是线性？如果你强行给H填一个常数矩阵，比如取某个参考点上的近似值，那么在偏离参考点的地方，线性近似误差会越来越大，滤波器输出的协方差根本不能反映真实误差。

更隐蔽的问题是过程模型。线性KF要求状态转移F是常量，但很多物理过程的微分方程本身就是非线性的：单摆的加速度和摆角的正弦成正比，飞行器的姿态动力学带转动惯量交叉项，车辆运动学模型里方向盘转角和轨迹曲率也是非线性关系。这时候用一个常数F去描述系统，等于把模型误差硬塞进过程噪声Q里。如果Q设小了，滤波器会“自信”地收敛到错误状态；Q设大了，估计结果又噪声巨大，滤波器失去意义。

1.2 非线性的两类来源：运动模型和观测模型

我把工程里常见的非线性来源分成两类，方便后面理解EKF的两种线性化需求。

第一类是运动模型非线性。状态变量之间的演化关系不是简单的矩阵乘法。比如一个二维匀转弯运动（constant turn，CT）目标，位置更新里包含三角函数，转角和速度还耦合在公式里。再比如摆系统：角度导数等于角速度是线性的，但角速度导数里有sin(θ)项，这是典型的非线性状态方程。针对这类系统，EKF需要对状态转移函数f(x)求雅可比矩阵，用F_Jac代替线性KF里的常数F。

第二类是观测模型非线性。状态本身可能是线性演化的，但传感器读数不是状态的线性函数。最典型的就是“笛卡尔坐标状态 + 极坐标观测”，也就是我开头说的场景。此外还有GPS定位中的伪距观测、无线电测向中的到达角、相机标定中的重投影坐标，全是非线性函数。针对这类系统，EKF需要对观测函数h(x)求雅可比矩阵，用H_Jac代替线性KF里的常数H。

一个完整的EKF实现里，这两类雅可比矩阵至少要处理对一类，很多项目是两类都要处理。这也是EKF和线性KF最大的区别：每个时刻都要重新计算矩阵，不能像线性KF那样提前离线算好。

1.3 EKF的总体思路：在每个工作点做“局部线性化”

EKF的基本思想不复杂：既然全局线性做不到，那就把非线性部分在每个时刻的估计点附近做一阶泰勒展开。这就像你爬山时看脚下的山坡，远处是起伏的曲线，但脚下那一小块地方可以用一个斜坡面去逼近。泰勒展开保留一阶项，丢掉二阶及以上项，得到的就是雅可比矩阵。

所以EKF的递推流程和标准KF几乎一模一样：预测→计算新息→算卡尔曼增益→更新状态和协方差。区别只有两点：预测值不是F乘以x，而是直接调用非线性函数f(x)计算；更新增益和协方差时，用的不是常数H，而是当前预测点上的雅可比矩阵H_Jac。搞清楚这一点，EKF就算掌握一半了。

但这里有一个必须牢记的前提：局部线性化在非线性系统里是有代价的，而且“局部”二字决定了一切。如果系统在单个采样周期内运动范围很小、非线性函数比较平滑，一阶近似就足够准，EKF表现和线性KF一样稳定。如果系统强非线性、采样周期大、初始误差离谱，一阶项不够用，滤波器就会发散。这就是为什么后面要讲UKF和粒子滤波，但在那之前，先把EKF玩明白。

2. EKF的核心：在预测和更新两个环节分别做线性化

2.1 状态预测阶段的线性化：从连续系统到一步雅可比

先看通用形式。设系统为：

x_k = f(x_{k-1}) + w_k

z_k = h(x_k) + v_k

其中w_k是过程噪声，v_k是观测噪声，都假设为零均值高斯白噪声。f和h是任意的可微函数。EKF要做的事情，是在每次递推时对f和h分别求导。

预测阶段，严格的做法是先对状态转移函数f在上一时刻的估计值x̂_{k-1}处做泰勒展开，忽略二阶以上项，得到：

x̂_k^- = f(x̂_{k-1})

协方差预测为：

P_k^- = F_Jac * P_{k-1} * F_Jac^T + Q

其中F_Jac是f对x的雅可比矩阵在x̂_{k-1}处的取值。如果系统的状态转移本身是线性的，那F_Jac就是原来的常数F，等式退化成标准KF。

实际工程里，很多非线性系统是用连续微分方程描述的，写成dx/dt = f_continuous(x)。这时候求一步预测雅可比有个特别实用的近似：用欧拉离散化，得到

x_k ≈ x_{k-1} + dt * f_continuous(x_{k-1})

F_Jac ≈ I + dt * J

其中J是连续系统雅可比矩阵∂f_continuous/∂x在当前点处的值。I是单位阵。这个近似在dt不是特别大时精度足够，而且写代码特别方便。

举个例子，单摆系统。状态取x = [θ; ω]，θ是摆角，ω是角速度。连续动态为：

dθ/dt = ω

dω/dt = -(g/L) * sin(θ)

那么J = [0, 1; -(g/L)*cos(θ), 0]。离散化后一步预测写成：

θ_k = θ_{k-1} + dt * ω_{k-1}

ω_k = ω_{k-1} - dt * (g/L) * sin(θ_{k-1})

对应的F_Jac = I + dt * J = [1, dt; -dt*(g/L)*cos(θ_{k-1}), 1]。注意这个矩阵里右下角是1，但实际离散系统里通常还会加一点阻尼项或过程噪声来吸收离散化误差。这种“先得连续雅可比再离散”的办法，对大多数机械系统、车辆模型和机电系统都够用，也是我在Matlab里最常用的方式。

2.2 观测更新阶段的线性化：测距测角模型雅可比怎么求

观测更新阶段同样做一阶泰勒展开。先把观测值算出来：

ẑ_k = h(x̂_k^-)

然后求雅可比H_Jac = ∂h/∂x在当前预测状态x̂_k^-处的值，用在卡尔曼增益里：

S = H_Jac * P_k^- * H_Jac^T + R

K = P_k^- * H_Jac^T * S^{-1}

如果我不用常数H，那这些式子看起来和标准KF一样，只是每个时刻的H都要重新算。关键难点就是求H_Jac。

还是用测距测角模型。状态x = [px; py; vx; vy]，观测为距离r和方位角θ，观测函数：

r = sqrt(px^2 + py^2)

θ = atan2(py, px)

对px和py分别求偏导，得到2×4的雅可比矩阵：

H_Jac = [px/r, py/r, 0, 0; -py/(r^2), px/(r^2), 0, 0]

第一行是距离对位置的偏导，物理意义是位置的单位方向向量；第二行是方位角对位置的偏导，注意分母有r^2，这就带来了一个重要注意事项：当目标离雷达很近时，r很小，方位角雅可比会变得很大，观测噪声的微小变化会被放大，滤波器容易反常。这是我实际调试中真实碰到过的，后面专门讲。

如果你手头有现成的传感器模型，但不想手推雅可比，还有一条路：数值差分。利用中心差分公式

∂h_i/∂x_j ≈ (h_i(x + εe_j) - h_i(x - εe_j)) / (2ε)

写一个通用函数就可以验证手推导的结果。我强烈建议在第一次实现EKF时这么做，因为雅可比矩阵写错是最隐蔽、最致命的错误——协方差矩阵照样在缩小，但状态估计偏得离谱。

2.3 线性化误差从哪里来：EKF什么时候不可以信

EKF的一个关键缺点被很多教程一笔带过：它没有考虑泰勒展开的高阶项。当系统在强非线性区域工作，或者采样周期dt太大时，一阶近似不够，滤波器就会把“近似误差”当成真实的后验分布，协方差不合理地缩小，最终发散。

工程上有一个粗糙的判断标准：如果每个采样周期内，系统状态的变化范围足够小，并且f和h在这一点附近近似一条直线，那么EKF是可靠的。反之，如果状态在短时间内会发生大幅突变（比如剧烈机动目标、快速变化的姿态角），或者非线性函数在高曲率区域（比如距离极近、角度接近±90度），EKF就很容易翻车。

还有一个同样重要的点：EKF默认噪声是高斯的。如果过程噪声或观测噪声是重尾分布、多峰分布，即使线性化做对了，结果也只是“勉强能用”，因为高斯假设本身就错了。这时候应该考虑的不是EKF，而是粒子滤波这类非参数方法。

3. Matlab实现：一个测距测角目标跟踪EKF的完整闭环

3.1 仿真场景设计：目标运动与传感器模型

为了让代码不绕弯子，我选一个能直接体现EKF价值的场景：雷达位于原点，目标在二维平面做匀速直线运动，传感器只能输出极坐标系下的距离r和方位角θ，测量噪声是高斯白噪声。目标实际位置用笛卡尔坐标生成，但EKF只能通过距离和角度去反推。

这里状态方程是线性的，观测方程是非线性的，正好可以只聚焦观测阶段的雅可比计算。如果你要同时验证过程模型非线性的情况，把代码里的恒速模型换成单摆或CT模型即可，公式我上一节已经给出来了。

仿真参数如下：

角度噪声必须用弧度参与计算，1°≈0.0175 rad，很多人在仿真里直接填1，结果角度噪声被放大了57倍，这是常见的调参失误。后面第4章我会专门讲这个问题。

3.2 EKF主循环代码和关键细节

下面是完整的Matlab实现，我故意分成了“真实数据生成”“EKF初始化和主循环”“结果可视化”三块，方便你拆开测试。

%% 1. 真实轨迹生成（匀速直线模型） dt = 0.1; T = 0:dt:100; N = length(T); xTrue = zeros(4, N); xTrue(:, 1) = [50; 30; 2; 1]; F = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; for k = 1:N-1 xTrue(:, k+1) = F * xTrue(:, k); end %% 2. 观测数据生成（距离 + 方位角） sigma_r = 1.0; sigma_theta = deg2rad(1.0); R = diag([sigma_r^2, sigma_theta^2]); z = zeros(2, N); for k = 1:N px = xTrue(1, k); py = xTrue(2, k); z(1, k) = sqrt(px^2 + py^2) + sigma_r * randn; z(2, k) = atan2(py, px) + sigma_theta * randn; end

%% 3. EKF初始化 xEKF = zeros(4, N); xEKF(:, 1) = [40; 40; 0; 0]; % 初始状态不精确也没关系 P = diag([100, 100, 10, 10]); % 初始协方差，数量级和状态量匹配 Q = diag([0.1, 0.1, 0.05, 0.05]); % 过程噪声，先给一个适中的值 for k = 2:N % ---- 预测 ---- xPred = F * xEKF(:, k-1); PPred = F * P * F' + Q; % ---- 观测雅可比 ---- px = xPred(1); py = xPred(2); rPred = sqrt(px^2 + py^2); H = [px/rPred, py/rPred, 0, 0; -py/(rPred^2), px/(rPred^2), 0, 0]; % ---- 更新 ---- zPred = [rPred; atan2(py, px)]; y = z(:, k) - zPred; y(2) = mod(y(2) + pi, 2*pi) - pi; % 角度残差归一化到 [-pi, pi] S = H * PPred * H' + R; K = PPred * H' / S; xEKF(:, k) = xPred + K * y; % Joseph形式稳定协方差更新 I4 = eye(4); P = (I4 - K*H) * PPred * (I4 - K*H)' + K * R * K'; end

这段代码里有两个细节值得单独说。第一，xPred是用常数F算的，因为匀速直线模型是线性的；但如果换成第2章的单摆模型，这里就得改成手动调用非线性函数并重算F_Jac。第二，协方差更新我用了Joseph形式，不是朴素的P = (I - K*H)*PPred。Joseph形式计算量稍大，但能保证P矩阵在数值上保持对称半正定，这一步在EKF长时间运行、状态维度较高时非常关键。

%% 4. 结果可视化 t = 1:N; figure; subplot(2,1,1); plot(xTrue(1,:), xTrue(2,:), 'k-', 'LineWidth', 1.5); hold on; plot(xEKF(1,:), xEKF(2,:), 'r--', 'LineWidth', 1.5); xlabel('px (m)'); ylabel('py (m)'); legend('真实轨迹','EKF估计轨迹'); title('目标跟踪轨迹对比'); grid on; subplot(2,1,2); posErr = sqrt((xTrue(1,:)-xEKF(1,:)).^2 + (xTrue(2,:)-xEKF(2,:)).^2); plot(t*dt, posErr, 'b-', 'LineWidth', 1.2); xlabel('时间 (s)'); ylabel('位置误差 (m)'); title('EKF位置误差随时间变化'); grid on;

我用Matlab R2023b跑过这段代码，没有调用任何工具箱，基础版就能运行。新版本Matlab下mod、atan2、randn都是基础函数，不用担心兼容性。

3.3 结果怎么看：误差曲线和残差才是试金石

跑完这段仿真，最直接的结果是位置误差曲线会在前几秒快速下降，之后收敛到一个稳定水平。如果初始位置给得离谱，前期误差波形会稍微大一点，但EKF的反馈修正能让它拉回来，这正是卡尔曼滤波的纠错能力。

还有一个更值得关注的指标是残差（innovation）序列。在EKF正常工作时，新息应当是零均值、白噪声性质的，不会持续偏正或偏负。我习惯把y(1)和y(2)单独画出来看：如果距离残差一直为正，说明模型假设有偏置，最可能是目标速度估计错了；如果角度残差偶尔出现跳变，大概率是角度绕圈问题没处理好。别小看这几个残差，调EKF时它们比状态估计曲线更诚实，滤波器内部的问题会先反映在残差统计上。

4. 调试EKF最容易翻车的几个点：我的完整排查链路

4.1 雅可比矩阵写错是头号死因：用数值差分验证

雅可比矩阵错误有个特点：状态估计可能在仿真前几秒还算正常，随后逐渐漂移，或者P矩阵异常缩小，看起来一切“收敛”了但结果完全不对。我遇到过不止一次，最后定位都是用数值差分找到的。

在Matlab里写一个通用数值雅可比函数非常容易：

function numJ = numJacobian(h, x, epsVal) n = length(x); h0 = h(x); numJ = zeros(length(h0), n); if nargin < 3 epsVal = 1e-6; end for i = 1:n xp = x; xm = x; xp(i) = xp(i) + epsVal; xm(i) = xm(i) - epsVal; numJ(:, i) = (h(xp) - h(xm)) / (2*epsVal); end end

然后假设你有一个观测函数hRangeBearing(x)，可以比较手推的H和numJacobian算出来的结果。注意eps的选择：太大会引入截断误差，太小会受浮点精度影响，1e-6对大多数位置量级是安全的。

如果你发现数值雅可比和解析雅可比在某个区域差异很大，大概率是手推漏项了。这也是我建议的调试顺序：先验证雅可比，再调噪声矩阵，否则后边所有参数调整都是在错误地基上糊墙。

4.2 Q、R、P0的工程设置思路：别再说调参靠玄学

很多教程把Q、R的调参说成“根据需要调整”，等于没说。我实际用下来，这三个矩阵是有逻辑可循的。

R矩阵代表传感器测量噪声，这是最不该“拍脑袋”的矩阵。如果你手头有一批静态观测数据，直接算样本方差填入R即可；如果没有，去查传感器手册里的精度指标，比如“距离精度1m（1σ）”“角度精度1°（1σ）”，换算成方差填入R。值得提醒的是角度量必须统一到弧度，填角度制的方差会让滤波器对角度的信任程度出现几十倍的偏差，效果就是你看到的轨迹震荡。

Q矩阵代表过程模型对真实系统的不确定性。它没有R那么直接，但工程上可从“模型误差的量级”估算。比如匀速直线模型，目标实际可能有轻微加减速，那么过程噪声的方差可以按预期的加速度波动来折算。固定时间步dt下，如果模型状态是位置和速度，那么加速度扰动a对位置和速度的影响分别是0.5adt^2和a*dt，所以Q的对应元素可以这样估算。一个方便的经验是，先设置一个让你觉得“噪声不太大”的Q，观察残差，如果残差均值非零且逐渐扩大，说明Q偏小、模型误差没有被充分吸收。如果轨迹跟随噪声过大，再适当减小。

P0是初始协方差，表示你对初始状态的不确定程度。如果初始位置误差大致在10m，那P0对应元素可以设100（方差）。P0设太小，滤波器会过早认为自己已经收敛，后续新观测改不动状态；P0设太大，前期状态估计会剧烈摆动。合理的做法是让P0和Q的数量级关系匹配，不要相差几十个数量级。

我习惯把这些参数按下面的表格来检查和调优：

4.3 滤波器发散时的分步排查方法

EKF发散是每个用它的工程师都会遇到的事。我的排查链路基本固定，按顺序走能省很多时间。

第一步，看残差序列。如果残差均值明显不为零，说明模型本身有偏，优先检查状态转移函数和观测函数是否正确。如果残差在某个时刻突然异常增大，优先看那个时刻发生了什么：是不是目标方位角接近±π导致atan2跳变？是不是距离接近0导致H矩阵元素爆炸？

第二步，检查角度归一化。方位角观测差很容易出现2π级别的跳变，如果不把残差归一化到[-π, π]，卡尔曼增益会把这种“假的大新息”当成真实误差，状态被瞬间拉飞。我代码里那行y(2) = mod(y(2)+pi, 2*pi) - pi就是为了处理这个问题。

第三步，检查P矩阵是否保持对称正定。如果用了朴素的P = (I - K*H)*PPred，数值误差可能让P失去对称性。改用Joseph形式，或者每次更新后执行P = 0.5*(P+P')强制对称。

第四步，检查R矩阵和Q矩阵的数量级是否匹配。一个非常常见的案例：用户觉得自己“调好了R”，其实角度标准差填的是度数而非弧度，导致EKF对角度观测的信任度被严重低估，滤波器几乎不更新角度信息，轨迹自然发散。

最后再说一个我自己的真实教训：有一次仿真，目标离雷达很近，大概两三米，我去掉了距离噪声，只保留角度噪声，结果滤波器在目标经过雷达正下方时崩溃。原因就是距离接近零时，方位角雅可比的分母r^2太小，H矩阵数值爆炸。遇到这种靠近奇点的场景，要么保证距离足够远，要么在雅可比计算里加一个最小距离阈值。这个细节写代码时不容易想到，但实际飞行器、车辆经过雷达附近时一定会遇到。

5. EKF扛不住的时候该怎么办：与UKF、粒子滤波的选型比较

5.1 EKF的三类失效场景

把EKF用熟之后，你应该能分辨什么时候该换工具了。根据我自己的项目经验，以下三类场景最容易让EKF失效。

一是强非线性系统。比如目标做急转弯，状态方程和观测方程在一个采样周期内非线性程度很高，一阶泰勒展开的误差已经大到无法忽略。此时EKF的估计可能发散，即使P矩阵看起来正常，实际位置误差也很大。

二是初始误差过大。EKF的线性化点依赖于当前的估计值，如果初始估计离真值太远，线性化点一开始就是错的，滤波器可能永远无法收敛到真值。这类问题在纯方位跟踪（bearing-only tracking）里尤其明显。

三是非高斯噪声。EKF从KF继承了对高斯分布的假设，如果传感器噪声存在粗大误差（野值）、多峰分布或重尾特性，EKF的输出就不是最优估计。这时候与其费劲调Q、R，不如换用对分布不做强假设的粒子滤波。

5.2 UKF比EKF强在哪里，代价是什么

无迹卡尔曼滤波（UKF）是EKF最直接的升级选项。它不对方程做泰勒展开，而是选取一组sigma点，把这组点通过真实的非线性函数传播，再从传播后的点中重构均值和协方差。本质上是“用样本点的传播来近似分布”，而不是“用一阶导数来近似函数”。

这意味着UKF不需要解析求雅可比矩阵，对强非线性的鲁棒性明显更好，而且对于高斯分布，UKF的近似精度可以达到三阶，优于EKF的一阶。代价是计算量更大：状态维度n下，UKF每次需要生成2n+1个sigma点，每个点都要调用一次非线性函数。如果你的系统状态维度只有4维，多算9次函数几乎可以忽略；但如果你做高维状态估计（比如50维以上的组合导航），sigma点的数量会急剧膨胀，这时候又得回头考虑EKF或者降维方案。

另一个常被忽略的问题是，UKF也需要设置sigma点相关的参数，比如α、β、κ。虽然这些参数相对不敏感，但也存在“换了参数结果差异很大”的情况。

5.3 工程选型时的个人建议

我现在的选型习惯是这样的：先评估系统非线性的强度和计算资源。如果状态维度不高、非线性函数平滑、采样周期足够小，优先用EKF，因为代码简单、调试链路成熟、Matlab里文档多。如果发现EKF参数怎么调都发散，或者系统里有明显的强非线性（比如大角度姿态变化、近距离测距），直接换UKF，省去反复调雅的痛苦。如果噪声明显非高斯、存在大量野值，那就别在卡尔曼框架里挣扎了，上粒子滤波，同时在重采样策略上多花功夫。

5.4 最后一条实在话：先把R矩阵标定准确

如果非要说一个最影响EKF工程落地质量的参数，我会把票投给R矩阵。原因很简单：R是唯一能从传感器数据直接标定的矩阵，Q和P0多少带点模型主观性，而R是物理量。

我刚接触EKF时，习惯在仿真里随手填R，觉得“反正都是噪声”。后来在真实传感器数据上做实验，发现静态测量数据算出来的R和仿真值能差一个量级，直接导致滤波器在真实场景下表现糟糕。从那以后，我的流程变成：拿到新传感器，先采集几百组静态数据，算方差填R；再用仿真数据确认滤波器基本行为；最后接入真实数据微调Q。这个顺序倒过来，你可能要在参数里熬好几天。

对于还在学EKF的朋友，我的建议是先把这篇文章里的仿真复现跑通，然后把角度噪声标准差改成度数再跑一次，亲眼看看轨迹为什么会散架，再改回来。这样比背十遍公式都管用。