卡尔曼滤波飞行轨迹预测：状态建模、Q/R整定与发散排查

简介：面向飞行器轨迹预测与卡尔曼滤波学习者的MATLAB实践包，适合已具备一定信号处理与矩阵运算基础、希望把滤波理论落到代码中的读者。内容围绕线性动态系统建模、状态预测与测量更新展开，可用于处理带噪声的GPS或传感器数据，帮助理解位置、速度与加速度估计以及飞行控制、导航和目标追踪中的滤波流程。压缩包共6个文件，以3个txt数据与说明、1个m脚本、1个docx文档及1个url链接组成，整体约129KB，结构轻量，便于快速查阅与复现实验。已有4690人学习下载。读者可获得卡尔曼滤波核心实现、飞行数据示例、结果可视化与参数调试思路，并据此对比原始轨迹与滤波结果，为进一步尝试EKF、UKF等变体打基础。

1. 卡尔曼滤波：为什么「会自己纠错」的预测器比拟合曲线更可靠

雷达每帧吐出来的坐标都带着噪声，很多人第一反应是把历史点攒起来做一次最小二乘拟合，或者干脆上一个滑动平均。方向一开始就偏了。卡尔曼滤波做飞行物体运动轨迹预测，核心动作并不是平滑，而是拿「运动模型说目标应该在哪个位置」和「传感器说目标在哪个位置」按各自的不可信程度做一次加权，加权系数每帧现算、只用上一帧的结果，不需要存历史数据。这也是它能在机载、弹载这类内存和算力都紧的平台上跑起来的原因。这个机制值得拆开看的地方在于：你只要动过 Q 和 R 两个矩阵，就会亲眼看到同一条轨迹的输出从严重滞后变到剧烈抖动，中间像个黑匣子。玄学感来自没看到协方差在传播什么。下面按状态建模、预测-更新双循环、噪声整定、发散排查的顺序，把 MATLAB 里那几十行代码背后的东西讲透。

2. 状态空间建模：飞行物体的运动怎么写成 F、H、Q 三个矩阵

卡尔曼滤波的第一行代码之前，真正决定成败的是建模。滤波器的估计质量上限由模型决定，算法本身只负责在模型约束下把观测信息揉进去。飞行物体的轨迹预测尤其吃这一点，因为目标可能匀速巡航，也可能突然拉起做机动，模型选错了，后面调参调到天亮也救不回来。

常见的实现思路是把连续物理系统写成状态空间形式：

x(k+1) = F * x(k) + w(k) w ~ N(0, Q) z(k) = H * x(k) + v(k) v ~ N(0, R)

第一个方程叫过程模型，描述目标自己怎么运动；第二个叫观测模型，描述传感器怎么看到它。w 和 v 是两个高斯噪声，Q 和 R 是它们的协方差。卡尔曼滤波的所有推导都建立在这两个假设上，尤其是「噪声零均值、高斯、白」这三条。飞行物体轨迹预测里最容易违反的就是白噪声假设——大气扰动是有相关时间的，不是每帧独立。先记住这个隐患，第 4 章会回来算账。

2.1 状态向量选型：位置、速度、加速度三者的取舍

状态向量 x 装的是「你想估的量」。飞行物体最常用的是六维：

x = [px, py, pz, vx, vy, vz]'

位置三个、速度三个。为什么不加速度？因为加速度在匀速段接近零，把它放进状态里，滤波器会不停地在噪声里「估」出一个不存在的加速度，反而引入抖动。这就是所谓的模型阶数与观测能力的匹配问题：你的传感器只能测位置，却想估加速度，那加速度的估计实际上是靠位置差分推出来的，噪声会被放大一个量级。

但反过来，如果目标确实在做持续加速（比如助推段），六维模型就会出现系统性滞后——滤波器总觉得目标该匀速走，观测却一直在前面，于是新息长期偏正。这时候该上九维匀加速模型（CA）：

x = [px, py, pz, vx, vy, vz, ax, ay, az]'

选择原则很朴素：目标的机动时间尺度如果远大于采样周期，用 CV；如果在一个采样窗口内速度有明显变化，用 CA。中间还有一条路叫「当前统计模型」，把加速度建模成一阶马尔可夫过程，机动强度用一个时间常数量化，工程上用得多，代价是多一个待调参数。

