七种卡尔曼滤波变体在雷达目标跟踪中的原理与Matlab实现

做雷达数据处理那几年，我最怕的就是目标一旦机动，卡尔曼滤波器的航迹就开始“发飘”。明明量测数据分布还算正常，滤波器自己却越走越偏，甚至直接把目标跟丢。后来我把手头这套“基本离散Kalman、固定增益Kalman、平方根Kalman、遗忘因子Kalman、扩大P Kalman、自适应Kalman、有限K减小Kalman”的Matlab代码完整过了一遍，才真正理解每一种变体不是花架子——它们对应着雷达跟踪里最常遇到的数值问题、机动响应问题和噪声未知问题。这套代码适合正在做目标跟踪、组合导航、状态估计相关课题的学生和工程师，也适合刚接触卡尔曼滤波、想系统搞懂各变体之间区别的初学者。本文就把这七种滤波器的原理差异、适用场景、Matlab实现关键点一次性讲透，并附上可参考的工程实践方案。

1. 状态估计与卡尔曼滤波：为什么雷达轨迹会“发飘”

1.1 卡尔曼滤波在雷达跟踪里的基本逻辑

雷达对目标做跟踪，本质上干的是这么一件事：每个扫描周期，雷达给出一组量测（距离、方位、俯仰），然后我们要根据这些量测估计目标当前的位置和速度，并预测下一个周期的位置。由于雷达量测必然带噪声，目标运动又可能随时改变（转弯、加速、减速），我们不太可能直接拿量测点去当航迹点用。拿噪声很大的量测直接连线，画出来就是一把“散弹枪”，完全没法看。

卡尔曼滤波的做法，是用一个递推估计过程把量测信息和目标运动模型结合起来。状态向量里放位置和速度，运动模型描述目标大致怎么走，量测模型描述雷达怎么观测目标。每一拍先由运动模型预测一步，再用量测更新修正预测结果。预测和更新的权重由协方差矩阵P和增益矩阵K决定——P大说明当前估计不确定，K就大，滤波器会更相信量测；P小说明模型预测可信，K就小，滤波器会更相信预测。

这套逻辑在匀速直线运动、量测噪声平稳的场景下非常好使。标准Kalman递推只有五条方程：状态预测、协方差预测、增益计算、状态更新、协方差更新，几十行代码就能跑起来，Matlab里实现更是半小时的事。

但是，工程里一旦遇到下面三类问题，标准Kalman就会明显力不从心：

• 目标机动：匀速直线模型是假的，目标一旦拐弯，模型预测会持续偏置，而滤波器此时比较信任模型，航迹就跟不上目标。
• 数值退化：长时间运行时，P矩阵受舍入误差影响会失去对称正定性，甚至变成负定，滤波结果直接发散。
• 噪声参数不准：Q和R设得不合理，滤波器要么过于平滑导致滞后，要么过于激进导致抖动。

这三点，恰好对应这套代码里七种滤波器的设计动机。

1.2 标准卡尔曼滤波的“槽点”与变体谱系

先给这七种滤波器画个谱系，心里有个整体框架就好理解了。

第一类，冲着“模型不准、目标会机动”去的有遗忘因子Kalman、扩大P Kalman和有限K减小Kalman。它们思路本质上都是降低老信息权重、提升新量测在滤波结果中的参与度，只是实现手段不同：遗忘因子给预测协方差乘个衰减因子；扩大P在检测到异常时人为放大P矩阵；有限K减小则直接给增益K设下限，不让它缩到太小。

第二类，冲着“噪声参数未知、不好调”去的是自适应Kalman。它利用量测新息的统计特性来在线估计量测噪声方差R，甚至过程噪声方差Q，减少人工调参的负担。

第三类，冲着“数值稳定性”去的是平方根Kalman。它不直接递推P矩阵，而是递推P的Cholesky分解因子，从机理上保证协方差矩阵的非负定性，适合长时间连续运行。

固定增益Kalman比较特殊，它是在滤波已经进入稳态以后，把K矩阵固定下来不再每步重算，牺牲一点自适应性换计算量的大幅下降。

