企业官网建设流程全解析

搞状态估计这块的人，大概率都绕不开这三个名字：EKF、UKF、PF。不管是做目标跟踪、组合导航，还是机器人定位、自动驾驶感知，只要系统模型带上非线性，经典卡尔曼滤波就有点使不上劲了。这时候，扩展卡尔曼、无迹卡尔曼、粒子滤波就成了最常见的三个选项。

网上关于这三种算法的资料其实不少，但大多各讲各的，要么只给理论推导，要么只放代码，很少有人把三者放到同一个仿真框架下，用同一个问题去对比它们的估计精度、实现难易度和计算开销。这篇文章就用一个典型的强非线性场景——纯方位目标跟踪，把EKF、UKF、PF在Matlab里完整实现一遍，直接比较三种算法的估计结果，顺便聊聊我自己在调参和改代码时踩过的坑。

内容偏工程实践，适合刚接触非线性滤波的研究生、做嵌入式状态估计的工程师，以及想快速上手Matlab仿真的同学。我会从原理、代码、结果对比到避坑经验一次性讲清楚，看完你就能直接动手改自己的模型。

1. 三种滤波算法的核心思想与原理解读

1.1 为什么线性卡尔曼滤波解决不了非线性问题

经典卡尔曼滤波（KF）的本质是最小均方误差意义下的最优线性估计器，它要求系统状态方程和量测方程都是线性的，噪声满足高斯分布。在这种条件下，KF能给出解析的最优解，而且计算量极小。

但现实工程问题几乎没有纯线性的。比如我们跟踪一个目标，用雷达测距离和方位角，量测方程里就有三角函数；又比如无人机在转弯机动时，状态转移方程里含有角度变化的非线性项。这时候再用KF，相当于拿直线去拟合曲线，误差会越来越大，甚至在强非线性场景下直接发散。

解决非线性滤波的核心思路只有一个：想办法处理非线性函数作用在概率分布上的传递问题。EKF、UKF、PF三个算法，本质上是三条不同的路径去逼近同一个答案。

1.2 EKF：一阶泰勒展开，把非线性掰直

EKF的思路最直接——既然非线性不好处理，那就把非线性函数在当前状态估计值附近做一阶泰勒展开，忽略高阶项，得到一个近似的线性模型，然后套用标准KF的递推框架。

用数学语言说，对于非线性状态方程 x(k+1)=f(x(k))+w(k) 和量测方程 z(k)=h(x(k))+v(k)，EKF在每次递推时计算雅可比矩阵：

F(k)=∂f/∂x|_{x̂(k)}

H(k)=∂h/∂x|_{x̂(k|k-1)}

有了这两个矩阵，原来的非线性系统就变成了时变的线性系统，之后的预测、更新步骤和标准KF完全一样。

这个方案的好处是实现简单、计算量小，很多工程里沿用至今。但它有两个明显的弱点：一是雅可比矩阵的计算本身就容易出错，尤其在高维状态下手工推导非常繁琐；二是一阶线性化在系统非线性强度高时，误差会迅速累积，甚至导致滤波器发散。另外，EKF隐含假设了状态分布经过非线性变换后仍然是高斯分布，这个假设在强非线性下经常不成立。

我记得第一次在项目里用EKF做纯方位跟踪时，就因为量测方程的雅可比推导少了一项，结果滤波结果直接飘了，排查了半天才找到问题。这个细节后面会专门讲。

1.3 UKF：用sigma点传递分布，绕开雅可比矩阵

UKF的思路完全不同，它不去线性化非线性函数，而是选择对状态分布做近似。既然高斯分布完全由均值和协方差描述，那我就选取一组确定性采样点（称为sigma点），这些点经过非线性函数传递后，再用加权统计的方法还原出新的均值和协方差。

这个过程叫无迹变换（Unscented Transform）。具体来说，如果状态维度是n，那么需要选取2n+1个sigma点，每个点带有一个权重。这些点经过非线性函数 h(x) 或者 f(x) 后，不需要计算任何雅可比矩阵，直接对变换后的点集求加权均值和加权协方差即可。

UKF的理论精度比EKF高一阶：EKF的局部线性化对均值和协方差的估计误差是三阶量级，而UKF对均值的估计精度可以达到三阶，协方差达到二阶。更重要的是，UKF不需要推导雅可比矩阵，只要知道非线性函数本身就能实现，这在实际工程中节省了大量开发时间。

