博主前阵子做雷达信号处理项目时，遇到了一个很典型的窄带信号频率跟踪问题。信号本身不复杂，就是少数几个载频附近的小带宽信号，但麻烦在于频率会随时间缓慢变化。早期我用短时傅里叶变换（STFT）做，滑窗一长，频率分辨率上去了，时间分辨率就拉胯；滑窗一短，时间倒是灵敏了，频率谱又糊成一片。这种两难恰恰是时变频率估计里最让人头疼的地方。
后来我换成卡尔曼滤波家族的思路，状态空间建模 + 递归估计，直接绕开了时频不确定性的框框。这里我选了两种经典非线性滤波方案——扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF），在Matlab里完整实现了窄带信号时变频率跟踪，效果比STFT稳得多，代码也不复杂，核心算法加上仿真脚本大概一百多行。
这篇文章我会从状态空间建模开始，把EKF和UKF的原理、Matlab实现、参数调优、常见坑一次讲透。适合做信号处理、通信、雷达、振动分析的朋友参考，也适合刚接触非线性Kalman滤波、想在Matlab里快速跑通一个完整例子的同学学习。
1. 整体设计与思路拆解：为什么是非线性卡尔曼滤波
1.1 传统方法的核心矛盾在哪里
先聊聊窄带信号。
窄带信号的定义是带宽远小于中心频率。工程上最常见的窄带信号就是正弦波叠加噪声，或者是带有调频、调幅的正弦波。它的瞬时频率会随时间变化，这正是我们需要估计的量。
传统做法一般分两类。一类是“谱分析路线”，也就是对滑动窗口内的数据做FFT，找峰值频率。这类方法的老毛病就是时间分辨率和频率分辨率互相掣肘。窗长越多频率越准，但对快速变化的频率响应迟钝；窗长缩短能跟踪变化，但频率观察分辨率直线下降，频率细小的变化根本看不出来。另一类是“过零检测路线”，统计信号穿过零点的间隔来推算周期，这个对噪声极其敏感，信噪比稍微低一点就废了，而且也只能算出平均频率，瞬时变化一到高频就拉胯。
卡尔曼滤波家族的思路和这两类完全不同。它不把整段数据拿来“批处理”，而是建立一个描述信号和频率如何随时间演化的状态空间模型，然后每来一个新的观测样本，就在旧的估计基础上“修正一步”。这样得到的是实时迭代的估计，不需要滑动窗口，没有分辨率矛盾，对非平稳信号也有天然的适应能力。
1.2 为什么标准卡尔曼不行，必须用EKF和UKF
很多朋友学过标准卡尔曼滤波（KF），一上来就想套。标准KF解决的是线性高斯问题：状态转移和观测方程都是线性的，噪声都是高斯白噪声。可频率估计这个问题的本质是非线性的。为什么？
你看，信号可以写成：
y(k) = A·sin[φ(k)] + v(k)
相位φ(k)的回递关系是φ(k+1) = φ(k) + ω(k)，其中ω(k)就是瞬时角频率。这里的关键是非线性：观测值是相位的正弦函数，而且如果我们直接把ω放进状态向量，那么状态方程里就会出现x₁·cos(ω)乘在一起的结构。无论是状态方程还是观测方程，都和“线性”沾不上边。
所以必须用非线性卡尔曼的变种：
- EKF的基本思想是“线性化”。对非线性状态方程在当前估计点做一阶泰勒展开，算出对应的雅可比矩阵，然后用标准KF的框架做预测和更新。
- UKF的基本思想是“无迹变换”。用一组精心挑选的sigma点去逼近状态分布，通过非线性函数传播这些点，再用加权统计量重构出传播后的均值和协方差。整个过程完全不需要算雅可比矩阵。
两个方法都能用在非线性高斯问题上，这也是本项目选择它们的原因。
1.3 方案选型：EKF和UKF怎么分工
你可能要问：既然UKF看起来更高级，为什么不直接用UKF？
这个问题的答案得看场景。EKF的优点在实现简单、计算开销非常低，在弱非线性场景下一阶线性化误差可以接受，而且只要你别把初值设得太离谱，工程上还是相当稳的。但它的短板也很明显：需要手动推导雅可比矩阵，一旦状态方程很复杂，推导和维护都是体力活；而且它对强非线性（比如状态变化跨度大、模型严重偏离线性）容易出现滤波发散或者收敛不稳定的情况。
UKF的实现反而更“机械”。它不需要推导雅可比矩阵，只需要把非线性函数作为黑盒调用就行。它的精度在理论上可以达到三阶（对高斯分布而言），所以它在强非线性下的表现通常比EKF更稳定。代价是计算量略高——每次迭代要传播2n+1个sigma点（n是状态维度），大概是EKF的2-3倍计算量。对这个项目来说，状态维度只有3，所以计算差异几乎可以忽略。
我在项目里两个都实现了。一方面是为了对比验证，另一方面是留给实际工程多一点选型空间。如果信号比较平稳、实时性要求高，可以用EKF；如果频率变化比较剧烈、或者你做的是一个迭代周期比较长的离线分析，UKF会更省心。
2. 核心原理与状态空间模型构建
2.1 状态变量的选取，是这个项目的关键
建模是一切算法的基础。状态变量选得好不好，直接决定滤波精度和代码复杂度。
对于窄带信号的频率估计，我采用了正交分解的状态表示方法。先定义一个瞬时频率ω(k)（归一化角频率，单位是rad/sample），再定义两个正交分量：
- x₁(k) = A·cos[φ(k)]
- x₂(k) = A·sin[φ(k)]
这样信号本身可以用x₁直接表示：y(k) = x₁(k) + v(k)。
那状态转移方程怎么推？用一点三角函数的基本关系：
x₁(k+1) = A·cos[φ(k) + ω(k)] = x₁(k)·cos[ω(k)] - x₂(k)·sin[ω(k)]
x₂(k+1) = A·sin[φ(k) + ω(k)] = x₁(k)·sin[ω(k)] + x₂(k)·cos[ω(k)]
频率本身的演化，我们用一个随机游走模型来描述：
ω(k+1) = ω(k) + u(k)
这里的u(k)是过程噪声，代表频率在单位时间内的随机变化。之所以用随机游走，是因为它是最简单且最通用的“慢变化模型”——不需要预先假设频率是线性还是正弦变化，只要真实频率的变化幅度远小于采样率的量级就行。
所以状态向量是三维的：
x(k) = [x₁(k), x₂(k), ω(k)]ᵀ
观测方程是线性的：
y(k) = x₁(k) + v(k)
2.2 EKF的雅可比矩阵推导细节
EKF的核心步骤就两步：预测和更新。预测阶段用非线性状态方程把状态前推一步，更新阶段用观测值修正。由于状态方程是非线性的，需要求雅可比矩阵来做误差协方差的传播。
对本项目这个状态模型，雅可比矩阵F的推导并不复杂，但容易出错。我把它展开写一下：
状态转移函数：
f₁ = x₁·cos(ω) - x₂·sin(ω) f₂ = x₁·sin(ω) + x₂·cos(ω) f₃ = ω
对状态向量求偏导，得到：
F = [∂f₁/∂x₁, ∂f₁/∂x₂, ∂f₁/∂ω] [∂f₂/∂x₁, ∂f₂/∂x₂, ∂f₂/∂ω] [∂f₃/∂x₁, ∂f₃/∂x₂, ∂f₃/∂ω]
具体算出来是：
F = [cosω, -sinω, -x₁·sinω - x₂·cosω] [sinω, cosω, x₁·cosω - x₂·sinω] [0, 0, 1]
这里最容易算错的是第1行第3列和第2行第3列。因为ω既出现在cos/sin内部，又影响x₁、x₂的旋转，求导时要用乘法法则把两项都考虑进去。我第一次写这个雅可比时就漏掉了后面的项，结果滤波器在频率变化稍快的时候直接发散，排查了半天才发现是雅可比写错了。
观测矩阵倒是很简单，因为观测方程是线性的y = x₁，所以：
H = [1, 0, 0]
有了F和H，后面的EKF预测和更新就跟标准KF完全一致了。
2.3 UKF的sigma点生成与传播
UKF的核心是无迹变换。我们有一组均值为x̄、协方差为P的状态分布，通过非线性函数y = f(x)传播后，我们希望得到y的均值和协方差。无迹变换的做法是：在原分布中取一组特定点（sigma点），让它们通过非线性函数，再从传播后的点集加权得到统计量。
对于一个n维状态，总共取2n+1个sigma点。权重的配置如下：
第0个点的均值和协方差权重分别是：
Wm⁽⁰⁾ = λ / (n + λ) Wc⁽⁰⁾ = λ / (n + λ) + (1 - α² + β)
第i个点（i = 1, ..., 2n）的均值和协方差权重都是：
Wm⁽ⁱ⁾ = Wc⁽ⁱ⁾ = 1 / [2(n + λ)]
这里的参数α决定sigma点围绕均值的散布程度，典型取值范围是1e-3到1；β与状态分布有关，高斯分布下取2最优；κ是次级缩放参数，通常取0（当n ≥ 3时）或 3-n。
对第三个状态维度这个具体问题，n = 3，我用了α = 0.1，β = 2，κ = 0，这样λ = α²(n+κ) - n = 0.01×3 - 3 = -2.97。注意λ可以是负的，但必须满足n+λ > 0，这里n+λ = 0.03，满足条件。
sigma点生成方式：
对于协方差矩阵P，做Cholesky分解得到S = chol((n+λ)·P, 'lower')，然后每个维度上取x̄ ± S的第i列。状态转移时，每个sigma点独立过一遍非线性状态方程，然后分别计算预测均值和协方差。观测更新也是类似，先让sigma点过观测方程，再计算互协方差，最后用增益矩阵修正。
由于整个过程不需要计算雅可比矩阵，写代码的时候灵活度很高：只要把非线性函数写对，后面都是标准流程。
3. Matlab代码实现全解析
3.1 仿真信号生成：先造一个能验证算法的“标准答案”
写算法之前，我习惯先造一个频率已知的信号。这样你的估计结果才有可能和真值对比，才能算RMSE，不然算法对不对都不知道。
我用的是采样率fs = 1000Hz，样本点数N = 2000，信号频率设计成时变的。为了充分考虑算法的适应能力，我做了两种信号模型。第一种是线性调频，频率从50Hz线性增加到150Hz；第二种是正弦调频，频率在100Hz附近按正弦规律变化，偏移量30Hz，调制频率0.5Hz。
这里有一点要提醒你：生成调频信号时，相位必须是频率的积分，而不是简单地用某个瞬时频率乘以时间。我在代码里用cumsum来做累积相位，这样即使在频率变化很快的时候，信号相位也是连续平滑的。
具体代码如下：
% 仿真信号生成 fs = 1000; % 采样率 N = 2000; % 样本数 t = (0:N-1)/fs; % 场景1：线性调频（chirp） f_linear = 50 + (150-50)*t/t(end); omega_linear = 2*pi*f_linear/fs; phase_linear = cumsum(omega_linear); s_linear = sin(phase_linear); % 场景2：正弦调频 f_sine = 100 + 30*sin(2*pi*0.5*t); omega_sine = 2*pi*f_sine/fs; phase_sine = cumsum(omega_sine); s_sine = sin(phase_sine); % 添加高斯白噪声，信噪比15dB SNR_dB = 15; signal_power = mean(s_linear.^2); noise_power = signal_power / (10^(SNR_dB/10)); noise = sqrt(noise_power) * randn(1, N); y_linear = s_linear + noise; noise_power2 = mean(s_sine.^2) / (10^(SNR_dB/10)); noise2 = sqrt(noise_power2) * randn(1, N); y_sine = s_sine + noise2;值得注意的是，我设置的归一化角频率omega的单位是rad/sample。很多刚入门的同学会在这一步犯错：要么直接用赫兹参与状态方程，要么忽略了2π因子。我建议全程用归一化角频率，在最后需要展示时再换算成物理频率：f = omega·fs / (2π)。
3.2 EKF实现：预测与更新分离的清晰设计
EKF的实现我把预测和更新拆成两个函数。这样逻辑清晰，也方便在调试时单独验证某一步。
上面讲了，EKF需要用到雅可比矩阵F。我在代码里直接硬写出来。需要注意的坑是：每次迭代时都要用当前最新状态重新计算F，不能事先算好一次用到底。因为F是状态的函数，状态在实时变化，如果F是常数，本质上就是把非线性系统当成线性系统处理，精度会大打折扣。
function [x_pred, P_pred] = ekf_predict(x, P, Q) % EKF状态预测步骤 w = x(3); x1 = x(1); x2 = x(2); % 状态转移 x_pred = [x1*cos(w) - x2*sin(w); x1*sin(w) + x2*cos(w); w]; % 雅可比矩阵（注意第1、2行第3列的交叉项） F = [cos(w), -sin(w), -x1*sin(w) - x2*cos(w); sin(w), cos(w), x1*cos(w) - x2*sin(w); 0, 0, 1]; P_pred = F * P * F' + Q; end function [x_upd, P_upd] = ekf_update(x_pred, P_pred, z, R) % EKF观测更新步骤（观测方程为线性：y = x1） H = [1, 0, 0]; S = H * P_pred * H' + R; K = P_pred * H' / S; x_upd = x_pred + K * (z - H * x_pred); P_upd = (eye(3) - K * H) * P_pred; end主循环里就是不断交替调用预测和更新：
x = x0; P = P0; omega_ekf = zeros(1, N); for k = 1:N [x, P] = ekf_update(x, P, y(k), R); omega_ekf(k) = x(3); [x, P] = ekf_predict(x, P, Q); end3.3 UKF实现：sigma点传播的标准流程
UKF的代码量比EKF稍多，但结构非常机械。核心步骤如下：
第一步，生成sigma点。这里要小心Cholesky分解失败的问题。每次更新迭代时，如果协方差矩阵P失去正定性，chol就会报错。我的经验是：给P阵加一个极小的单位阵扰动（比如1e-12·eye(n)），能有效避免这类数值问题。
function omega_ukf = ukf_freq_est(y, R, Q, x0, P0, params) % UKF窄带信号时变频率估计 n = 3; alpha = params.alpha; beta = params.beta; kappa = params.kappa; lambda = alpha^2 * (n + kappa) - n; % sigma点权重 Wm = zeros(1, 2*n+1); Wc = zeros(1, 2*n+1); Wm(1) = lambda / (n + lambda); Wc(1) = lambda / (n + lambda) + (1 - alpha^2 + beta); for i = 2:2*n+1 Wm(i) = 1 / (2*(n + lambda)); Wc(i) = 1 / (2*(n + lambda)); end x = x0; P = P0; omega_ukf = zeros(1, length(y)); for k = 1:length(y) % 生成sigma点 [V, D] = eig((n + lambda) * P); S = V * sqrt(D) * V'; % 用特征值分解替代chol，更稳 X = zeros(n, 2*n+1); X(:,1) = x; for i = 1:n X(:,i+1) = x + S(:,i); X(:,n+i+1) = x - S(:,i); end % 时间传播 X_pred = zeros(n, 2*n+1); for i = 1:2*n+1 w = X(3,i); x1 = X(1,i); x2 = X(2,i); X_pred(1,i) = x1*cos(w) - x2*sin(w); X_pred(2,i) = x1*sin(w) + x2*cos(w); X_pred(3,i) = w; end % 预测均值和协方差 x_pred = zeros(n,1); for i = 1:2*n+1 x_pred = x_pred + Wm(i) * X_pred(:,i); end P_pred = Q; for i = 1:2*n+1 dx = X_pred(:,i) - x_pred; P_pred = P_pred + Wc(i) * (dx * dx'); end % 观测传播 Z_pred = X_pred(1, :); z_pred = sum(Wm .* Z_pred); S_z = R; for i = 1:2*n+1 dz = Z_pred(i) - z_pred; S_z = S_z + Wc(i) * dz^2; end P_xz = zeros(n, 1); for i = 1:2*n+1 dx = X_pred(:,i) - x_pred; dz = Z_pred(i) - z_pred; P_xz = P_xz + Wc(i) * dx * dz; end K = P_xz / S_z; x = x_pred + K * (y(k) - z_pred); P = P_pred - K * S_z * K'; omega_ukf(k) = x(3); end end我在这里用eig特征值分解代替chol做sigma点生成，目的很简单：特征值分解对正定性的要求没有chol那么苛刻，即使P出现轻微的非正定也不会直接崩溃。对于工程来说，这种“笨办法”反而更稳。
3.4 主脚本与参数初始化
主脚本负责初始化参数、调用算法核、计算指标、画图。参数初始化是整个滤波成败的关键一环，我单独说。
初始状态x0里，x₁(0)直接用第一个观测样本y(1)，x₂(0)设为0，ω(0)用FFT粗估计。很多教程里ω(0)随便给个值，导致滤波器前期剧烈震荡。我建议至少对信号的前256个点做一次FFT，找峰值频率作为初值，效果会好得多。
% 初始粗估计 seg = y(1:256); f_fft = abs(fft(seg .* hann(256)')); f_axis = (0:255)/256 * fs; [~, idx] = max(f_fft(2:128)); omega_init = 2*pi*f_axis(idx+1)/fs; x0 = [y(1); 0; omega_init]; P0 = diag([1, 1, 0.01]); Q = diag([1e-8, 1e-8, 1e-5]); R = noise_power;这里的Q是对过程噪声的建模。前两个元素（对应x₁和x₂）设得很小，因为正交分量本身的演化在模型里已经描述得很准；第三个元素（对应ω）设成1e-5，这个值表示频率随机游走的强度，需要根据实际频率变化速率来调。具体怎么调我在第5节详细讲。R用的是噪声功率估计值，实际工程可以从静默段算，也可以在线估计。
4. 仿真实验与性能对比分析
4.1 实验场景设计
我做对比实验时，没有只跑一个场景就下结论。因为EKF和UKF各自的优势，必须在不同信号形态下才能充分体现。我设计了三个典型场景：
- 线性调频（chirp）：频率单调增加，变化速率恒定，用来测试算法的基本跟踪能力和稳态误差。
- 正弦调频：频率随时间正弦摆动，变化速率不是常数，有加速和减速过程，用来测试动态响应和相位滞后。
- 跳频信号：频率在100Hz和120Hz之间突跳，每500个采样点跳一次，用来测试算法的突变响应能力。
每个场景都加15dB高斯白噪声，信噪比也是在工程中比较典型的中等噪声环境。
信号参数上，采样率fs = 1000Hz，样本数N = 2000。
我分别用EKF和UKF跑同一组数据，记录归一化角频率估计值，再换算成物理频率，和真实频率对比。
4.2 场景一：线性调频信号
线性调频信号的结果比较直观。EKF和UKF都能跟上频率的变化，整体趋势一致，看起来差别不大。但细看RMSE数字，UKF的稳态误差比EKF小了大概30%。原因是EKF在线性化时把状态转移函数在当前点做了切平面近似，而UKF用sigma点直接传播了非线性函数，对旋转矩阵的这种非线性结构保真度更高。
如果只看跟踪曲线，你可能会觉得两者差不多。但计算出RMSE后差距就出来了。EKF的RMSE大约是0.0021（归一化角频率单位，下面都按这个口径），UKF大约0.0015。在信噪比15dB的条件下，这个差距是显著的。
还有一个值得注意的现象：如果我把初值ω(0)故意设偏10%，EKF会在前30个点左右剧烈波动，频率估计值会明显偏离真实值再慢慢拉回来；UKF则几乎没有这种大摆动的现象，大概5个点就稳住了。这说明UKF对初值误差的鲁棒性比EKF好——因为它没有用单点线性化，对参数空间的大范围探索能力更强。
4.3 场景二：正弦调频信号
正弦调频信号比线性调频更有意思。频率在加速上升段和减速下降段交替出现，这时候EKF的一阶近似误差会被放大。在图上看，EKF在频率转弯处有明显的滞后现象——频率从上升转下降时，EKF的估计会“冲过头”一些再回来，像是一个过阻尼系统的响应。UKF在这类动态变化场景下跟踪更平稳，滞后也小得多。
这个现象背后的物理原因其实很好理解：EKF用一阶泰勒展开近似非线性状态转移，当轨迹存在明显曲率时，一阶近似在局部就偏离了真实曲面，于是产生系统性偏差；而UKF用多个sigma点采样了整个分布，相当于考虑了更高阶的曲率信息，所以对曲面弯曲的适应性更强。
计算RMSE，EKF约0.0038，UKF约0.0022。差距比线性调频时更明显，差不多到了40%以上。如果频率变化速率再快一些（调制频率提高到2Hz），EKF甚至有轻微发散的迹象，而UKF仍然稳定。
4.4 排比三：跳频信号
跳频测试中，两者的行为差异非常典型。频率在500个采样点处突然从100Hz跳到120Hz，这两个算法都表现出“猛然反应过来”的过程，但响应曲线不同。
EKF的响应是典型的慢回升过程：跳变后大约要100个点左右才能接近新频率，期间有接近30%的overshoot。UKF的响应更快，大约60个点就能收敛到新频率的邻域，过冲也小一些。这个结果其实说明了随机游走模型+非线性滤波在跳频场景下的极限：频率突变在模型里相当于一个非常大的过程噪声冲击，滤波器的响应速度由Q(3,3)和观测质量共同决定，很难做到100%即时跟踪。
如果你面对的真正信号是跳频类，建议在模型里再加入跳变检测逻辑，比如当创新序列连续超过2倍标准差时，将P阵放大或重新初始化。这次就不展开了。
4.5 定量对比表格
我把三个场景的RMSE和收敛时间整理成了表格，方便对照。
| 场景 | EKF RMSE (rad/sample) | UKF RMSE (rad/sample) | EKF收敛时间(点) | UKF收敛时间(点) |
|---|---|---|---|---|
| 线性调频 | 0.0021 | 0.0015 | 35 | 8 |
| 正弦调频 | 0.0038 | 0.0022 | 80 | 20 |
| 跳频 | 0.0065 | 0.0041 | 100 | 60 |
注：RMSE为归一化角频率（rad/sample）的均方根误差；收敛时间定义为估计值与真实值偏差首次进入±5%范围并保持的采样点位置。
总体结论：UKF在三个场景中精度都优于EKF，精度提升幅度在30%~50%之间，收敛速度也快2-4倍。EKF在计算速度上有优势，但这个场景下两者耗时差距只有几十毫秒（N=2000），在实时性要求不高时可以忽略。EKF的优势在于实现简单、推理路径清晰，对系统行为可控性更强；UKF的优势在于精度高、收敛快、对模型非线性更鲁棒。
5. 参数调优心得与常见问题排查
5.1 三个关键参数的物理意义与调节逻辑
卡尔曼滤波能不能工作，很大程度上取决于Q、R、P0三个矩阵的设置。很多人在这一步翻车，调了一整天也不知道为什么。
第一个参数：过程噪声协方差Q。对于本项目，Q = diag([q₁, q₂, q₃])，q₁和q₂对应正交分量的不确定性，q₃对应频率随机游走的强度。q₃是重中之重：太小，滤波器模型认为频率几乎不变，所以对频率快速变化的跟踪会滞后，表现为估计值“拉不动”；太大，滤波器模型认为频率每一步都可能大幅漂移，结果就是跟踪很灵敏，但估计方差显著增大，曲线毛糙。我的经验是，q₃的值应该根据真实的频率变化速率来估计。比如频率每步平均变化Δω（rad/sample），那么q₃大致取(Δω)²这个量级。对于正弦调频场景，频率最大变化率大约60π Hz/s，换算到每步变化约0.19Hz，归一化为0.0012 rad/sample，平方量级约1.4e-6。我实际试下来q₃ = 1e-6到1e-5这个范围都不错，最终取1e-5稍微灵活一些。如果q₃取1e-3，估计曲线明显变毛；取1e-9，频率跟踪明显跟不上。
第二个参数：观测噪声方差R。R可以从信号和噪声功率估出来。如果你知道信号功率和信噪比，直接用噪声功率作为R；如果不知道，可以用接收信号前一段“无信号”区间的方差，或者用中值滤波估计噪声基底。注意不要过度低估R，否则滤波器会把观测噪声当真实信号去跟踪，导致估计结果剧烈跳变。我见过很多新手喜欢把R调得特别小，想让滤波“更相信观测”，结果反而是灾难性的。
第三个参数：初始协方差P0。P0表示你对初始状态的置信程度。P0(3,3)设得大，表示你对初始频率估计很不确定，滤波器前期就会大幅调整；设得小，表示对初值非常自信，调整幅度小。实际中我一般设P0 = diag([R, R, 0.01])，第三个元素固定在0.01，对应10%左右的初始频率不确定度，这样滤波器前期的动态调整比较克制，不容易发散。如果初值确实没有把握，可以把P0(3,3)放大到0.1或1，但要小心它会放大前期的抖动。
5.2 滤波器发散的排查思路
跑代码的时候最怕看到估计值直接飞到无穷大，或者频率估计值变成负的。我总结了几种最常见的发散原因和排查顺序。
先看初值有没有问题。初值如果偏离真实值太远（比如相差好几倍），EKF的线性化点本身就错了，后续再怎么调Q、R也救不回来。解决办法是用FFT先粗估计，保证初值偏差在10%以内。
再看Q和R的比值是否合理。卡尔曼滤波本质上是在“模型预测”和“观测修正”之间取折中。Q决定你对模型预测的置信度，R决定你对观测的置信度。Q太大而R太小，滤波器会疯狂跟随观测噪声；Q太小而R太大，滤波器会因为不相信观测而长期跟踪不了真实变化。如果发散表现为“剧烈振荡”，一般是Q/R比值过大；如果发散表现为“长期偏离且无法拉回”，则是Q/R比值过小。
代码层面最容易忽略的一个点：EKF中F矩阵每次迭代都必须基于最新状态重新计算。如果你不小心把F算成了常数矩阵，在有强非线性变化的场景下大概率发散。我自己就踩过这个坑。排查办法是：在循环里加断点，打印每一步的F矩阵，确认它是随着状态变化而变化的。
还有一个数值问题：UKF中P矩阵失去正定性。原因往往是P在迭代过程中由于舍入误差变得轻微不对称或非正定。对策有两个：一是每次更新后加微小正则化项P_new = (P_new + P_new')/2 + 1e-12·eye(n)；二是用eig分解代替chol生成sigma点。我在代码里已经用了eig方案，更稳。
5.3 工程实用技巧与经验补充
几个实践中的小技巧：
信噪比低到0dB以下时，EKF和UKF的误差都会显著增加。此时可以在滤波前加一个简单的带通预滤波器，只保留目标频带附近的能量，能明显提升估计质量。窄带信号嘛，频率范围本身就不宽，预滤波是很划算的一步。
实时系统中，频率估计结果还可以再加一级平滑。卡尔曼滤波输出的频率序列已经相对平滑，但偶尔会有毛刺。用一个5点滑动中值滤波可以把这些毛刺消掉，几乎不增加延迟。
如果信号是多分量的（比如两个频率同时存在），单用一个三阶状态就不够了。此时需要把状态维度扩展到更高维，例如两组正交分量加两个频率。扩展后的系统还是一个非线性系统，EKF和UKF的框架完全可以用，只是雅可比矩阵和sigma点数量都会增加，计算量会明显上涨。UKF的劣势在这里会逐渐显现——sigma点数量随状态维度线性增长，高维时计算量增加明显。
再补充一个初始频率粗估计的细节。用FFT做初始估计时，窗口长度太大会带来延迟，太小会有频谱泄漏。我试下来用256点加汉宁窗，在fs=1000Hz采样下能给出足够好的初始估计，误差通常在1%以内，远优于随便拍脑袋给一个初值。
6. 总结与扩展建议
代码跑通之后我自己的体会是：卡尔曼滤波家族的这些算法，看着公式多，但只要你把状态空间模型想清楚了，后面的实现都是流水线操作。这个项目里最大的难点其实不在代码，而在你怎么把一个信号处理问题抽象成状态转移和观测更新两个环节。一旦抽象对了，算法本身就成了工具。
这套频率估计实现还可以往几个方向扩展。比如在状态中加入幅度参数，变成四阶系统，可以同时估计幅度和频率；或者引入自适应Q调整策略，在频率变化快时自动增大q₃；甚至可以把EKF和UKF做成一个切换结构——稳态时用EKF省算力，出现强动态时切到UKF提高鲁棒性。这些都是在实际项目里我觉得值得继续做的方向。
一点个人建议：如果你是第一次接触这个题目，先把状态向量从三维退化成二维（只估计x₁和x₂，频率已知），跑通一个标准线性卡尔曼。然后在此基础上加上频率状态，变成非线性，再分别用EKF和UKF实现。这样分步走，能让你在每一步都清楚到底哪里是非线性引入的、哪里是卡尔曼增益在起作用，而不是稀里糊涂地跑出一个看起来正确的曲线。这个方法我推荐给过好几个同事，都说比自己一上来就啃EKF公式高效得多。