提示：状态维数每加一维，矩阵运算量按平方增长。机载嵌入式平台跑 MATLAB 生成的代码时，六维和九维的实时性差距是能测出来的，别一上来就堆九维。

2.2 状态转移矩阵 F 的推导：匀速模型与匀加速模型差在哪

F 的物理含义是「不给任何新观测，光凭物理规律，状态从这一帧到下一帧怎么变」。三维匀速模型下，位置按速度积分，速度不变：

F = [ I3 T*I3 ] [ 0 I3 ]

T 是采样周期，I3 是三阶单位阵。写成 MATLAB 就是几行赋值。九维匀加速模型则是：

F = [ I3 T*I3 (T^2/2)*I3 ] [ 0 I3 T*I3 ] [ 0 0 I3 ]

位置吃速度的一次项和加速度的二次项，速度吃加速度的一次项。

这里有个容易被忽略的细节：T 必须是真实的帧间时间，不能拿标称帧率硬填。雷达帧率标称 20Hz，实际因为调度抖动，帧间隔可能在 45ms 到 55ms 之间跳。你按 50ms 算 F，等于每帧都给模型灌一个 5ms 的相位误差，几百帧累积下来就是轨迹整体漂移。血泪经验是：时间戳从数据里读，读不到就用实际到达时间，宁可带噪也别用常数。

另一种更严谨的写法是连续模型离散化。先把物理写成微分方程 x' = A x，再用矩阵指数 F = expm(A*T)。MATLAB 一行F = expm(A*T)就够。好处是 A 直接对应牛顿定律，改模型时不容易写错；坏处是 expm 每帧算一次太贵，工程上一般在初始化时算一次存起来，或者用泰勒展开取前几项。

2.3 观测矩阵 H 与噪声协方差 R 的落地

H 描述「状态里的哪些分量被传感器直接看到」。只测位置的话：

H = [ I3, 0(3x3) ]

在 MATLAB 里就是H = [eye(3), zeros(3)]。它的作用是把六维状态投影到三维观测空间，让预测的观测值和真实观测值能相减。

R 是观测噪声协方差，来源应该是传感器的实际指标，不是拍脑袋。做法是让目标静止，采几百帧，算每个通道的方差，对角线上填进去。如果三个轴噪声水平不同（比如测距精度和测角精度换算到笛卡尔后量级差很多），R 就该是非对角的完整矩阵，不能只填对角。R 填小了，滤波器会过度信任观测，输出跟着噪声一起抖；R 填大了，滤波器会无视观测，输出变成模型的自说自话，目标一机动就跟丢。

还有一个隐藏的坑：很多传感器原始输出是极坐标（距离、方位、俯仰），先转成笛卡尔再滤波是最省事的写法，但极坐标下的高斯噪声经过非线性变换后不再是高斯的，转出来的 x/y/z 噪声有偏、有相关。这个偏差在远距离、大角度时尤其明显，会以「系统性偏移」的形式混进 R 里，让 R 怎么调都不对。稳妥的做法要么在极坐标下做非线性滤波（第 6 章展开），要么用去偏转换公式把偏差补回去。

建模阶段结束时，手上应该有 F、H、Q、R 四个矩阵和初始状态 x0、初始协方差 P0。这六个东西定了，后面的滤波循环只是机械运算。

3. 预测-更新双循环：协方差矩阵到底在传播什么

卡尔曼滤波让人迷糊的地方是 P 这个协方差矩阵。x 好理解，就是估计值；P 是估计值的不确定度，它才是整个算法真正在「算」的东西。K 由 P 决定，P 又由上一帧的 P 和 Q 决定，整条链路是一个自洽的闭环。看懂 P 的传播，这个算法就没有黑匣子了。

3.1 预测步：为什么是 F P F' + Q

预测步干两件事：

x_pred = F * x_prev P_pred = F * P_prev * F' + Q

第一个式子是状态的惯性外推。第二个式子值得多说两句。P 是「我对 x 有多不确定」，当 x 经过线性变换 F 之后，不确定度也要跟着变换。线性变换下协方差的传播规律就是 F P F'，这在概率论里是标准结论：若 y = Fx，则 Cov(y) = F Cov(x) F'。而过程噪声 w 是这一帧新注入的不确定性，方差独立叠加，所以加 Q。

