简介卡尔曼滤波是信号处理中常用的最优估计算法本资源面向需要去除白噪声、恢复真实信号的研究者或开发者提供一份简洁的MATLAB实现。该卡尔曼滤波示例通过构建系统状态方程与观测方程结合预测和更新两个核心步骤可对受白噪声污染的测量序列进行实时滤波输出平滑的估计信号帮助理解噪声方差对滤波效果的影响。资源包仅含1个.m脚本压缩后大小约1KB代码短小精悍适合快速阅读、修改与运行无需复杂工程配置。目前已有456人学习使用适合正在学习卡尔曼滤波原理、希望在仿真环境中观察滤波效果的初学者或工程人员。通过运行脚本并调整参数可直观对比滤波前后信号加深对状态估计与噪声抑制的理解。1. 卡尔曼滤波去白噪声为什么它比低通滤波更适合你手头的那段信号把“卡尔曼滤波、白噪声去除、信号”这三个词放到一起往往意味着你手头有一段被噪声污染的离散采样序列噪声近似服从零均值高斯分布而你又需要把它还原成相对平滑的真实信号。这类需求在传感器数据处理、通信信号解调前处理、惯性导航对准甚至音频降噪里都会出现。一个容易忽略的结论是当噪声是白噪声且信号本身有一定连续性时卡尔曼滤波的平滑效果通常优于等阶数的一阶低通滤波因为它用统计模型代替了固定截止频率能自适应当前信噪比。这段内容面向的是至少会写一点 Python 或 MATLAB 的工程师你不需要理解完整的控制理论但需要把状态方程、观测方程、噪声方差这三个概念先立住。像cal.rar这种工程包里面核心部分往往就叫kalman restore做的事情就是把“带白噪声的信号”经过递推估计还原为“无白噪声的信号”下面直接从建模开始讲。2. 卡尔曼滤波去白噪声的建模状态方程、观测方程与参数含义2.1 先把一维信号装进状态空间模型卡尔曼滤波不是一种像移动平均那样只对窗口做运算的滤波器它是一个递推的状态估计器。它的起点是把“信号本身的变化规律”和“观测噪声的统计特性”拆成两个方程。假设真实信号序列为 x_k观测到的含噪信号为 z_k则观测方程写作z_k H * x_k v_k其中 H 是观测矩阵对于“直接观测信号本身”的一维场景H 1v_k 是白噪声均值为 0方差为 R。状态方程则描述相邻两个采样点之间信号如何变化最常见的做法是把它建模为一阶随机游走x_k A * x_{k-1} w_k这里 A 1代表相邻两个采样点之间信号没有确定性的趋势变化允许的差异全部由过程噪声 w_k 承担w_k 的方差为 Q。这个 Q 实际上是整个滤波器里最值得调的一个旋钮Q 越大滤波器越相信“信号本身可能变化很快”于是会减少平滑力度Q 越小滤波器越认为“当前值应该和上一刻差不多”于是会把更多噪声滤掉代价是信号突变处会出现滞后。提示不要一上来就套用教科书里的完整卡尔曼公式。先去分辨你的信号是“平稳随机过程”还是“带趋势的缓变过程”。对大多数白噪声去除场景A 1、H 1 就足够了直接写预测与更新两步比套用通用矩阵公式更容易排错。2.2 初始化 P 与增益 K卡尔曼滤波递归里最先影响的三个时刻卡尔曼滤波每次递推包含两步。预测步根据上一时刻的状态估计 x_hat(k-1) 和状态转移矩阵 A推算当前时刻的先验估计 x_pred同时把上一时刻的误差协方差 P 也做一次传递得到先验协方差 P_pred。更新步则用当前观测 z_k 计算卡尔曼增益 K然后对先验估计做修正K P_pred * H^T / (H * P_pred * H^T R) x_hat x_pred K * (z_k - H * x_pred) P (1 - K * H) * P_pred这段公式里最关键的是卡尔曼增益 K。当 R 远大于 P_pred 时K 趋近于 0说明当前观测噪声很大滤波器更相信预测值当 R 远小于 P_pred 时K 趋近于 1说明观测很可靠滤波器会直接跟随观测值。这也就解释了为什么 R 设置得过小会让输出看起来依然“毛刺感十足”滤波器认为每个观测都很可信于是几乎不做平滑。初始化时一般需要给两个量初始状态 x_hat(0) 可以直接取第一个观测值 z_0也可以取 0初始协方差 P(0) 通常取一个相对较大的数比如 1 或等于 R。P(0) 过大只会让刚开始几个点的增益偏大经过若干次递推后会自动收敛到稳态值因此不必花太多时间精调。下表是常用的参数设置依据参数建议初始值物理含义过大或过小的后果Q信号最大允许逐点变化量的平方比如 1e-3过程噪声方差约束信号变化剧烈程度过大导致滤波后噪声残留多过小导致信号细节被平滑掉阶跃响应变慢R按传感器或观测噪声方差标定比如 0.1观测噪声方差单位与信号幅值平方一致过小导致输出毛刺多过大会导致滤波结果严重滞后P(0)取 R 或略大比如 1.0初始估计误差方差只影响最初十多个点会自然收敛2.3 为什么说“白噪声去除”这一场景对模型要求很低白噪声有一个区别于有色噪声的性质它在时间上不相关功率谱是平坦的。这意味着你无法从上一个噪声样本去预测下一个噪声样本任何以“预测修正”为框架的估计算法都会天然吃亏。卡尔曼滤波恰好只要求噪声是零均值高斯分布也就是说只需要给出一个方差 R而不需要关心噪声的具体频谱形状这使得它在处理白噪声去除时比设计一个带通滤波器更省时。但这并不意味着卡尔曼滤波是无条件的。它要求被估计的信号满足“线性高斯”假设。如果你处理的是带有明显非线性特点的信号比如调频信号、载波相位跳变或目标轨迹机动应该考虑使用扩展卡尔曼滤波或无迹卡尔曼滤波而不是继续加大 Q。对于一维缓变信号一套 A 1、H 1 的线性模型已经足够应付绝大多数场合这也是为什么很多复现项目里kalman_restore脚本的代码行数并不多但调参过程却能折腾很久。3. 用 Python 复现卡尔曼滤波去噪从原始信号到干净信号3.1 生成带白噪声的测试信号并验证滤波器行为实践时我一般会先构造一个已知真实信号的数据集这样才能量化去噪效果。比较稳妥的测试信号是叠加了一个阶跃跳变的缓变正弦波既包含平稳段又包含信号突变点可以同时观察稳态平滑能力和动态跟随能力。import numpy as np import matplotlib.pyplot as plt np.random.seed(42) fs 200 # 采样率 200Hz t np.linspace(0, 5, fs * 5) # 真实信号低频正弦 在 2 秒处加入一个阶跃 true_signal np.sin(2 * np.pi * 0.5 * t) true_signal[t 2.5] 2.0 # 观测信号叠加均值为 0标准差为 0.3 的高斯白噪声 noise_std 0.3 z true_signal np.random.normal(0, noise_std, true_signal.shape) plt.figure(figsize(10, 4)) plt.plot(t, z, linewidth0.6, labelnoisy signal) plt.plot(t, true_signal, linewidth1.2, labeltrue signal) plt.legend() plt.show()这段代码里true_signal 是已知真实值z 是加入白噪声后的观测。噪声标准差 0.3 意味着 R 的理论值应设置在 0.09 附近。把真实信号和噪声信号一起画出来目的是在调参时能直接观察滤波输出是在“跟随噪声”还是“跟随信号”而不是仅靠信噪比数字判断。3.2 一维卡尔曼滤波主循环代码def kalman_1d(z, Q1e-3, R0.09, initial_stateNone): n len(z) x_hat np.zeros(n) P np.zeros(n) # 初始状态用第一个观测值初始协方差取 R避免前几个点震荡 if initial_state is None: x_hat[0] z[0] else: x_hat[0] initial_state P[0] R for k in range(1, n): # 预测步A 1因此先验估计等于上一时刻的估计 x_pred x_hat[k-1] P_pred P[k-1] Q # 更新步H 1 K P_pred / (P_pred R) x_hat[k] x_pred K * (z[k] - x_pred) P[k] (1 - K) * P_pred return x_hat, P filtered, P_hist kalman_1d(z, Q1e-3, R0.09)这段代码的核心是循环内的四行计算。x_pred 和 P_pred 对应预测步K 是当前时刻的卡尔曼增益x_hat[k] 是融合观测后的后验估计。P[k] 保存的是后验误差协方差它可以反映滤波器当前对估计结果的信任程度。由于状态转移矩阵 A 为 1预测步退化为直接沿用上一时刻的状态这种简化让代码逻辑非常容易排查。参数方面Q 取 1e-3 表示允许信号每两个采样点之间发生约 ±0.032 的缓慢漂移R 取 0.09 对应噪声标准差 0.3 的平方。如果你处理的信号幅值更大或噪声更强需要同步放大或缩小这两个值而不是沿用这套数字。3.3 评估去噪效果信噪比与残差统计光看图只能得到定性印象量化评估通常计算滤波前后的信噪比观察残差是否为白噪声性质。def snr(original, noisy): signal_power np.mean(original ** 2) noise_power np.mean((original - noisy) ** 2) return 10 * np.log10(signal_power / noise_power) snr_noisy snr(true_signal, z) snr_filtered snr(true_signal, filtered) print(fSNR before filter: {snr_noisy:.2f} dB) print(fSNR after kalman filter: {snr_filtered:.2f} dB) # 残差分析滤波残差 观测 - 滤波输出 residual z - filtered print(fResidual std: {np.std(residual):.4f}) print(fNoise std: {noise_std:.4f})残差标准差是一个很有用的判断依据。如果残差标准差明显大于你设定的噪声标准差说明滤波过弱预测输出没有有效吸收观测中的真实信号如果残差标准差明显小于噪声标准差说明滤波器已经不只滤掉了噪声还可能把信号中的高频成分也抹平了。正常情况下残差应该是一个零均值、分布近似高斯、与真实信号不相关的序列这正是白噪声去除后剩下的“不可压部分”。3.4 三个常见的复现坑与排查方法第一个坑是 Q 和 R 的数量级相差过大导致数值精度问题。比如 Q 取 1e-8、R 取 100 时P_pred 会长时间停留在极小值附近卡尔曼增益接近 0滤波输出几乎是一条直线。排查方法是打印前 50 步的 P 和 K 序列观察它们是否在一个合理的量级上收敛。第二个坑是拿移动平均窗口的心态去调 Q。移动平均通过窗口长度控制平滑度长度越长越平滑但卡尔曼滤波的平滑度不等于 Q 越小越好。当信号存在大阶跃时过小的 Q 会让滤波输出出现“爬坡”现象表现为阶跃处上升时间过长。解决办法是在检测到残差显著偏离均值时临时增大 Q或者把模型替换为带速度分量的常速度模型但后者会显著增加代码复杂度。第三个坑是初始状态设置不当。直接取 z[0] 在逻辑上最安全但当第一个观测点正好被较大噪声污染时初始误差会被前几十步逐步消化。如果你的应用对前 100 个点有严格要求可以考虑用前 10 个点的均值作为初始状态同时将 P(0) 设大一些让滤波器自己快速修正。3.5 在没有真实信号的情况下如何判断参数是否合适现实项目里你往往拿不到 true_signal所以无法直接计算信噪比。这时我一般会看两个指标一是滤波输出与观测残差的标准差是否接近你标定的 R 的平方根二是残差的自相关函数是否在非零滞后处接近 0。如果残差自相关在滞后 1 到 10 之间仍然有明显相关峰说明滤波后的误差不是白噪声而是信号被滤掉的高频成分或低频趋势需要重新调整 Q。4. 用预测残差在线标定 R并和一阶低通滤波做对比4.1 一个实用技巧从 innovation 序列反推 R卡尔曼滤波里z_k - x_pred被称为预测残差或 innovation它的理论方差等于P_pred R。当滤波器已经收敛到稳态时P_pred 会稳定在一个较小的值于是可以取一段窗口的 innovation 样本方差再减去 P_pred 的稳态值得到对 R 的更真实估计。这个方法在标定传感器噪声时非常有用因为你不需要准备一段“纯噪声”数据。def estimate_innovation_std(filtered, P_hist, window200): # 用滤波结果近似预测值计算 innovation 标准差 innovation z[1:] - filtered[:-1] p_pred P_hist[:-1] Q # 与滤波循环中的预测协方差一致 r_estimated np.var(innovation[-window:]) - np.mean(p_pred[-window:]) return max(r_estimated, 0.0) r_est estimate_innovation_std(filtered, P_hist) print(fEstimated R: {r_est:.4f}, theoretical R: {R:.4f})这里有个容易出错的地方innovation 的窗口应该取滤波器达到稳态之后的数据而不是包含初始收敛段。如果滤波还没有收敛就统计方差r_estimated 会被明显高估。另一个细节是P_pred 和 R 的单位都是信号幅值的平方如果你的信号经过归一化处理R 的值也会随之改变需要在代码中保持一致。4.2 与一阶低通滤波对比延迟和自适应性很多人会把卡尔曼滤波和一阶低通滤波放在一起比较因为两者的递推形式相似。一阶低通可以写成y_k y_{k-1} alpha * (z_k - y_{k-1})而卡尔曼滤波的更新步是x_hat_k x_pred K * (z_k - x_pred)。当 Q 固定、R 固定时稳态卡尔曼增益 K 是一个常数此时它等价于一个一阶低通滤波器alpha K。差别在于卡尔曼滤波在初期和信号发生突变时P 和 K 会自动变化从而提供比固定 alpha 更好的动态响应。对比维度一阶低通滤波卡尔曼滤波去白噪声参数数量1 个alpha2 个以上Q、R、初始 P稳态行为固定 alpha恒定延迟稳定后等效固定增益突变响应alpha 较小时滞后明显增益会自动抬升响应更快实现成本一行代码约十行循环代码噪声模型要求无显式模型需要高斯白噪声与方差 R如果你只是想把信号做平滑且没有突变要求一阶低通完全够用但当你需要在去白噪声的同时保留信号存在的阶跃或快速变化卡尔曼滤波这种“增益自适应”的优势就会体现出来。在生产环境中有一个更细致的做法先将卡尔曼滤波后的残差送入上述 R 估计函数把估计的 R 回填到滤波器参数中再跑一遍通常能一边降低稳态噪声一边保持突变的跟随能力。这个“二次标定”是卡尔曼滤波白噪声去除项目里最值得优先尝试的调优手段。本文还有配套的精品资源点击获取