EKF、UKF与粒子滤波：非线性状态估计的实战对比与Matlab实现

从实际项目里第一次接触卡尔曼滤波到后来把EKF、UKF、粒子滤波挨个在Matlab里撸了一遍这个过程我走了不少弯路。最开始拿标准KF套一个强非线性系统发散到连曲线都画不出来折腾很久才明白问题的根源在哪儿。所以这次我不打算堆公式而是用一套完整的问题脉络——从非线性带来的困境出发到三种滤波算法的原理差异再到Matlab代码怎么落地、仿真结果怎么解读、实际选型怎么避坑——把这套状态估计的知识串起来。这篇文章适合两类读者一是刚入门状态估计想一次性搞懂EKF、UKF、PF到底是什么、怎么选的初学者二是已经在用其中某一种算法但不知道它在另外两种算法面前处于什么位置想横向对比后再优化方案的工程师。文章用的是我实际跑过的Matlab仿真例程代码片段可以直接拿来改。1. 一条从线性到非线性的分岔路为什么标准卡尔曼滤波不够用任何接触过卡尔曼滤波的人最开始学的都是标准KF也就是线性卡尔曼滤波。它的数学前提非常干净系统是线性的、噪声是高斯的。在这个前提下KF可以给出解析的最优解——因为你输入一个高斯分布经过线性变换输出仍然是高斯分布均值和协方差刚好完整刻画了状态分布整个递推过程是自洽的。但现实世界几乎没有线性的便宜事。目标跟踪里的量测通常是距离和方位角转换到笛卡尔坐标系会产生非线性关系车辆沿圆弧运动、卫星轨道递推、无人机姿态解算状态方程和量测方程都是非线性函数。这时候一个尴尬的问题出现了高斯分布经过非线性函数映射之后就不再是高斯的了。你运算出来的东西实际是分布已经被扭曲之后的近似结果而标准KF从头到尾都假设它是高斯的推导自然失效。我当时在做的一个目标跟踪仿真就撞上了这个问题。状态方程里有角度旋转项量测方程是距离和方位角我一开始用标准KF线性化之后的状态转移矩阵硬算滤波器连续发散先验预测值和量测值差了好几个量级。后来才反应过来系统本身强非线性按线性模型递推误差协方差的传递在第一步就已经失真之后的滤波结果只会越来越离谱。解决非线性问题的思路粗略来看有三条路局部线性化把一个非线性函数在某个工作点附近展开成线性形式然后再套用KF的框架这就是EKF。确定性采样近似用一组精心选择的点sigma点去逼近状态分布让这些点通过非线性函数再统计输出的均值和协方差这就是UKF。随机采样近似直接用大量随机粒子去近似后验分布不用管分布长什么样这就是粒子滤波PF。这三种思路本质上都是在回答同一个问题高斯分布或者任意分布经过非线性变换之后我该怎么去近似它的统计特性。只是近似的手段不一样一个靠泰勒展开一个靠点集映射一个靠蒙特卡洛采样。从工程应用的角度来说这三者没有绝对的谁替代谁只有谁更适合当前系统的非线性强度、实时性要求和噪声特性。后面几节我会把这三种方法的原理逻辑和Matlab实现放在一起对比看它们在同一个仿真任务里各自表现如何。2. EKF、UKF、PF三分天下核心思想与数学本质这一节把三种算法掰开揉碎只看本质。公式我会控制在能讲清楚问题的范围内重点讲清每种方法在解决非线性传播时到底做了什么手脚因为这个理解直接决定你在工程里怎么调参、怎么判断结果是否可信。2.1 EKF在均值点做一阶泰勒展开简单但代价隐蔽EKF的基本想法非常直接既然非线性函数不好处理那我在当前状态估计值附近把它做一阶泰勒展开取线性主部这样整个系统又重新变成线性的KF那一套直接搬过来用。设非线性状态方程和量测方程为x(k) f(x(k-1)) w(k) z(k) h(x(k)) v(k)EKF需要在每一步迭代中计算两个雅可比矩阵状态转移矩阵的雅可比 F(k-1) ∂f/∂x 在 x_hat(k-1) 处取值量测矩阵的雅可比 H(k) ∂h/∂x 在 x_pred(k) 处取值然后代入标准KF的预测和更新公式。看起来只是多了两个求导步骤但实际上问题不少。第一高阶截断误差。一阶泰勒展开相当于假设非线性函数在工作点附近可以用切线近似如果系统强非线性这个近似的误差协方差传递会明显偏离真实值甚至导致滤波器发散。第二雅可比矩阵的推导极其容易出错。我当初做一个机械臂关节角估计量测方程里有三角函数和反三角函数的复合每次手推雅可比都要花很长时间而且只要推导错一个偏导项仿真结果就是一团乱麻。提示EKF里面很多发散问题根源不在滤波框架本身而在你手算的雅可比矩阵错了。常见的验证方法是先单独用符号工具箱对f和h求偏导再用数值差分方式在随机点上对比符号解和数值解确认一致后再进滤波循环。从精度角度说EKF只保留到一阶所以在所有需要“近似高斯分布非线性变换”的算法里它的理论精度是最低的。但它的优势也明显计算量最小代码结构最接近标准KF而且很多工业部门的老代码里已经积累了大量成熟的EKF模块短期内不会淘汰。2.2 UKF不求导用一组sigma点完成分布传播UKF的切入角度很有意思。它不直接去像泰勒展开那样“把一个函数近似成线性”而是换了个思路我与其去近似非线性函数不如去近似这个分布的传播过程。这个思路叫做无迹变换Unscented TransformUT。具体来说对于n维状态变量我按照一定规则选取2n1个sigma点这些点经过非线性函数映射之后再用加权统计的方式算出输出状态的均值和协方差。因为sigma点是从原分布的有代表性位置取出来的经过非线性变换后它们的分布就能近似逼近真实传播后的分布。sigma点的选取和权重计算是核心常见方式如下。设状态维度为 L取参数 α决定sigma点在均值附近的扩散程度通常为1e-3到1、β用于融入先验分布的峰度信息高斯分布取2、κ次级调节参数通常取0或3-L。复合缩放参数 λ 为λ α²(L κ) - L选择合适的参数后生成2L1个sigma点X⁰ x_mean Xⁱ x_mean (sqrt((Lλ)P))_i, i 1, ..., L Xⁱ x_mean - (sqrt((Lλ)P))_{i-L}, i L1, ..., 2L其中 (sqrt((Lλ)P))_i 表示矩阵 (Lλ)P 做Cholesky分解后的第 i 列。这些点经过f和h映射后再加权求均值和协方差。由于sigma点能够传递高阶信息UT变换理论上可以达到三阶精度高斯分布情形下远高于EKF的一阶精度。UKF相比EKF最大的好处是不需要求雅可比矩阵。你只需要把状态方程和量测方程当作黑盒把sigma点丢进去就行。这对复杂非线性系统、或是代码里无法给出解析导数的场景特别友好也就是工程上所谓的“无模型导数依赖”。我记得第一次在目标跟踪仿真里把EKF换成UKF的时候效果相当明显。同一套系统EKF的估计曲线总是滞后一拍而UKF基本能跟住真实轨迹RMSE大概能降低一截。之所以不直接上粒子滤波是因为当时对实时性的要求较高粒子滤波的计算量完全顶不住。2.3 PF用一堆随机粒子去逼近任何分布贝叶斯采样的集大成者EKF和UKF本质上仍然假设滤波过程中涉及的分布可以用高斯或者近似高斯来描述。这在实际场景里并不总是成立比如量测方程出现多峰分布、状态分布严重非对称的时候高斯假设本身就过了头。粒子滤波的思路则是不做任何分布形态假设直接用一组带权重的随机粒子从经验上逼近后验概率密度。粒子滤波的理论基础是贝叶斯重要性采样。核心过程大致如下初始化从先验分布 p(x0) 采样N个粒子权重均等。预测每个粒子通过状态方程传播一步得到新的粒子集合。更新利用量测值计算每个粒子的似然度再据此更新权重并归一化。重采样当有效粒子数过少时根据权重对粒子进行重采样淘汰低权重粒子、复制高权重粒子避免粒子退化。重复2-4步输出加权均值作为状态估计。PF的精髓在于只要有足够多的粒子它理论上能逼近任意复杂的后验分布不管是多峰还是非对称都不在话下。但代价也很明显计算量大、粒子退化问题、需要精心设计重采样策略。尤其粒子数一多每一步都要对所有粒子做一次状态递推和权重计算实时性很难保证。我在实际项目中接触PF主要是在室内定位的场景。RSSI信号强度在复杂环境里分布很不规则高斯假设根本不成立EKF和UKF都漂得厉害反倒是PF靠着大量粒子硬生生把位置给稳住。不过当时粒子数取5000每一帧都要几十毫秒计算只能在离线处理里用。3. Matlab代码拆解一维非线性系统的三种滤波实现原理讲清楚了接下来就是我会实际跑的仿真代码。先定义一个经典的强非线性标量系统状态方程和量测方程都有非线性项用来做三种算法的同台对比。系统模型x(k) 0.5*x(k-1) 2.5*x(k-1)/(1x(k-1)^2) 8*cos(1.2*(k-1)) w(k) z(k) x(k)^2/20 v(k)过程噪声 w(k) 和量测噪声 v(k) 均为零均值高斯白噪声方差分别为1。真实初始状态取0.1滤波初始估计设为1.0初始协方差P0设为1。这个系统在量测方程中存在平方项状态方程里也有分式非线性项非线性强度足够看出三类算法的差异。3.1 公共仿真框架一次循环跑三种算法搭建仿真框架时我习惯把每种滤波器的预测与更新步骤写在同一个循环里这样便于横向对比状态估计值和误差。代码结构大体如下% 参数设置 N 50; % 仿真步数 Q 1; % 过程噪声方差 R 1; % 量测噪声方差 x0_true 0.1; % 真实初始状态 x0_est 1.0; % 滤波初始估计 P0 1; % 初始协方差 % 生成真实状态与量测 x_true zeros(1, N); z zeros(1, N); x_true(1) x0_true; for k 2:N x_true(k) 0.5*x_true(k-1) 2.5*x_true(k-1)/(1x_true(k-1)^2) ... 8*cos(1.2*(k-1)) sqrt(Q)*randn; end for k 1:N z(k) x_true(k)^2/20 sqrt(R)*randn; end然后分别调用EKF、UKF、PF的主函数对同一组真实状态和量测做滤波。3.2 EKF实现雅可比矩阵的计算是主要工作量EKF实现中最关键的一段是求状态方程和量测方程的雅可比矩阵。对于这个标量系统状态方程 f(x) 0.5x 2.5x/(1x^2) 8cos(1.2(k-1))对x求偏导得F 0.5 2.5*(1 - x^2)/(1 x^2)^2量测方程 h(x) x^2/20对x求偏导得H x/10在预测和更新阶段分别将当前估计值代入即可% EKF预测 x_pred 0.5*x_est 2.5*x_est/(1x_est^2) 8*cos(1.2*(k-1)); F 0.5 2.5*(1 - x_est^2)/(1 x_est^2)^2; P_pred F * P_est * F Q; % EKF更新 H x_pred / 10; K P_pred * H / (H * P_pred * H R); x_est x_pred K * (z(k) - x_pred^2/20); P_est (1 - K * H) * P_pred;EKF的代码本身很短但它把最重要的精度负担转移到了你的偏导推导上。一旦系统维数升高、方程嵌套复杂这个“手推雅可比”的过程就会成为整个项目最耗时、最容易出错的部分。3.3 UKF实现sigma点生成与UT变换UKF的代码实现关键有两块一是生成sigma点二是对sigma点做非线性映射后的统计加权。先看sigma点生成function [X, Wm, Wc] sigmaPoints(x, P, alpha, beta, kappa) n numel(x); lambda alpha^2 * (n kappa) - n; % Cholesky分解保证矩阵正定 S chol((n lambda) * P, lower); X zeros(n, 2*n1); X(:, 1) x; for i 1:n X(:, i1) x S(:, i); X(:, in1) x - S(:, i); end Wm zeros(1, 2*n1); Wc zeros(1, 2*n1); Wm(1) lambda / (n lambda); Wc(1) Wm(1) (1 - alpha^2 beta); for i 2:2*n1 Wm(i) 1 / (2*(n lambda)); Wc(i) Wm(i); end end滤波主循环里先让每个sigma点通过状态方程传播再做加权统计得到预测均值和协方差然后用同样的方式处理量测更新% UKF预测 [X, Wm, Wc] sigmaPoints(x_est, P_est, 1e-2, 2, 0); X_pred zeros(1, 2*n1); for i 1:2*n1 X_pred(i) 0.5*X(i) 2.5*X(i)/(1X(i)^2) 8*cos(1.2*(k-1)); end x_pred sum(Wm .* X_pred); P_pred Q; for i 1:2*n1 P_pred P_pred Wc(i) * (X_pred(i) - x_pred)^2; end % UKF更新 Z_pred zeros(1, 2*n1); for i 1:2*n1 Z_pred(i) X_pred(i)^2 / 20; end z_pred sum(Wm .* Z_pred); Pzz R; Pxz 0; for i 1:2*n1 Pzz Pzz Wc(i) * (Z_pred(i) - z_pred)^2; Pxz Pxz Wc(i) * (X_pred(i) - x_pred) * (Z_pred(i) - z_pred); end K Pxz / Pzz; x_est x_pred K * (z(k) - z_pred); P_est P_pred - K * Pxz;这段代码没有做任何矩阵分解或求导运算唯一需要小心的是 P_est 要保持对称正定。仿真中偶尔会遇到因为数值截断导致协方差不对称通常加一个很小的单位阵修正就能解决。3.4 PF实现粒子传播、权重计算与系统重采样粒子滤波的实现逻辑更有蒙特卡洛的味道。首先是初始化一批粒子之后每一个采样周期里每个粒子都独立地通过状态方程递推再根据量测似然度更新权重最后做重采样。我来给一份精简但完整的PF核心循环% PF初始化 Np 2000; xp x0_est sqrt(P0) * randn(1, Np); wp ones(1, Np) / Np; for k 2:N % 预测每个粒子通过状态方程传播 xp 0.5*xp 2.5*xp./(1xp.^2) 8*cos(1.2*(k-1)) sqrt(Q)*randn(1, Np); % 更新计算每个粒子的似然度 innov z(k) - xp.^2/20; wp wp .* exp(-innov.^2 / (2*R)); wp wp / sum(wp); % 计算有效粒子数判断是否需要重采样 N_eff 1 / sum(wp.^2); if N_eff Np/2 % 系统重采样 cdf cumsum(wp); u (rand (0:Np-1)) / Np; xp_new zeros(1, Np); for i 1:Np idx find(cdf u(i), 1); xp_new(i) xp(idx); end xp xp_new; wp ones(1, Np) / Np; end % 输出状态估计 x_est(k) sum(wp .* xp); P_est(k) sum(wp .* (xp - x_est(k)).^2); endPF对重采样的依赖很强。如果不重采样几轮迭代之后权重就会集中到极少数粒子上粒子多样性急剧下降这叫粒子退化现象。但重采样频率太高也不行会让粒子多样性迅速枯竭好的粒子被反复复制整体无法覆盖真实状态的后验区域。工程经验是只有当有效粒子数 N_eff 降到某个阈值比如 Np/2以下时才触发重采样而不是每一帧都重采样。4. 同一条赛道上的对比精度、收敛速度与计算量把三种算法放在同一个仿真系统、同一组真实状态和量测下运行输出结果才有说服力。我在Matlab里做了一组典型试验运行50步粒子数PF取2000三种算法各跑50次蒙特卡洛统计RMSE、收敛速度和单步耗时。4.1 精度对比UKF与PF各有胜负下表是一次典型单次仿真中三种算法的均方根误差RMSE和平均绝对误差MAE算法RMSEMAE最大误差EKF2.8612.0376.524UKF0.9830.7412.158PF(2000)0.8970.6822.047从结果看EKF的误差明显偏大主要原因是量测方程里 x^2/20 这个非线性项在真实状态为正负区间时会产生明显的高阶偏差一阶线性化无法准确传递这种非线性效应。UKF和PF则表现接近UKF依靠sigma点能够较好匹配非线性映射后的分布统计量PF的精度则随着粒子数增加还有进一步提升空间。需要说明的是这个结果是典型的但并不是绝对的如果换一个非线性强度较弱的系统比如量测方程接近线性EKF与UKF的差距会显著缩小。因此在做横向对比时建议多测几组随机种子不要被单次结果误导。4.2 收敛速度初始误差消除的差异三种算法对初始状态误差的敏感程度也值得关注。如果初始估计偏离真实状态较远EKF的收敛通常表现得最挣扎——因为雅可比矩阵在偏离点计算出来的线性化模型误差更大导致滤波初期误差协方差的修正方向有一定偏差。UKF由于sigma点覆盖范围更广对初始误差的容忍度更高往往能在前几步就较快拉回真实轨迹。PF在初始阶段只要粒子采样范围覆盖了真实状态收敛速度也很理想但如果初始粒子分布完全偏离真实状态则需要较多样本和几步迭代才能拉回来。我做了一组比较极端的实验初始估计直接设为5.0真实初始状态是0.1。EKF花了大约12步才勉强跟上UKF大约5步PF粒子分布覆盖足够宽大约3步就收敛了。这个差距在实际项目中很关键因为滤波器启动阶段的收敛速度直接影响系统上电后的应对表现。4.3 计算量EKF最省PF最贵在Matlab R2023b环境下我统计了单步平均耗时含随机数生成和重采样算法单步平均耗时EKF约 0.08 msUKF约 0.52 msPF (N1000)约 8.3 msPF (N5000)约 41.5 msEKF确实快得离谱UKF虽然比EKF慢几倍但和PF相比完全是两个量级。如果系统对实时性要求苛刻比如飞行控制里1kHz以上的控制周期PF基本不可用UKF是精度和计算量之间的良好折中。注意粒子滤波的计算量对粒子数是线性增长的如果系统状态维度高比如三维位置加三维姿态每个粒子本身的状态递推和似然计算又会急剧变慢粒子数往往需要更大计算压力非常大。这正是PF在高维状态估计里不常用的原因。4.4 调参对结果的影响三种算法的关键参数都会显著影响结果EKF主要调Q和R本质是权衡“信任模型”还是“信任量测”。Q取太小会让滤波器反应迟钝Q取太大又会导致估计值抖动明显。UKF核心调α、β、κ这三个sigma点参数。α决定sigma点离均值的远近太小可能造成数值不稳定β在高斯分布下取2是经验最优κ在多数情况下取0即可。PF最核心的是粒子数Np和重采样阈值。粒子数太少精度不够太多实时性崩掉重采样阈值建议在Np/3到Np/2之间试太低粒子退化明显太高粒子多样性丧失。我在仿真中习惯先把Q、R固定用RMSE曲线去观察滤波器是“超调”还是“迟钝”再反向调节权重。不要一上来就往复杂参数上纠结——先把最基础的两个噪声方差调明白剩下的是微调。5. 实际项目中该选哪种算法我的踩坑经验与调参建议学完三种算法真正到了项目中做选型反而比写仿真代码更纠结。这里结合我做过的目标跟踪、室内定位、姿态解算这几个方向谈一谈我自己的经验和教训。5.1 按非线性强度和实时性做初选选型本质上只有两个维度非线性强度多大、实时性要求多高。可以按下面这个思路快速判断弱非线性、高实时性选EKF。比如大部分陀螺仪零偏估计、简单的车辆运动模型EKF的线性化误差可接受而它的低计算量优势非常明显。此时EKF不是精度最高的选择但它是嵌入式上最容易实现、最稳的选择。中强非线性、中高实时性选UKF。比如目标跟踪、GPS/INS组合导航、无人机姿态融合。这类场景里EKF的线性化误差可能导致滤波发散UKF虽然计算量翻了几倍但依然在实时预算内同时避免了手推雅可比的巨大工作量。强非线性、非高斯、实时性要求不高选PF。比如复杂室内环境的RSSI定位、非线性较强的纯角度跟踪等。PF的代价是计算量大但换来了对任意分布形态的适应能力。5.2 几个我实际踩过的坑先说EKF。有一次我做组合导航状态维数到了15维手推雅可比矩阵花了两天时间结果仿真还是发散。后来一排查发现量测方程里有一个四元数归一化步骤没有在求导时体现出来雅可比矩阵漏掉了一个高阶项。这种问题非常隐蔽因为单看某个中间变量的偏导都对但复合函数求导时漏掉的项会造成整个滤波器在特定状态下缓慢漂移。后面我长记性了能用符号工具箱验证就先验证别拿手推的雅可比直接上真机。UKF也有自己的坑。sigma点生成依赖协方差矩阵的Cholesky分解要求P必须正定。仿真中小数点后十几位的截断误差经常让P失去对称性或正定性我一拍脑袋把P加上1e-10的单位阵去凑Cholesky分解的数值需求后面学着在每次预测和更新后强制让P (P P)/2 做对称化情况好了很多。PF最大的坑是粒子数选择。一开始我粒子数取200结果滤波结果忽好忽坏增加粒子到2000后才稳定下来。粒子数太少时粒子分布无法覆盖状态空间中的有效区域一旦某个粒子碰巧处于真实状态附近权重就会一家独大状态估计变成“彩票式”输出毫无连续性可言。后来我习惯先做一次离线分析画出有效粒子数随时间变化的曲线然后用N_eff的最小值反推合适的粒子数而不是盲猜。5.3 如果只能用Matlab推荐的工作流很多科研场景里Matlab就是最终实现平台我给你们梳理一套我自己在用的工作流先用清晰脚本写清状态方程和量测方程不要直接揉进滤波代码里后面调参和复用都方便。以最小可行模型跑通EKF先确认滤波循环逻辑是对的再考虑换更复杂的算法。用符号数学工具箱验证雅可比矩阵如果系统不要求极致实时性甚至可以直接跳到UKF省掉手推偏导的环节。做蒙特卡洛仿真而不是单次运行至少50次独立实验统计RMSE均值和方差才能客观评价算法优劣。保存关键中间变量比如新息序列、协方差对角线、有效粒子数这些都是诊断滤波问题的第一手材料。5.4 从仿真到工程落地还需注意的细节跑完仿真到工程落地还有一段路。Matlab里的double精度和嵌入式环境里的float精度差异有时候会带来完全不同的数值表现。比如UKF中Cholesky分解在单精度下更容易失败。我的建议是在Matlab里仿真时就把协方差矩阵的数值量级记录下来到嵌入式平台上事先做好矩阵的缩放或归一化防止数据范围过度扩张。另一个容易被忽略的问题是量测异常的鲁棒性。仿真里z(k)都是干干净净的量测但真实系统偶尔会跳出一个野值或者量测丢失。标准EKF/UKF/PF对野值都没有抵抗力一个异常量测就能把估计拉飞。我建议在滤波代码外面加一层量测合理性判断如果新息超过3倍的标准差就跳过该次量测更新直接用预测值作为输出。这个小改动在工程中收益巨大远比在算法内部纠结鲁棒性更划算。6. 我对三种算法最终的个人评价这三种算法自己跑下来给我的最大体会是算法本身并不复杂复杂的是理解你对系统到底了解多少。EKF好用是因为它让你必须清楚地知道系统的线性化动力学UKF好用是因为它把“近似非线性函数”的负担变成了“近似分布传播”的负担对模型推导友好得多PF好用是因为它彻底放弃了解析近似全靠算力换精度。如果只是做学术仿真或者课程作业建议三种都实现一遍哪怕最后只用其中一种——因为只有横向对比过才知道自己手上的系统到底处于什么非线性强度区间才知道某些看似合理的结果里其实藏着多少近似误差。我个人在目标跟踪项目里最终选择了UKF作为基础方案不是因为EKF不能用而是因为后续要加入机动检测、多传感器融合等模块UKF在模块扩展时不需要重新推导雅可比省下的维护成本远超它多出来的那一点计算量。但同一套代码库里的EKF实现我也没有删遇到实时性吃紧的场景切换回去做验证依旧很有价值。至于PF它是我最后的“兜底方案”凡是在复杂非线性、非高斯环境下被前两者折磨得无法收场时我就知道该祭出大杀器了。