代价是增加了少量计算量，因为每个时刻要多算2n+1个点的函数值。不过对于大多数嵌入式平台来说，这个开销完全可接受。

1.4 PF：用随机粒子逼近任何分布

PF的思路和前面两个截然不同——它干脆放弃了对高斯分布的假设。粒子滤波的核心思想是用一组带权重的随机样本（粒子）来逼近状态的后验概率分布。只要粒子数量足够多，理论上可以逼近任意分布，不管它是高斯、双峰还是更复杂的形状。

标准的粒子滤波流程是：从初始分布采样得到一批粒子，然后每个时刻先通过状态方程预测所有粒子的位置，再用量测更新计算每个粒子的权重（通常用似然函数计算），最后根据权重对粒子进行重采样，解决粒子退化问题。

这个方案最明显的优势是能处理强非线性、非高斯场景；缺点是计算量远大于EKF和UKF，而且在低维状态空间、高斯噪声场景下，它的精度不一定拼得过UKF，因为粒子数有限时，随机采样的统计波动反而会引入误差。

粒子数怎么选是个经验活。太少会退化甚至发散，太多则实时性堪忧。我在自己测试中常用500到5000个粒子，具体取决于状态维度和系统的非线性强度。后面在性能对比部分，我会给出具体的测试数据。

2. 仿真场景设计与Matlab实现核心代码

2.1 测试场景：纯方位目标跟踪模型

为了公平比较三种算法，我选了一个经典且非线性强烈的场景：二维平面内，目标做近似匀速直线运动，观测站固定在原点，只能测量目标相对观测站的方位角。

状态向量定义为 x=[px, vx, py, vy]^T，即x方向位置和速度、y方向位置和速度。状态转移方程是线性匀加速运动模型：

px(k+1)=px(k)+vx(k)·T

vx(k+1)=vx(k)

py(k+1)=py(k)+vy(k)·T

vy(k+1)=vy(k)

其中 T 是采样周期，取 T=1s。过程噪声 w(k) 假设为高斯白噪声，协方差矩阵为 Q。

量测方程为：

z(k)=atan2(py(k), px(k)) + v(k)

也就是方位角等于目标位置的反正切值，v(k) 是量测高斯白噪声，方差为 R。

这个模型的非线性体现在量测方程里：反正切函数在目标接近原点时斜率变化剧烈，对滤波算法的非线性处理能力是很大的考验。我故意让目标从x=1000m, y=1000m附近出发，斜向靠近观测站附近，这样方位角会在短时间内大幅变化，三种算法的差异会非常明显。

仿真时长设100秒，每种算法跑50次蒙特卡洛，统计平均均方根误差和耗时。

2.2 EKF实现关键代码

EKF的核心是预测和更新两步，更新时需要量测方程的雅可比矩阵。对于这个场景，量测方程为 z=atan2(py,px)，求偏导后得到：

H=[-py/(px^2+py^2), 0, px/(px^2+py^2), 0]

这里要注意的是，px 和 py 用预测值代入。很多人在这一步直接用当前量测更新后的状态值，顺序搞反就会出问题。

EKF的Matlab核心代码如下：

function [x_pred, P_pred] = ekf_predict(x, P, F, Q) x_pred = F * x; P_pred = F * P * F' + Q; end function [x_upd, P_upd] = ekf_update(x_pred, P_pred, z, R) px = x_pred(1); py = x_pred(3); d2 = px^2 + py^2; H = [-py/d2, 0, px/d2, 0]; z_pred = atan2(py, px); S = H * P_pred * H' + R; K = P_pred * H' / S; x_upd = x_pred + K * (z - z_pred); P_upd = (eye(4) - K * H) * P_pred; end

主循环里就是交替调用这两个函数。我在实际运行时发现，如果初始协方差矩阵 P 设置得过大，EKF在前几步容易出现数值振荡，所以一般会把初始P的对角元设为目标初始位置误差的平方，比如10000左右，而不是随意给个大数。

2.3 UKF实现关键代码

UKF不需要雅可比矩阵，只需要在预测和更新时生成sigma点。以更新步骤为例，核心代码如下：