理解这一点的关键在「不确定性只会增加」。没有观测的时候，P 只会因为 F P F' 和 Q 而变大——这是符合直觉的，你闭着眼睛推演目标位置，推得越久越不敢确定。F 中速度乘以 T 那一项，会让位置的不确定度里混入速度的不确定度，所以 P 的非对角块会越来越显著，这在物理上对应「速度估不准，位置就越走越偏」。

Q 的构造也不是随便填。对六维 CV 模型，假设加速度是白噪声、谱密度为 q，标准做法是：

Q = q * [ (T^4/4)*I3 (T^3/2)*I3 ] [ (T^3/2)*I3 T^2*I3 ]

位置块是 T 的四次方，速度块是 T 的平方，交叉项是三次方。这个结构的由来是把加速度积分两次得到位置、积分一次得到速度，然后对白噪声做积分求方差。工程上很多人图省事直接Q = q*eye(6)，这等于给位置和速度注入同样量级的不确定性，物理量纲都不对，调出来的参数自然没法迁移到别的采样率上。q 的量纲是加速度谱密度，单位 m²/s³，取值应该和目标的最大机动加速度量级挂钩。

3.2 卡尔曼增益：K 是「最优权重」而非经验系数

更新步的第一件事是算新息：

y = z - H * x_pred S = H * P_pred * H' + R K = P_pred * H' / S

y 是「观测减预测」，S 是 y 的协方差（预测的不确定度投影到观测空间，再加上传感器噪声）。K 的表达式看着像矩阵除法，其实是在做一个加权平均：如果 P_pred 大（我对预测没底），K 就大，结果偏向观测；如果 R 大（传感器不准），K 就小，结果偏向预测。

这就是开头说的「按不可信程度加权」的数学形式。而且它是最优的——在「噪声高斯、模型线性」两个前提下，K 使估计误差的协方差迹最小。这个最优性可以被证明，不是经验公式。换句话说，你不需要调 K，K 是自己算出来的；你能调的只有 Q 和 R，它们决定了 K 往哪边偏。

MATLAB 里K = P_pred * H' / S用的是 mrdivide，内部按最小二乘解，比inv(S)数值稳定性好。S 是观测维数大小的方阵，三维观测就是 3×3，求逆代价可忽略，但inv在 S 病态时会放大误差。

3.3 更新步与协方差的形式选择

x_new = x_pred + K * y P_new = (I - K*H) * P_pred

上面这个 P 更新式是最常见的写法，但它在数值上有缺陷：当 K 接近最优、P 已经很小的时候，(I - K H) 的两个大数相减会产生舍入误差，P 可能失去正定性甚至变成非对称。P 一旦不正定，下一帧的 S 就可能出问题，滤波器直接崩。

工程上一般用 Joseph 形式：

A = I - K*H P_new = A * P_pred * A' + K * R * K'

它在代数上等价于简化式，但结构上保证了半正定，因为两项都是「矩阵乘自身转置」的形式。代价是多两次矩阵乘法，对三维问题完全可以接受。这个改动属于典型的「多写两行换一个后悔药」，我在任何要长时间运行的滤波器里都会用它。

3.4 MATLAB 里一个可跑的滤波循环

把上面几节拼起来，一个可以独立运行的单步函数长这样：

function [x, P] = kfStep(x, P, z, F, H, Q, R) % kfStep 卡尔曼滤波单步：一次预测 + 一次更新 % x: 状态列向量 P: 状态协方差 z: 当前观测 % F/H/Q/R: 模型矩阵，由外部按物理模型构造 % ---------- 预测 ---------- x = F * x; % 状态外推，不依赖任何观测 P = F * P * F' + Q; % 不确定性传播并叠加过程噪声 % ---------- 更新 ---------- y = z - H * x; % 新息：观测与预测的差 S = H * P * H' + R; % 新息的协方差 K = P * H' / S; % 卡尔曼增益，用 / 避免 inv(S) x = x + K * y; % 状态修正 A = eye(size(P)) - K * H; P = A * P * A' + K * R * K'; % Joseph 形式，保证 P 半正定 end