这么一看就清楚了：很多教材只讲标准Kalman，但工程上真正可用的，往往是这些变体在特定场景下的组合。下面一个一个拆。

2. 七种卡尔曼滤波变体逐一拆解

2.1 基本离散Kalman：所有变体的地基

基本离散Kalman是整个代码库的起点。它假设系统满足线性离散模型：

x(k) = F * x(k-1) + w(k-1) z(k) = H * x(k) + v(k)

其中w和v是互不相关的高斯白噪声，协方差分别为Q和R。

递推过程分两步。预测步：

x_pred = F * x_est P_pred = F * P * F' + Q

更新步：

K = P_pred * H' * inv(H * P_pred * H' + R) x_est = x_pred + K * (z - H * x_pred) P = (I - K * H) * P_pred

这套递推在Matlab里写起来特别“直给”，就是矩阵乘来乘去。但这个基本形式是整个代码库的地基，后面所有变体都只是在这五条方程上做局部改动，有的改预测步，有的改增益步，有的改协方差更新步。

用基本Kalman做雷达跟踪，最重要的不是把方程背熟，而是把状态向量、F、H、Q、R都定义对。目标做匀速直线运动时，状态向量通常取[x, y, vx, vy]，F是常速运动模型，H是位置观测矩阵。这一步做对了，基本Kalman才有意义，否则后面所有变体都是在错误的模型上做精细化。

2.2 固定增益Kalman：算力受限场景的“快刀”

固定增益Kalman的思路是：标准Kalman在递推一段时间后会进入稳态，P矩阵不再显著变化，K矩阵也收敛到一个常数。既然K基本稳定，那就不如直接把这个稳态K算出来、写死，省掉每步求逆运算。

K矩阵的计算包含H * P_pred * H' + R的求逆，在状态维数高、采样率高或者嵌入式平台上，这一步是实时的负担。固定增益后，整个递推只剩：

x_pred = F * x_est x_est = x_pred + K_fixed * (z - H * x_pred)

计算量骤减，一个周期内几乎只有矩阵乘和加减法。

工程上确定K_fixed有两种做法。一种是用离线脚本把标准Kalman跑几十到几百步，取K的收敛值。另一种是求离散代数Riccati方程的解。后一种理论更严谨，不过那种表达式确实不太直观。我在实践中更推荐前一种：直接拿一段与真实场景一致的量测数据，先跑标准Kalman，观察K矩阵对角线收敛到什么量级，然后取一个安全的保守值。

固定增益的代价也很清楚：如果目标运动模型变了，比如从匀速突然变成转弯，K固定会导致滤波器的机动响应能力被锁死，很难再跟上目标。所以固定增益Kalman适合运动规律相对稳定的目标，比如直线巡航的民航客机、直线段传输的皮带定位，不适合做高机动目标的全程跟踪。

2.3 平方根Kalman：数值稳定性优先

平方根Kalman建立的动机非常实际：协方差矩阵P在递推过程中必须保持对称正定。但计算机里浮点数有舍入误差，长时间递推后，P矩阵的对称性可能被破坏，甚至出现负特征值，滤波器就崩了。

平方根Kalman的基本思路是，不直接递推P，而是递推P的Cholesky分解：

P = S * S'

预测步和更新步都直接作用于S，只在需要K的时候临时重构P。这样哪怕某一步数值很差，S * S'在数学上也天然是非负定的，P矩阵退化的概率被大大降低。

实现上，我常用一种带Joseph形式的平方根更新，虽然代码比标准Kalman多几行，但稳定性实实在在。预测步：

P_pred = F * (S * S') * F' + Q S_pred = chol(P_pred, 'lower')

更新步：

K = P_pred * H' * inv(H * P_pred * H' + R) P_upd = (eye(n) - KH) * P_pred * (eye(n) - KH)' + K * R * K' S_upd = chol(P_upd, 'lower')

这里的Joseph形式是数值线性代数里很经典的处理方式，它不需要假设(eye(n)-K*H)和P_pred可交换，对各种数值条件都更稳健。每步用chol分解强制半正定，是工程上“粗暴但有效”的兜底手段。

