第一次用线性卡尔曼滤波做目标跟踪的时候我踩过一个很典型的坑目标明明在匀速直线运动但一旦把雷达观测换成距离和方位角滤波器就开始发疯估计轨迹绕着真值来回震荡甚至直接飞出屏幕。后来才反应过来问题不在算法实现而在“线性”这两个字——真实世界里几乎没有多少系统是严格线性的雷达测的是斜距和方位角卫星定轨里有与位置三次方成反比的摄动力电池SOC估计中开路电压和SOC是一条带滞回的非线性曲线。这些事情线性卡尔曼滤波KF根本接不住。扩展卡尔曼滤波Extended Kalman FilterEKF就是为这种情况准备的它把非线性的状态方程和观测方程在当前工作点附近做一阶泰勒展开得到雅可比矩阵然后用标准的卡尔曼滤波框架继续递推。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 turnCT目标位置更新里包含三角函数转角和速度还耦合在公式里。再比如摆系统角度导数等于角速度是线性的但角速度导数里有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_kz_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 RK 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模型即可公式我上一节已经给出来了。仿真参数如下参数值含义dt0.1 s采样周期T100 s仿真时长初始位置[50, 30] m目标起点初始速度[2, 1] m/s目标匀速速度σ_r1.0 m距离观测噪声标准差σ_θ1.0°角度观测噪声标准差角度噪声必须用弧度参与计算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(:, k1) 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即可如果没有去查传感器手册里的精度指标比如“距离精度1m1σ”“角度精度1°1σ”换算成方差填入R。值得提醒的是角度量必须统一到弧度填角度制的方差会让滤波器对角度的信任程度出现几十倍的偏差效果就是你看到的轨迹震荡。Q矩阵代表过程模型对真实系统的不确定性。它没有R那么直接但工程上可从“模型误差的量级”估算。比如匀速直线模型目标实际可能有轻微加减速那么过程噪声的方差可以按预期的加速度波动来折算。固定时间步dt下如果模型状态是位置和速度那么加速度扰动a对位置和速度的影响分别是0.5adt^2和a*dt所以Q的对应元素可以这样估算。一个方便的经验是先设置一个让你觉得“噪声不太大”的Q观察残差如果残差均值非零且逐渐扩大说明Q偏小、模型误差没有被充分吸收。如果轨迹跟随噪声过大再适当减小。P0是初始协方差表示你对初始状态的不确定程度。如果初始位置误差大致在10m那P0对应元素可以设100方差。P0设太小滤波器会过早认为自己已经收敛后续新观测改不动状态P0设太大前期状态估计会剧烈摆动。合理的做法是让P0和Q的数量级关系匹配不要相差几十个数量级。我习惯把这些参数按下面的表格来检查和调优矩阵作用相对可靠的设定方法过大/过小的典型表现R观测噪声静态数据样本方差、传感器手册指标过小状态跟随噪声抖动过大响应迟钝、误差大Q过程噪声按模型误差量级和dt换算过小残差持续偏置、滤波“过于自信”过大噪声淹没信号P0初始不确定度按初始误差平方估算过小收敛慢、更新不动过大前期震荡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*(PP)强制对称。第四步检查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每次需要生成2n1个sigma点每个点都要调用一次非线性函数。如果你的系统状态维度只有4维多算9次函数几乎可以忽略但如果你做高维状态估计比如50维以上的组合导航sigma点的数量会急剧膨胀这时候又得回头考虑EKF或者降维方案。另一个常被忽略的问题是UKF也需要设置sigma点相关的参数比如α、β、κ。虽然这些参数相对不敏感但也存在“换了参数结果差异很大”的情况。5.3 工程选型时的个人建议我现在的选型习惯是这样的先评估系统非线性的强度和计算资源。如果状态维度不高、非线性函数平滑、采样周期足够小优先用EKF因为代码简单、调试链路成熟、Matlab里文档多。如果发现EKF参数怎么调都发散或者系统里有明显的强非线性比如大角度姿态变化、近距离测距直接换UKF省去反复调雅的痛苦。如果噪声明显非高斯、存在大量野值那就别在卡尔曼框架里挣扎了上粒子滤波同时在重采样策略上多花功夫。滤波器适用场景计算复杂度主要限制EKF弱非线性、高斯噪声、状态维度适中低只需每次算雅可比强非线性易发散需要解析/数值雅可比UKF中强非线性、高斯噪声占主导中2n1个sigma点传播高维状态时sigma点数量大参数需核实PF任意非线性、非高斯噪声高粒子数量和维度指数相关粒子退化、重采样开销大、实时性差5.4 最后一条实在话先把R矩阵标定准确如果非要说一个最影响EKF工程落地质量的参数我会把票投给R矩阵。原因很简单R是唯一能从传感器数据直接标定的矩阵Q和P0多少带点模型主观性而R是物理量。我刚接触EKF时习惯在仿真里随手填R觉得“反正都是噪声”。后来在真实传感器数据上做实验发现静态测量数据算出来的R和仿真值能差一个量级直接导致滤波器在真实场景下表现糟糕。从那以后我的流程变成拿到新传感器先采集几百组静态数据算方差填R再用仿真数据确认滤波器基本行为最后接入真实数据微调Q。这个顺序倒过来你可能要在参数里熬好几天。对于还在学EKF的朋友我的建议是先把这篇文章里的仿真复现跑通然后把角度噪声标准差改成度数再跑一次亲眼看看轨迹为什么会散架再改回来。这样比背十遍公式都管用。