参数说明：x和P是上一帧的输出，第一帧用初始猜测；z是当前帧观测，必须是列向量且维数和 H 的行数一致；F、Q由采样周期和目标运动模型决定；H、R由传感器配置决定。函数无状态，所以能直接塞进任何循环，也方便做单元测试——给一个恒定输入，输出应该收敛到那个输入。

主循环里要注意两件事：一是初始化，P0别设太小。位置初始化误差有多大，P0 的位置块就填多大，速度初始化如果靠两点差分得到，方差要按差分公式算（大约 2*R/T²），拍脑袋填 1 会让滤波器在启动那几秒极度自信，观测全被当成噪声丢掉，表现就是「开头一段轨迹贴着模型走，跟真值差很远」。二是丢帧处理，某一帧没有观测时不要跳过整个循环，只跑预测步、把 P 加上 Q 就行，下一帧观测来了自然会修正回来。跳过预测会让时间戳和 F 失配，等于人为制造模型误差。

T = 0.05; % 采样周期 50ms n = 6; F = eye(n); F(1,4) = T; F(2,5) = T; F(3,6) = T; H = [eye(3), zeros(3)]; q = 9; % 加速度谱密度，按最大机动 3g 量级估 Q = q * [ (T^4/4)*eye(3), (T^3/2)*eye(3); (T^3/2)*eye(3), T^2*eye(3) ]; R = diag([4, 4, 9]); % 三个轴的观测方差，来自静态标定 x = [0; 0; 0; 0; 0; 0]; P = diag([100, 100, 100, 25, 25, 25]); for k = 1:size(Z, 2) if all(~isnan(Z(:, k))) % 有观测才更新，无观测只预测 [x, P] = kfStep(x, P, Z(:, k), F, H, Q, R); else x = F * x; P = F * P * F' + Q; end Xest(:, k) = x; end

这段循环跑通只说明代码没错，不代表滤波器可用。真正的验证在第 4 章和第 6 章。

4. 噪声整定：Q 与 R 的调参为什么像玄学

Q 和 R 是滤波器里唯二需要人调的东西，也是「玄学感」的主要来源。但它们的物理含义其实很明确：Q 是「我有多不相信模型」，R 是「我有多不相信传感器」。把这两个解释记牢，调参就变成了有方向的试探，而不是瞎试。

4.1 Q 大和 R 大分别会发生什么

把 K 的表达式拆开看极端情况最清楚。当 R → 0，K → H⁻¹（伪逆意义下），滤波器完全信任观测，输出就是观测本身，噪声原样通过。当 Q → 0，P_pred 会随时间衰减到零，K 也趋近零，滤波器完全信任模型，输出是一条光滑的、按 F 外推的曲线，观测等于被无视。

实际调参就在这两个极端之间找平衡：

一个实用的起步方法是：q 按目标最大机动加速度的平方量级估，R 按传感器静态标定方差填，跑一遍看新息序列，再微调。别一上来就对着输出曲线调到「看起来顺眼」——看起来顺眼很可能是因为滤波过度平滑，滞后被视觉掩盖了。

还有一个量纲问题。Q 和 R 的相对大小才决定 K，绝对大小不影响稳态增益（在定常系统里 K 会收敛到一个常值，只和 F、H、Q、R 的比例有关）。所以换采样率之后必须重算 Q，因为 Q 里含 T 的幂次。改了 T 不重算 Q，等于偷偷改了信噪比假设，这是换平台时最常见的翻车点。

4.2 用新息序列做一致性检验

调参最怕「不知道对不对」。新息序列 y(k) 提供了一个可量化的判据。理论上，如果模型和噪声假设都正确，y(k) 应该是零均值白噪声，其归一化平方：

NIS(k) = y(k)' * S(k)^-1 * y(k)

服从自由度为观测维数 m 的卡方分布，均值等于 m。这个统计量叫归一化新息平方，是滤波器自检的标准工具。

nis = zeros(N, 1); for k = 1:N % 用当前帧的预测值算新息，注意不要用更新后的 x yk = Z(:, k) - H * Xpred(:, k); Sk = H * Ppred(:, :, k) * H' + R; nis(k) = yk' / Sk * yk; end m = size(H, 1); fprintf('NIS 均值 = %.2f，理论值 = %d\n', mean(nis), m); % 经验判据：实测均值落在 [0.5m, 1.5m] 内算基本一致