平方根Kalman在雷达跟踪里的应用，主要不是提升精度，而是保证长时间连续滤波不“犯病”。雷达系统经常要几百上千秒连续工作，量测周期又短，如果P矩阵中途退化，航迹断点和高低跳跃就都来了。用平方根形式，相当于给滤波器加了一道数值保险。

2.4 遗忘因子Kalman：老数据慢慢“不重要”

遗忘因子Kalman的引入根源于一个朴素的直觉：目标已经拐弯了，过去那些匀速直线模型下的量测，就不该再对未来估计有太大影响。标准Kalman里，P矩阵和K矩阵的取值隐含了历史所有量测的“投票权”，历史越长，新量测的话语权越低。这在目标状态平稳时是好事，但目标一旦机动，就是灾难。

遗忘因子的改法只有一处，在协方差预测步：

P_pred = alpha * F * P * F' + Q

其中alpha是一个略大于1的因子，比如1.01到1.05。这个乘数让预测协方差在每次递推中被人为放大一点，等效于历史量测的权重被指数级遗忘。alpha越大，遗忘越快，滤波器对新鲜量测响应越敏感，但噪声抑制能力也越差。

具体数值的含义，可以从信息角度理解。遗忘因子相当于每拍衰减一部分历史信息，信息量随时间呈指数衰减，所以alpha再小也会产生效果，只是快慢不同。实际用的时候，我一般先把alpha设成1.02，然后观察航迹在机动段的延迟情况，如果跟不上就往上调，如果太抖就往下调。

遗忘因子Kalman最大的价值是方法极简，只动一个数，非常容易接入现有系统。它的缺点是整个状态向量共享同一个遗忘因子，位置和速度的遗忘速度没法分离。某些场景下，这会导致速度估计偏震荡。

2.5 扩大P卡尔曼：机动检测后的“急刹车”

扩大P Kalman和遗忘因子的目标相同，都是应对机动，但策略完全不同。它不是持续放大P矩阵，而是先做机动检测，确认目标确实有异常机动时，才人为放大P矩阵。

检测思路用的是新息特性。定义新息：

d = z - H * x_pred

在模型匹配、噪声分布正常的情况下，d服从零均值高斯分布，协方差为：

S_d = H * P_pred * H' + R

于是可以构造归一化新息平方：

gamma = d' * inv(S_d) * d

gamma近似服从自由度为量测维数的卡方分布。如果gamma明显超过某个阈值（比如在自由度2下取9.21，对应置信度99%），就说明实际量测与模型预测明显不符，大概率是目标机动了。

检测到机动之后，扩大P的做法是：

P_pred = rho * P_pred

rho通常取3到10。P被放大后，下一拍的K也会跟着变大，滤波器就敢大幅更新状态，快速跟上机动目标。

扩大P Kalman的关键在阈值和放大系数怎么设。阈值太小，机动检测误报多，滤波器经常处于“亢奋”状态，噪声抑制能力下降；阈值太大，机动检测迟钝，等检测到时机动已经走了一段。我自己常用的阈值在量测维数m下取 chi2inv(0.99, m)，放大系数先取5，在仿真里做一次扫描调优。

这套策略的工程价值是“平时省着用、关键时敢用力”，比遗忘因子那种全程持续放大更省信息。缺点是要额外处理机动检测逻辑，且如果目标频繁做小幅机动，检测会反复触发，滤波结果会显得躁动。

2.6 自适应Kalman：噪声未知就自己量

自适应Kalman处理的是另一个痛点：Q和R靠人工拍脑袋设定，拍得不准滤波效果就拉胯。实际雷达系统里，量测噪声方差可能随目标距离、环境杂波变化，很难用一个固定的R描述全程。

自适应的常见工程实现是用新息序列实时估计R。原理基于前面构造的量测新息协方差：

S_d = H * P_pred * H' + R

在滑动窗口内统计实际新息样本协方差Cov_d，然后用：

R_hat = Cov_d - H * P_pred * H'

来反推量测噪声方差。简单实现可以在每个时刻维护一个长度为N的滑动窗口，存最近N个新息向量，每N拍更新一次R_hat，替换掉滤波器里的固定R。

