前段时间在帮一个同学看目标跟踪的课程作业他整整一晚都耗在 MATLAB 里面最后发给我一张锯齿状乱跳的滤波曲线。问题非常典型他拿标准卡尔曼滤波去处理雷达的距离、方位角量测而量测方程里带了个 atan2。线性滤波器和非线性量测模型硬凑在一起结果当然不忍直视。后来我给他改成一个最简单的扩展卡尔曼滤波EKF仿真核心循环也就几十行效果立刻正常。这篇文章就是我把那次调试过程重新复盘后整理出来的内容适合刚接触滤波、想弄懂 EKF 原理、又希望在 MATLAB 里亲手跑通一个完整仿真的同学。1. 从普通卡尔曼到EKF非线性量测是怎么逼着我们改方案的1.1 线性KF的舒适区高斯经过线性变换还是高斯标准卡尔曼滤波之所以形式漂亮前提条件是整个系统“线性高斯”。状态转移方程和量测方程都必须是线性的也就是写成矩阵乘法的形式比如x_k F * x_{k-1} w、z_k H * x_k v。这句话看着简单但很多人一开始没意识到它的分量有多重。高斯分布有一个很漂亮的性质一个高斯随机变量经过线性变换后仍然是高斯分布。正因为这个“形状不变”的性质卡尔曼滤波里用均值和协方差这两个参数就可以完整描述状态分布预测和更新过程中的矩阵运算也都能精确推导出来。这相当于你在一条直道上开车方向和速度都稳定用一个简单的线性模型就能长时间准确预测你下一时刻在哪。麻烦在于现实世界里的量测几乎很少是纯线性的。目标距离、方位角、图像坐标、卫星伪距这些观测量和状态之间天生就是非线性关系。一旦非线性进入系统高斯分布经过非线性函数之后就不再是高斯了可能变得偏斜、长尾甚至出现多峰。这时候你还硬要用均值加协方差去描述信息就已经丢了卡尔曼滤波那套精确公式自然不再成立。1.2 一个atan2毁掉的高斯假设很多同学踩坑都是从这个地方开始的状态还是老样子位置和速度但量测从直角坐标变成了距离和方位角。距离量测是r sqrt(px^2 py^2)方位角量测是theta atan2(py, px)。这两个表达式里面带了开方、反正切都不是线性函数。你看第一行公式好像还凑合真正让滤波崩溃的是 atan2。当目标在空间里移动时方位角的变化速度并不是均匀的离原点远的时候角度变化慢离原点近的时候角度变化极快如果目标从原点附近掠过角度可能瞬间跳变上百个角度。这种强非线性关系下高斯噪声经过 atan2 之后分布会被明显扭曲再用一个高斯分布去近似误差会越来越大。更直接的说法是你把 atan2 塞进线性卡尔曼滤波的量测更新里等式左右两边的单位、维度、物理意义全都对不上滤波器的校正量是靠量测新息乘上增益算出来的而这个新息在非线性量测下会被严重放大或压缩。结果就是滤波估计值来回摆动甚至直接发散。我那位同学跑出的锯齿状曲线就是这种“半线性半非线性”混搭的典型症状。1.3 泰勒展开在估计点附近把曲线“掰直”EKF 的思路说起来其实很朴素既然非线性函数本身没法直接处理那我就在当前估计点附近把非线性函数做一阶泰勒展开用切线代替原来的曲线。只要非线性程度不是特别剧烈而且滤波误差不是太大在估计点附近这个小范围内用切线近似原函数是足够可靠的。数学上就是h(x) ≈ h(x_pred) H * (x - x_pred)其中 H 是个雅可比矩阵里面放的是 h 对状态各分量的偏导数。这个 H 在 EKF 里替代了标准卡尔曼滤波中的量测矩阵 H后续增益计算、协方差更新的形式和标准 KF 完全一致。换句话说EKF 做的就是把非线性问题在当前点局部线性化然后再套用卡尔曼滤波的完整框架。这里有个非常关键的操作细节雅可比矩阵必须在预测值x_pred处计算而不是在真值、更不是在上一时刻状态处计算。因为你实际能拿到的最新状态估计就是预测值滤波器在这里做线性化才是自洽的。很多人写代码时把 H 写成常数矩阵或者放到初始化阶段算一次就不管了这种做法在简单场景可能还能勉强跑一旦目标运动范围变大线性化点早已跑远误差就会迅速积蓄起来。2. 建模先行状态方程、量测方程和坐标系里的坑2.1 为什么选“匀速直线距离/方位角”这个组合在做仿真之前要先定模型。我这里选的是二维平面内的匀速直线运动模型量测是距离和方位角。这个组合在课程作业和工程入门里出现频率极高原因也简单状态方程是线性的量测方程是非线性的正好能体现 EKF 相对标准 KF 的价值又不至于一上来就被强烈的模型非线性折磨到没法调试。这种模型对应雷达、激光雷达、声呐等一大批真实传感器场景。传感器放在原点测量目标相对自己的距离和角度这是很常见的观测方式。比起直接量测px和py的传感器距离角度量测更贴近工程实际也更有代表性。状态变量我取四维x [px, py, vx, vy]^T分别代表 x 轴位置、y 轴位置、x 轴速度、y 轴速度。匀速直线运动的意思是速度向量不变位置按速度乘以时间步长往前推。之所以不上带加速度的模型是因为这个 demo 的核心是让你理解 EKF 的量测更新过程模型越简单问题越容易暴露。2.2 F、G、Q、R四个矩阵各自管什么状态转移矩阵 F 负责描述模型怎么“猜”下一时刻状态。在匀速直线模型里如果时间步长是 dt那么 F 写成矩阵就是F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];位置加上速度乘时间速度保持不变。这个 F 是线性部分所以不需要做任何线性化。接下来是 G 和 Q。G 是过程噪声输入矩阵它定义了随机扰动是怎么作用到状态上的。我这里用连续白噪声模型离散化后得到的标准形式G [dt^2/2 0; 0 dt^2/2; dt 0; 0 dt]; Q q * (G * G);式子里的 q 是自己调的系数可以理解为“目标偏离匀速直线运动强度的加速度噪声功率谱密度”。为什么需要过程噪声因为真实目标不可能完全匀速哪怕只是轻微转弯或风速扰动模型也会有误差。过程噪声 Q 就是给滤波器一个缓冲让它不要过于相信模型预测。R 是量测噪声协方差矩阵这里直接取距离噪声方差和角度噪声方差。传感器标称精度一般都会给标准差比如距离标准差 5 米、角度标准差 2 度那就把标准差平方后放到 R 的对角线上。R 代表你对量测的信任程度R 越小滤波器越相信量测增益会相应变大。2.3 角度单位与初始状态最容易被忽略的细节角度单位这个问题看着小炸起代码来毫不含糊。MATLAB 里的 atan2 返回的是弧度所以角度噪声标准差也必须换算成弧度。我见过不少代码里把“2 度”直接写进 R 矩阵结果距离量的方差量级和角度方差量级差得天壤地别滤波器里的量纲直接乱套输出曲线一团糟。还有一个隐蔽问题是角度回绕。真实方位角在 -180 度和 180 度附近时微小的真实变化可能让测量值从 179 度跳到 -179 度新息算出来是 -358 度滤波器以为目标突然转了将近一整圈。EKF 里必须有“角度回绕处理”常见做法是把角度差换算到 [-pi, pi] 区间内。这个细节不做滤波结果就会突刺频出。初始化同样值得注意。位置初值最好用第一帧量测反推也就是把第一帧距离和方位角转成直角坐标。速度初值给 0 是一个合理的保守选择因为一开始确实不知道目标速度。初始协方差 P 要反映这种“不确定性”位置的不确定性可以从量测噪声推导速度的不确定性则要设得大一些否则滤波一开始就会过度自信后面很难校正过来。3. MATLAB仿真主框架生成真值、量测和滤波记录3.1 先用固定随机种子造一条“真实轨迹”仿真第一步是造真值。我们需要一条完全确定、没有噪声的轨迹然后在上面叠加量测噪声。这样后续才有“标准答案”用来计算误差否则滤波好坏根本没法评判。我这里设时间步长dt 0.1秒仿真时长 30 秒目标从原点出发x 方向和 y 方向速度都是 10 米每秒。这样一条轨迹足够长又不会因为转弯等运动让模型复杂化。为了让结果可复现我会在开头固定随机种子clear; clc; close all; rng(42); dt 0.1; T 30; N round(T / dt); tVec (0:N-1) * dt; X_true zeros(4, N); X_true(:, 1) [0; 0; 10; 10]; for k 2:N F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; X_true(:, k) F * X_true(:, k-1); end sigma_r 5.0; sigma_theta 2 * pi / 180; R diag([sigma_r^2, sigma_theta^2]); Z zeros(2, N); for k 1:N r sqrt(X_true(1,k)^2 X_true(2,k)^2); theta atan2(X_true(2,k), X_true(1,k)); Z(1,k) r sigma_r * randn; Z(2,k) theta sigma_theta * randn; end这里的量测噪声我设的是高斯白噪声这符合 EKF 的基本假设。如果你手头的传感器噪声其实是重尾分布那 EKF 的效果会打折扣不过那是后话初学阶段先把标准情况跑通最重要。3.2 滤波器初始化从第一帧量测反推初值初值往往决定着你滤波前几秒会不会“抽风”。很多教材直接告诉你初始状态设成什么却不解释为什么导致遇到新问题就不知道从哪里下手。我的做法是把第一帧量测当成唯一可信的信息源用它反推位置。x_est zeros(4, N); P_est zeros(4, 4, N); r0 Z(1, 1); theta0 Z(2, 1); x_est(:, 1) [r0 * cos(theta0); r0 * sin(theta0); 0; 0]; P_est(:, :, 1) diag([sigma_r^2, sigma_r^2, 20, 20]);位置方向的不确定度和量测噪声一致取sigma_r^2作为方差速度方向完全未知所以我把方差设成 20 而不是更小的数。如果第一帧量测噪声特别大位置初值会偏一点但较大的 P 会让滤波器在后续量测到来后快速修正。反而是那些“拍脑袋给初值又把 P 设得特别小”的做法会让滤波器困在错误的初始估计里出不来。3.3 主循环加记录预测更新只占十几行真正的 EKF 主循环不长核心就四步预测状态、预测协方差、计算增益、更新状态和协方差。下面这段代码是完整的滤波循环q 0.5; G [dt^2/2 0; 0 dt^2/2; dt 0; 0 dt]; Q q * (G * G); for k 2:N % 1. 状态预测 F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; x_pred F * x_est(:, k-1); P_pred F * P_est(:, :, k-1) * F Q; % 2. 量测预测与雅可比矩阵 px x_pred(1); py x_pred(2); r_pred sqrt(px^2 py^2); theta_pred atan2(py, px); H [px/r_pred, py/r_pred, 0, 0; -py/(r_pred^2), px/(r_pred^2), 0, 0]; z_pred [r_pred; theta_pred]; % 3. 卡尔曼增益 S H * P_pred * H R; K P_pred * H / S; % 4. 更新 innovation Z(:, k) - z_pred; innovation(2) atan2(sin(innovation(2)), cos(innovation(2))); x_est(:, k) x_pred K * innovation; P_est(:, :, k) (eye(4) - K * H) * P_pred; end主循环写完之后剩下的就是把x_est和X_true一起画出来再算误差。这个框架非常通用后面想换模型、换传感器基本只需要改动 F 和 H 这两处循环结构不用大改。4. 核心代码逐段拆解H矩阵、增益和协方差更新4.1 雅可比矩阵H写法不难难在不写错雅可比矩阵是整个 EKF 里最容易写错又最难排查的地方。这个例子里量测是两个分量状态是四个分量所以 H 是 2 行 4 列。第一行对应距离量测对状态的偏导第二行对应方位角量测对状态的偏导。距离量测的偏导相对直观d(r)/d(px) px / r d(r)/d(py) py / r方位角量测的偏导则来自 atan2 的导数公式d(theta)/d(px) -py / r^2 d(theta)/d(py) px / r^2注意分母里那个 r 的平方。有些同学会把第二行写成-py/r加px/r那错的就太远了。量纲检查是个好办法距离对位置求导后应该是无量纲量而角度对位置求导后的量纲应该是“1/长度”。你写完 H 以后先看一眼量纲是否合理能提前排掉一半错误。还有一个实务细节当r_pred非常接近 0 时雅可比会变成很大的数甚至无穷大。在真值轨迹里我可以让目标远离原点但真实场景中目标确实可能穿越传感器正下方附近。遇到这种情况要么在雅可比计算中加一个小量防止除零要么改用其他坐标系去描述量测。初学者先记住这个坑至少知道异常数据是从哪来的。4.2 卡尔曼增益和新息角度差先做回绕处理卡尔曼增益的计算和标准 KF 完全一样先算新息协方差S H * P_pred * H R再用K P_pred * H / S算出增益。这一步的含义是“根据预测和量测的不确定度给两者分配权重”。预测协方差越大增益越偏向量测量测噪声越大增益越偏向预测。真正需要额外处理的只有角度量测的新息。MATLAB 里atan2(sin(innovation(2)), cos(innovation(2)))这行代码做的事就是角度回绕。它把角度差压到 [-pi, pi] 范围内避免因为跨越 ±180 度边界而产生一个荒谬的大新息。我曾经在一个项目里漏掉这行处理滤波效果一会好一会坏找了两天才发现是角度跨边界问题。这里也要强调增益 K 是个矩阵它在更新状态和状态误差协方差时起到“分配校正量”的作用。状态更新的公式是x_est x_pred K * innovation每个状态分量被校正的量不是独立的而是通过 K 关联起来。因为距离和方位角量测都同时携带位置信息甚至对角速度方向也有约束所以 K 矩阵会自动把这些信息揉进四个状态分量的修正里。4.3 协方差更新简单版和Joseph形式怎么选协方差更新写成P (eye(4) - K * H) * P_pred这是大多数教材里的标准写法。它在理论推导上是正确的而且在仿真的数值精度范围内基本够用。很多实际项目会改用 Joseph 形式P (eye(4) - K * H) * P_pred * (eye(4) - K * H) K * R * K;Joseph 形式的优势是数值稳定性更好协方差更不容易因为舍入误差而失去对称正定性。在单精度浮点、长时间递推或者矩阵条件数极差的场景下这个优势会被放大。但对于我们这个 MATLAB 仿真实例double 精度下简单版完全没问题。不过有件事比选哪种更新形式更重要协方差矩阵必须保持对称正定。如果你发现滤波后期 P 矩阵对角线出现负值通常不是协方差更新公式的问题而是前面的 H 或 Q、R 等矩阵填错了导致 P 被污染。先查雅可比和噪声矩阵不要一上来就怀疑数值稳定性。5. 仿真结果该怎么看RMSE、一致性和发散排查5.1 别只看RMSE还要看误差均值和P矩阵很多同学跑完滤波之后只算一个 RMSE数字小就说“效果好”数字大就说“滤波不好”这其实会掩盖很多问题。RMSE 是整体误差的平方平均再开方它对大误差特别敏感但看不出误差是否有偏。比如滤波结果一直在真值旁边偏 3 米RMSE 可能是 3 米左右但单独看每一帧误差均值也是 3 米这说明存在系统性偏差往往是模型误差或者初始化不对而不是随机噪声太大。我更习惯的做法是同时画三样东西位置误差曲线、误差均值与标准差、P 矩阵对角线开方后的 1-sigma 曲线。第三样尤其重要因为它能检验滤波器是否“自洽”。如果实际误差的标准差远大于 P 矩阵给出的标准差说明滤波器过于乐观也就是把不确定性低估了反过来如果实际误差远小于 P 矩阵描述的不确定性说明滤波器过于保守还有进一步调小的空间。在一个设计合理的 EKF 里滤波误差的大约 68% 应该落在 P 矩阵给出的 1-sigma 范围内95% 落在 2-sigma 范围内。这个“一致性”检验比单纯看 RMSE 要可靠得多它直接告诉你滤波器对自身不确定性的估计是否准确。5.2 发散自查清单从单位到雅可比的排查链路如果滤波发散了不要慌更不要盲目调大 Q。我一般按下面的链路逐项排查每步都能快速定位问题。症状可能原因排查方法位置估计严重偏移曲线呈爆炸状量测矩阵单位或量纲错误检查 R 矩阵里角度方差是否误用“度”误差曲线在某个时刻突然跳刺角度跨 ±180 度边界检查新息是否做了角度回绕滤波轨迹明显滞后真值过程噪声 Q 设置过小增大 q 后观察滞后是否改善滤波整体平滑但偏离真值较远初始化偏差大或 P0 过小重新从第一帧量测反推初值放大 P0增益 K 几乎为 0量测被忽略R 取值远大于真实噪声用传感器标称精度重新设定 R误差曲线围绕真值剧烈抖动过程噪声 Q 过大减小 q让滤波器更信任模型第一排查项永远是角度单位这是最低成本、最高收益的检查。第二项是雅可比你可以在真值附近手动算一次偏导数值和代码输出做对比。第三项才轮到 Q 和 R 的调参。顺序不能反否则你很可能把一个单位错误当成噪声问题反复调参调试一晚上都找不到根源。5.3 Q和R的调参原则我踩出来的几条经验Q 和 R 的调参确实有点像是在“凭感觉”但这个感觉背后有规律。我的习惯是先把 R 固定成传感器标称精度换算出来的值不去动它然后单独调整 q。因为 R 有明确的物理意义传感器说明书上写着什么精度你就用什么精度乱调 R 反而会让结果失去可信度。Q 的初始值我会给得比较小比如 q0.01然后观察滤波轨迹的平滑程度。如果轨迹平滑但真值变化时滤波器跟不上误差在每次机动后明显变大说明过程噪声低估需要增大 q。如果轨迹抖得厉害每一帧都紧紧贴着量测说明过程噪声高估需要减小 q。调到“贴合但不抖动”的状态基本就是比较合理的位置。有些同学会把 q 当成万能旋钮滤波效果不好就调大一点直到看起来不错。这种做法很容易把滤波器调到“过拟合”某一个特定仿真轨迹。我的建议是固定随机种子调好参数后换一个不同的运动模式或者换一组随机种子再跑一次这样才能确认参数不是只在一条轨迹上有效。6. 仿真之外这个模型还能改造成什么6.1 升级成UKF或粒子滤波的必要性判断EKF 的一阶线性化在某些场景下会明显受限。如果量测方程非线性程度很强比如距离量测的噪声本身就大或者目标距离传感器非常近导致角度变化剧烈那么一阶泰勒展开可能造成较大的截断误差。这时候可以考虑无迹卡尔曼滤波UKF或者粒子滤波但不是所有场景都值得升级。我的判断标准是先跑 EKF观察误差一致性和滤波是否经常发散。如果发散频繁或者 1-sigma 曲线和实际误差差得很远再考虑换方法。如果只是偶尔某个时段误差偏大往往先调参就能解决没必要立刻上更复杂的算法。升级到 UKF 不是要重写整个框架主循环结构仍然是“预测-更新”区别只在于用一组 Sigma 点去传递分布而不是靠雅可比矩阵做线性化。粒子滤波则适合强非线性、非高斯甚至多峰分布的场景但计算量增长很快。初学者没必要一步到位先把 EKF 跑透再横向对比其他滤波器才能真正理解各自的长处和短板。6.2 换传感器时只需要动哪几个地方最后说点实用的。如果你后续要在自己的项目里把这个 EKF 改造成别的传感器配置主循环不需要动需要改的只有四处量测函数 h、雅可比 H、量测噪声协方差 R 以及初始化时 P 的位置分量。比如把距离/方位角量测换成直角坐标量测那 H 就会变成简单的选择矩阵EKF 也就退化成了标准 KF。这时候你就更能体会 EKF 的通用性它把大量不同传感器模型统一到了同一个递推框架下。我个人现在的习惯是接任何新的滤波项目都先写一个最简 EKF 跑通全流程再根据效果决定要不要升级成更复杂的方法。这个习惯帮我避开了很多“一上来就上复杂算法最后不知道是建模错还是算法错”的坑。希望这套 MATLAB 仿真代码和调试思路也能让你少走一点我当时走过的弯路。