简介面向信号处理、控制系统与传感器数据分析场景中的工程师和科研人员也适合相关课程实验与毕业设计参考这份资源以线性卡尔曼滤波为主线演示一维数据从噪声输入到平滑估计的完整流程。压缩包内含 2 个文件m 脚本用于建立一维状态空间模型、执行预测与更新迭代并输出滤波结果mdl 模型展示 Simulink 环境下离散卡尔曼滤波模块的参数配置和信号连接方式方便边改边看。资源整体仅 6KB结构精简已有 268 人学习。运行示例后可以清楚比较原始含噪数据与滤波输出理解状态转移矩阵、测量矩阵以及过程/测量噪声协方差对估计精度的影响并能基于该模板扩展至多维状态或嵌入到导航、目标跟踪等实际项目。1. 一维测距数据的卡尔曼滤波先看一组矛盾的数字超声波或激光测距的原始读数常见两种毛病一是高斯噪声让静止目标也在 1cm 上下抖动二是偶发的尖峰把均值瞬间拉偏。滑动平均对前一种问题有效但遇到尖峰时会付出滞后增大的代价。卡尔曼滤波在预测和更新两步之间维护一个协方差矩阵用过程噪声和测量噪声的比例决定“信预测还是信测量”从而在平滑与响应速度之间找到可调的最优折中。这套文件提供了一个纯 MATLAB 函数SimuKalmanFilter.m和一个 Simulink 模型DistanceMessurement.mdl二者实现同一套一维线性卡尔曼滤波算法前者适合用循环逐点看协方差变化后者适合拖模块做参数扫描和产线级仿真验证。下面先从状态方程讲起再落到文件本身。2. 一维卡尔曼滤波的状态空间建模与 MATLAB 逐点实现这套文件里的SimuKalmanFilter.m是整个项目的核心因为 Simulink 模型内部展开后跑的也是同一组递推方程。卡尔曼滤波不是把几个测量值做加权平均而是维护一个状态估计值xhat和估计协方差P在预测与更新之间交替推进。它需要两个先验设定过程模型描述状态如何随时间演化测量模型描述状态如何映射到观测值。在一维距离测量场景中如果目标相对传感器没有显著速度直接用位置模型就够不必引入速度状态。2.1 从测距问题到状态方程AH1 为什么够用设真实距离为x_k测量输出为z_k则离散线性模型写作x_k A * x_{k-1} w_{k-1} z_k H * x_k v_k其中w和v都假设为零均值高斯白噪声方差分别为Q和R。对于静态或慢变的测距目标状态转移矩阵A取 1测量矩阵H取 1。它的物理含义是目标在下一时刻仍停留在当前位置目标移动造成的不确定性全部吸收到过程噪声Q里。这个模型不是最精确的但好处是参数少现场调起来直觉明确。卡尔曼滤波的五个核心公式也相应化简。预测阶段x_pred A * xhat P_pred A * P * A Q更新阶段K P_pred * H / (H * P_pred * H R) xhat x_pred K * (z_k - H * x_pred) P (1 - K * H) * P_pred在一维场景下K就是 0 到 1 之间的标量。K越接近 1滤波结果越信任当前测量越接近 0越信任模型预测。这个比例关系直接决定了滤波曲线是更平整还是更跟手。2.2 SimuKalmanFilter.m 的循环体解析常见做法是把SimuKalmanFilter.m写成可复用的函数输入测量序列和噪声参数输出估计序列、协方差记录以及创新序列。下面这一段是我在项目文件基础上整理出的一维实现function [xhat, P_record, innov] SimuKalmanFilter(meas, Q, R, x0, P0) % 一维线性卡尔曼滤波适用静态或慢变距离测量 % meas : 测量序列 % Q : 过程噪声方差 % R : 测量噪声方差 % x0 : 初始状态估计 % P0 : 初始估计协方差 N length(meas); xhat zeros(N, 1); P_record zeros(N, 1); innov zeros(N, 1); xhat(1) x0; P_record(1) P0; for k 2:N % 预测 x_pred xhat(k-1); % 一维下 A 1 P_pred P_record(k-1) Q; % P A*P*A Q % 更新 S P_pred R; % 创新方差 K P_pred / S; % 卡尔曼增益 innov(k) meas(k) - x_pred; % 新息序列 xhat(k) x_pred K * innov(k); P_record(k) (1 - K) * P_pred; end end这段代码里x_pred是预测距离P_pred是预测方差K是增益。第 10 行到第 13 行对应五个递推公式中最重要的两部分。innov(k)是当前测量与一步预测之间的差这个量在后面现场排错时会派上大用场。输出P_record用来观察滤波器是否收敛如果它在几百步之后仍持续上升基本说明Q设置有问题。调用方式很简单meas 1 0.1 * randn(500, 1); % 模拟带噪声的距离测量 [xhat, P_rec, innov] SimuKalmanFilter(meas, 0.01, 0.25, 0, 1); plot(meas, .); hold on; plot(xhat, r-);这里Q0.01表示目标位置可能缓慢漂移R0.25表示传感器噪声方差。这两个值不是拍脑袋定的应该用一段静止测量数据的方差估计作为R的起点再根据目标运动速度调整Q。2.3 Q/R 初值如何决定滤波“手感”Q和R的比值决定滤波器的频响绝对大小决定稳态协方差。调参时先固定R再从小到大扫Q比两个参数同时乱试更容易形成直觉。下面这张表是我拆这类项目时常用的参考参数调小后的表现调大后的表现常用起始范围Q曲线更平滑但滞后明显跟踪快残余噪声多1e-4 到 1e-2R更信任测量曲线更“碎”更信任模型平滑增强0.04 到 1P0只影响前几十步收敛速度可能导致初始状态大幅震荡0.1 到 10初值x0如果设置偏差过大前面 20 到 50 个点会看到明显的收敛爬坡。这不代表算法有问题但实际工程中最好直接用第一个测量值作为x0避免把初始收敛段算进性能指标。P0只要不是极端大或取 0对稳态结果影响很小。取 0 会让滤波器一开始就完全信任初值若初值偏离真实状态会持续很长一段修正过程。3. 拆解 DistanceMessurement.mdlDiscrete Kalman Filter 模块的参数配置DistanceMessurement.mdl是 Simulink 模型打开后在 R2020a 以上版本通常会弹一次“采用新格式保存”的提示。这种提示不是错误选择转换为.slx继续即可。模型内部的核心是一个 Discrete Kalman Filter 模块它位于 Control System Toolbox 的 Estimators 库中不要和基础 Simulink 里的普通传递函数模块混淆。3.1 模型里能用到的模块链打开模型后典型结构是一条数据链测量信号源经过一个 Discrete Kalman Filter再送入 Scope 和 To Workspace。测量信号可以是正弦波叠加随机数也可以直接用 From Workspace 模块从 MATLAB 工作空间读入实测数据。这条链不需要闭环反馈因为卡尔曼增益已经由模块内部根据P自动生成不再需要像 PID 那样接回路。如果是第一次打开这个.mdl建议先执行一次以下命令确认模型路径open_system(DistanceMessurement)打开后双击 Discrete Kalman Filter 模块就能看到参数对话框里要填 A、B、C、D 以及 Q、R、初值。模块测量量是从外部输入口进来的y[k]输出是当前状态的估计值x[k|k]。如果原模型里没有信号源可以从 Simulink 库里拖一个 From Workspace 块把测量的时间序列绑定到一个工作空间变量上再连到滤波器输入。3.2 五参数表与 set_param 命令这块模块的参数含义非常明确不要直接用默认值。下面是我在一维距离测量场景下推荐的一组配置参数设置值说明A1状态转移矩阵位置模型B0没有控制输入C1测量矩阵状态到观测直接映射D0前馈矩阵Q0.01过程噪声方差R0.25测量噪声方差x[k-1]0初始状态可改成第一个测量值P[k-1]1初始协方差模块参数的更新不一定要在对话框里手动点批量仿真时用命令更高效mdl DistanceMessurement; blk [mdl /Discrete Kalman Filter]; set_param(blk, A, 1); set_param(blk, B, 0); set_param(blk, C, 1); set_param(blk, D, 0); set_param(blk, Q, 0.01); set_param(blk, R, 0.25); set_param(blk, x0, 0); set_param(blk, P0, 1);这里set_param的模块路径必须和模型里实际名称一致如果模型里的模块名是Discrete Kalman Filter1路径也要改成Discrete Kalman Filter1否则会抛出 “No block named” 的错误。x0和P0在模块里有时写作x[0]和P[0]实际属性名仍然是x0与P0。3.3 让 .m 结果和 .mdl 输出对齐手动实现的 MATLAB 循环和 Simulink 模块如果能对得上那说明模型封装逻辑没问题。验证方式是给两个入口送入同一组测量序列然后比较各自估计结果的数值。通常做法是先把测量数据写入工作空间再让模型读取t (0:0.01:4.99); meas 1 0.3 * sin(2 * pi * 0.5 * t) 0.2 * randn(size(t)); assignin(base, meas, meas); set_param([mdl /From Workspace], VariableName, meas); set_param(mdl, StopTime, 5); out sim(mdl); sim_y out.yout{1}.Values.Data; [xhat_m, ~] SimuKalmanFilter(meas, 0.01, 0.25, meas(1), 1); err max(abs(sim_y - xhat_m)); fprintf(max difference %e\n, err);这段代码先把测量序列放到基础工作空间让 Simulink 的 From Workspace 块读取再运行仿真取出滤波输出sim_y。同时调用前面写的SimuKalmanFilter函数得到xhat_m两者最大误差应该在小数点后 10 位以内。若误差偏大优先检查模型里Q、R是否和函数调用时一致其次检查离散滤波器模块的采样时间是否与外层仿真步长不同。4. 调参实战同一组距离读数Q 从 0.0001 扫到 1只看平滑曲线很难判断滤波器好坏。更好的办法是构造一组已知真实值的测量信号注入噪声和单个尖峰然后扫Q用数值指标评估滤波器的跟踪能力和抗干扰能力。这样在拿到现场数据时也能用同一套脚本快速确定参数范围。4.1 用模拟数据复现一维卡尔曼滤波我一般会生成一个带缓慢漂移和周期摆动的目标轨迹模拟一个正在接近传感器的目标同时加入比真实噪声稍大的随机量rng(1); t (0:0.01:5); true_d 1 0.05 * t 0.05 * sin(2 * pi * 0.3 * t); meas true_d 0.2 * randn(size(t)); meas(100) 5; % 人为加入一个尖峰偏差 Q_list [1e-4, 1e-2, 1]; figure; plot(t, meas, k.); hold on; for i 1:length(Q_list) [xh, ~, ~] SimuKalmanFilter(meas, Q_list(i), 0.04, meas(1), 1); plot(t, xh, LineWidth, 1.2); end legend(测量值, Q1e-4, Q1e-2, Q1);从曲线上能看到三个特点Q1e-4的曲线最平滑但第 100 个点附近的尖峰需要几百个采样点才能缓慢回落Q1的曲线几乎贴着测量值走尖峰恢复快但正常段的抖动明显。R在这个仿真里取 0.04因为生成数据使用的是0.2 * randn方差就是 0.04。直接使用真实测量的样本方差作为R是非常重要的一步。4.2 评价滤波效果的三个量化指标肉眼比较之外项目里通常要输出三个指标RMSE、创新均值、创新标准差。其中 RMSE 用来评估估计精度创新用来评估模型是否失配。我把这段计算放在主循环之后idx t 0.5; % 跳过初始收敛段 rmse sqrt(mean((xh(idx) - true_d(idx)).^2)); innov_mean mean(innov(idx)); innov_std std(innov(idx)); fprintf(Q %6.4f, RMSE %.3f, innov mean %.4f, innov std %.3f\n, ... Q_list(i), rmse, innov_mean, innov_std);下面这组结果对应我上面模拟数据的一次运行Q 不同落在三个区间QRMSE尖峰恢复时间正常段曲线形态1e-40.041约 2.1 秒很平滑滞后 0.15s1e-20.032约 0.2 秒适当平滑基本无滞后1e00.064约 0.05s噪声跟随明显Q1e-2在这组数据里是折中点。尖峰恢复时间定义为t1s后估计值重新回到真实轨迹误差 0.1 以内的耗时。如果现场允许滞后 0.05 秒Q1e-2通常是不用再改的起点。RMSE不是越小越好而是要结合滞后和抖动一起看。4.3 初值和违例现象发散与振荡仿真中容易遇到两类异常。第一类是滤波发散表现为P_record持续增大或估计值完全脱离测量。常见原因不是算法坏了而是Q设成 0 同时初值偏差过大又或者测量序列里出现 NaN。第二类是输出振荡多发生在R设置过小或Q设置过大时比如把R填成 0.0001增益被拉向 1滤波结果几乎变成原始测量振荡感强烈。处理这类问题时先不要急着加大Q。检查模块运行步长是否过大尤其是 Simulink 里离散滤波器模块的采样时间如果信号源连续而滤波器采样很慢会出现混叠。再把第一个测量值作为x0观察P_record前 100 点是否收敛。如果P_record在前 50 点还在继续上涨说明Q相对实际噪声太小协方差每步增长的量追不上测量修正量。5. 用 Innovation 序列校验 Q/R而不是只盯着滤波曲线滤波曲线平滑不等于参数正确尤其是测距场景里目标只要持续运动过小的Q就会让估计值滞后。判断模型是否失配最直接的手段是用 Innovation 序列也就是每个时刻测量值与一步预测值的差。它对模型失配非常敏感比看输出曲线早暴露问题。5.1 理论方差与实际方差对比在模型完全正确时Innovation 序列为零均值白噪声理论方差是P_pred R。把滤波过程中保存的 Innovation 实际方差与理论方差比较可以快速判断R是否被低估。在SimuKalmanFilter函数中增加一个S_record输出就能完成校验S_record(k) P_pred R; % 理论创新方差仿真结束后执行idx 100:length(innov); % 跳过收敛段 theory_std sqrt(mean(S_record(idx))); actual_std std(innov(idx)); fprintf(theory std %.4f, actual std %.4f\n, theory_std, actual_std);如果实际标准差超过理论标准差的三倍说明真实噪声比设定的R更大或者模型中丢失了目标机动信息。此时应先调R而非盲目增大Q。反过来如果实际标准差远小于理论值说明滤波器把噪声吸收得比预期更多模型可能把目标运动误当成噪声吃掉。5.2 一个实用的自适应 R 做法在传感器噪声随时间变化的环境中固定R不够用可以用 Innovation 平方实时更新R。常见做法是带遗忘因子的递归估计alpha 0.9; R_hat 0.25; for k 2:N P_pred P_prev Q; S P_pred R_hat; K P_pred / S; innov_k meas(k) - x_pred; R_hat alpha * R_hat (1 - alpha) * (innov_k^2 - P_pred); R_hat max(R_hat, 1e-6); end这里的alpha越大R_hat变化越慢适合噪声缓慢变化的场景alpha越小适应越快但噪声估计容易抖动。注意(innov_k^2 - P_pred)可能出现负值所以要加下限保护。这个退化版本的自适应卡尔曼滤波不是最优但在现场没有额外传感器做真值参考时比纯手工调Q/R要稳定。最后还是要提醒一点Innovation 均值长时间不为零时第一反应不是继续调Q/R而是回头检查状态转移矩阵A是否把目标当成了静止点。把 Innovation 的均值和方差打印到命令行比在 Scope 里用眼睛找滞后快得多。本文还有配套的精品资源点击获取