实际操作中要注意两点。第一，剔除故障值，窗口内的新息如果出现很大的离群点，说明可能是目标机动而不是噪声变大了，这种样本应该排除，否则R_hat会被严重拉大，滤波器会过度平滑。第二，R_hat必须做下限保护，比如每个分量不小于初始R的0.1倍，防止R_hat趋近零导致K炸掉。

自适应Q比自适应R更麻烦，因为Q不好从量测直接反推，误调很容易让P矩阵漂移导致发散。我做工程时，如果必须自适应Q，会用一个约束很强的慢变估计器，并且给P矩阵加一个下限保护，防止P收敛到接近零。

在雷达跟踪里，自适应R的收益往往很明显，特别是目标从近距离转移到远距离，或者从高信噪比区域进入低信噪比区域，量测噪声确实在变，自适应Kalman能明显减少航迹抖动和丢失。

2.7 有限K减小Kalman：给增益下限做保险

有限K减小Kalman这个名字，我理解它强调的就是给增益K加下限。标准Kalman在收敛后，P会趋近一个稳态值，K也随之缩到很小。这个状态下滤波器对量测反馈很弱，目标一旦机动，拉回来需要几十拍，航迹表现为明显滞后。

“有限K减小”的做法，是给K的对角元素或者范数设置一个下限，当计算出的K小于下限时，取下限值参与更新：

K = max(K, K_min)

这样滤波器的增益不会因为时间推移而无限缩小，始终保留一定比例的“新鲜量测接入能力”。

为什么这么做在工程上很实用？因为真实目标经常是“大部分时间平稳、偶尔机动”，而标准Kalman在平稳期把K压得特别低，等机动时刻来了根本来不及恢复。有限K减小相当于给航迹加了一根软弹簧，不让她完全僵硬。

K_min的设定要匹配量测噪声水平。量测噪声大，K_min要定得小一点，否则航迹会被噪声牵着走。量测噪声小，K_min可以适当大一点。我的做法是先跑标准Kalman，观察收敛后K对角元素的量级，然后把它乘以0.3到0.5作为K_min的初始值，再在仿真中调整。

这种方法的优点是实现简单到极致，只需在增益计算后加一行max操作。缺点是它和遗忘因子一样，不加区分地对各状态分量统一设限，无法对各通道独立精细调节。

3. 实操环节：在Matlab里把7个滤波器跑起来

3.1 场景建模与观测生成

为了把这七种滤波器放在同一平台上对比，我建了一个带中途转弯的雷达目标跟踪场景。目标先沿x轴匀速直线运动，在第50秒突然做一个接近90度的转弯，转弯后再匀速直线运动一段时间，模拟典型的高机动目标。

状态向量取四维：[px, py, vx, vy]，采样周期dt=1秒，总时长150步。匀速运动模型的状态转移矩阵：

F = [1, 0, dt, 0; 0, 1, 0, dt; 0, 0, 1, 0; 0, 0, 0, 1];

观测量取目标的平面位置[px, py]，所以观测矩阵：

H = [1, 0, 0, 0; 0, 1, 0, 0];

量测噪声为高斯白噪声，标准差取10米，所以R为：

R = 100 * eye(2);

过程噪声反映目标加速度的随机变化。用白噪声加速度模型，Q的计算方式为：

q = 0.5; 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];

生成真实航迹后，按每拍叠加观测噪声得到量测序列。后面所有滤波器都吃同一组量测数据，保证对比公平。

3.2 七种滤波器的核心实现差异

七种滤波器共用同一个“预测-更新”骨架，区别只在中间几行。代码骨架可以这样组织：

for k = 1:N % 预测 x_pred = F * x_est(:, k-1); P_pred = F * P * F' + Q;

% 变体差异点A：遗忘因子在这里乘alpha % 变体差异点B：扩大P在这里做机动检测与放大 % 变体差异点C：平方根形式在这里用chol因子 % 增益 K = P_pred * H' / (H * P_pred * H' + R); % 变体差异点D：固定增益在这里直接用K_fixed % 变体差异点E：有限K减小在这里做max(K, K_min) % 更新 z_pred = H * x_pred; x_est(:, k) = x_pred + K * (z(:, k) - z_pred); P = (eye(4) - K * H) * P_pred;