如果 NIS 均值远大于 m，说明新息比预期大——要么 Q 设小了（模型跟不上真实机动），要么 R 设小了（低估了传感器噪声）。如果远小于 m，说明滤波器对自己的不确定度估计过于悲观，Q 或 R 设大了，实际精度比它自己以为的高。这个判据比肉眼看曲线靠谱得多，因为它把「预测的不确定度」也纳入了比较。

进一步还能看新息的自相关性。如果 y(k) 和 y(k-1) 明显相关，说明模型漏了动态——典型情形是目标在加速而你用了匀速模型，新息会呈现同号连跑。画一下xcorr(nis - m)或者直接看 y 的符号序列，很容易发现这种模式。

4.3 自适应与强跟踪：当噪声统计特性在变

真实场景里 Q 和 R 不是常数。目标从巡航切到机动，等效的过程噪声突然变大；传感器在强杂波环境下 R 也会变。固定参数的滤波器在这种时候会失衡，要么机动段跟丢，要么平稳段抖。

工程上有几类应对。一是新息协方差匹配，用滑动窗口内的实测新息协方差去反推 Q，Sage-Husa 类方法属于这一类，思路直白但容易在窗口太短时估出负定的 Q，需要加保护。二是渐消记忆，每帧把 P 乘一个略大于 1 的因子再预测，相当于人为告诉滤波器「别太自信」，这就是强跟踪滤波器的核心动作，实现只需要改一行，代价是平稳段精度略降。三是多模型并行，同时跑 CV 和 CA 两个滤波器，按各自的新息似然度加权融合输出，也就是交互式多模型（IMM）的基本形态，代价是算力翻倍、状态转移概率还要调。

选哪种看约束。算力紧、机动不频繁，渐消因子最划算；机动模式有限且可枚举，IMM 效果好但参数多；纯粹想省事，就老老实实把 q 按最大机动估，接受平稳段的一点抖动。

5. 排查清单：滤波发散与轨迹抖动的踩坑记录

下面这些是飞行物体轨迹预测里反复出现的现场问题，按现象、原因、解决的顺序记下来，方便对照。

5.1 发散类：轨迹越跑越偏，最后彻底飞掉

现象：前几十帧还贴着真值，之后误差单调增大，P 的迹也在涨，最后输出变成一条直线飞出去。

原因一：时间戳和 F 失配。数据里的帧间隔不是常数，代码里按标称 T 构造 F，等效于每帧注入一点模型误差，误差被 P 的传播放大。解决：从数据里读真实时间戳，每帧按实际 dt 重建 F 和 Q；实在读不到，就把到达时间记下来当 dt 用。

原因二：Q 设得极小甚至为 0。P_pred 会单调衰减到接近奇异，K 趋近 0，滤波器变成纯外推，一旦模型和真实运动有偏差就再也拉不回来。解决：Q 必须给正定值，加速度谱密度按物理量级估，别为了曲线好看把它压到 1e-9。

原因三：R 被设成奇异矩阵或含零对角元。S = H P H' + R 求逆时数值爆炸，K 出现巨值，一帧就把状态拉飞。解决：检查 R 的对角元是否都为正值，用cond(S)打印条件数，超过 1e10 就要警惕。

5.2 抖动与滞后类：曲线不好看，但没发散

现象一：静止或匀速段输出毛刺明显，相邻帧之间位置跳动几个像素量级。

原因：Q 相对 R 太大，K 偏大，观测噪声直接漏到输出。解决：先确认 R 是否按实际标定填的，很多人的 R 是随手写的 1，而实际传感器方差可能是 4 或 9；R 修正后 Q 往往可以同步下调。

现象二：目标转弯时输出明显切不进去，轨迹滞后真值零点几秒。

原因：Q 太小或 R 太大，滤波器过度信任匀速模型。解决：先看 NIS 均值，滞后通常伴随 NIS 长期偏大；调大 q 或者改用 CA 模型。注意别用「把 R 改小」这条捷径，那会同时把抖动带进来。

