简介面向飞行器轨迹预测与卡尔曼滤波学习者的 MATLAB 实践包围绕带噪声传感器数据的状态估计与轨迹预测展开适合具备一定信号处理、概率统计基础并希望把理论落到代码中的读者。压缩包共 6 个文件约 129KB以 txt 飞行与经纬度数据、m 滤波实现源码为主另附 docx 说明文档和 url 使用提示便于按数据读取、参数初始化、预测更新与结果可视化几个环节对照学习。已有 4690 人学习下载。包内提供 20 组飞行数据样本、经纬度序列与卡尔曼滤波主程序可用于复现位置、速度估计并与原始观测对比也能作为扩展卡尔曼滤波、无迹卡尔曼滤波等变体实验的起点说明文档与源码提示有助于理解参数调整和运行入口适合课程设计、科研预研与飞行控制、导航、目标追踪方向的入门实践。1. 从一条抖到没法看的航迹说起卡尔曼滤波做飞行轨迹预测值不值得上手雷达或者光电跟踪设备回传的飞行目标坐标直接画出来几乎是一条毛刺曲线逐帧差分算速度更是噪声爆炸外推两百毫秒就飘出几十米。很多做飞行物体运动轨迹预测的同学第一次遇到这种数据本能反应是加低通滤波或者滑动平均结果延迟大到目标拐弯时彻底跟不上。这个 Matlab 工程就是拿卡尔曼滤波把这条链路重新做了一遍从状态方程建模、预测更新循环到球坐标观测下的扩展卡尔曼滤波再到 RMSE 和 NEES 指标评估全部是可读可改的脚本而不是黑盒函数。适合三类人做无人机或飞行器仿真需要一份能跑通的参考实现、做雷达数据处理课程设计需要交得出实验结果、以及手里有实测航迹想验证滤波参数的老手。Matlab 的好处是矩阵运算和画图都是现成的把注意力全放在模型和参数上这也是我建议先用它把流程跑通再考虑移植的原因。2. 工程骨架一份 Matlab 轨迹预测工程该切成哪几块拿到一份算法工程我第一件事不是看滤波核心而是看目录怎么切的。切得清楚的工程改一个模型不用翻五个文件切得乱的调一次参数能把自己绕进去。这个工程的分层思路很朴素真值航迹生成、观测生成、滤波器、评估、可视化五块各管一件事中间靠结构体传数据。2.1 目录分层从航迹生成到指标评估的六个模块工程结构大致如下文件名做了简化作用是让每一层职责单一文件 / 目录职责主要函数start_sim.m仿真总入口串起生成、滤波、评估、画图无纯脚本traj/traj_gen.m生成真值航迹支持匀速段与机动段拼接traj_cv、traj_catraj/obs_gen.m由真值生成带噪观测支持直角坐标与球坐标obs_cart、obs_sphfilters/kf_linear.m线性卡尔曼滤波CV 与 CA 共用kf_predict、kf_updatefilters/ekf_sph.m球坐标观测下的 EKF含雅可比矩阵ekf_stepeval/metrics.mRMSE、NEES、新息统计rmse_pos、nees_calcplot/plot_all.m三维航迹对比、误差曲线、协方差椭圆plot3、errorbar这样切的好处是数据流向单向traj_gen出来的真值X_true是 N×6 的矩阵obs_gen出来的观测Z是 N×3 或 N×2滤波器只吃Z和cfg吐出来的X_est、P_hist直接喂给评估和画图。中间任何一环想换实现只要保证输入输出的维度约定不变其他文件一行都不用动。我一般会在start_sim.m顶部写死三个约定真值矩阵列顺序是[x y z vx vy vz]观测矩阵列顺序是[r az el]或[x y]时间统一用秒。这三个约定不写清楚后面换个人接手必定踩坑。还有一点容易忽略P_hist一定要存下来。只存状态估计的话你永远不知道滤波器是「估得准」还是「自以为估得准」。协方差轨迹是排查发散问题的第一手证据后面第 5 章会反复用到。2.2 状态向量与运动模型CV、CA、Singer 各自的适用面状态向量决定了滤波器能表达什么样的运动。这个工程里出现三种模型选哪个取决于目标在观测窗口内怎么飞。匀速模型 CV 是最常用的状态取位置和速度% 二维 CV 模型的状态向量排列: x [px; py; vx; vy] % 三维时把位置和速度各扩成三行即可 x [px; py; vx; vy];对应的状态转移矩阵就是位置对速度的积分关系采样间隔 dt 一旦确定F 里没有第二个自由参数。CV 模型的隐含假设是加速度为零实际飞行目标在巡航段近似成立一旦进入转弯或者爬升模型失配会立刻体现在新息上。匀加速模型 CA 在状态里再加一组加速度% 二维 CA 模型: x [px; py; vx; vy; ax; ay] % F 的位置行含 dt^2/2 项速度行含 dt 项 F [1 0 dt 0 dt^2/2 0; 0 1 0 dt 0 dt^2/2; 0 0 1 0 dt 0; 0 0 0 1 0 dt; 0 0 0 0 1 0; 0 0 0 0 0 1];CA 的问题在于它假设加速度恒定实际机动目标的加速度是变化的所以过程噪声要给得更大否则滤波器对自己的预测过度自信观测进来了也拉不回来。Singer 模型介于两者之间把加速度建模成一阶马尔可夫过程用一个时间常数描述机动的持续性。它的状态维度和 CA 一样只是 F 里加速度那一块变成了衰减项exp(-dt/tau)。飞行目标在大气扰动下的匀速飞行、缓慢转弯用 Singer 比 CV 和 CA 都贴。工程实践里更常见的做法是上交互多模型 IMM让 CV、CA、Singer 并行跑按似然加权输出代价是计算量翻几倍、参数量翻几倍调参难度也跟着上。课程设计或者单目标验证我建议先把 CV 跑通再单独加 CA 对比别一上来就 IMM。2.3 参数集中管理用一个 cfg 结构体贯穿仿真与滤波散落在各个脚本里的魔法数字是 Matlab 工程的头号可维护性杀手。这个工程的做法是把所有可调参数收进一个cfg结构体在入口脚本里一次性定义往下传。% start_sim.m 中的参数区全部单位统一到国际单位制 cfg.dt 0.05; % 采样间隔 50 ms对应 20 Hz 数据率 cfg.N 1200; % 总帧数60 s 仿真时长 cfg.q 0.8; % 过程噪声强度谱密度单位 m^2/s^3 cfg.sigma_r 30; % 距离量测标准差单位 m cfg.sigma_az 0.3*pi/180; % 方位角量测标准差单位 rad cfg.sigma_el 0.3*pi/180; % 俯仰角量测标准差单位 rad cfg.P0_diag [100^2 100^2 100^2 10^2 10^2 10^2]; % 初始协方差对角线 cfg.model CV; % CV | CA | SINGERcfg.q、cfg.P0_diag、三个sigma是后续调参时唯一需要动的四个地方。单位统一这条必须强调距离用米、角度用弧度、时间用秒只要有一处混进毫秒或者度量纲对不上滤波器不会报错只会给你一条缓慢发散的曲线排查起来非常费劲。我习惯在start_sim.m开头加一行断言比如assert(abs(cfg.dt-0.05)1e-9)改参数的时候先把单位确认一遍。3. 滤波主循环拆解预测与更新这两步背后的东西卡尔曼滤波的公式在教科书上只有五行但真正写成能跑的代码坑全在细节里协方差为什么必须对称、增益为什么用左除而不是求逆、非线性观测的雅可比矩阵怎么推。这一章把循环拆开逐段看。3.1 状态转移矩阵 F 与过程噪声 Q 的构造F 好写Q 才是真正决定滤波效果的东西。工程上普遍用连续白噪声加速度模型离散化让 Q 的强度只由一个标量q控制避免逐元素调参。function [F, Q] build_cv_3d(dt, q) % 三维匀速模型状态 x [px py pz vx vy vz] % dt 采样间隔(s)q 过程噪声强度(m^2/s^3) F kron(eye(3), [1 dt; 0 1]); % 三个轴各自独立克罗内克积拼装 G kron(eye(3), [dt^2/2; dt]); % 噪声输入矩阵加速度积分到位置和速度 Q q * (G * G); % 离散过程噪声协方差 Q (Q Q) / 2; % 数值对称化避免累积误差 endkron(eye(3), [1 dt; 0 1])这一步是把一维的[1 dt; 0 1]复制到三个坐标轴上得到 6×6 的块对角矩阵。G同理每一轴对应[dt^2/2; dt]这一列代表单位加速度噪声如何影响该轴的位置和速度。q越大滤波器越不相信自己的预测越跟着观测走q越小输出越平滑但机动段滞后越明显。这个单参数设计的好处是调参有直觉观测噪声大就把 q 调小目标机动强就把 q 调大。如果换成 CA 模型G要扩成三维加速度对位置的二重积分形式Q的结构也随之变化但思路完全一样都是先写噪声如何输入、再乘强度。3.2 预测步先验估计与协方差外推预测步只做两件事把状态往前推一个 dt把协方差按 F 传播并叠加过程噪声。function [x_pred, P_pred] kf_predict(x, P, F, Q) % 输入: 上一时刻后验状态 x、后验协方差 P、状态转移 F、过程噪声 Q % 输出: 当前时刻先验状态与先验协方差 x_pred F * x; % 状态外推纯线性运算 P_pred F * P * F Q; % 协方差外推Q 在此注入不确定性 P_pred (P_pred P_pred) / 2;% 强制对称抑制浮点误差累积 end协方差外推这一行是整个算法的心脏。F*P*F描述的是「原来的不确定性如何被运动放大」Q描述的是「模型本身有多不可信」。两者叠加之后如果P_pred的某个方向特别大说明滤波器在那个方向上心里没底下一步更新时卡尔曼增益就会相应变大更多地采纳观测。这就是卡尔曼滤波自适应的来源不是什么神秘机制。不加对称化那一行也能跑但跑个几千帧之后P会因为浮点误差慢慢失去对称性极端情况下特征值出现负值随后就是 NaN。血泪经验是这一行几乎不花时间加上就完事。3.3 更新步新息、卡尔曼增益与 Joseph 形式协方差更新步是观测进来的地方也是最容易写出数值问题的地方。function [x_upd, P_upd, nu, S] kf_update(x_pred, P_pred, z, H, R) % 输入: 先验状态与协方差、观测 z、观测矩阵 H、观测噪声协方差 R nu z - H * x_pred; % 新息观测与预测的差 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益用左除代替 inv(S) x_upd x_pred K * nu; % 后验状态 A eye(size(P_pred)) - K * H; P_upd A * P_pred * A K * R * K; % Joseph 形式保证半正定 P_upd (P_upd P_upd) / 2; end几个细节值得单独说。第一卡尔曼增益用/ S而不是inv(S)*Matlab 的左除内部走的是矩阵分解数值稳定性明显更好尤其是 R 很小、S 接近奇异的时候。第二协方差更新用了 Joseph 形式而不是教科书上的(I-KH)P前者在理论上恒为半正定后者在 K 计算有微小误差时会悄悄破坏正定性。第三nu和S一定要往外输出新息序列是判断滤波器有没有失配的最直接指标白噪声、零均值说明模型匹配出现持续偏置说明模型跟不上nu*inv(S)*nu长期大于状态维数说明 R 给得太小或者 Q 给得太大。H 和 R 的构造跟观测类型绑定。直角坐标直接观测位置时H 就是挑出位置那三行的选择矩阵R 是对角阵对角元是各轴量测方差。这一块保持简单把复杂度留给后面的球坐标观测。3.4 非线性观测下的 EKF雅可比矩阵一个符号写错就发散真实跟踪设备给的往往不是直角坐标而是距离、方位角、俯仰角。观测函数变成非线性的卡尔曼滤波要线性化也就是 EKF。观测量z [r; az; el]状态x [px py pz vx vy vz]观测函数function z_pred obs_model(x) % 由状态预测观测量输出 [距离; 方位角; 俯仰角] px x(1); py x(2); pz x(3); rho sqrt(px^2 py^2); % 水平距离方位角分母要用 r sqrt(px^2 py^2 pz^2); z_pred [r; atan2(py, px); atan2(pz, rho)]; end雅可比矩阵是观测函数对状态的偏导只有位置那三列非零function H jacob_sph(x) px x(1); py x(2); pz x(3); rho2 px^2 py^2; rho sqrt(rho2); r2 rho2 pz^2; r sqrt(r2); H zeros(3, 6); % 距离对位置的偏导 H(1,1) px/r; H(1,2) py/r; H(1,3) pz/r; % 方位角对位置的偏导注意分母是水平距离平方 H(2,1) -py/rho2; H(2,2) px/rho2; H(2,3) 0; % 俯仰角对位置的偏导 H(3,1) -px*pz/(r2*rho); H(3,2) -py*pz/(r2*rho); H(3,3) rho/r2; end这三个公式是最容易抄错的地方尤其是俯仰角那一行分母里同时出现r2和rho少一个就发散。我的习惯是写完立刻做数值验证取一个已知状态用有限差分算一遍雅可比和自己的解析式逐元素对比误差超过 1e-6 就说明推错了。% 雅可比自检解析解 vs 中心差分开发阶段必跑 x_test [1200; -800; 300; 0; 0; 0]; H_ana jacob_sph(x_test); H_num zeros(3, 6); eps_step 1e-6; for i 1:6 dx zeros(6,1); dx(i) eps_step; H_num(:,i) (obs_model(x_testdx) - obs_model(x_test-dx)) / (2*eps_step); end assert(max(abs(H_ana(:) - H_num(:))) 1e-6, 雅可比矩阵推导有误);EKF 的预测步和线性 KF 完全一样更新步把H*x_pred换成obs_model(x_pred)把H换成当前先验处的雅可比。线性化误差在观测非线性强的场景下会体现为收敛变慢甚至震荡这时候可以考虑 UKF但那是第 6 章的事。4. 完整仿真跑通从合成航迹到误差曲线前面的模块单独看都简单串起来跑通并且结果合理才算真正掌握。这一章按执行顺序走一遍给出可以直接抄的代码。4.1 造数据匀速段加机动段的合成航迹与球坐标观测真值航迹要包含匀速和机动两段否则滤波器的模型失配根本测不出来。function [X_true, t] traj_gen(cfg) % 生成三段式航迹匀速巡航 - 协调转弯 - 匀速爬升 dt cfg.dt; N cfg.N; t (0:N-1) * dt; X_true zeros(N, 6); p [5000; 0; 1000]; % 初始位置(m) v [-180; 60; 5]; % 初始速度(m/s)约 0.7 马赫量级 for k 1:N if k 400 k 700 % 机动段给一个持续 15 s 的侧向加速度模拟协调转弯 a [2*cos(0.02*k); 3*sin(0.02*k); 0.5]; else a [0; 0; 0]; end v v a * dt; p p v * dt; X_true(k, :) [p, v]; end end观测生成把真值转到球坐标再叠噪声这一步的噪声标准差必须和cfg里给滤波器的一致否则评估结果没有意义。function Z obs_gen(X_true, cfg) % 真值转球坐标观测叠加高斯噪声 N size(X_true, 1); Z zeros(N, 3); for k 1:N p X_true(k, 1:3); rho sqrt(p(1)^2 p(2)^2); r norm(p); z_true [r; atan2(p(2), p(1)); atan2(p(3), rho)]; Z(k, :) (z_true [cfg.sigma_r; cfg.sigma_az; cfg.sigma_el] .* randn(3,1)); end end注意机动段的加速度不是常数也不是标准的协调转弯模型这是故意的。真实目标不会按教科书飞用一段「不那么标准」的机动去测滤波器结论才有参考价值。4.2 主循环滤波器在时间轴上的推进主循环要做的事情很固定初始化、逐帧预测、逐帧更新、记录。% start_sim.m 主循环片段 X_est zeros(cfg.N, 6); P_hist zeros(6, 6, cfg.N); x [Z(1,1)*cos(Z(1,3))*cos(Z(1,2)); % 用首帧观测反算初始位置 Z(1,1)*cos(Z(1,3))*sin(Z(1,2)); Z(1,1)*sin(Z(1,3)); 0; 0; 0]; P diag(cfg.P0_diag); R diag([cfg.sigma_r^2, cfg.sigma_az^2, cfg.sigma_el^2]); [F, Q] build_cv_3d(cfg.dt, cfg.q); for k 1:cfg.N if k 1 [x, P] kf_predict(x, P, F, Q); % 预测 end H jacob_sph(x); % 在当前先验处线性化 z Z(k, :); nu z - obs_model(x); nu(2) mod(nu(2) pi, 2*pi) - pi; % 角度新息绕回 [-pi, pi] nu(3) mod(nu(3) pi, 2*pi) - pi; S H * P * H R; K P * H / S; x x K * nu; A eye(6) - K * H; P A * P * A K * R * K; P (P P) / 2; X_est(k, :) x; P_hist(:, :, k) P; end初始速度给 0 是保守做法让滤波器自己从观测里把速度估出来。如果首帧观测质量差可以把P0_diag里速度那几项给大一点让前几十帧快速收敛。角度新息的绕回处理绝对不能省方位角从 179 度跳到 -179 度时不做绕回会得到一个接近 2π 的新息滤波器会以为目标瞬移了协方差瞬间炸开。4.3 评估RMSE、NEES 与航迹对比图怎么读位置 RMSE 是最直观的指标但要分段落统计匀速段和机动段分开看否则机动段的误差会被匀速段稀释。function rmse rmse_pos(X_est, X_true, idx) % 按给定帧索引计算三维位置 RMSE err X_est(idx, 1:3) - X_true(idx, 1:3); rmse sqrt(mean(sum(err.^2, 2))); endNEES 检验的是「滤波器对自己的不确定性估计准不准」比 RMSE 更能暴露问题function nees nees_calc(X_est, X_true, P_hist, idx) % 归一化估计误差平方理论均值应接近状态维数 nees zeros(numel(idx), 1); for i 1:numel(idx) e (X_true(idx(i), 1:3) - X_est(idx(i), 1:3)); P P_hist(1:3, 1:3, idx(i)); nees(i) e / P * e; end nees mean(nees); end三维位置对应 3 个自由度NEES 的长期均值应该落在 3 附近。如果稳定在 10 以上说明协方差给得太小滤波器过度自信如果长期低于 1说明协方差给得太大滤波器太保守观测的信息没吃进去。这个指标调参的时候比 RMSE 好用得多因为它不依赖真值量级。画图上三维航迹对比图看整体形状误差随时间曲线看收敛速度新息序列看模型匹配程度协方差椭圆二维投影看不确定性的方向。四张图配合着看基本能定位所有常见问题。4.4 Q 和 R 怎么定一个工程经验公式加试凑Q 和 R 的绝对数值没有意义比值才有意义。R 一般来自设备手册或者实测统计是已知量Q 才是要调的。一个粗糙但好用的起点是让q * dt^3和位置量测方差同量级也就是假设一个采样周期内模型误差的量级接近量测噪声。实际调的时候按这个起点上下浮动一个数量级看 NEES 落在哪。如果 NEES 偏大说明 Q 给小了往上调偏小就往下调。调到 NEES 落在 2 到 5 之间再回头看看 RMSE 和航迹图通常就差不多了。机动段误差明显大于匀速段这不是调参能解决的是 CV 模型本身的局限。要么上 CA、要么上自适应 Q、要么上 IMM三条路在第 6 章展开。5. 避坑与排查五个让滤波器当场发散的现场下面这几条都是我自己和身边人真实翻过的车按「现象 → 原因 → 解决」写遇到了直接对号入座。第一条跑几百帧之后状态变成 NaN。现象是前几十帧还好好的跑到某个时刻X_est突然全变 NaNP_hist里出现 Inf。原因是协方差更新用了(I-KH)*P这种简化形式在 R 很小、增益接近 1 的时候I-KH的数值误差把P推出正定域特征值变负下一步开方或者求逆就炸了。解决是把更新改成 Joseph 形式A*P*A K*R*K并且每次更新后强制对称化。这一步几乎不增加计算量但能把这类问题彻底堵死。第二条目标转弯时滤波器明显滞后误差尖峰。现象是匀速段 RMSE 只有几米一进机动段直接冲到几十米误差曲线上出现一个明显的鼓包机动结束后又降回去。原因是 CV 模型假设加速度为零机动期间模型持续给出错误预测新息出现系统性偏置但 Q 给得小增益不够大滤波器拉不回来。解决有三条路把 q 调大代价是匀速段精度下降、切换到 CA 模型、或者上新息驱动的自适应 Q。最简单的验证方法就是看新息序列机动段新息均值明显偏离零基本可以确诊。第三条方位角在 ±π 附近跳变协方差爆掉。现象是目标飞越正北或者正南方向时滤波器突然发散几帧之后又自己恢复。原因是方位角观测从 179 度跳到 -179 度直接相减得到约 -358 度的新息滤波器以为观测和预测差了将近一整圈。解决是在算新息之后立刻做角度绕回nu mod(nu pi, 2*pi) - pi。这里有个细节Matlab 的wrapToPi在 Mapping Toolbox 里不是所有环境都有用mod自己写更保险也不会因为工具箱缺失报错。第四条整体误差始终偏大怎么调 Q 都没用。现象是 RMSE 稳定在一个偏大的值上NEES 也正常说明滤波器「自认为」估得不错但和真值就是差一截。八成是单位或者量纲的问题时间用了毫秒但 F 里按秒算角度用了度但 R 里按弧度算或者位置用了千米而噪声标准差用了米。这类错误的隐蔽性在于代码不报错只是结果系统性偏移。解决办法是仿真开始前把所有输入的单位打印一遍跟cfg里的注释逐个核对。第五条初始几十帧收敛特别慢甚至被拉偏。现象是滤波器启动后很长一段时间估计值偏离真值慢慢才拉回来。原因是初始协方差P0给得太小滤波器对初始状态过度自信观测进来了也只肯小步修正。解决是把P0的位置项给到量测噪声方差的几十倍速度项给得更夸张一些让前几十帧的增益接近 1快速吃进观测。初始状态本身给得粗糙没关系协方差给对了滤波器自己会收敛。6. 二次开发从二维匀速模型推到三维机动目标跟踪跑通 CV 之后绝大多数人的下一步是把模型换掉让它在真实机动目标上也能用。这里给一条我实际走过的改造路径按改动量从小到大排。改造项改动位置工作量效果状态扩到三维build_cv_3d、obs_model、jacob_sph小支持高度维飞行目标必备换 CA 或 Singerbuild_*函数、F维数中机动段误差明显下降自适应 Q主循环里按新息调整q中兼顾匀速精度和机动响应上 IMM新增模型集与概率更新大复杂机动下最稳调参最烦换 UKF替换线性化环节中强非线性观测下更准第一步扩三维基本是机械劳动但有两个地方必须同步改obs_model里的rho要改成水平距离而不是总距离jacob_sph里俯仰角的分母要跟着变。我见过有人只改了状态维数没改雅可比跑出来的结果看着能用实际上俯仰通道一直在缓慢发散。第二步换模型建议先用 CA 验证一遍。改法是把build_cv_3d复制一份改成九维G里加速度项的二重积分形式写对其余代码一行不动。如果 CA 在机动段比 CV 好、在匀速段比 CV 差说明改对了符合预期。第三步自适应 Q 是我最推荐的折中方案。思路很简单维护一个新息滑动窗口窗口内新息的加权平方和超过阈值就把q放大回落之后再调回去。% 新息驱动的自适应 Q放在主循环更新步之前 win 10; % 滑窗长度 if k win nu_win nu_buf(:, end-win1:end); stat mean(sum(nu_win.^2, 1)); % 窗口内新息能量 q_adapt cfg.q * max(1, stat / cfg.q_ref); [~, Q] build_cv_3d(cfg.dt, min(q_adapt, 100*cfg.q)); % 上限保护 endcfg.q_ref是匀速段标定出来的新息能量基准100*cfg.q是防止目标长时间机动把 Q 推到无穷大的保护上限。这个方案不需要维护多个模型代码量增加不到二十行实测在持续十几秒的机动段能把误差尖峰压掉一半以上。验证方法上我建议固定随机种子把改动前后的 RMSE 和 NEES 曲线画在同一张图上对比同时看新息序列的自相关。自相关明显非零说明还有模型失配没吃掉这时候再考虑上 IMM。别一上来就堆模型先把单模型的极限摸清楚后面加复杂度才知道收益从哪来。从那以后我每次改滤波器的状态维数或者观测模型都强制先跑一遍雅可比的有限差分自检再跑一遍 NEES这两个过了才看 RMSE。这个习惯帮我省掉了无数次「结果看着还行其实早就偏了」的返工。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/11/4 17:23:28 · 阅读 1.2k Matlab卡尔曼滤波飞行轨迹预测与EKF工程实践
简介面向飞行器轨迹预测与卡尔曼滤波学习者的 MATLAB 实践包围绕带噪声传感器数据的状态估计与轨迹预测展开适合具备一定信号处理、概率统计基础并希望把理论落到代码中的读者。压缩包共 6 个文件约 129KB以 txt 飞行与经纬度数据、m 滤波实现源码为主另附 docx 说明文档和 url 使用提示便于按数据读取、参数初始化、预测更新与结果可视化几个环节对照学习。已有 4690 人学习下载。包内提供 20 组飞行数据样本、经纬度序列与卡尔曼滤波主程序可用于复现位置、速度估计并与原始观测对比也能作为扩展卡尔曼滤波、无迹卡尔曼滤波等变体实验的起点说明文档与源码提示有助于理解参数调整和运行入口适合课程设计、科研预研与飞行控制、导航、目标追踪方向的入门实践。1. 从一条抖到没法看的航迹说起卡尔曼滤波做飞行轨迹预测值不值得上手雷达或者光电跟踪设备回传的飞行目标坐标直接画出来几乎是一条毛刺曲线逐帧差分算速度更是噪声爆炸外推两百毫秒就飘出几十米。很多做飞行物体运动轨迹预测的同学第一次遇到这种数据本能反应是加低通滤波或者滑动平均结果延迟大到目标拐弯时彻底跟不上。这个 Matlab 工程就是拿卡尔曼滤波把这条链路重新做了一遍从状态方程建模、预测更新循环到球坐标观测下的扩展卡尔曼滤波再到 RMSE 和 NEES 指标评估全部是可读可改的脚本而不是黑盒函数。适合三类人做无人机或飞行器仿真需要一份能跑通的参考实现、做雷达数据处理课程设计需要交得出实验结果、以及手里有实测航迹想验证滤波参数的老手。Matlab 的好处是矩阵运算和画图都是现成的把注意力全放在模型和参数上这也是我建议先用它把流程跑通再考虑移植的原因。2. 工程骨架一份 Matlab 轨迹预测工程该切成哪几块拿到一份算法工程我第一件事不是看滤波核心而是看目录怎么切的。切得清楚的工程改一个模型不用翻五个文件切得乱的调一次参数能把自己绕进去。这个工程的分层思路很朴素真值航迹生成、观测生成、滤波器、评估、可视化五块各管一件事中间靠结构体传数据。2.1 目录分层从航迹生成到指标评估的六个模块工程结构大致如下文件名做了简化作用是让每一层职责单一文件 / 目录职责主要函数start_sim.m仿真总入口串起生成、滤波、评估、画图无纯脚本traj/traj_gen.m生成真值航迹支持匀速段与机动段拼接traj_cv、traj_catraj/obs_gen.m由真值生成带噪观测支持直角坐标与球坐标obs_cart、obs_sphfilters/kf_linear.m线性卡尔曼滤波CV 与 CA 共用kf_predict、kf_updatefilters/ekf_sph.m球坐标观测下的 EKF含雅可比矩阵ekf_stepeval/metrics.mRMSE、NEES、新息统计rmse_pos、nees_calcplot/plot_all.m三维航迹对比、误差曲线、协方差椭圆plot3、errorbar这样切的好处是数据流向单向traj_gen出来的真值X_true是 N×6 的矩阵obs_gen出来的观测Z是 N×3 或 N×2滤波器只吃Z和cfg吐出来的X_est、P_hist直接喂给评估和画图。中间任何一环想换实现只要保证输入输出的维度约定不变其他文件一行都不用动。我一般会在start_sim.m顶部写死三个约定真值矩阵列顺序是[x y z vx vy vz]观测矩阵列顺序是[r az el]或[x y]时间统一用秒。这三个约定不写清楚后面换个人接手必定踩坑。还有一点容易忽略P_hist一定要存下来。只存状态估计的话你永远不知道滤波器是「估得准」还是「自以为估得准」。协方差轨迹是排查发散问题的第一手证据后面第 5 章会反复用到。2.2 状态向量与运动模型CV、CA、Singer 各自的适用面状态向量决定了滤波器能表达什么样的运动。这个工程里出现三种模型选哪个取决于目标在观测窗口内怎么飞。匀速模型 CV 是最常用的状态取位置和速度% 二维 CV 模型的状态向量排列: x [px; py; vx; vy] % 三维时把位置和速度各扩成三行即可 x [px; py; vx; vy];对应的状态转移矩阵就是位置对速度的积分关系采样间隔 dt 一旦确定F 里没有第二个自由参数。CV 模型的隐含假设是加速度为零实际飞行目标在巡航段近似成立一旦进入转弯或者爬升模型失配会立刻体现在新息上。匀加速模型 CA 在状态里再加一组加速度% 二维 CA 模型: x [px; py; vx; vy; ax; ay] % F 的位置行含 dt^2/2 项速度行含 dt 项 F [1 0 dt 0 dt^2/2 0; 0 1 0 dt 0 dt^2/2; 0 0 1 0 dt 0; 0 0 0 1 0 dt; 0 0 0 0 1 0; 0 0 0 0 0 1];CA 的问题在于它假设加速度恒定实际机动目标的加速度是变化的所以过程噪声要给得更大否则滤波器对自己的预测过度自信观测进来了也拉不回来。Singer 模型介于两者之间把加速度建模成一阶马尔可夫过程用一个时间常数描述机动的持续性。它的状态维度和 CA 一样只是 F 里加速度那一块变成了衰减项exp(-dt/tau)。飞行目标在大气扰动下的匀速飞行、缓慢转弯用 Singer 比 CV 和 CA 都贴。工程实践里更常见的做法是上交互多模型 IMM让 CV、CA、Singer 并行跑按似然加权输出代价是计算量翻几倍、参数量翻几倍调参难度也跟着上。课程设计或者单目标验证我建议先把 CV 跑通再单独加 CA 对比别一上来就 IMM。2.3 参数集中管理用一个 cfg 结构体贯穿仿真与滤波散落在各个脚本里的魔法数字是 Matlab 工程的头号可维护性杀手。这个工程的做法是把所有可调参数收进一个cfg结构体在入口脚本里一次性定义往下传。% start_sim.m 中的参数区全部单位统一到国际单位制 cfg.dt 0.05; % 采样间隔 50 ms对应 20 Hz 数据率 cfg.N 1200; % 总帧数60 s 仿真时长 cfg.q 0.8; % 过程噪声强度谱密度单位 m^2/s^3 cfg.sigma_r 30; % 距离量测标准差单位 m cfg.sigma_az 0.3*pi/180; % 方位角量测标准差单位 rad cfg.sigma_el 0.3*pi/180; % 俯仰角量测标准差单位 rad cfg.P0_diag [100^2 100^2 100^2 10^2 10^2 10^2]; % 初始协方差对角线 cfg.model CV; % CV | CA | SINGERcfg.q、cfg.P0_diag、三个sigma是后续调参时唯一需要动的四个地方。单位统一这条必须强调距离用米、角度用弧度、时间用秒只要有一处混进毫秒或者度量纲对不上滤波器不会报错只会给你一条缓慢发散的曲线排查起来非常费劲。我习惯在start_sim.m开头加一行断言比如assert(abs(cfg.dt-0.05)1e-9)改参数的时候先把单位确认一遍。3. 滤波主循环拆解预测与更新这两步背后的东西卡尔曼滤波的公式在教科书上只有五行但真正写成能跑的代码坑全在细节里协方差为什么必须对称、增益为什么用左除而不是求逆、非线性观测的雅可比矩阵怎么推。这一章把循环拆开逐段看。3.1 状态转移矩阵 F 与过程噪声 Q 的构造F 好写Q 才是真正决定滤波效果的东西。工程上普遍用连续白噪声加速度模型离散化让 Q 的强度只由一个标量q控制避免逐元素调参。function [F, Q] build_cv_3d(dt, q) % 三维匀速模型状态 x [px py pz vx vy vz] % dt 采样间隔(s)q 过程噪声强度(m^2/s^3) F kron(eye(3), [1 dt; 0 1]); % 三个轴各自独立克罗内克积拼装 G kron(eye(3), [dt^2/2; dt]); % 噪声输入矩阵加速度积分到位置和速度 Q q * (G * G); % 离散过程噪声协方差 Q (Q Q) / 2; % 数值对称化避免累积误差 endkron(eye(3), [1 dt; 0 1])这一步是把一维的[1 dt; 0 1]复制到三个坐标轴上得到 6×6 的块对角矩阵。G同理每一轴对应[dt^2/2; dt]这一列代表单位加速度噪声如何影响该轴的位置和速度。q越大滤波器越不相信自己的预测越跟着观测走q越小输出越平滑但机动段滞后越明显。这个单参数设计的好处是调参有直觉观测噪声大就把 q 调小目标机动强就把 q 调大。如果换成 CA 模型G要扩成三维加速度对位置的二重积分形式Q的结构也随之变化但思路完全一样都是先写噪声如何输入、再乘强度。3.2 预测步先验估计与协方差外推预测步只做两件事把状态往前推一个 dt把协方差按 F 传播并叠加过程噪声。function [x_pred, P_pred] kf_predict(x, P, F, Q) % 输入: 上一时刻后验状态 x、后验协方差 P、状态转移 F、过程噪声 Q % 输出: 当前时刻先验状态与先验协方差 x_pred F * x; % 状态外推纯线性运算 P_pred F * P * F Q; % 协方差外推Q 在此注入不确定性 P_pred (P_pred P_pred) / 2;% 强制对称抑制浮点误差累积 end协方差外推这一行是整个算法的心脏。F*P*F描述的是「原来的不确定性如何被运动放大」Q描述的是「模型本身有多不可信」。两者叠加之后如果P_pred的某个方向特别大说明滤波器在那个方向上心里没底下一步更新时卡尔曼增益就会相应变大更多地采纳观测。这就是卡尔曼滤波自适应的来源不是什么神秘机制。不加对称化那一行也能跑但跑个几千帧之后P会因为浮点误差慢慢失去对称性极端情况下特征值出现负值随后就是 NaN。血泪经验是这一行几乎不花时间加上就完事。3.3 更新步新息、卡尔曼增益与 Joseph 形式协方差更新步是观测进来的地方也是最容易写出数值问题的地方。function [x_upd, P_upd, nu, S] kf_update(x_pred, P_pred, z, H, R) % 输入: 先验状态与协方差、观测 z、观测矩阵 H、观测噪声协方差 R nu z - H * x_pred; % 新息观测与预测的差 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益用左除代替 inv(S) x_upd x_pred K * nu; % 后验状态 A eye(size(P_pred)) - K * H; P_upd A * P_pred * A K * R * K; % Joseph 形式保证半正定 P_upd (P_upd P_upd) / 2; end几个细节值得单独说。第一卡尔曼增益用/ S而不是inv(S)*Matlab 的左除内部走的是矩阵分解数值稳定性明显更好尤其是 R 很小、S 接近奇异的时候。第二协方差更新用了 Joseph 形式而不是教科书上的(I-KH)P前者在理论上恒为半正定后者在 K 计算有微小误差时会悄悄破坏正定性。第三nu和S一定要往外输出新息序列是判断滤波器有没有失配的最直接指标白噪声、零均值说明模型匹配出现持续偏置说明模型跟不上nu*inv(S)*nu长期大于状态维数说明 R 给得太小或者 Q 给得太大。H 和 R 的构造跟观测类型绑定。直角坐标直接观测位置时H 就是挑出位置那三行的选择矩阵R 是对角阵对角元是各轴量测方差。这一块保持简单把复杂度留给后面的球坐标观测。3.4 非线性观测下的 EKF雅可比矩阵一个符号写错就发散真实跟踪设备给的往往不是直角坐标而是距离、方位角、俯仰角。观测函数变成非线性的卡尔曼滤波要线性化也就是 EKF。观测量z [r; az; el]状态x [px py pz vx vy vz]观测函数function z_pred obs_model(x) % 由状态预测观测量输出 [距离; 方位角; 俯仰角] px x(1); py x(2); pz x(3); rho sqrt(px^2 py^2); % 水平距离方位角分母要用 r sqrt(px^2 py^2 pz^2); z_pred [r; atan2(py, px); atan2(pz, rho)]; end雅可比矩阵是观测函数对状态的偏导只有位置那三列非零function H jacob_sph(x) px x(1); py x(2); pz x(3); rho2 px^2 py^2; rho sqrt(rho2); r2 rho2 pz^2; r sqrt(r2); H zeros(3, 6); % 距离对位置的偏导 H(1,1) px/r; H(1,2) py/r; H(1,3) pz/r; % 方位角对位置的偏导注意分母是水平距离平方 H(2,1) -py/rho2; H(2,2) px/rho2; H(2,3) 0; % 俯仰角对位置的偏导 H(3,1) -px*pz/(r2*rho); H(3,2) -py*pz/(r2*rho); H(3,3) rho/r2; end这三个公式是最容易抄错的地方尤其是俯仰角那一行分母里同时出现r2和rho少一个就发散。我的习惯是写完立刻做数值验证取一个已知状态用有限差分算一遍雅可比和自己的解析式逐元素对比误差超过 1e-6 就说明推错了。% 雅可比自检解析解 vs 中心差分开发阶段必跑 x_test [1200; -800; 300; 0; 0; 0]; H_ana jacob_sph(x_test); H_num zeros(3, 6); eps_step 1e-6; for i 1:6 dx zeros(6,1); dx(i) eps_step; H_num(:,i) (obs_model(x_testdx) - obs_model(x_test-dx)) / (2*eps_step); end assert(max(abs(H_ana(:) - H_num(:))) 1e-6, 雅可比矩阵推导有误);EKF 的预测步和线性 KF 完全一样更新步把H*x_pred换成obs_model(x_pred)把H换成当前先验处的雅可比。线性化误差在观测非线性强的场景下会体现为收敛变慢甚至震荡这时候可以考虑 UKF但那是第 6 章的事。4. 完整仿真跑通从合成航迹到误差曲线前面的模块单独看都简单串起来跑通并且结果合理才算真正掌握。这一章按执行顺序走一遍给出可以直接抄的代码。4.1 造数据匀速段加机动段的合成航迹与球坐标观测真值航迹要包含匀速和机动两段否则滤波器的模型失配根本测不出来。function [X_true, t] traj_gen(cfg) % 生成三段式航迹匀速巡航 - 协调转弯 - 匀速爬升 dt cfg.dt; N cfg.N; t (0:N-1) * dt; X_true zeros(N, 6); p [5000; 0; 1000]; % 初始位置(m) v [-180; 60; 5]; % 初始速度(m/s)约 0.7 马赫量级 for k 1:N if k 400 k 700 % 机动段给一个持续 15 s 的侧向加速度模拟协调转弯 a [2*cos(0.02*k); 3*sin(0.02*k); 0.5]; else a [0; 0; 0]; end v v a * dt; p p v * dt; X_true(k, :) [p, v]; end end观测生成把真值转到球坐标再叠噪声这一步的噪声标准差必须和cfg里给滤波器的一致否则评估结果没有意义。function Z obs_gen(X_true, cfg) % 真值转球坐标观测叠加高斯噪声 N size(X_true, 1); Z zeros(N, 3); for k 1:N p X_true(k, 1:3); rho sqrt(p(1)^2 p(2)^2); r norm(p); z_true [r; atan2(p(2), p(1)); atan2(p(3), rho)]; Z(k, :) (z_true [cfg.sigma_r; cfg.sigma_az; cfg.sigma_el] .* randn(3,1)); end end注意机动段的加速度不是常数也不是标准的协调转弯模型这是故意的。真实目标不会按教科书飞用一段「不那么标准」的机动去测滤波器结论才有参考价值。4.2 主循环滤波器在时间轴上的推进主循环要做的事情很固定初始化、逐帧预测、逐帧更新、记录。% start_sim.m 主循环片段 X_est zeros(cfg.N, 6); P_hist zeros(6, 6, cfg.N); x [Z(1,1)*cos(Z(1,3))*cos(Z(1,2)); % 用首帧观测反算初始位置 Z(1,1)*cos(Z(1,3))*sin(Z(1,2)); Z(1,1)*sin(Z(1,3)); 0; 0; 0]; P diag(cfg.P0_diag); R diag([cfg.sigma_r^2, cfg.sigma_az^2, cfg.sigma_el^2]); [F, Q] build_cv_3d(cfg.dt, cfg.q); for k 1:cfg.N if k 1 [x, P] kf_predict(x, P, F, Q); % 预测 end H jacob_sph(x); % 在当前先验处线性化 z Z(k, :); nu z - obs_model(x); nu(2) mod(nu(2) pi, 2*pi) - pi; % 角度新息绕回 [-pi, pi] nu(3) mod(nu(3) pi, 2*pi) - pi; S H * P * H R; K P * H / S; x x K * nu; A eye(6) - K * H; P A * P * A K * R * K; P (P P) / 2; X_est(k, :) x; P_hist(:, :, k) P; end初始速度给 0 是保守做法让滤波器自己从观测里把速度估出来。如果首帧观测质量差可以把P0_diag里速度那几项给大一点让前几十帧快速收敛。角度新息的绕回处理绝对不能省方位角从 179 度跳到 -179 度时不做绕回会得到一个接近 2π 的新息滤波器会以为目标瞬移了协方差瞬间炸开。4.3 评估RMSE、NEES 与航迹对比图怎么读位置 RMSE 是最直观的指标但要分段落统计匀速段和机动段分开看否则机动段的误差会被匀速段稀释。function rmse rmse_pos(X_est, X_true, idx) % 按给定帧索引计算三维位置 RMSE err X_est(idx, 1:3) - X_true(idx, 1:3); rmse sqrt(mean(sum(err.^2, 2))); endNEES 检验的是「滤波器对自己的不确定性估计准不准」比 RMSE 更能暴露问题function nees nees_calc(X_est, X_true, P_hist, idx) % 归一化估计误差平方理论均值应接近状态维数 nees zeros(numel(idx), 1); for i 1:numel(idx) e (X_true(idx(i), 1:3) - X_est(idx(i), 1:3)); P P_hist(1:3, 1:3, idx(i)); nees(i) e / P * e; end nees mean(nees); end三维位置对应 3 个自由度NEES 的长期均值应该落在 3 附近。如果稳定在 10 以上说明协方差给得太小滤波器过度自信如果长期低于 1说明协方差给得太大滤波器太保守观测的信息没吃进去。这个指标调参的时候比 RMSE 好用得多因为它不依赖真值量级。画图上三维航迹对比图看整体形状误差随时间曲线看收敛速度新息序列看模型匹配程度协方差椭圆二维投影看不确定性的方向。四张图配合着看基本能定位所有常见问题。4.4 Q 和 R 怎么定一个工程经验公式加试凑Q 和 R 的绝对数值没有意义比值才有意义。R 一般来自设备手册或者实测统计是已知量Q 才是要调的。一个粗糙但好用的起点是让q * dt^3和位置量测方差同量级也就是假设一个采样周期内模型误差的量级接近量测噪声。实际调的时候按这个起点上下浮动一个数量级看 NEES 落在哪。如果 NEES 偏大说明 Q 给小了往上调偏小就往下调。调到 NEES 落在 2 到 5 之间再回头看看 RMSE 和航迹图通常就差不多了。机动段误差明显大于匀速段这不是调参能解决的是 CV 模型本身的局限。要么上 CA、要么上自适应 Q、要么上 IMM三条路在第 6 章展开。5. 避坑与排查五个让滤波器当场发散的现场下面这几条都是我自己和身边人真实翻过的车按「现象 → 原因 → 解决」写遇到了直接对号入座。第一条跑几百帧之后状态变成 NaN。现象是前几十帧还好好的跑到某个时刻X_est突然全变 NaNP_hist里出现 Inf。原因是协方差更新用了(I-KH)*P这种简化形式在 R 很小、增益接近 1 的时候I-KH的数值误差把P推出正定域特征值变负下一步开方或者求逆就炸了。解决是把更新改成 Joseph 形式A*P*A K*R*K并且每次更新后强制对称化。这一步几乎不增加计算量但能把这类问题彻底堵死。第二条目标转弯时滤波器明显滞后误差尖峰。现象是匀速段 RMSE 只有几米一进机动段直接冲到几十米误差曲线上出现一个明显的鼓包机动结束后又降回去。原因是 CV 模型假设加速度为零机动期间模型持续给出错误预测新息出现系统性偏置但 Q 给得小增益不够大滤波器拉不回来。解决有三条路把 q 调大代价是匀速段精度下降、切换到 CA 模型、或者上新息驱动的自适应 Q。最简单的验证方法就是看新息序列机动段新息均值明显偏离零基本可以确诊。第三条方位角在 ±π 附近跳变协方差爆掉。现象是目标飞越正北或者正南方向时滤波器突然发散几帧之后又自己恢复。原因是方位角观测从 179 度跳到 -179 度直接相减得到约 -358 度的新息滤波器以为观测和预测差了将近一整圈。解决是在算新息之后立刻做角度绕回nu mod(nu pi, 2*pi) - pi。这里有个细节Matlab 的wrapToPi在 Mapping Toolbox 里不是所有环境都有用mod自己写更保险也不会因为工具箱缺失报错。第四条整体误差始终偏大怎么调 Q 都没用。现象是 RMSE 稳定在一个偏大的值上NEES 也正常说明滤波器「自认为」估得不错但和真值就是差一截。八成是单位或者量纲的问题时间用了毫秒但 F 里按秒算角度用了度但 R 里按弧度算或者位置用了千米而噪声标准差用了米。这类错误的隐蔽性在于代码不报错只是结果系统性偏移。解决办法是仿真开始前把所有输入的单位打印一遍跟cfg里的注释逐个核对。第五条初始几十帧收敛特别慢甚至被拉偏。现象是滤波器启动后很长一段时间估计值偏离真值慢慢才拉回来。原因是初始协方差P0给得太小滤波器对初始状态过度自信观测进来了也只肯小步修正。解决是把P0的位置项给到量测噪声方差的几十倍速度项给得更夸张一些让前几十帧的增益接近 1快速吃进观测。初始状态本身给得粗糙没关系协方差给对了滤波器自己会收敛。6. 二次开发从二维匀速模型推到三维机动目标跟踪跑通 CV 之后绝大多数人的下一步是把模型换掉让它在真实机动目标上也能用。这里给一条我实际走过的改造路径按改动量从小到大排。改造项改动位置工作量效果状态扩到三维build_cv_3d、obs_model、jacob_sph小支持高度维飞行目标必备换 CA 或 Singerbuild_*函数、F维数中机动段误差明显下降自适应 Q主循环里按新息调整q中兼顾匀速精度和机动响应上 IMM新增模型集与概率更新大复杂机动下最稳调参最烦换 UKF替换线性化环节中强非线性观测下更准第一步扩三维基本是机械劳动但有两个地方必须同步改obs_model里的rho要改成水平距离而不是总距离jacob_sph里俯仰角的分母要跟着变。我见过有人只改了状态维数没改雅可比跑出来的结果看着能用实际上俯仰通道一直在缓慢发散。第二步换模型建议先用 CA 验证一遍。改法是把build_cv_3d复制一份改成九维G里加速度项的二重积分形式写对其余代码一行不动。如果 CA 在机动段比 CV 好、在匀速段比 CV 差说明改对了符合预期。第三步自适应 Q 是我最推荐的折中方案。思路很简单维护一个新息滑动窗口窗口内新息的加权平方和超过阈值就把q放大回落之后再调回去。% 新息驱动的自适应 Q放在主循环更新步之前 win 10; % 滑窗长度 if k win nu_win nu_buf(:, end-win1:end); stat mean(sum(nu_win.^2, 1)); % 窗口内新息能量 q_adapt cfg.q * max(1, stat / cfg.q_ref); [~, Q] build_cv_3d(cfg.dt, min(q_adapt, 100*cfg.q)); % 上限保护 endcfg.q_ref是匀速段标定出来的新息能量基准100*cfg.q是防止目标长时间机动把 Q 推到无穷大的保护上限。这个方案不需要维护多个模型代码量增加不到二十行实测在持续十几秒的机动段能把误差尖峰压掉一半以上。验证方法上我建议固定随机种子把改动前后的 RMSE 和 NEES 曲线画在同一张图上对比同时看新息序列的自相关。自相关明显非零说明还有模型失配没吃掉这时候再考虑上 IMM。别一上来就堆模型先把单模型的极限摸清楚后面加复杂度才知道收益从哪来。从那以后我每次改滤波器的状态维数或者观测模型都强制先跑一遍雅可比的有限差分自检再跑一遍 NEES这两个过了才看 RMSE。这个习惯帮我省掉了无数次「结果看着还行其实早就偏了」的返工。希望帮到你。本文还有配套的精品资源点击获取