最近在做窄带信号的瞬时频率估计时我把扩展卡尔曼滤波器EKF和无迹卡尔曼滤波器UKF都拉出来遛了一圈。先说结论用Matlab实现这两条路线代码量差得不多但UKF在强非线性和频率突跳场景下明显更稳EKF胜在计算量小、推导直观。窄带信号的时变频率估计本质上要回答一个问题给定一串带噪声的观测点怎么把每个时刻的瞬时频率实时抽出来这个问题做通信、振动分析、生物电信号处理的人都会撞上这篇就把我从建模到调参、从仿真到踩坑的完整过程展开讲一遍。1. 从谱峰搜索到状态估计窄带时变频率问题的本质1.1 谱峰法为什么不行很多人拿到窄带信号第一反应是滑窗FFT。窗口一开把每个窗内的主峰频率当成当前时刻的频率。这个方法在频率基本不变、信噪比还不错的场合确实能用可一旦频率变化稍快窗长和频率分辨率这对矛盾立刻暴露窗太长一个窗内频率已经“跑”了一段距离估计出来的是平均频率而不是瞬时频率窗太短频率分辨率不够峰值会跳来跳去。等频率真的出现突跳或者窄带干扰时谱峰法基本就崩了。我再举个实际感受。一个从100Hz线性扫到200Hz的窄带信号采样率1000Hz信噪比20dB用256点滑窗FFT做出来的频率曲线滞后非常明显而且峰值在接近200Hz时还会出现“拖尾”。这个场景让我彻底转向了状态空间方法。1.2 卡尔曼滤波家族在频率估计中的定位卡尔曼滤波解决的是“从噪声观测中估计随时间变化的状态”这一类问题。窄带信号的当前相位、瞬时频率、幅度天然就是一组动态状态。只要把信号模型写成状态方程观测方程就能用递推的方式逐点更新频率估计。标准卡尔曼滤波器只适用于线性高斯系统而窄带信号的观测方程里必然带cos函数这是强非线性。于是扩展卡尔曼滤波器EKF和无迹卡尔曼滤波器UKF成了最顺手的两条路。我的理解是EKF做着“把非线性函数一阶泰勒展开”的近似UKF则用一组精心挑选的sigma点去传播概率分布不展开、不求导。后面你会发现正是这个“不求导”的特点让UKF在很多相位类观测模型里避免了一阶近似带来的偏差。2. 建立可滤波的模型状态向量、转移方程与观测方程2.1 状态向量怎么选带通采样后的实窄带信号最常见的离散形式是y_k A cos(θ_k) v_k其中A是幅度θ_k是瞬时相位v_k是观测噪声。瞬时频率是相位的导数在离散时间下f_k (θ_k - θ_{k-1}) / (2π Δt)所以状态向量至少应该包含相位和频率x_k [θ_k; f_k]如果幅度A也随时间漂移那就再把A加进来变成x_k [A_k; θ_k; f_k]。幅度模型一般也写成随机游走相当于我们相信A“缓慢变化但具体规律未知”。实际中A是否进状态要权衡加进去状态维度加一EKF的雅可比矩阵多一列UKF的sigma点数量也跟着涨但换来的是对幅度变化的适应能力。2.2 复解析信号 vs 实信号观测很多文献喜欢用I/Q复解析信号建模把窄带信号写成z_k A_k exp(jθ_k) n_k然后观测拆成实部虚部两个通道。这种方式好处是实部虚部对相位和幅度都有完整观测滤波器更容易收敛。但工程落地时如果你手里只有一路ADC采出来的实信号那就得老老实实用实信号观测方程。我这里选择实信号观测因为更贴近很多实际采集场景。代价是观测方程的非线性更强恰好适合用来对比EKF和UKF。2.3 状态转移方程与观测方程推导状态转移方程用的是随机游走加相位累积θ_k θ_{k-1} 2π f_{k-1} Δt w_θ f_k f_{k-1} w_f这里w_θ和w_f是过程噪声代表模型没把握的部分。θ的转移不是随意的它来自相位的物理定义相位增量等于角频率乘时间步长。f的转移用随机游走表示瞬时频率在每个采样点可以有一点漂移但漂移量被w_f的方差约束。写成矩阵形式就是x_k F x_{k-1} w_k其中F [1, 2πΔt; 0, 1]观测方程是z_k A cos(θ_k) v_k到这里模型建立完毕。这一步是整个工程的地基模型建不好后面EKF和UKF再折腾也白搭。3. 扩展卡尔曼滤波器雅可比矩阵推导与使用中的坑3.1 EKF的整体流程EKF的思想很简单既然观测方程非线性那就把它在当前状态估计值附近做一阶泰勒展开得到线性的近似然后套用标准卡尔曼滤波的更新公式。递推分两步预测步x_pred F x_prev P_pred F P_prev F Q更新步S H P_pred H R K P_pred H / S x_new x_pred K (z_k - h(x_pred)) P_new (I - K H) P_pred这里Q是过程噪声协方差矩阵R是观测噪声方差。真正麻烦的是H也就是观测函数h(x)对状态向量的雅可比矩阵。3.2 雅可比矩阵推导观测函数是h(x) A cos(θ)对状态x [θ; f]求偏导∂h/∂θ -A sin(θ) ∂h/∂f 0所以雅可比矩阵H [-A sin(θ), 0]如果状态里加了幅度A观测变成h A cosθ那么∂h/∂A cosθ ∂h/∂θ -A sinθ ∂h/∂f 0H [cosθ, -A sinθ, 0]这个H在代码里每一拍都要重新计算因为θ每时每刻都在变。很多新手栽跟头的地方就在这里H算错一个符号或者忘记更新滤波器立刻就发散。3.3 一阶线性化在相位跟踪中的隐患EKF在窄带频率估计中的问题不是实现复杂而是“只保留一阶项”这个近似在强非线性下不够用。相位θ的观测是通过cos函数映射的当θ处于±π附近时cos的局部曲率变化非常剧烈一阶泰勒展开的误差会变大。这个偏差反映到滤波结果上就是频率估计出现缓慢的偏置尤其当A有一定误差时更明显。我在实际仿真里见过一种情况真值是线性调频信号EKF在大部分时间能跟住但相位接近π边界时频率估计会出现周期性抖动幅度不大却很烦人。EKF还有个容易被忽视的问题雅可比矩阵里包含 -A sinθ如果A估计偏小等效于滤波器给的观测增益偏低频率更新变慢跟踪滞后变大。所以用EKF做频率跟踪时幅度A要尽量准备准要么把它放进状态向量一起估计要么用其他手段实时估计幅度。4. 无迹卡尔曼滤波器sigma点的艺术与工程实现4.1 无迹变换在做什么UKF的核心是“无迹变换”。它不再把非线性函数做展开而是选择一组确定的样本点sigma点这些点经过非线性函数传播后用它们的统计量来近似变换后的均值和协方差。数学上可以证明对任意非线性函数这种基于sigma点传播的均值和协方差估计至少能达到二阶精度而EKF只有一阶精度。这在实际中的表现就是UKF对相位的强非线性没有那么敏感频率估计更平滑。一个直觉理解EKF是把一条曲线在某一点用切线代替UKF是取曲线上一段区间内的多个点把它们的“平均行为”作为结果。后者自然更接近真实分布。4.2 sigma点生成与权重假设状态向量维度是L均值为x协方差为P。UKF生成2L1个sigma点χ_0 x χ_i x (γ √P)ii1,...,L χ{iL} x - (γ √P)_ii1,...,L这里γ √(Lλ)λ α²(Lκ) - L。α控制sigma点离均值的距离通常取0.01到1之间κ是次级缩放参数高斯分布下常取3-L或0β用于合并先验分布信息高斯分布取2。权重为W_m^0 λ/(Lλ) W_c^0 λ/(Lλ) (1 - α² β) W_m^i W_c^i 1/[2(Lλ)]i1,...,2L需要注意的是W_c^0可能是负值这是正常现象但会给协方差计算带来数值风险。后面代码部分我会讲怎么处理。4.3 参数整定与数值陷阱UKF没有雅可比矩阵避免了求导麻烦但引入了参数整定问题。α、β、κ选不好滤波效果可以明显变差。α太小sigma点离均值太近非线性影响体现不出来效果接近EKFα太大sigma点离均值太远局部线性假设又被破坏。我的经验是α0.01到0.1之间比较稳κ3-L在L≤3时表现正常β2基本是高斯场景默认值。数值上最大的坑是sqrt(P)。Matlab里直接用sqrt(P)对非正定矩阵会报错或者算出NaN。每轮更新后P矩阵因为浮点误差可能轻微不对称或非正定。我习惯在生成sigma点前做一次对称化P_sym (P P) / 2然后下三角Cholesky分解L_chol chol(P_sym, lower)如果chol报错说明协方差矩阵数值病态优先检查过程噪声Q是不是给得太小、状态是否可观测。5. Matlab代码落地一个能跑通的EKF/UKF对比框架5.1 仿真场景设计我用三个典型场景来测线性调频、频率突跳、正弦调频。先说线性调频。生成一个从100Hz线性扫到200Hz的信号采样率1000Hz时长2秒幅度A1观测噪声标准差0.05。Matlab生成代码如下fs 1000; dt 1/fs; N 2000; t (0:N-1)*dt; f0 100; k 50; % Hz/s扫频斜率 phase 2*pi*(f0*t 0.5*k*t.^2); A 1.0; x_true A*cos(phase); sigma_v 0.05; z x_true sigma_v*randn(size(x_true)); true_freq f0 k*t;频率跳变场景则把频率设计成前1秒100Hz后1秒150Hz便于观察滤波器重新收敛速度。5.2 滤波器初始化与参数配置状态向量取x [θ; f]初始状态我故意设偏一点比如θ00.1f080Hz让滤波器自己收敛。初始协方差P0反映对初值的信任程度。频率初值可能偏差很大所以P0(2,2)要给大一些P0 [1e-2, 0; 0, 40^2];过程噪声协方差Q是调参重点。我们的状态模型是随机游走Q越小滤波器越相信模型跟踪越平滑但跟不上突跳Q越大滤波器越依赖观测跟踪越快但噪声越大。线性调频场景下每个采样点频率真实变化约0.05Hz我给q_f一个小值频率突跳场景则不得不调大。q_theta 1e-8; q_f 0.1^2; Q [q_theta, 0; 0, q_f]; R sigma_v^2;这里q_f0.1^2的含义是每个采样点允许频率随机游走的标准差约为0.1Hz。对100Hz量级的载频这个量级是合理的起点。5.3 EKF核心循环x [0.1; 80]; P P0; ekf_freq zeros(1, N); ekf_freq(1) x(2); for k 2:N % 预测 x_pred [x(1) 2*pi*x(2)*dt; x(2)]; F [1, 2*pi*dt; 0, 1]; P_pred F*P*F Q; % 更新 theta_pred x_pred(1); A_est 1.0; z_pred A_est * cos(theta_pred); H [-A_est * sin(theta_pred), 0]; S H * P_pred * H R; K P_pred * H / S; x_new x_pred K * (z(k) - z_pred); P_new (eye(2) - K * H) * P_pred; x x_new; P P_new; ekf_freq(k) x(2); end这段代码去掉了很多工程保护但逻辑主线是清晰的。注意H在每次更新时重新计算这是EKF的关键。如果想跟踪幅度把x扩展成三维H也要加一列cosθ。5.4 UKF核心循环UKF相对长一些我写成几个子步骤。L 2; alpha 0.05; kappa 3 - L; beta 2; lambda alpha^2 * (L kappa) - L; gamma sqrt(L lambda); Wm zeros(1, 2*L1); Wc zeros(1, 2*L1); Wm(1) lambda / (L lambda); Wc(1) lambda / (L lambda) (1 - alpha^2 beta); for i 2:2*L1 Wm(i) 1 / (2*(L lambda)); Wc(i) 1 / (2*(L lambda)); end x [0.1; 80]; P P0; ukf_freq zeros(1, N); ukf_freq(1) x(2); for k 2:N % 生成sigma点 P_sym (P P) / 2; L_chol chol(P_sym, lower); X zeros(L, 2*L1); X(:, 1) x; for i 1:L X(:, i1) x gamma * L_chol(:, i); X(:, iL1) x - gamma * L_chol(:, i); end % 状态转移传播 X_pred zeros(L, 2*L1); for i 1:2*L1 X_pred(1, i) X(1, i) 2*pi*X(2, i)*dt; X_pred(2, i) X(2, i); end x_pred zeros(L, 1); for i 1:2*L1 x_pred x_pred Wm(i) * X_pred(:, i); end P_pred zeros(L, L); for i 1:2*L1 diff_x X_pred(:, i) - x_pred; P_pred P_pred Wc(i) * (diff_x * diff_x); end P_pred P_pred Q; % 观测传播 Z_pred zeros(1, 2*L1); for i 1:2*L1 Z_pred(i) A * cos(X_pred(1, i)); end z_pred 0; for i 1:2*L1 z_pred z_pred Wm(i) * Z_pred(i); end Pzz R; Pxz zeros(L, 1); for i 1:2*L1 diff_z Z_pred(i) - z_pred; Pzz Pzz Wc(i) * (diff_z^2); diff_x X_pred(:, i) - x_pred; Pxz Pxz Wc(i) * (diff_x * diff_z); end K Pxz / Pzz; x_new x_pred K * (z(k) - z_pred); P_new P_pred - K * Pzz * K; x x_new; P P_new; ukf_freq(k) x(2); end注意UKF更新后的协方差我用的是Joseph形式的一种等价写法P_new P_pred - K Pzz K。这个形式能更好地维持对称性。也可以直接用标准形式P_pred - KSK效果类似。5.5 性能评估指标评估频率估计质量我用两个指标。一个是估计频率与真实瞬时频率之间的均方根误差RMSERMSE sqrt(mean((f_est - f_true).^2))另一个是收敛时间尤其在频率突跳场景下定义滤波器从跳变时刻到估计值进入真实值±5%范围内所需的时间。这两个指标加在一起能同时反映“跟踪精度”和“动态响应速度”。单看RMSE不够因为一个滤波器可能很平滑但反应很慢RMSE反而小单看收敛时间也不够因为收敛快可能是以噪声大为代价。6. 实测对比与调参经验我踩过的那些坑6.1 线性调频场景EKF和UKF都能跟但响应速度不同线性调频信号下EKF和UKF都能跟上100Hz到200Hz的扫频跟踪的均值误差都不大。区别在于细节EKF的频率曲线在高频段出现略微滞后UKF的曲线紧贴真实频率但高频噪声稍大。这个结果符合理论预期。EKF的一阶线性化在相位快速变化时引入了近似误差滞后是近似误差的直接体现UKF通过sigma点传播保留了更多非线性信息滞后更小。如果信噪比降到10dB以下EKF的滞后会变得更明显偶尔还会在相位翻折处出现短暂失锁。UKF此时依然能保持跟踪代价是代码运行时间大约增加30%到40%。实时性要求极高的场合这个差距要考虑进去。6.2 频率跳变场景滤波发散与协方差崩溃频率跳变是真正的试金石。我在100Hz突跳到150Hz的场景下EKF和UKF的表现差距非常明显。EKF在q_f0.1^2时跳变后大约需要0.4秒才追到150Hz附近把q_f调到1^2追赶时间缩短到0.2秒但稳频段噪声明显增大。如果q_f继续调大滤波器会变得“神经质”频率估计在100Hz附近就抖得厉害。UKF在相同参数下追赶时间大约比EKF快20%到30%。原因仍然是无迹变换对相位观测的强非线性更鲁棒同样的Q值下等效观测增益更合理。更极端的场景是跳变量超过初始频率偏差很远。我试过从100Hz跳到300HzEKF在某些初值条件下会出现协方差矩阵非正定Matlab直接报错UKF依靠协方差对称化处理没有崩。所以代码里那行P_sym(PP)/2不是可有可无是关键时刻的救火队员。6.3 过程噪声、观测噪声与P0怎么调过程噪声Q是频率跟踪里最敏感的参数。我的经验公式是先估算频率每秒可能变化的最大速率f_dot_max然后每个采样点的频率变化量大约是f_dot_max * dt。q_f取这个变化量的平方再乘以一个0.1到1的系数。举例频率每秒最多变化50Hzdt0.001则每采样点变化0.05Hz。q_f从(0.01)²到(0.05)²之间调试。跳变场景要按跳变量除以期望收敛时间来折算。观测噪声R的估计有个简单办法取一段没有信号或只有噪声的数据直接算方差。如果你用ADC采的是带内信号可以先用带通滤波器把带外噪声滤掉再估计带内噪声方差否则R偏大会让滤波器对观测“信任不足”跟踪滞后。初始协方差P0不要给太小。频率初值有可能偏得非常远P0(2,2)至少给成最大可能误差的平方。我习惯给一个偏大的P0让滤波器前几十个点快速收敛然后观察频率曲线是否收敛到真实值附近。如果P0给得太小滤波器会过度信任错误的初值表现为频率估计长时间停在初值附近甚至不收敛。6.4 对工程应用的几点体会跑完这三组场景我对窄带时变频率估计的工程落地有了几个明确的认识。如果系统是线性调频、频率变化比较平缓EKF完全够用计算量小实现简单调试方便。如果频率可能出现跳变、信噪比偏低或者相位非线性影响明显UKF的优势是实打实的。代价不过多几十行代码和30%左右的计算时间在今天的处理器上通常可以接受。代码层面有几个细节值得强调。第一角度状态θ会持续增长长时间运行可能达到很大数值虽然cos和sin计算不受影响但数值上建议每步对θ做范围折叠x(1) mod(x(1) pi, 2*pi) - pi;第二如果滤波器发散不要急着调Q和R先检查观测方程里的A和真实信号幅度是否一致。A差太多所有后续参数调整都是白费。第三UKF的chol分解报错时先尝试调alpha把alpha从0.05调大到0.1往往能缓解因为sigma点范围宽了协方差数值更稳定。最后分享一个实际项目里很有用的组合拳先用短时FFT粗估计初始频率比如256点FFT找峰值然后用这个值初始化卡尔曼滤波器的频率状态P0(2,2)按FFT峰值频率分辨率的一半平方来设。这样既避免了滤波器从离谱初值缓慢收敛的尴尬又保留了卡尔曼滤波实时跟踪的优势。我拿这个方法处理过实测的旋转机械振动信号效果比单独用任何一种方法都稳。