现象三：启动后头一两秒输出几乎不走，落后真值一大截。

原因：P0 设得太小，滤波器开局就极度自信，把最初几帧观测全当噪声忽略。解决：P0 的位置块按初始定位误差填，速度块按两点差分的方差填，宁大勿小；或者前几帧直接以观测为输出，等 P 收敛后再切到滤波。

5.3 实现类：代码逻辑没错但结果不对

现象一：MATLAB 仿真里完美，移植到嵌入式后慢慢发散。

原因：单精度浮点下 (I - K H) P 的减法抵消误差被放大，P 失去正定性。解决：换 Joseph 形式更新 P，并对 P 做对称化P = (P + P')/2；必要时用平方根滤波（Cholesky 分解后传播 S），代价是代码量翻倍。

现象二：极坐标传感器数据转笛卡尔后，滤波结果有固定方向的偏移，角度越大越明显。

原因：非线性变换把高斯噪声变成有偏非高斯，偏差被当成真实状态吸收。解决：要么在极坐标下建观测模型做 EKF，要么用去偏转换测量（把转换后的均值修正回无偏），别硬用线性 H。

现象三：偶尔一帧观测是野值（多径、遮挡导致的跳点），之后轨迹被拽偏很久才恢复。

原因：卡尔曼滤波对野值没有鲁棒性，一个大新息会被 K 按比例吸收进去。解决：加新息门限，y' * S^-1 * y超过卡方阈值（比如 m 自由度的 99% 分位）就判为野值，这一帧只预测不更新。门限别设太紧，否则真实机动也会被误杀。

注意：门限判据里的 S 必须用当前帧的 P_pred 算，不能用更新后的 P，否则阈值偏小，正常观测也会被拒。

6. 线性假设的边界：EKF、UKF 与验证方式的选择

前面所有推导都建立在「模型线性、噪声高斯」上。飞行物体轨迹预测里，这两条最先崩的是线性。传感器给极坐标、目标做蛇形机动、坐标系要从地心系转到站心系，任何一处非线性都会让 H 或 F 不再是常矩阵。EKF 的处理方式是在当前估计点做一阶泰勒展开，用雅可比矩阵代替常矩阵。它的优点是改动量小，把 H 换成H(x)、F 换成雅可比，滤波循环一个字不用动；缺点是线性化误差在强非线性区会引入偏差，而且雅可比推错一个符号，现象和参数调错一模一样，排查起来很折磨。UKF 换了个思路，不去线性化函数，而是选一组 sigma 点穿过非线性函数再统计均值和协方差，精度到二阶，代价是每帧要算 2n+1 次函数，n 是状态维数，六维就是 13 次，机载上要算清楚预算。

我的判断标准是看非线性程度落在哪个区间。角度误差小于十几度、转换关系接近线性，EKF 够用；大角度、强机动、状态分布明显非高斯，直接上 UKF 或者把问题重参数化（比如在极坐标下建状态），别在 EKF 上反复调参硬扛。

至于验证，光看输出曲线贴合真值是不够的——那只能证明滤波器在平滑，不能证明它在正确融合。我一般分三层做：第一层是闭环仿真，用已知真值生成观测，看位置 RMSE 和速度 RMSE 是否随 R 的减小而下降，如果 R 减半精度不动，说明模型误差是主导，调 R 没意义；第二层是蒙特卡洛，同一段真值加不同随机种子跑几十次，统计估计误差的均值和协方差，和滤波器自己报的 P 对比，P 报小了就是过度自信，这是发散的前兆；第三层是残差白性检验，第 4 章的 NIS 和自相关都在这层，它能在没有真值的实测数据上给出判断，是真机上唯一可用的自检手段。

有一件事我踩过坑：早期做某飞行器轨迹预测项目时，把全部精力放在让曲线好看上，Q 一路往下压，输出的确丝滑，交付后目标一做机动就跟丢。回头查 NIS 才发现均值长期是理论值的三倍多，滤波器一直在报警，只是没人看。从那以后我每次调完参数，都强制走一遍 NIS 统计和残差自相关，把这两个数当成和精度指标同等重要的验收项，不看曲线先看它们。希望这套顺序能帮你在自己的数据上少绕几圈。

本文还有配套的精品资源，点击获取