function [z_pred, S, Pxz] = unscented_transform(x_pred, P_pred, f_handle, Q, alpha, beta, kappa) n = length(x_pred); lambda = alpha^2 * (n + kappa) - n; % 生成sigma点 sqrtP = chol((n + lambda) * P_pred, 'lower'); X = zeros(n, 2*n+1); X(:,1) = x_pred; for i = 1:n X(:, i+1) = x_pred + sqrtP(:, i); X(:, n+i+1) = x_pred - sqrtP(:, i); end % 通过非线性函数传递sigma点 Y = zeros(size(X)); for i = 1:2*n+1 Y(:, i) = f_handle(X(:, i)); end % 加权统计 Wm = [lambda/(n+lambda), repmat(1/(2*(n+lambda)), 1, 2*n)]; Wc = Wm; Wc(1) = Wm(1) + (1 - alpha^2 + beta); z_pred = Y * Wm'; S = zeros(size(z_pred,1)); Pxz = zeros(n, size(z_pred,1)); for i = 1:2*n+1 dz = Y(:,i) - z_pred; dx = X(:,i) - x_pred; S = S + Wc(i) * (dz * dz'); Pxz = Pxz + Wc(i) * (dx * dz'); end S = S + R; % 加上量测噪声 end

UKF的sigma点参数 alpha、beta、kappa 对滤波精度有直接影响。一般取 alpha=1e-3，beta=2，kappa=0 是比较稳妥的默认值，但如果你发现滤波结果对初值敏感，可以适当调大 alpha 到 0.1 左右试一试。

2.4 PF实现关键代码

粒子滤波的实现比前两者复杂一些，核心步骤包括：粒子预测、权重计算、归一化、重采样。这里给出最常用的SIR（序贯重要性重采样）粒子滤波的核心代码：

function [particles, weights] = pf_update(particles, weights, z, f_handle, Q, R) N = length(weights); n = size(particles, 1); % 粒子预测：每个粒子通过状态方程加噪声 for i = 1:N particles(:, i) = f_handle(particles(:, i)) + mvnrnd(zeros(n,1), Q)'; end % 权重计算：用似然函数 for i = 1:N px = particles(1, i); py = particles(3, i); z_pred = atan2(py, px); innov = z - z_pred; % 确保角度差在[-pi, pi]内 innov = atan2(sin(innov), cos(innov)); weights(i) = weights(i) * exp(-0.5 * innov^2 / R) / sqrt(2*pi*R); end % 归一化权重 weights = weights / sum(weights); % 重采样 N_eff = 1 / sum(weights.^2); if N_eff < N * 0.5 [particles, weights] = resample(particles, weights); end end

重采样函数通常用系统重采样，实现简单且效果不错。这部分代码不复杂，但要注意几个细节：角度差一定要处理成[-pi, pi]区间，否则在目标跨过±180度分界线时权重会算错，导致滤波突变；重采样阈值的选取也会影响性能，设太严格会导致粒子多样性快速下降，设太宽松则粒子退化严重。

3. 三种算法在测试场景中的性能对比

3.1 估计精度对比：RMS误差

在相同轨迹、相同噪声条件下，用500个粒子的PF与EKF、UKF做了50次蒙特卡洛仿真，统计了位置估计的均方根误差（RMSE）。结果如下表：

这个结果可能和很多人想的不一样：粒子滤波并不是精度最高的。原因在于这个场景是高斯噪声，而且状态维度只有4维，UKF的确定性采样在这种条件下反而比随机采样更高效。PF要发挥优势，需要非高斯噪声或者状态分布呈现明显多峰的场景。

不过，PF的分布表示能力确实更强大。我在另一个双峰量测模型下做过测试（两个传感器给出两个方位角），EKF和UKF直接失效，而PF还能保持有效跟踪。选择算法一定要看具体场景，而不是盲目追求“更高级”的算法。

3.2 一致性对比：NEES检验

滤波一致性衡量的是滤波器给出的协方差估计是否与实际误差匹配。如果一个滤波器报出的协方差很小，但实际误差很大，说明它过于自信，工程上这是很危险的，因为你会基于错误的置信区间做决策。

我计算了三种算法的归一化估计误差平方（NEES），理论值在状态维度为4时约为4。测试结果：UKF的NEES最接近理论值，大约在3.8到4.3之间波动；EKF的NEES偏高，达到6到8，说明它的协方差估计偏小、过于乐观；PF的NEES也不稳定，在粒子数不足时波动特别大，增加粒子数后才慢慢靠近理论值。

这个差异说明了UKF在协方差传播上的优势：sigma点方法对非线性变换的统计特性刻画得更准确，协方差估计自然更可信。EKF的雅可比线性化本质上是对变换的局部逼近，协方差的传播误差会随非线性强度增大而放大。

3.3 计算量与实时性对比

我在同一台机器（Intel i5，MATLAB R2023b）上统计了100秒仿真共100步的运行时间：

从实时性角度看，EKF和UKF都适合部署在资源受限的平台上，PF则更适合离线分析或者有较强计算能力的系统。如果你的系统有严格的毫秒级实时要求，PF基本可以放弃。

我在测试中还试过用10000个粒子的PF，耗时直接飙到60毫秒以上，而且精度提升并不显著。这说明PF在粒子数达到一定数量后存在边际效应，盲目增加粒子数只会浪费算力。

3.4 鲁棒性对比：初值误差与噪声失配

另一个在实际工程中很关键的性能指标是对模型失配的鲁棒性。我做了两组测试：第一组把初始位置误差加到500米，第二组把过程噪声Q增大到原值的10倍但滤波器内部保持原值。

结果是：EKF在初始误差大时收敛速度最慢，甚至出现短暂的负值位置估计（明显不合理但滤波器自己没发现）；UKF适应性强，初值误差大时也能较快收敛；PF在初始误差大的情况下表现也还可以，但粒子数少时会出现粒子坍缩到错误区域的问题。Q增大但滤波器不知情时，EKF和UKF都能维持跟踪但精度下降，PF受影响最大，因为粒子分布逐渐偏离真实状态，重采样后又无法补救。

这些测试说明了一个结论：如果系统模型比较可信且非线性不极端，UKF是性价比最高的选择；如果系统存在严重非线性或噪声模型不明确，PF的多模态表示能力能提供额外的鲁棒性，但代价是计算量和调参难度。

4. 常见问题与实战避坑指南

4.1 EKF雅可比矩阵推导错误的典型表现

EKF最大的坑就是雅可比矩阵容易算错。常见错误包括：偏导项漏掉、顺序搞反、复合函数链式法则不对等。如果滤波结果出现“看起来在跟踪但误差很大”或者“突然跳变”的现象，八成是雅可比矩阵的问题。

我的建议是：在写代码前，务必用符号推导工具（比如Matlab的Symbolic Toolbox）验证一遍雅可比表达式。代码里可以在初始化阶段加一个数值微分检查，用中心差分法近似雅可比，和解析雅可比对比，如果差异超过1e-3，多半是解析推导有问题。这个检查30秒就能写完，能省下大量排查时间。

4.2 UKF协方差矩阵不正定的处理

UKF实现中，生成sigma点需要计算 sqrt((n+lambda)P)，如果P矩阵不是正定对称，Cholesky分解就会报错。这个问题在长时间运行后特别容易出现，原因是数值误差累积导致协方差矩阵丢失正定性。

解决办法有几种：最常用的是在每次递推后对P做一个对称化处理，即 P=(P+P')/2；更彻底的方法是用特征值分解代替Cholesky分解，对负特征值进行截断或取绝对值。我在代码里用的是第二种方法，虽然在运行时略微增加了计算开销，但稳定性好很多。

另外要留意的是，在sigma点权重的计算公式中，如果 lambda 选得不当，可能出现负权重，导致协方差矩阵的更新结果非正定。这时需要调整 alpha 或者 kappa 参数。

4.3 粒子滤波的重退化与多样性丧失

粒子滤波最常见的两个问题是“粒子退化”（少数粒子权重极大，其他粒子权重趋近于0）和“粒子多样性丧失”（重采样后大量重复粒子，导致无法表达真实分布）。

解决退化问题的核心是重采样，但重采样本身又会带来多样性问题。一个实用的改进方法是使用正则化粒子滤波（Regularized Particle Filter），在重采样后对粒子施加一个小的高斯扰动，增加多样性。如果你不想改算法，也可以采取一个简单土办法：重采样后的粒子在更新过程中加入一个极小的噪声，相当于人为维持多样性。

还有一个细节：计算有效粒子数 N_eff 时，如果粒子数很少（比如少于100），N_eff 的估计波动很大，阈值判断容易误触发。这种情况下建议固定每一代都进行重采样，而不是依赖自适应阈值。

4.4 噪声协方差Q和R的调参实战经验

滤波器的性能在很大程度上取决于Q和R的设置，而不是算法本身。很多人拿着默认参数直接跑，发现效果差，就怪算法不行，其实问题往往出在噪声参数上。

Q和R设置的基本原则是：Q反映你对系统模型的信任程度，Q越大表示模型越不可靠、滤波器越依赖量测；R反映量测噪声的方差，R越大表示量测越不可靠、滤波器越依赖预测。两者之间的比值（Q/R）决定了滤波器对量测和模型的权重分配，比它们的绝对值更重要。

一个务实的调参方法是：先用实际数据估计量测噪声方差R（比如记录静止状态下传感器的测量抖动），然后从小到大扫描Q，找到估计误差最小的值。不要追求理论最优，工程上“够用”比“最优”更重要。我在这个场景测试中，Q取了0.1，R取了1，效果就很稳定。

4.5 角度量测的周期性处理

纯方位跟踪还有一个隐蔽的坑：方位角的量测值在±180度之间存在跳变。如果目标从179度运动到-179度，角度差的绝对数值是358度，但实际变化只有2度。如果滤波器不做周期性处理，这个跳变会被当作一个巨大的量测误差，导致滤波结果剧烈震荡甚至发散。

处理办法是在计算新息（innovation）时，把角度差转换到[-pi, pi]区间。代码我已经在PF部分给过了，EKF和UKF的量测更新部分也要同样处理。

% 角度差归一化 innov = z - z_pred; innov = atan2(sin(innov), cos(innov));

很多教科书上的例子都回避了这个细节，但实际做仿真和工程时几乎一定会遇到。我在第一次仿真中就没处理这个，结果目标越过分界线时滤波误差突然爆炸，排查了很久才发现原因。

4.6 代码性能优化的几点建议

如果仿真参数很大（比如跑1000次蒙特卡洛），Matlab代码的性能就很重要。这里分享几个优化技巧：

一是避免在循环中动态分配数组。粒子滤波的循环次数等于粒子数，如果循环体内有数组拼接操作，整体速度会慢一个数量级。预分配粒子存储矩阵，在循环外一次性申请好，能显著提速。

二是向量化计算。粒子更新中，如果状态方程和量测方程可以向量化，尽量对整个粒子集用矩阵运算，而不是逐个粒子循环。比如纯方位跟踪里，所有粒子的方位角可以用一条atan2(particles(3,:), particles(1,:))一次性算完。

三是用parfor跑蒙特卡洛外循环。如果你的机器有多核，500次蒙特卡洛仿真用parfor能节省一半以上的时间。不过要注意的是，随机数生成要在每次循环内部用不同的流，否则多个 worker 生成的随机序列可能相关。

四是关闭不必要的数据可视化。仿真过程中如果实时画轨迹图，会严重拖慢运行速度。建议在仿真阶段只存数据，最后再统一画图。

5. 拓展：如何把这段代码应用到其他非线性系统

看到这里，你可能已经发现了，这个仿真的框架其实不局限于纯方位跟踪。状态方程和量测方程换成你自己的模型，噪声矩阵也替换掉，整个流程可以原样跑通。

我自己常用的一个转换套路是这样的：拿到一个新系统，先确定状态维度和量测维度，写出状态方程和量测方程的Matlab函数句柄；然后准备好初始状态均值、初始协方差、Q和R；接着把三种算法的函数套上去，跑一遍仿真看结果是否符合预期；最后做蒙特卡洛统计。整个过程基本不需要改动滤波算法的核心代码，只需要适配模型函数。

如果你用的是组合导航概念，状态量是位置速度姿态（9维以上），UKF的sigma点数量会随维度线性增长，计算量大约变成9维的5倍左右，仍然可控；PF在9维状态下需要上万粒子才能维持稳定，计算量会非常惊人，这时就要慎重考虑了。

最后再分享一个我个人的判断习惯：拿到一个非线性滤波问题，第一选择我会用UKF，因为它不需要推导雅可比又有较高的精度；如果系统是非高斯噪声或者量测存在多峰，就换PF；如果嵌入式平台算力有限且模型非线性不是很强，才用EKF。这套决策逻辑在多个项目中帮我节省了不少时间，希望你用得上。