end

固定增益Kalman的K_fixed需要离线先取。我的做法是先用标准Kalman跑完整个场景，取第80到第150步K矩阵各元素的平均值，作为后续固定增益使用的K_fixed。这里取均值而不是最后一步的值，是为了减小瞬时波动的影响。

平方根Kalman的实现差异在P的表示。初始时计算：

S = chol(P0, 'lower');

然后在每拍预测时先从S重构P_pred再做chol分解，更新时用Joseph形式再求一次chol。虽然多算了几个矩阵乘法，但换来的是长期稳定。

遗忘因子Kalman的关键行就一句：

P_pred = alpha * (F * P * F') + Q;

alpha取1.02。如果目标是高机动，可以取1.05，但要注意航迹会变“毛糙”。

扩大P Kalman在预测后、增益前插入机动检测。用chi2inv(0.99, 2)作为阈值，归一化新息平方超过阈值时，将P_pred乘上放大系数5。

自适应Kalman我用滑动窗口法，窗口长度取30。窗口内保存最近30拍的归一化新息向量，每30拍利用新息样本协方差更新R_hat，并重新赋值给R。R_hat取出后要加一个下限保护：

R_hat = max(R_hat, 0.1 * R0);

有限K减小Kalman在增益计算后执行：

K = max(K, K_min);

K_min我取标准Kalman稳态K对角元素的0.3倍，两组对角分别对应x位置和y位置的增益下限。注意这里max是逐元素比较，不是取整体范数的最大值。

3.3 结果评估：误差与稳健性怎么比

跑完之后，最重要的就是对比结果。只看一张航迹图容易误判，我习惯做四类评估：

第一，真实轨迹与估计轨迹叠加图。用plot画真实航迹，用hold on叠加量测点和滤波航迹，能直观看出滤波器在匀速段和转弯段的贴合度。

第二，位置误差曲线。计算每拍估计位置与真实位置的欧氏距离，plot出来。重点关注转弯发生后第50到第70拍这段，这是各变体拉开差距的地方。

第三，位置误差的均值和最大值。均值反映整体跟踪精度，最大值反映最差情形下的失控程度。比如标准Kalman在这个场景里位置误差均值可能在15到25米之间，但在转弯段的峰值可能冲到60米以上；遗忘因子Kalman能把峰值压低到40米左右，但平稳段误差可能略有增加。

第四，P矩阵的对角元素曲线。P的对角元素收敛过程能看出滤波器是否健康。如果P曲线在某个时刻急剧放大，说明机动检测触发了扩大P或遗忘因子在持续放大，这是设计意图，不用慌。如果P曲线出现负数或NaN，那就是数值稳定性出问题了，要检查是不是哪里把P算成了非正定。

一句话：误差均值看跟踪品质，误差峰值看极端可靠性，P曲线看数值健康度。三个维度综合起来，才能判断一个滤波器在这个场景下到底合不合适。

4. 常见问题与排查技巧实录

4.1 滤波发散：从哪里开始查

滤波发散是卡尔曼滤波实践里最常见的“鬼故事”。表现是估计值突然偏离真实轨迹，然后越走越远，再也拉不回来。排查时我的顺序是固定的。

先看Q、R、P0是不是设置合理。Q设得过大，滤波器会过分相信量测，航迹变成量测噪声的“复印机”；R设得过大，滤波器会过分相信模型，目标机动后完全跟不上。这三兄弟的设置顺序也很重要，我一般先用一段平稳数据把R设定好，再根据目标机动水平调Q，最后用P0吸收初始不确定性。

再看量测数据里有没有野值。雷达在某些角度、某些距离上会出现异常大的偏差，如果没做野值剔除，一个巨大的新息就能把滤波器状态拉飞。工程上一定先做野值检测，用新息卡方检验把超过阈值的新息标记出来，决定是剔除还是限幅。

最后检查模型本身。如果目标做蛇形机动而模型里只有匀速，任何参数都救不回来，得换模型或者上交互式多模型，这是另一个范畴的问题了。

4.2 P矩阵非正定与数值退化

长时间滤波后P矩阵非正定，在Matlab里往往表现为估计结果突然NaN，或者chol函数报错。原因有两类：一类是舍入误差长期累积，一类是代码逻辑里P_pred被算成了非对称矩阵。

前者靠平方根Kalman解决，后者要检查P_pred的计算顺序。MATLAB里矩阵乘法不保证对称，F * P * F'理论上对称，但浮点操作后往往不是完美对称。一个很有效的工程小技巧是每个周期加一行：

P = (P + P') / 2;

强制对称化。虽然治标不治本，但能避免很多莫名其妙的问题。本质上，这也是为什么平方根Kalman在工程里被广泛使用的原因——直接从表示形式上规避了这类问题。

4.3 Q和R怎么定：没时间精调时按比例先搭

很多读者拿到代码后会卡在Q和R的数值设定上。我的习惯是，没有精确标定数据时，先从物理意义出发给个量级。

R可以从雷达的标称测量精度直接换算。比如测距误差标准差是10米，方位误差标准差是0.5度，距离1000米时方位引起的横向误差大约1000 * tan(0.5°)约8.7米，综合一下R对角元可以取100左右，和位置量测标准差10米的平方是一个量级。

Q反映的是过程噪声，也就是目标运动模型没有描述完的部分。目标最大加速度大约多少，可以估算a_max，那么Q的对角元素大致按a_max^2乘以一个时间相关的系数来定。没有太精确的想法时，从标准Kalman开始跑，调q的数量级，先看误差量级变化趋势，再微调。记住一个口诀：Q决定平滑程度，R决定灵活程度，两者比值才是真正影响滤波特性的关键。

4.4 目标突然机动后滤波器“僵住”怎么办

“僵住”是机动响应的典型症状。标准Kalman在平稳段把P压得很小，K也很小，目标一转弯，滤波器却依然按照原来的速度外推，航迹直接画出一条直线穿过去。等K慢慢恢复，目标早跑远了。

这类问题优先考虑三个手段：遗忘因子、扩大P和有限K减小。三者的取舍可以参考下表：

实际工程中，我经常把扩大P和平方根Kalman组合使用：平方根保证数值稳定，扩大P保证机动响应。两个方法修改的位置不同，互不冲突。自适应Kalman则通常作为一个前级，先把R估算准确，再配合其他变体使用。

如果你测试时发现目标一机动，滤波器还是慢半拍，先别急着加大遗忘因子。先画出新息序列，看看新息在机动的第一拍是不是真的远离零。如果新息确实异常但滤波器没反应，说明扩大P的检测没触发，检查阈值是否太高。如果新息本身就不敏感，那问题在前端的观测模型或数据对齐，不是滤波器的锅。

5. 个人经验的一点补充

这套代码我反复读过很多遍，也改过很多版本。如果你和我一样是半路出家搞工程，我建议你不要一上来就用最复杂的变体。先把基本离散Kalman跑通，把Q、R、P0三个参数搞明白，再逐步加入平方根、遗忘因子、自适应这些机制。每一步都只加一个变化，用同一个场景对比前后差别，这样才能真正练出手感。

调参的时候，也别迷信脚本里那一组默认值。雷达频率变了、目标类型变了、甚至量测坐标系变了，参数都得重新审视一遍。尤其是Q这一项，它代表的不是真实噪声，而是你对模型的“不信任程度”，这个度只能靠实际数据反复磨合。

最后再分享一个小技巧：调卡尔曼滤波参数时，不要只盯误差均值看，画一张包含真实轨迹、量测、滤波轨迹的图，用眼睛看一遍。很多时候，误差均值很漂亮，但航迹曲线在机动段明显拖着一条“长尾巴”；反过来也有误差峰值高但曲线形态平稳的。滤波器的价值最终是让航迹曲线既平滑又能跟着目标走，数值指标只是辅助判断的依据。把这些变体都跑一遍，你会发现卡尔曼滤波不是一道公式题，而是一门需要针对场景做权衡的手艺。