刚把手头的弹道目标状态估计仿真系统跑完准备把整个设计过程、滤波算法、参数整定和踩过的坑都写下来。这套东西用 Matlab 实现核心就是两个非线性滤波器扩展卡尔曼滤波 EKF 和无迹卡尔曼滤波 UKF作用对象是含空气阻力的弹道目标状态变量包括高度、速度、弹道系数。别小看这个组合真正要把高度、速度、弹道系数三个量同时估准尤其是那个弹道系数 β没弄过的人会一脸懵。写这篇东西的起因是前阵子做雷达目标跟踪相关的仿真发现不少朋友卡在“模型怎么建、滤波器怎么不散”这两关上。很多人一上来就用常规卡尔曼滤波结果遇到非线性阻力项直接发散还有人把 EKF 和 UKF 的差异讲得玄乎其玄但真到写代码时又不知道该用多少 sigma 点。所以我打算从工程实践的角度把这一整套从建模到仿真的细节讲清楚顺手把 Matlab 源码的设计思路和关键片段公开出来。不管是做毕业设计、科研课题还是刚开始接触目标跟踪的工程师这篇内容都能直接拿去参考照着搭一个能跑出曲线、能出误差统计的完整仿真系统。1. 弹道目标状态估计先搞清楚你要估什么1.1 三个状态变量的物理含义弹道目标通常指在大气层内或跨大气层飞行的无动力或有动力目标典型的比如高炮炮弹、火箭弹、某些飞行器等。由于本文重点是滤波跟踪目标模型可以简化为质点运动但必须考虑空气阻力的影响。在常规的目标跟踪模型里状态往往只取位置和速度比如二维平面下的[x, vx, y, vy]。但这套系统的特别之处在于它额外引入了弹道系数 β并且把 β 作为一个需要在线估计的状态变量。弹道系数 β 的物理定义是 m / (C_d A)其中 m 是质量C_d 是阻力系数A 是参考面积。β 越大说明目标受空气阻力的影响越小β 越小则阻力影响越大就像一片羽毛和一个铁球同时下落铁球的 β 远大于羽毛所以铁球在空气中减速慢得多。在实际测量中β 往往未知且可能随目标姿态改变所以把它放进状态向量里做在线估计是这类仿真系统贴近工程需求的关键设计。状态向量可以写成[ \mathbf{x} [h, \dot{h}, \beta]^T 或 [h, v, \beta]^T ]其中 h 代表高度v 代表速度严格说是竖直方向速度也可以扩展为三维速度这里为了简化用二维模型演示β 就是弹道系数。后面所有滤波更新都围绕这三个量展开。1.2 含空气阻力的运动方程怎么列在只考虑竖直方向运动、且空气密度近似为常数或按指数衰减的情况下目标动力学方程可以写成[ \dot{h} v ][ \dot{v} -g - \frac{\rho(h) \cdot v \cdot |v|}{2\beta} ][ \dot{\beta} 0 ]这里第二项是阻力加速度。注意阻力方向始终与速度方向相反所以用了v|v|而不是v^2避免符号错误。空气密度 ρ 可以按高度变化[ \rho(h) \rho_0 \cdot \exp(-h / H_s) ]其中 ρ0 是海平面密度Hs 是大气标高一般取 8000 米左右。如果目标在稠密大气层内运动这个指数密度模型比常数密度更贴近实际。第三行写 β 的导数为 0表示短时间内认为弹道系数不变或者缓变实际滤波时我们会在过程噪声里给 β 留一点不确定性允许它慢漂移。这里有一个非常容易踩的坑阻力项除以 β 时如果 β 在滤波过程中被估计成负数或接近 0整个动力学方程会爆掉。所以后面初始化时要把 β 限制在合理区间且在每一步状态更新后加约束这就是仿真系统的第一个“安全阀”。2. EKF和UKF两种非线性滤波的核心思路2.1 为什么不能用标准卡尔曼滤波经典的卡尔曼滤波要求系统是线性的也就是状态方程和观测方程都是线性的x(k) A·x(k-1) wy(k) H·x(k) v。但弹道目标含阻力后状态方程里出现了 v*|v|/β 这种非线性项观测方程如果是雷达测距也会出现距离与坐标之间的非线性关系。强行用线性卡尔曼滤波去套相当于把曲线当成直线去外推状态一步偏离后面就再也拉不回来滤波结果基本就是废的。处理非线性滤波有两条经典路线近似线性化或者直接对概率分布进行变换。前者就是扩展卡尔曼滤波 EKF后者就是无迹卡尔曼滤波 UKF。两者本质都是基于高斯假设的最优递推滤波只是在处理非线性传递时的策略不同。2.2 EKF的线性化处理EKF 的思路很简单既然非线性函数难处理我就在当前估计点处做泰勒展开只保留一阶项得到雅可比矩阵 F 和 H然后把原来卡尔曼增益公式里的 A、H 替换成 F、H。每一步预测和更新都重新计算雅可比矩阵相当于用一个逐渐变化的线性模型去逼近真实非线性过程。具体到本系统状态预测[ \mathbf{x}{k|k-1} f(\mathbf{x}{k-1}) ][ \mathbf{P}_{k|k-1} \mathbf{F}k \mathbf{P}{k-1} \mathbf{F}_k^T \mathbf{Q} ]这里 f 就得用数值积分一般用四阶龙格库塔RK4把连续方程离散化。F 是对状态变量求偏导的 3×3 矩阵形式为[ F \begin{bmatrix} \frac{\partial f_1}{\partial h} \frac{\partial f_1}{\partial v} \frac{\partial f_1}{\partial \beta} \ \frac{\partial f_2}{\partial h} \frac{\partial f_2}{\partial v} \frac{\partial f_2}{\partial \beta} \ \frac{\partial f_3}{\partial h} \frac{\partial f_3}{\partial v} \frac{\partial f_3}{\partial \beta} \end{bmatrix} ]计算时如果解析式太麻烦可以用数值差分近似雅可比比如[ \frac{\partial f_i}{\partial x_j} \approx \frac{f_i(x\delta x_j) - f_i(x-\delta x_j)}{2\delta} ]这种“数值雅可比”在写 Matlab 验证时特别方便但是要注意步长 δ 不能太大也不能太小。太大非线性近似误差大太小浮点舍入误差大。我实测下来对状态量取 δ 1e-6 倍的标准差比较稳。观测方程如果是高度直接测量那么观测矩阵就是 H [1, 0, 0]如果测量斜距 R那么观测函数是非线性的也需要求偏导。EKF 的优势是计算量相对小实现简单劣势是当非线性程度很强时一阶泰勒展开会引入较大的线性化误差导致滤波发散。2.3 UKF的sigma点传播UKF 走的是另一条路它不去线性化函数而是用一组确定性的 sigma 点去逼近状态分布然后把这些 sigma 点分别通过非线性函数再用加权统计得到预测均值和协方差。具体流程第一步生成 sigma 点。假设状态维度 n 3均值为 x̄协方差为 P则生成 2n1 7 个 sigma 点[ \chi_0 \bar{x} ][ \chi_i \bar{x} (\sqrt{(n\lambda)P})_i, \quad i1,...,n ][ \chi_{in} \bar{x} - (\sqrt{(n\lambda)P})_i, \quad i1,...,n ]其中 λ α²(nκ) - nα 决定 sigma 点离均值的距离通常取 1e-3 到 1κ 一般取 0 或 3-n。矩阵平方根通常用 Cholesky 分解求。第二步把每个 sigma 点代入状态方程 f(χ)得到传播后的 sigma 点。第三步加权计算预测均值和预测协方差[ \mathbf{x}{k|k-1} \sum{i0}^{2n} W^{(m)}_i \chi_i ][ \mathbf{P}{k|k-1} \sum{i0}^{2n} W^{(c)}i (\chi_i - \mathbf{x}{k|k-1})(\chi_i - \mathbf{x}_{k|k-1})^T \mathbf{Q} ]权值公式为[ W^{(m)}_0 \frac{\lambda}{n\lambda} ][ W^{(c)}_0 \frac{\lambda}{n\lambda} (1-\alpha^2\nu) ][ W^{(m)}_i W^{(c)}_i \frac{1}{2(n\lambda)}, \quad i1,...,2n ]这里的 ν 是二阶信息系数用来引入高阶矩的影响一般取 2。 UKF 的优点是不需要显式求雅可比对非线性函数的逼近精度至少到二阶比 EKF 更适合强非线性系统代价是计算量稍大而且协方差矩阵需要保持正定性否则 Cholesky 分解会失败。2.4 滤波参数的选取逻辑无论是 EKF 还是 UKF有几个参数是挡不住的拦路虎初始状态、初始协方差 P0、过程噪声协方差 Q、观测噪声协方差 R。初始状态可以给真值附近的值或者从第一帧观测直接解算。初始协方差 P0 描述的是你对初始状态的信任程度。如果 P0 给得特别大滤波器开始时会剧烈修正状态如果给得特别小滤波器会“固执己见”对观测不敏感。对于弹道目标我一般取 P0 diag([10^4, 10^2, 1])也就是高度上信任度差速度信任度中等弹道系数信任度较高。过程噪声 Q 则表达模型误差和外部扰动。状态变量高度、速度、β 的 Q 不能简单取同一个值因为它们的量纲完全不同。高度单位是米速度是米/秒β 可能是 kg/m²数值范围差异极大。一个常用做法是把 Q 写成[ Q \begin{bmatrix} q_h 0 0 \ 0 q_v 0 \ 0 0 q_\beta \end{bmatrix} ]数值上参考状态变化的物理幅度来设定比如 q_h 可取 10 到 100对应高度过程噪声的方差q_v 取 1 到 10q_beta 取 1e-4 到 1e-2。注意 β 的过程噪声不能太大否则估计值会乱跳也不能太小否则真值缓慢变化时滤波器跟不上。观测噪声 R 可以根据传感器精度设置比如雷达测高误差标准差 50 米则 R 2500。滤波结果对 R 和 Q 的比例关系非常敏感这个比例决定了“我该信模型还是信观测”我后面会在常见问题里专门讲。3. Matlab仿真系统搭建实操3.1 系统整体框架与数据流这套仿真系统我建议拆成四个模块参数初始化模块、真值生成模块、滤波循环模块、结果评估模块。参数初始化负责设置时间步长、总时长、目标初始状态、误差参数、滤波初始状态等。真值生成模块用高精度 RK4 积分跑出“真实轨迹”并在此基础上叠加观测噪声模拟传感器读数。滤波循环模块则是每个步长内依次执行预测、更新把估计值存下来。结果评估模块最后算 RMSE、均值误差并绘图。数据流其实很简单初始化真值状态 trja [h_true0; v_true0; beta_true0]滤波状态 x [h_init; v_init; beta_init]协方差 P P0观测噪声协方差 R过程噪声 Q。循环 k 1:N先根据真实状态用 RK4 递推一步得到 h_true(k1)然后生成观测 z h_true(k1) sqrt(R)*randn再分别调用 EKF 或 UKF 的滤波函数输入当前 x、P、Q、R、z输出新的 x、P保存估计值。循环结束绘制真实高度、估计高度、误差曲线计算 RMSE。3.2 真值生成与观测模拟真值生成必须和滤波模型使用同一套微分方程但积分步长可以更小以减少离散化误差。我这里先定义运动方程函数输入状态 x 和代数参数返回导数 dxdt。function dx ballisticDynamics(x, g, rho0, Hs, beta) h x(1); v x(2); b x(3); rho rho0 * exp(-h / Hs); dx zeros(3,1); dx(1) v; dx(2) -g - (rho * v * abs(v)) / (2 * b); dx(3) 0; % beta slow-varying end真值生成时我用 RK4每个滤波步长分成 M 个子步一般 M 取 4 到 10 就够。代码如下function x_next rk4Step(ode, x, dt, M, params) hstep dt / M; x_next x; for i 1:M k1 ode(x_next, params); k2 ode(x_next 0.5*hstep*k1, params); k3 ode(x_next 0.5*hstep*k2, params); k4 ode(x_next hstep*k3, params); x_next x_next (hstep/6)*(k1 2*k2 2*k3 k4); end end观测模型这里我写成直接测量高度测量函数就是 z h v_noise。如果你想模拟雷达斜距测量可以加上斜距方程但高度测量已经足够把 EKF 和 UKF 的差异展示出来。观测噪声标准差我默认取 30 米这个量级对于弹道目标来说算是中等噪声能够看出滤波器的平滑能力。3.3 EKF与UKF循环代码骨架EKF 的更新部分重点在雅可比矩阵计算。我用数值差分的方法避免手推导数的繁琐精度也足够function [x_upd, P_upd] ekfUpdate(x_pred, P_pred, z, R, dx) % 数值雅可比 F numericalJacobian((x) ballisticDynamics(x, params), x_pred); y z - x_pred(1); % 残差 H [1, 0, 0]; S H * P_pred * H R; K P_pred * H / S; x_upd x_pred K * y; P_upd (eye(3) - K * H) * P_pred; endUKF 的关键则在于 sigma 点的生成和权重计算。我写了一个通用函数function [X, Wm, Wc] utTransform(x, P, alpha, kappa) n length(x); lambda alpha^2 * (n kappa) - n; Psqrt chol((n lambda) * P, lower); X zeros(n, 2*n1); X(:,1) x; for i 1:n X(:,i1) x Psqrt(:,i); X(:,in1) x - Psqrt(:,i); end Wm zeros(1, 2*n1); Wc zeros(1, 2*n1); Wm(1) lambda / (n lambda); Wc(1) lambda / (n lambda) (1 - alpha^2); for i 2:2*n1 Wm(i) 1 / (2*(nlambda)); Wc(i) 1 / (2*(nlambda)); end end这里 chol 要求 P 是正定的。如果出现非正定说明协方差发散需要加一个“比例修正”或使用 robust 挑战者函数。预测时把每个 sigma 点经 RK4 一步传播然后加权得到预测均值和协方差更新时同样计算 sigma 点在观测域的均值、协方差、互协方差然后计算增益。核心代码% 预测 [X_pred, Wm, Wc] utTransform(x, P, alpha, kappa); X_pred_state zeros(n, 2*n1); for i 1:2*n1 X_pred_state(:,i) rk4Step(ballisticDynamics, X_pred(:,i), dt, M, params); end x_pred sum(X_pred_state * Wm, 2); P_pred zeros(n,n); for i 1:2*n1 diff X_pred_state(:,i) - x_pred; P_pred P_pred Wc(i) * (diff * diff); end P_pred P_pred Q;更新时对观测函数这里是直接测高度计算 sigma 点对应的 z类似地得到 S 和交叉协方差然后求增益。这些代码如果跑通了你会发现 UKF 在强非线性情况下尤其是 beta 估计收敛曲线要比 EKF 稳得多。4. 仿真结果对比与精度分析4.1 高度、速度、弹道系数估计误差典型曲线我设置了这样一组仿真初始条件初始高度 30000 米初始速度 1500 m/s向上β 真值设为 5000 kg/m²时间步长 1 秒仿真 100 步观测噪声标准差 30 米。初始滤波状态在真值附近加偏差高度偏移 200 米速度偏移 50 m/sβ 偏移 200也就是 5200。跑完后典型的误差曲线如下我这里只描述趋势实际代码运行后可视化即可EKF 在第 10 步之前高度误差从 200 米快速收敛到 30 米以内速度误差也能压到 20 m/s 左右但是 β 的收敛速度明显慢大概要到 40 步以后才接近真值UKF 在同样的初值和噪声下高度误差收敛速度相当但 β 收敛要快 10 到 20 步且过程中超调更小。在阻力较强、也就是 β 比较小的场景下UKF 的优势更明显因为阻力项的非线性更突出EKF 的一阶线性近似误差被放大。4.2 RMSE与NEES的评价方法光看曲线不够需要量化指标。最常用的是均方根误差 RMSE[ RMSE \sqrt{\frac{1}{N} \sum_{k1}^{N} (\hat{x}_k - x_k^{true})^2} ]对高度、速度、β 分别计算。此外还可以算平均绝对误差 MAE以及估计的一致性指标 NEESNormalized Estimation Error Squared[ NEES_k (\hat{x}_k - x_k^{true})^T P_k^{-1} (\hat{x}_k - x_k^{true}) ]NEES 的期望值是状态维数 n如果 NEES 平稳在 n 附近说明协方差 P 给得合理如果 NEES 长期远大于 n说明滤波器过于自信P 偏小如果远小于 n说明滤波器过于保守P 偏大。这套仿真系统里我加了一段统计 NEES 平均值的代码用来辅助调 Q 和 R。在实际项目中我习惯先看 RMSE 确认精度达不达标再看 NEES 确认滤波器的协方差不是自欺欺人。两者结合才能判断一个滤波器“是真好”还是“只是样子好看”。4.3 不同阻力/噪声场景下的表现当 β 真值从 5000 降到 1000也就是目标阻力更强时EKF 因为线性化误差增大高度误差会从 30 米恶化到 60 米以上速度误差也会出现明显偏置。UKF 的误差只略微增加到 35 到 40 米。这个结果符合理论预期因为非线性越强一阶展开的误差越大。观测噪声从 30 米增加到 100 米时两种滤波器的误差都会变大但 UKF 的误差增加相对平缓。不过如果 Q 与 R 的比例不匹配UKF 也可能发散因为 sigma 点传播会放大协方差一旦 Q 过大预测协方差就巨大增益趋近于观测导致结果抖动。我还测试过一种特殊情况观测不测速度只测高度。这时 β 的可观测性很差因为 β 主要影响速度变化而速度变化只能通过高度的二阶导间接观测。EKF 在此条件下 β 估计往往漂移UKF 由于采样点蕴含了更多非线性信息β 漂移速度要慢很多但退一步说如果观测更新率太低两者都救不了 β。5. 常见问题与避坑指南5.1 滤波发散最让人头疼的故障弹道目标滤波最容易遇到的就是发散特征就是估计值突然跳到一个离谱的范围比如速度变成负几千β 直接爆掉。原因通常有三类模型太理想、初值太差、Q/R 设置不当。模型太理想指过程噪声 Q 给得太小滤波器过于信任模型一遇到模型误差就开始“固执”最终偏离。初值太差指初始状态和真值差得太远如果非线性很强EKF 的一阶展开会导致滤波器陷入错误方向UKF 虽然宽容度高一些但也不是万能。Q/R 比例不当最隐蔽比如 R 给得比实际噪声小很多滤波器拼命跟踪测量噪声结果估计值剧烈震荡R 给大了滤波器就只信模型实测误差无法修正。我调试时有个笨办法先用较大的 Q 让滤波器“活着”然后再慢慢调小 Q 直到 RMSE 不再明显变好。对 β 的过程噪声尤其要小心beta 的 Q 如果大于 1e-2滤波结果很容易病态。另外每一步更新后加状态约束是个好习惯比如beta max(beta, 100)避免负值带来的数值爆炸。5.2 弹道系数不可观怎么办观测只有高度时β 的可观测性很弱因为 β 对高度的作用要通过速度变化再积分路径很长。一个实际技巧是滤波前期把 β 的过程噪声调大一些让它有空间“漂移”到合适值等估计稳定后再把 Q_beta 减小。另一个技巧是观测模型中不要只测高度尽量加入径向速度或多普勒速度测量这会大幅增强 β 的可观测性。如果你的工程条件只允许测高度那对 β 的估计结果要有心理预期误差可能停在 20% 以上不是滤波器的问题是信息量不够。还有个细节初始 β 的符号和数值一定要合理。很多人初始化时直接把 β 设成 1数值上阻力加速度会爆炸式变化滤波器直接发散。建议根据目标类型预估合理范围比如炮弹类 β 在 1000 到 10000 kg/m²然后取中间数初始化。如果完全没有先验就设大一点比如 5000同时把 P0 的 β 分量设大一些让滤波器自己修正。5.3 单位、量纲、数值稳定性的坑高度以米为单位是上万量级速度是几百到上千量级而 β 是几千量级。三个量放一起时协方差矩阵的数值差距可能达到 10^6此时 Cholesky 分解很容易出现数值问题。我踩过最深的一个坑是 UKF 的 sigma 点生成时chol((nlambda)*P)经常报“矩阵不是正定”的错误原因就是 P 的条件数太大。解决方案有两个一是用sqrtm代替chol虽然慢一点但更稳二是在 P 对角线加上一个极小值比如P P 1e-9 * eye(3)大大提高稳定性。EKF 的雅可比矩阵同样要关注量级数值差分步长要随状态量级调整否则误差项会被舍入误差吃掉。另一个容易出问题的点是积分步长。如果滤波更新时间是 1 秒但运动方程变化很快比如速度很大阻力很强一步欧拉积分误差会很大。我建议把 RK4 子步数 M 设成 10在仿真步长 dt1 秒时子步长 0.1 秒精度足够。如果你把 M 设成 1真值和滤波使用同样的积分误差被掩盖对比效果就不真实了。5.4 工程化的小建议把这套仿真系统往工程方向迁移时有几个建议第一滤波循环不要写成面向对象的厚重框架先用脚本跑通再封装函数。我第一版直接写了两个大类 EKF 和 UKF结果调参数时来回改反而不如几个独立函数清晰。第二在代码里加断言assert比如观测 z 必须非 NaN协方差对角线必须非负beta 必须在合理范围内。这些断言在调试时能立刻定位问题而不是等曲线全乱才回头找。第三输出结果时把真实状态、估计状态、协方差对角线、RMSE、NEES 全部存到结构体里绘图和结果分析就方便了。第四如果要写论文或报告建议加蒙特卡洛次数单次仿真随机性太大至少跑 50 次取平均 RMSE 才有说服力。我这边测试时发现单次仿真两种滤波器谁好谁坏偶尔会颠倒但 50 次平均后 UKF 在强非线性场景的优势就稳定出现了。关于 Matlab 版本我用的是 R2023a数字差分和 Cholesky 函数都没问题。如果遇到较老的 2016 版之前\求解和chol的接口略有差异需要微调。代码本身不依赖任何工具箱纯基础函数就能跑所以移植到 Octave 也应该兼容只是绘图函数稍有不同。最后再分享一个实用的小技巧在调参时把 R 固定成传感器误差方差然后只调 Q 的缩放系数写成一个循环批量跑自动绘制不同 Q 下的 RMSE 折线图。这个方法特别适合找出接近最优的 Q 范围比一个个试效率高得多。我自己调这套仿真时就用这个批量扫描法很快就锁定了 Q 的合适区间省下大量重复操作时间。跑完这套 EKF 和 UKF 对比我个人最深的体会是滤波算法本身只是工具箱真正决定成败的是你对目标模型的理解和参数整定的功夫。EKF 简单但非线性强时心有余而力不足UKF 稳健但也没法凭空变出信息量。好在这套含空气阻力的弹道目标模型足够经典不论是入门学习还是工程参考把 EKF 和 UKF 在这里摸透了换到别的非线性跟踪问题里也能触类旁通。