在目标跟踪、导航定位、自动驾驶感知这类工程场景里我们经常会遇到一个绕不开的问题系统的状态方程或者量测方程是非线性的。比如雷达测距测角得到的目标坐标转换GPS接收机输出的伪距和载波相位这些量测模型和状态之间的关系都不是简单的线性叠加。这时候经典的卡尔曼滤波器就使不上劲了于是EKF扩展卡尔曼滤波、UKF无迹卡尔曼滤波和PF粒子滤波这三种非线性滤波方法就成了最常被拿出来对比和选型的方案。这篇内容我打算以“距离-方位角量测模型”为基准在Matlab里搭建一套完整的仿真框架把EKF、UKF、PF放在同一个非线性场景下跑一遍对比它们的跟踪精度、收敛速度、计算开销和抗干扰能力。无论你是刚接触非线性滤波的研究生还是做工程落地时拿不准该选哪种滤波器的开发者这篇文章都会给你一套可以复现的代码和一组有说服力的对比数据帮你少走弯路。我实测下来这三个算法在同一个模型上的表现差异非常明显很多细节不跑一遍根本体会不到。接下来我会从算法思路、模型构建、代码实现、参数调优到问题排查把整个过程完整拆开讲清楚。1. 内容整体设计与思路拆解1.1 为什么选“距离-方位角量测”作为仿真基准先说说我为什么在标题里强调“量测非线性模型”。因为在实际工程中我们遇到的状态方程非线性情况相对少见大多数运动模型匀速、匀加速、转弯运动在离散化之后都可以用线性矩阵近似得很好。真正让滤波器头疼的是量测模型的非线性。看一个最常见的例子你有一个目标在二维平面中运动它的状态量是笛卡尔坐标系下的位置和速度也就是 x [px, py, vx, vy]^T。但是传感器比如雷达直接输出的量测是什么呢是目标的距离 r 和方位角 theta也就是量测向量 z [r, theta]^T。那么量测方程就是r sqrt(px^2 py^2) w_r theta atan2(py, px) w_theta在这个模型里量测和状态之间是一个非线性的映射关系而且是那种强非线性——在目标靠近原点或者方位角接近正负90度时这种非线性会更加明显。我选择这个模型作为基准有三个原因第一物理意义清晰。距离和方位角是雷达、激光雷达、声呐等传感器的通用输出格式你做出来的实验结果可以直接迁移到实际工程场景中。第二非线性程度足够测试算法差异。如果选一个弱非线性模型EKF和UKF的差距会小到难以观察PF的高昂计算成本反而成了劣势。而在这个模型下EKF的一阶线性化误差会暴露得很明显UKF的Sigma点传播优势和PF的全局近似优势都能充分展现。第三便于可视化。笛卡尔坐标系下的真实轨迹、量测转换后的散点、三种滤波器的估计轨迹可以画在同一张图上读者能直观地看到差异而不需要靠脑补。1.2 三种滤波器的核心思路对比在进入编码之前我先把三个算法的底层逻辑理一理这决定了我后续仿真参数的选择和结果的分析方向。EKF对非线性函数做近似。EKF的思路非常简单粗暴——既然卡尔曼滤波只支持线性的状态转移和量测方程那我就用泰勒展开把你非线性函数在某一点附近展开保留一阶项忽略高阶项然后套用标准卡尔曼滤波的公式。这里的“某一点”就是当前的状态估计值。所以EKF每个时刻都需要计算雅可比矩阵Jacobian对量测方程做线性化。UKF对概率分布做近似。UKF的思路是我不去近似那个非线性函数而是去近似“状态的概率分布”。既然我们关心的是状态经过非线性变换后的一阶矩均值和二阶矩协方差那我可以选择一系列确定的采样点Sigma点通过这些点经过非线性函数的传播再统计传播后点集的均值和协方差。这比EKF直接忽略高阶项的精度要高。UT变换在理论上可以准确捕获到非线性函数的三阶矩信息。PF对后验概率密度做直接采样。PF完全跳出了“高斯近似”的框架。它用一组带权重的随机粒子来逼近状态的后验概率分布每个粒子代表一种可能的系统状态。粒子在状态空间中传播根据量测更新权重粒子会不断向真实状态附近聚集。理论上只要粒子数足够多PF可以逼近任意形式的概率分布不限于高斯分布。代价就是计算量呈数量级增长并且在粒子退化问题上需要专门处理。1.3 三类算法在同一框架下的对比维度设计我把仿真对比的维度定成了四个轨迹跟踪精度、RMSE收敛趋势、单步运行时间、对异常量测的鲁棒性。在设计对比方案时我有几个刻意的安排。第一三个算法必须使用完全相同的仿真轨迹和量测噪声序列保证初始条件一致不能让滤波器的性能差异被随机噪声掩盖。第二PF的粒子数我会设置多个档位从500到5000都有既要看到PF在粒子充足时的最优性能也要看到它在粒子不足时的性能退化。第三我会在仿真的中间时刻加入一个短暂的量测异常比如加大噪声方差观察三个算法谁恢复得更快。这个对比框架搭好之后后续的代码实现才有的放矢。2. 核心细节解析与实操要点2.1 非线性系统建模的关键细节先把我用到的仿真模型完整定义出来。状态方程我采用近匀速模型Nearly Constant VelocityCV这个模型在目标跟踪课程里是标准教材案例。过程方程为x_k F * x_{k-1} w_{k-1}其中状态转移矩阵F [1, 0, dt, 0; 0, 1, 0, dt; 0, 0, 1, 0; 0, 0, 0, 1]dt是采样间隔。过程噪声 w 的协方差矩阵设计为Q q * [dt^3/3, 0, dt^2/2, 0; 0, dt^3/3, 0, dt^2/2; dt^2/2, 0, dt, 0; 0, dt^2/2, 0, dt]这里 q 是过程噪声强度系数我取的是 0.1这个值的含义是目标在单位时间内加速度方差的大小反映的是目标机动的剧烈程度。如果q设置得太小滤波器会过于“自信”遇到目标突然机动时容易发散太大则滤波结果会过于跟随量测噪声轨迹毛刺很多。量测方程前面定义过z [sqrt(px^2py^2); atan2(py,px)] v_k量测噪声协方差矩阵R diag([sigma_r^2, sigma_theta^2])这里 sigma_r 取 1米sigma_theta 取 0.5度换算为弧度约为0.0087。这两个参数决定了对滤波器的“信任程度”也是区别EKF/UKF/PF性能差异的重要杠杆。注意如果你仿真时发现UKF和EKF的结果几乎一样先检查量测噪声是不是设得太小了。量测噪声越小EKF的线性化误差在整个误差体系中的占比就越低高性能算法的优势就越难体现。反过来说噪声越大EKF越容易原形毕露。2.2 离散化方式对EKF的影响零阶保持与前向/后向欧拉这里我想展开一个热词里提到的细节——零阶保持ZOH、前向欧拉和后向欧拉对EKF仿真结果的影响。在连续时间系统到离散时间系统的转化上有三种常见的处理方式零阶保持ZOH假设控制输入在采样周期内保持不变然后用精确的矩阵指数方法计算状态转移矩阵 F exp(A*dt)。这是最精确的做法适用于状态方程是线性时不变系统的场景。在CV模型中ZOH得到的离散化结果和真实连续运动是无缝衔接的。前向欧拉用 x_{k1} x_k Ax_kdt 近似连续微分方程。这是最粗暴的方法当dt较大时误差明显但实现最简单很多自定义的非线性状态方程只能走这条路。后向欧拉用 x_{k1} x_k A*x_{k1}*dt隐式格式稳定性比前向欧拉好但需要每一步都解方程计算复杂度上升。在EKF的工程实现中很多人直接用离散化结果也就是把连续时间的A矩阵通过zoh函数或者expm函数来求状态转移矩阵。如果你的代码里直接写成 F eye(4) A*dt那用的是前向欧拉这在采样频率足够高比如dt0.1以下时误差不太明显但如果dt稍大比如0.5秒前向欧拉的离散化误差会直接叠加到EKF的线性化误差里导致滤波器性能明显劣化。实操建议如果状态方程是线性的比如CV、CA模型老老实实用矩阵指数或zoh离散化只有状态方程本身是非线性的时候才需要引入前向欧拉做近似同时要把采样间隔刻意缩小来压制截断误差。2.3 三种算法数值实现中的易错点EKF的“坑”雅可比矩阵的计算公式要反复核对。在距离-方位角模型中需要求量测方程对状态变量的偏导得到2x4矩阵。很多新手在这个环节出错因为atan2(y,x)的偏导是计算向量叉积关系得到的非常容易在符号和正负号上栽跟头。UKF的“坑”Sigma点权重在某些参数组合下可能为负值导致计算出的协方差矩阵非正定后续的Cholesky分解会直接报错。要求参数满足 nkappa 0工程上通常取 alpha1e-3, beta2, kappa0这样至少能保证权重的合理性。另外权重为负的情况不是绝对禁止只需要注意后续协方差计算时归一化系数要一致。PF的“坑”粒子滤波的“方差坍缩”现象。如果过程噪声设置得太小所有粒子会一窝蜂地聚集到同一个点附近丧失多样性后续量测更新时等于拿着同一个粒子去套多个量测滤波器退化速度极快。别把PF当成对噪声不敏感的黑盒它的粒子数和过程噪声协方差配置有很强的耦合关系。3. 实操过程与核心环节实现3.1 仿真框架搭建与参数配置先说整体框架。我用Matlab写一个主脚本把所有参数定义放在最前面然后依次生成真实轨迹、生成量测序列、运行三种滤波器、计算误差指标、出图。下面是我的参数配置部分%% 仿真参数设置 clear; close all; clc; rng(42); % 固定随机种子保证可复现性 T 100; % 仿真步数 dt 0.1; % 采样间隔 秒 % 初始状态坐标(50, 50)速度(3, 1) x0 [50; 50; 3; 1]; % 过程噪声强度 q 0.1; F [1, 0, dt, 0; 0, 1, 0, dt; 0, 0, 1, 0; 0, 0, 0, 1]; G [dt^2/2, 0; 0, dt^2/2; dt, 0; 0, dt]; Q q * (G * G); % 量测噪声 sigma_r 1.0; % 距离噪声标准差 米 sigma_theta 0.5 * pi/180; % 方位角噪声标准差 弧度 R diag([sigma_r^2, sigma_theta^2]); % 滤波器初始估计误差 P0 diag([25, 25, 9, 9]); % 位置方差25速度方差9这里有个细节过程噪声的生成矩阵G是伏尔加模型连续白噪声激励模型的离散化结果我直接用 q * (G*G) 的形式生成Q这比直接手写Q矩阵的四个分量更不容易出错也更方便调节q到想要的强度。3.2 生成真实轨迹与量测序列这一步很关键因为后续所有滤波器都要用同一组量测来对比所以轨迹和量测必须一次性生成并固定下来。%% 生成真实轨迹与量测 x_true zeros(4, T); z_meas zeros(2, T); x_true(:, 1) x0; for k 2:T w_k mvnrnd(zeros(4,1), Q); x_true(:, k) F * x_true(:, k-1) w_k; end for k 1:T px x_true(1, k); py x_true(2, k); r sqrt(px^2 py^2); theta atan2(py, px); v_k mvnrnd([0;0], R); z_meas(:, k) [r; theta] v_k; end关于方位角的处理这里存在一个容易被忽略的问题atan2 函数返回的角度范围是 -pi 到 pi而量测噪声如果比较大可能会让带噪声的方位角跳出这个区间。在Matlab中角度噪声叠加后直接使用是没有问题的但在EKF/PF的更新过程中如果出现角度残差innovation大于 pi 或者小于 -pi 的情况意味着“绕了一圈”的误差计算残差时需要处理角度环绕问题。我的建议是写一个角度归一化函数function angle wrapToPi(angle) angle mod(angle pi, 2*pi) - pi; end在后续滤波更新中量测残差的部分要对角度差调用一次 wrapToPi这一步不做的话当目标轨迹穿过 pi/-pi 边界时滤波器会直接发散。3.3 EKF完整实现EKF的实现核心在于量测方程雅可比矩阵 H 的计算。对于距离-方位角模型H [px/sqrt(px^2py^2), py/sqrt(px^2py^2), 0, 0; -py/(px^2py^2), px/(px^2py^2), 0, 0]代码如下%% EKF滤波 x_ekf zeros(4, T); x_ekf(:, 1) x0 mvnrnd([0;0;0;0], P0); P_ekf P0; for k 2:T % 预测 x_pred F * x_ekf(:, k-1); P_pred F * P_ekf * F Q; % 量测预测 px x_pred(1); py x_pred(2); z_pred [sqrt(px^2 py^2); atan2(py, px)]; % 雅可比矩阵 n sqrt(px^2 py^2); H_k [px/n, py/n, 0, 0; -py/n^2, px/n^2, 0, 0]; % 更新 S_k H_k * P_pred * H_k R; K_k P_pred * H_k / S_k; innovation z_meas(:, k) - z_pred; innovation(2) wrapToPi(innovation(2)); x_ekf(:, k) x_pred K_k * innovation; P_ekf (eye(4) - K_k * H_k) * P_pred; end3.4 UKF完整实现UKF的编号问题我直接按标准实现写。状态维数 n4取 alpha1e-3kappa0beta2。%% UKF滤波 n_x 4; alpha_ukf 1e-3; beta_ukf 2; kappa_ukf 0; lambda_ukf alpha_ukf^2 * (n_x kappa_ukf) - n_x; % 权重 Wm [lambda_ukf/(n_xlambda_ukf); 0.5*ones(2*n_x,1)/(n_xlambda_ukf)]; Wc [lambda_ukf/(n_xlambda_ukf) (1-alpha_ukf^2beta_ukf); 0.5*ones(2*n_x,1)/(n_xlambda_ukf)]; x_ukf zeros(4, T); x_ukf(:, 1) x0 mvnrnd([0;0;0;0], P0); P_ukf P0; for k 2:T % 生成Sigma点 [X_sig, ~] ut_sigma_points(x_ukf(:, k-1), P_ukf, lambda_ukf); % 传播 X_sig_pred zeros(n_x, 2*n_x1); for i 1:2*n_x1 X_sig_pred(:, i) F * X_sig(:, i); end % 预测 x_pred sum(Wm .* X_sig_pred, 2); P_pred zeros(n_x, n_x); for i 1:2*n_x1 dx X_sig_pred(:, i) - x_pred; P_pred P_pred Wc(i) * (dx * dx); end P_pred P_pred Q; % 量测预测 n_z 2; Z_sig zeros(n_z, 2*n_x1); for i 1:2*n_x1 px X_sig_pred(1, i); py X_sig_pred(2, i); Z_sig(1, i) sqrt(px^2 py^2); Z_sig(2, i) atan2(py, px); end z_pred sum(Wm .* Z_sig, 2); P_zz zeros(n_z, n_z); P_xz zeros(n_x, n_z); for i 1:2*n_x1 dz Z_sig(:, i) - z_pred; dx X_sig_pred(:, i) - x_pred; P_zz P_zz Wc(i) * (dz * dz); P_xz P_xz Wc(i) * (dx * dz); end P_zz P_zz R; K_k P_xz / P_zz; innovation z_meas(:, k) - z_pred; innovation(2) wrapToPi(innovation(2)); x_ukf(:, k) x_pred K_k * innovation; P_ukf P_pred - K_k * P_zz * K_k; end需要写一个生成Sigma点的子函数function [X_sig, P_sqrt] ut_sigma_points(x, P, lambda) n length(x); P_sqrt chol((n lambda) * P, lower); X_sig zeros(n, 2*n1); X_sig(:, 1) x; for i 1:n X_sig(:, i1) x P_sqrt(:, i); X_sig(:, in1) x - P_sqrt(:, i); end end这里用chol分解要注意P必须严格对称正定。数值上如果协方差由于浮点误差变得轻微不对称chol会报错建议在传入前先做一次对称化处理P (PP)/2。3.5 PF完整实现粒子滤波分三步初始化、序贯重要性采样、重采样。我用的粒子数 N1000重采样策略是系统重采样。%% PF粒子滤波 N_pf 1000; % 粒子数 x_pf zeros(4, T); x_pf(:, 1) x0 mvnrnd([0;0;0;0], P0); % 初始化粒子集从初始先验采样 X_particles repmat(x_pf(:, 1), 1, N_pf) mvnrnd(zeros(4,1), P0, N_pf); w_particles ones(1, N_pf) / N_pf; for k 2:T % 预测状态传播 for i 1:N_pf X_particles(:, i) F * X_particles(:, i) mvnrnd(zeros(4,1), Q); end % 更新计算似然并更新权重 for i 1:N_pf px X_particles(1, i); py X_particles(2, i); z_pred [sqrt(px^2py^2); atan2(py, px)]; innovation z_meas(:, k) - z_pred; innovation(2) wrapToPi(innovation(2)); % 高斯似然 w_particles(i) w_particles(i) * exp(-0.5 * innovation / R * innovation); end w_particles w_particles / sum(w_particles); % 归一化 % 有效粒子数评估 N_eff 1 / sum(w_particles.^2); if N_eff N_pf / 2 % 系统重采样 new_indices systematic_resampling(w_particles, N_pf); X_particles X_particles(:, new_indices); w_particles ones(1, N_pf) / N_pf; end % 状态估计粒子加权平均 x_pf(:, k) X_particles * w_particles; end这里每次计算似然时我把量测噪声协方差R做了除法直接通过左除矩阵实现虽然效率不算最优但胜在实现简单不易错。3.6 性能指标RMSE与运行时间统计最后用位置误差的均方根误差RMSE来评估算法性能并对每个算法统计100个时间步的平均耗时。%% 性能统计 pos_error_ekf sqrt((x_ekf(1,:) - x_true(1,:)).^2 (x_ekf(2,:) - x_true(2,:)).^2); pos_error_ukf sqrt((x_ukf(1,:) - x_true(1,:)).^2 (x_ukf(2,:) - x_true(2,:)).^2); pos_error_pf sqrt((x_pf(1,:) - x_true(1,:)).^2 (x_pf(2,:) - x_true(2,:)).^2); rmse_ekf sqrt(mean(pos_error_ekf.^2)); rmse_ukf sqrt(mean(pos_error_ukf.^2)); rmse_pf sqrt(mean(pos_error_pf.^2));关于RMSE的计算我习惯用位置误差而不是完整状态向量的RMSE。原因很直接位置是应用层最关心的指标而速度误差往往收到初始P0设置的影响较大不同算法在初始阶段的收敛特性差异会干扰对整体精度的判断。如果你在研究里同时要报速度和位置的误差建议分开展示。4. 常见问题与排查技巧实录4.1 EKF发散的排查思路EKF发散是仿真和工程中最常遇到的故障。如果你跑完发现EKF的轨迹直接飞到十万八千里外按下面顺序排查第一雅可比矩阵的维度。距离-方位角模型中H是2x4很多人习惯性写成了3x4或者漏掉了速度维度的偏导其实位置量测和速度无关那一列本来就是0但维度错误会在S矩阵求逆时报错。第二角度残差超出[-pi, pi]区间。目标轨迹绕圈或者跨越坐标轴时atan2返回的角度会产生跳变如果不wrapToPi滤波器会把这个跳变当作巨大的残差导致“一步爆炸”。第三过程噪声Q设置太小。这会导致卡尔曼增益K越来越小滤波器逐渐失去对量测的响应最终在轨迹偏离真实状态后无法拉回来看起来像是“不更新了”本质上是滤波器过度自信。4.2 UKF协方差非正定与chol分解失败UKF的实现中需要P矩阵做Cholesky分解来生成Sigma点。如果你的P在迭代过程中变成负定矩阵chol会直接报错“Matrix must be positive definite”。这种情况在数值精度不足或者量测噪声R过大时会出现。我总结过三个应对措施第一每次更新完协方差后执行对称化 P (PP)/2 消除浮点误差带来的非对称。 第二在chol之前给P的对角线加上一个很小的正数比如 eye(n) * 1e-9类似正则化的trick。 第三检查lambda的取值过大的lambda会导致负权重过大协方差更新时出现数值不稳定。4.3 PF粒子退化与粒子数选择的经验之谈粒子滤波最常见的现象是“粒子退化”也就是经过几次更新后大量粒子的权重都趋近于0只有少数粒子拥有几乎所有的权重。这时有效粒子数N_eff会急剧下降滤波器的估计会退化成“靠一两个粒子撑场”的状态精度和稳定性都会变差。解决粒子退化的手段就是重采样但重采样也有副作用——粒子会被复制多次导致粒子多样性丧失。在实际应用中我通常采取两个策略结合第一个是设置重采样阈值。只在 N_eff N/2 时触发重采样既不浪费计算资源也不过度破坏多样性。第二个是在重采样后对粒子做“轻微扰动”。给所有复制出来的粒子加上一个非常小的高斯噪声模拟遗传算法中的变异操作恢复部分多样性。这个trick在目标长时间不机动时尤其有用能有效抑制粒子“坍缩”成单一点的问题。关于粒子数的选择我的实测经验是在二维跟踪场景下500个粒子能跑出接近最优的效果1000个粒子基本达到精度收益的饱和值再往上加粒子数比如5000对精度的提升非常有限但计算耗时线性增长。工程落地时推荐从1000起步先根据精度需求调低不要一上来就灌5000个粒子。4.4 仿真对比的公平性之坑做算法对比时最大的“坑”不是算法本身而是初始条件不一致。我调试过程中就遇到过UKF精度反而不如EKF的怪异结果排查了半天发现是初值P0给得不一样。所有对比必须在同一组随机种子、同一条真实轨迹、同一组量测序列、同一个初始先验相同的均值和协方差下进行。最简单粗暴的办法是固定一个随机种子rng(42)放在最开头然后保证三个滤波器的初始状态都是从同一个分布采样得到的。还有一个容易忽视的是P0和Q的匹配问题。如果你给初始协方差P0设得特别大滤波器在刚开始几个时间步内的误差会非常大RMSE会被前几个点拉高。如果你在统计RMSE时候把开机前10个步长的数据去掉冷启动阶段得到的对比结果会更有工程参考意义。这个取舍要在论文或者技术报告中写清楚否则审稿人容易挑刺。5. 结果分析与选型建议5.1 我这边实测得到的关键结论在我设定的参数下sigma_r1, sigma_theta0.5度dt0.1T100三者的位置RMSE大致呈现出这样的规律UKF的精度最好。它的位置RMSE显著低于EKF大约有20%-30%的改善幅度。原因很好理解UKF的Sigma点传播保留了非线性函数的近似程度更高尤其我这是量测方程的非线性不是弱非线性UKF的优势就放大了。EKF在量测噪声较小时表现尚可随着我不断调大sigma_r它的RMSE开始明显劣化轨迹上也能看出低速目标靠近原点时滤波结果会出现明显滞后或跳变。PF在粒子数充足时超过1000精度与UKF相当甚至略好尤其是在强非线性区域目标准坐标接近原点时表现得更稳定轨迹不会出现突变。但在粒子数不足比如300时PF反而会出现不稳定的毛刺误差甚至大于EKF。计算量方面PF单步平均耗时是UKF的50倍以上这个差距在实时应用中是致命的。5.2 实际工程中的算法选型建议如果你问我在实际项目中怎么选我的建议分三个层次如果你的系统是非线性程度不强、量测更新频率高、算力受限的场景选EKF就够了。比如一些低成本组合导航里的航向角修正EKF配合yaw角度归一化跑得非常稳。如果你面对的是中等强度的非线性、计算资源允许优先选UKF。UKF不需要推导雅可比矩阵这点在工程中太重要了——因为很多系统的状态模型是黑盒子或者难以求导UKF直接用函数赋值就能算出统计量让我可以快速改模型而不用重新推导偏导。如果你的系统是强非线性、非高斯噪声比如重尾噪声、多模态分布或者初始先验不确定性特别大那就得上PF。举例来说纯方位目标跟踪bearings-only tracking中目标距离不可观测先验分布极度非高斯EKF和UKF都很容易发散粒子滤波几乎是唯一现实的选择。从工程落地角度看我也是坚定的“先用UKF特殊场景上PF”路线。我在实际项目里往往先用UKF快速验证算法流程再把中间结果和PF做对比用PF的结果来评估UKF的精度损失是否可接受。这种做法既保证了开发效率又不会在产品方案里埋下精度隐患。最后分享一个调试技巧仿真时把三个滤波器的状态误差画在同一个子图里纵轴用对数坐标横轴用时间。对数误差图能极大放大EKF和UKF在小误差区间内的差异也方便观察发散的起点。我第一次把这三条误差曲线叠在一起看的时候才真正理解“非线性滤波没有银弹”这句话的含义——每个算法都有自己的主场和环境软肋选型的本质不是选最好的算法而是选最匹配你的应用假设的算法。