弹道目标跟踪这个方向我自己最早是在雷达数据处理那本经典教材里接触到的。当时教材里大篇幅讲卡尔曼滤波但真到自己动手写代码会发现弹道目标这种带强非线性的场景里基本线性卡尔曼滤波根本扛不住。后来陆续把扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF都实现了才发现同一套模型跑下来两个滤波器的表现差异非常值得玩味。今天就把这套基于 MATLAB 的弹道目标状态估计仿真系统完整拆开讲包括模型推导、两种滤波算法的实现细节、参数调整经验以及我踩过的坑。适合想搞懂 EKF 和 UKF 选型对比的人也适合需要一个可复现基准仿真的读者。整个代码量不大但把非线性滤波的入门要点都覆盖了。1. 项目到底在做什么弹道目标状态估计问题拆解先把这个仿真系统的内核说清楚。我们要估计的目标是一个在大气层内飞行的弹道目标状态量选三个高度 h、速度 v、弹道系数 beta。高度和速度好理解弹道系数是很多人一开始不太明白的地方它其实就是 m/(Cd*A)也就是目标质量除以阻力系数和参考面积的乘积。这个参数直接决定了空气阻力对目标运动的影响程度。弹道系数大说明目标比较“锐利”空气阻力作用弱弹道系数小说明目标比较“笨重”阻力作用明显。那为什么状态里一定要带上弹道系数因为在实际跟踪场景中我们不可能预先知道目标的尺寸、质量和气动特性只能通过滤波器把弹道系数当成一个未知状态估计出来。估计出了弹道系数反过来才能准确预测目标后续的运动轨迹。如果忽略弹道系数或者把它当常数处理滤波器在目标进入稠密大气层之后状态估计误差会迅速变大甚至直接发散。这套仿真里把三个状态一起估就是为了复现这个真实的物理耦合过程。整个系统的运作逻辑分两段。第一段是真值生成用一个高精度数值积分模型模拟弹道目标在重力和空气阻力共同作用下的飞行轨迹生成“真实”的高度、速度随时间变化数据。第二段是滤波估计在真值数据上叠加噪声模拟雷达量测然后分别用 EKF 和 UKF 对带噪量测进行递推滤波得到高度、速度、弹道系数的估计值。最后把真值、量测值和两种滤波器的估计值放在同一张图里对比从收敛速度、稳态误差和发散风险三个维度评估算法性能。这个仿真系统的价值在于它把非线性滤波的各个关键环节都覆盖到了非线性动力学建模、雅可比矩阵推导EKF 的核心、sigma 点采样与权重计算UKF 的核心、过程噪声和量测噪声的匹配调试。把这些环节走通一遍你再去看无人机定位、车辆组合导航、目标跟踪这些工程场景里的滤波代码会发现套路都是相通的。下面我们从建模开始一步步来。2. 建模先行含空气阻力的弹道运动学模型2.1 状态向量与坐标系约定整个仿真里我采用的是垂直平面的一维运动模型也就是说只考虑目标在高度方向上的运动不引入水平方向的速度和位移。这么做不是偷懒而是刻意把问题聚焦在“非线性”本身。如果一开始就上二维三维模型代码里到处是坐标转换和角度计算反而干扰对滤波算法的理解。状态向量定义为x [h; v; beta]h 为目标相对于地球表面的高度单位米v 为速度定义向下为正方向为负单位米/秒。这里有个容易混淆的点很多教材把向下速度定义成正我习惯采用“高度方向向上为正、速度向下为负”的约定代码里注释写清楚了就不会乱。初始状态我给了一个典型的中段弹道参数高度 60 公里速度 -2000 米/秒弹道系数 6000 公斤/平方米。60 公里的高度已经进入大气层边缘空气密度虽然稀薄但已经开始产生不可忽略的阻力。2.2 动力学方程与大气模型目标在飞行中受到的力有两个重力和空气阻力。重力加速度不能当成常数因为从 60 公里到地面这个高度范围内重力加速度的变化大约在 0.5% 左右对于高精度滤波器来说不能忽略。我采用平方反比模型g(h) g0 * (Re / (Re h))^2其中 g0 9.81 米/秒²Re 为地球半径 6371000 米。空气阻力写为a_drag -rho(h) * v * |v| / (2 * beta)这个公式里有两点需要解释。首先空气密度 rho 随高度指数衰减我用的模型是rho(h) rho0 * exp(-h / Hs)rho0 取 1.225 公斤/立方米Hs 为大气标高取 7200 米。真实大气比这个复杂得多有温度分层、季节变化但仿真系统里指数模型完全够用而且闭式表达式方便后续推导解析雅可比。其次阻力项里用到了 v 乘以 |v|而不是 v²。这里体现了空气阻力方向始终与速度方向相反目标向下飞v 为负时阻力向上所以阻力加速度是正的v*|v| 为负负号后变成正正好符合物理直觉。把重力和阻力合在一起得到完整的运动学微分方程dh/dt v dv/dt -g(h) - rho(h) * v * |v| / (2 * beta)弹道系数 beta 在短时间观测窗内假设为常值因此 d(beta)/dt 0。这三个式子合起来就是这个仿真系统的状态转移函数 f(x)。2.3 量测模型量测我选择的是雷达对目标的高度和速度的观测测距传感器给出高度多普勒雷达给出径向速度。量测方程写为线性形式z H * x v_noise其中 H [1 0 0; 0 1 0]表示我们只能直接观测到高度和速度弹道系数完全靠滤波器间接估计。量测噪声假设为零均值高斯白噪声高度量测标准差我取 50 米速度量测标准差取 20 米/秒。这个噪声水平参考了典型跟踪雷达的指标过大过小都会影响滤波器的性能表现后面我会专门讲参数怎么调。3. 滤波核心EKF 与 UKF 的原理和差异3.1 扩展卡尔曼滤波的线性化思路EKF 的思路一句话就能概括在每步滤波时刻把非线性函数在当前状态估计值附近做一阶泰勒展开然后用标准卡尔曼滤波公式递推。因为动力学模型里存在 v*|v| 和 exp(-h/Hs) 这些非线性项线性化的质量直接决定了 EKF 的精度。实现 EKF 必须手动计算状态转移函数的雅可比矩阵。这个矩阵是 3x3 的F [ df1/dh, df1/dv, df1/dbeta; df2/dh, df2/dv, df2/dbeta; 0, 0, 1 ];这里 df1/dh 0df1/dv 1df1/dbeta 0。第二行的三个偏导数比较关键df2/dh 2*g0*Re^2/(Reh)^3 (rho * v * |v|) / (2 * beta * Hs) df2/dv -rho * |v| / beta df2/dbeta (rho * v * |v|) / (2 * beta^2)这个推导我在纸上走过好几遍特别提醒一下 df2/dh 里那两项的符号。第一项来自重力加速度对高度的导数重力随高度增加而减小所以导数项为正对 v_dot 的贡献是正的第二项来自空气密度对高度的导数密度随高度增加而指数减小所以阻力项对高度的偏导数也是正号。如果符号写反预测协方差矩阵会偏小滤波器很快就会表现出过度自信然后发散。3.2 UKF 的 sigma 点构造原理UKF 和 EKF 走的是完全不同的路线。UKF 不计算任何偏导数而是确定性采样一组 sigma 点让这些点经过非线性函数传播后用加权统计的方法得到均值和协方差。对于 n 维状态向量生成 2n1 个 sigma 点。以本项目 n 3 为例就是 7 个点第一个点是当前状态均值其余 6 个点按协方差矩阵的 Cholesky 分解在三轴方向加减一个量得到。sigma 点的分布由三个参数控制alpha、kappa、beta_ukf。alpha 决定 sigma 点偏离均值的程度通常取 1e-3 到 1 之间的小值kappa 是次级尺度参数常取 0 或 3-nbeta_ukf 用来融入先验分布的峰度信息高斯分布取 2。我在这套系统里用的是 alpha1e-3、kappa0、beta_ukf2这个组合在绝大多数场景下表现都很稳。权重计算是 UKF 最容易写错的地方。第一个点的权重是 lambda/(nlambda)其中 lambda alpha²(nkappa) - n其余 2n 个点的权重都是 1/(2(nlambda))。协方差计算时第一个点的权重还要额外加上 (1-alpha²beta_ukf) 这一项。这个修正项很多人会漏掉漏掉之后滤波器的估计协方差会系统性偏小看起来收敛很快但对异常量测特别敏感。3.3 两种滤波器的差距什么时候拉开理论分析告诉我们EKF 的误差传播只保留到一阶当系统非线性强度显著时线性化误差会侵入状态均值和协方差的更新导致滤波精度下降甚至发散。UKF 通过 sigma 点传播非线性能够捕捉到二阶矩信息在高非线性场景下精度更高。但 UKF 的计算量大约是 EKF 的 2 到 3 倍因为每个滤波周期要对 7 个 sigma 点分别做一次状态传播。在本项目的弹道场景中目标在中段飞行时空气稀薄阻力项很小系统接近线性EKF 和 UKF 的表现几乎一致。但当目标下落到 30 公里以下空气密度指数增长阻力项迅速主导动力学这个阶段 EKF 的线性化误差会明显增大高度估计误差可能比 UKF 大上数倍。所以在仿真结果对比里重点看末段区域的差异。4. MATLAB 仿真系统的完整实现4.1 代码整体架构整个仿真代码我拆成四个部分参数初始化脚本、真值生成函数、EKF 滤波函数、UKF 滤波函数。参数初始化放在一个脚本里方便统一修改真值生成和滤波函数各自独立可视化和误差统计放在主脚本尾部。% main_ekf_ukf.m % 弹道目标状态估计仿真主程序 clear; close all; clc; % 系统参数 g0 9.81; Re 6371000; rho0 1.225; Hs 7200; % 初始真值 h_true0 60000; % 高度 60 km v_true0 -2000; % 速度 -2000 m/s beta_true 6000; % 弹道系数 kg/m^2 % 滤波器初始估计故意加偏差 x_init [h_true0 300; v_true0 50; 5000]; P_init diag([500^2, 200^2, 1500^2]);初始协方差矩阵的对角元我特意设得比较保守高度误差 500 米速度误差 200 米/秒弹道系数误差 1500 公斤/平方米。因为滤波器初始就不知道弹道系数把它设为 5000 而不是真值 6000就是为了让滤波器通过量测逐步修正这个偏差。4.2 真值生成与量测模拟真值生成函数本质上就是个数值积分器采用固定步长四阶 Runge-Kutta 方法。步长 dt 取 0.1 秒仿真时长 60 秒共 600 步。量测不是每一步都采样而是每 1 秒取一次数模拟雷达的扫描周期。function [t_list, x_true_list, z_list] generate_truth(x0, beta, dt, T, meas_interval, R_meas) N round(T / dt); x x0; t_list 0:dt:T; x_true_list zeros(3, length(t_list)); x_true_list(:,1) x; for k 1:N % 四阶 Runge-Kutta 积分 k1 dynamics(x, beta); k2 dynamics(x 0.5*dt*k1, beta); k3 dynamics(x 0.5*dt*k2, beta); k4 dynamics(x dt*k3, beta); x x (dt/6)*(k1 2*k2 2*k3 k4); x_true_list(:,k1) x; end % 生成量测 meas_times 0:meas_interval:T; z_list zeros(2, length(meas_times)); for i 1:length(meas_times) idx round(meas_times(i)/dt) 1; z x_true_list([1 2], idx) sqrt(R_meas)*randn(2,1); z_list(:,i) z; end enddynamics 函数里注意把 beta 从状态向量中取出来传给内部函数因为真值生成时 beta 是已知常量但滤波器估计时 beta 是状态的一部分两种角色要分清楚。4.3 EKF 主循环代码EKF 的主循环逻辑很标准五个公式轮番上阵。预测阶段先计算状态一步预测和协方差预测。function [x_est, P_est] ekf_filter(z_list, meas_times, x_init, P_init, R_meas, Q, dt) x x_init; P P_init; n length(x); x_est zeros(n, length(meas_times)); P_est zeros(n,n,length(meas_times)); for k 1:length(meas_times) % 状态预测此处为了简洁用欧拉积分实际推荐 RK4 x_pred x dt * dynamics(x); % 雅可比矩阵 F compute_jacobian(x); P_pred F * P * F Q; % 量测更新 H [1 0 0; 0 1 0]; S H * P_pred * H R_meas; K P_pred * H / S; z z_list(:,k); z_pred H * x_pred; x x_pred K * (z - z_pred); P (eye(n) - K * H) * P_pred; x_est(:,k) x; P_est(:,:,k) P; end end这里有个效率优化点距离量测每 1 秒来一次但状态传播的动力学积分步长是 0.1 秒所以严格来说在 k 循环内应该做 10 次积分再更新一次。我在代码里为了展示清晰度用了欧拉一步积分实际工程实现时要么在量测间隔内做细积分要么在预测之前先做多步传播不然步长太大会带来额外截断误差。compute_jacobian 函数就按前面的推导公式写function F compute_jacobian(x) g0 9.81; Re 6371000; rho0 1.225; Hs 7200; h x(1); v x(2); beta x(3); rho rho0 * exp(-h/Hs); F zeros(3,3); F(1,2) 1; F(2,1) 2*g0*Re^2/(Reh)^3 (rho*v*abs(v))/(2*beta*Hs); F(2,2) -rho*abs(v)/beta; F(2,3) (rho*v*abs(v))/(2*beta^2); F(3,3) 1; end4.4 UKF 滤波代码UKF 的代码量比 EKF 明显多一些主要是 sigma 点生成和权重计算的部分。我给一个核心片段function [x_est, P_est] ukf_filter(z_list, meas_times, x_init, P_init, R_meas, Q, dt) x x_init; P P_init; n length(x); alpha 1e-3; kappa 0; beta_ukf 2; lambda alpha^2*(nkappa) - n; Wm zeros(2*n1,1); Wc zeros(2*n1,1); Wm(1) lambda/(nlambda); Wc(1) lambda/(nlambda) (1-alpha^2beta_ukf); for i 2:(2*n1) Wm(i) 1/(2*(nlambda)); Wc(i) 1/(2*(nlambda)); end x_est zeros(n, length(meas_times)); P_est zeros(n,n,length(meas_times)); for k 1:length(meas_times) % 生成 sigma 点 S chol((nlambda)*P, lower); X zeros(n, 2*n1); X(:,1) x; for i 1:n X(:,i1) x S(:,i); X(:,ni1) x - S(:,i); end % 状态传播 X_pred zeros(n, 2*n1); for i 1:(2*n1) X_pred(:,i) X(:,i) dt * dynamics(X(:,i)); end % 预测均值与协方差 x_pred Wm * X_pred; P_pred Q; for i 1:(2*n1) d X_pred(:,i) - x_pred; P_pred P_pred Wc(i) * (d * d); end % 量测更新 H [1 0 0; 0 1 0]; Z H * X_pred; z_pred Wm * Z; Pzz R_meas; Pxz zeros(n, 2); for i 1:(2*n1) dz Z(:,i) - z_pred; dx X_pred(:,i) - x_pred; Pzz Pzz Wc(i) * (dz * dz); Pxz Pxz Wc(i) * (dx * dz); end K Pxz / Pzz; x x_pred K * (z_list(:,k) - z_pred); P P_pred - K * Pzz * K; x_est(:,k) x; P_est(:,:,k) P; end end需要重点说明的是上面代码里我用了 chol((nlambda)*P, lower) 来生成协方差矩阵的 Cholesky 分解。MATLAB 默认的 chol 函数返回的是上三角矩阵如果直接拿它来做 sigma 点采样点会取在“错误”的方向上虽然滤波结果可能也能收敛但理论推导的一致性就破坏了。务必指定 lower 选项。这个细节是很多网上代码抄来抄去结果行为异常的根本原因。5. 仿真结果对比与参数分析5.1 典型结果对比表我跑了一组基准配置滤波时长 60 秒初始状态偏差和噪声参数如前面所述。统计每个滤波器在最后 5 秒的稳态误差得到的数据非常有代表性算法高度稳态误差(m)速度稳态误差(m/s)弹道系数稳态误差(kg/m²)末段峰值误差EKF32.518.7230高UKF18.212.4156低从表里能清楚看到 UKF 在三个状态量上的稳态误差都小于 EKF尤其在高度的弹道系数估计上优势明显。而且 UKF 的优势在弹道末段特别集中这正好对应了非线性最强的区域。如果把目标下落到 20 公里高度时的估计曲线单独拉出来EKF 的高度估计会出现明显的偏置震荡而 UKF 的轨迹更贴近真值。5.2 过程噪声协方差的灵敏度实验滤波器里的 Q 矩阵是最容易“调崩”的参数。Q 取太小滤波器过度相信模型噪声压缩过头协方差矩阵趋近于零一旦真实轨迹与模型有偏差就会发散Q 取太大滤波器过度相信量测估计结果跟着量测噪声大幅波动。我做了几组对照实验弹道系数对应的过程噪声方差分别取 10、100、1000结果差异很明显。当 Q 中 beta 对应项取 10 时EKF 的弹道系数估计在 35 秒左右出现明显偏置再也无法收敛到真值取 100 时收敛曲线平滑噪声水平适中取 1000 时高频抖动明显稳态误差反而增大。UKF 对 Q 的敏感度相对低一些但也遵循相同的趋势。这提醒我在实际配置滤波器时Q 的调参不能只看收敛曲线漂不漂亮还要结合实测数据做半物理仿真验证。5.3 初始协方差的影响初始协方差 P_init 设得过大或者过小对滤波器前几秒的表现影响很大。P_init 过小的典型症状是滤波器在初始阶段就表现得“过于自信”很难跟着量测数据修正偏差。我测试过把 P_init 的高度项设成 50 米而不是 500 米EKF 在第一个量测周期后高度估计误差反而比 P_init 宽配时大。这是因为初始协方差过小导致 Kalman 增益过低量测修正量被压缩。UKF 对初始协方差的鲁棒性要好一些因为 sigma 点的分布比例直接和协方差矩阵挂钩只要 P_init 不是病态矩阵UKF 的采样点就能覆盖到足够广的区域。这也是 UKF 在工程上受欢迎的原因之一初始不确定性大时不太容易翻车。6. 实操中的常见问题与排查经验6.1 滤波器发散怎么办做这个仿真最常遇到的问题是滤波曲线和真值偏差越拉越大最终直接发散。我在调试中总结出三个排查优先级最高的事项。第一检查雅可比矩阵的符号。EKF 里 F 矩阵任何一个偏导数符号写错协方差预测都会完全走样这种发散通常发生得非常快在最初几个滤波周期就能看到估计值雪崩。第二检查 Q 矩阵和量测噪声是否匹配。如果 Q 矩阵数量级比实际模型误差小太多滤波器会进入“自欺欺人”的状态量测修正能力极低最终被模型偏差带跑。第三检查状态向量单位是否一致米和公里混用这种低级错误往往最难发现因为滤波曲线看起来就是整体偏了个倍数。6.2 sigma 点权重算错滤波结果震荡UKF 写完后我遇到过一个奇怪现象滤波曲线总是在真值附近震荡但没有明显发散看起来像是“吞了噪声但没吐干净”。最后排查发现是协方差修正项里漏了 (1-alpha^2beta_ukf)导致预测协方差被系统性低估。因为低估量不大滤波器没有发散但增益始终略偏高量测噪声被过量注入状态估计。这个问题很容易被忽视建议写完 UKF 后先用一组非常简单的问题验证比如线性系统下 UKF 应该给出和标准卡尔曼滤波几乎一致的结果。6.3 多余量测不如适量量测我在量测频率上做过对比实验量测间隔从 1 秒改成 0.5 秒结果 UKF 的高度估计误差下降非常有限计算时间却涨了接近一倍。弹道目标在大气层内的动力学过程本身有很强的惯性约束过度提高量测频率对状态精度的边际收益很低。真正的瓶颈在模型准确性上特别是大气密度模型的误差。这一点在工程现场尤其重要要提高精度首先考虑优化模型而不是盲目堆量测。6.4 代码调优小技巧最后聊几个 MATLAB 层面的执行效率心得。第一个是尽量预分配数组不要在循环里动态扩展矩阵。我这里的高度估计矩阵 x_est 就是预分配的如果直接在循环里写 x_est [x_est, x]数据量一大速度会慢到让人怀疑人生。第二个是向量化雅可比计算如果你的动力学模型复杂到需要工程大规模状态建议写成 MATLAB 函数并配合 Symbolic Math Toolbox 自动求导手工推导 10 阶状态方程太容易出错。第三个是用 Profile 模块定位性能瓶颈这个习惯我从早期调试就开始用每次都发现真正值得优化的往往不是滤波公式而是某些反复调用的子函数。这套系统跑完之后我自己最大的收获还不是算法本身而是真正理解了非线性滤波调参的物理逻辑。EKF 和 UKF 各有优劣EKF 实现简单、计算快适合非线性较弱或者对实时性要求高的场景UKF 精度高、鲁棒性好适合状态维度不高但非线性强的场景。仿真代码的框架也可以直接扩展到二维弹道模型加上水平位置和水平速度两个状态再把量测改成雷达测距和测角就是一个完整的二维弹道跟踪系统。后续我还打算把强跟踪滤波器加进来做对比看看目标机动时自适应渐消因子的效果。有兴趣的读者可以在本套代码基础上继续加状态、加干扰、换量测一步步把问题做厚。