简介这份资源是《Kalman filtering-theory and practice using MATLAB》第三版2008的PDF电子书及配套MATLAB代码合集面向信号处理、导航制导、控制系统与数据分析方向的学习者和工程人员帮助读者在掌握卡尔曼滤波理论的同时通过可运行代码完成从公式推导到工程实现的过渡。压缩包共66个文件约4.59MB以61个m脚本为主体辅以txt说明、mat数据、pdf原书与doc文档覆盖基本卡尔曼滤波、扩展卡尔曼滤波、平方根滤波、UD分解、Bierman与Potter算法、RTS平滑、惯性导航与GNSS组合等章节示例代码按章节组织便于对照书中内容逐章练习。目前已有499人学习下载。读者可借助这些脚本理解状态转移矩阵与观测矩阵的构建、滤波器初始化与更新流程并在传感器融合、轨道预测、自动驾驶等场景中迁移应用适合作为课程学习与项目开发的参考材料。1. 从一段飘忽的陀螺仪数据说起这套卡尔曼滤波资源到底能干什么如果你做过姿态解算、目标跟踪或者传感器融合大概率遇到过这种场景MPU6050 静止时角度还算稳一旦电机转起来解算出来的姿态角就开始飘加速度计和陀螺仪的数据像是两个人在吵架谁都不服谁。这时候老工程师通常会甩给你三个字——上卡尔曼。但真翻开理论推导满屏的协方差矩阵和贝叶斯递推很容易让人从入门到放弃。这套《Kalman filtering-theory and practice using MATLAB》资源本质上就是来解决这个断层的。它包含一份系统讲解卡尔曼滤波理论到工程实现的 PDF以及配套的 MATLAB 代码。理论部分不是纯数学炫技而是从最小二乘、递归估计一路推到卡尔曼滤波的五个核心方程代码部分则把每一章的算法都落成了可运行的 .m 文件覆盖线性卡尔曼、扩展卡尔曼EKF、无迹卡尔曼UKF等常见变体。适合谁正在做惯性导航、雷达跟踪、电池 SOC 估算、传感器融合的工程师以及被课程设计逼到墙角、需要一份能跑通代码的学生。你不需要是数学科班出身但得能看懂矩阵运算愿意动手改参数。2. 五个方程背后的工程直觉先搞懂为什么这么算2.1 从递归最小二乘到卡尔曼增益很多人一上来就背卡尔曼的五个方程背完也不知道为什么增益 K 要长成那个样子。这套资源的 PDF 在第二章花了很大篇幅讲递归最小二乘RLS其实就是在为卡尔曼做铺垫。RLS 解决的是我有一堆带噪声的观测怎么递推地估计一个未知常量。卡尔曼在此基础上多了一步——状态本身在动于是引入了状态转移矩阵 F 和过程噪声 Q。核心直觉是这样的卡尔曼增益 K 本质上是在做加权平均权值由两个东西决定——预测的不确定度 P 和观测的不确定度 R。P 大、R 小说明你更信观测K 就大反过来K 就小。PDF 里用了一个很直观的标量例子演示这个权衡代码包里对应的脚本是kf_scalar_demo.m。我建议第一次跑的时候把 R 调大十倍看估计曲线怎么变得迟钝再把 Q 调大看它怎么变得神经质。这种手感比看十页推导都管用。提示不要跳过 RLS 那一章直接看卡尔曼。RLS 里的递推协方差更新和卡尔曼的 P 更新是同一个数学结构理解了它后面 P 矩阵的物理意义就顺了。2.2 预测与更新两步走的数据流卡尔曼滤波的工程实现说到底就是一个循环里做两件事预测和更新。预测步用上一时刻的状态和协方差推当前时刻的先验估计更新步用当前观测把先验修正成后验。这套资源的代码结构很清晰每个示例都遵循同一个骨架% 以 kf_linear_demo.m 为例核心循环骨架 for k 2:N % 预测步 x_pred F * x_est(:,k-1); % 状态外推 P_pred F * P_est(:,:,k-1) * F Q; % 协方差外推 % 更新步 y z(:,k) - H * x_pred; % 新息观测残差 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 x_est(:,k) x_pred K * y; % 后验状态估计 P_est(:,:,k) (eye(n) - K * H) * P_pred; % 后验协方差 end这段代码里F 是状态转移矩阵Q 是过程噪声协方差H 是观测矩阵R 是观测噪声协方差。四个矩阵定好了滤波器的行为基本就定了。资源里的 PDF 对每个矩阵的物理含义都有解释但真正调参的时候你会发现 Q 和 R 的比值才是决定滤波效果的关键。常见做法是先把 R 设成传感器手册里的噪声方差然后调 Q 来平衡响应速度和平滑度。如果 Q 给得太大估计会跟着噪声跳Q 太小又会滞后于真实状态变化。2.3 扩展卡尔曼非线性系统的线性化代价实际工程里纯线性系统很少更多的是非线性比如雷达测距测角、无人机姿态动力学。这套资源用了一整章讲扩展卡尔曼滤波EKF核心思路就是在每个时刻对非线性函数做一阶泰勒展开用雅可比矩阵代替原来的 F 和 H。代码包里ekf_pendulum_demo.m是一个单摆角度估计的例子状态方程里有 sin 项观测方程也是非线性的。EKF 的坑在于雅可比矩阵的推导。PDF 里给了详细的偏导数推导过程但自己动手推的时候很容易漏项。我的血泪经验是先用符号计算工具把雅可比矩阵求出来再手写进代码别硬推。资源里有一个jacobian_check.m脚本用数值差分验证解析雅可比的正确性这个习惯非常值得保留。另外EKF 的线性化误差在状态变化剧烈时会变大如果发现滤波发散先检查是不是采样周期太长导致单步变化过大。2.4 无迹卡尔曼不用雅可比的替代方案如果雅可比矩阵实在难求或者非线性太强导致 EKF 效果差无迹卡尔曼UKF是另一个选择。这套资源在最后一章介绍了 UKF核心是用一组 sigma 点来传播均值和协方差避免了解析求导。代码包里ukf_demo.m展示了同样的单摆问题用 UKF 怎么解对比 EKF 的结果你会发现 UKF 在非线性强的时候估计更稳但计算量也更大。UKF 的参数主要是 sigma 点的分布参数 alpha、beta、kappa。资源里给的默认值适用于大多数情况但如果状态维度很高需要适当调整 alpha 来控制 sigma 点的散布范围。我一般会先用默认参数跑一遍看估计效果如果协方差收敛太慢或者太快再微调这几个参数。注意 UKF 的权重计算里有一个负权重的问题可能导致协方差矩阵非正定代码里用了 Cholesky 分解来保证数值稳定性这个细节在 PDF 里有说明。3. 把代码跑起来环境配置与第一个可复现示例3.1 MATLAB 版本与工具箱依赖这套代码对 MATLAB 版本的要求不算苛刻R2016b 之后的版本基本都能跑。但有几个函数需要注意chol在旧版本里对非正定矩阵会报错新版本有cholupdate可以增量更新expm和ode45在大部分版本里都有。资源里没有用到太偏门的工具箱主要依赖的是基础矩阵运算和信号处理工具箱里的randn、cov这些。如果你用的是开源替代方案Octave 也能跑大部分脚本但ode45的接口略有差异需要改一下调用方式。注意代码包里有一个startup.m脚本负责把各个子目录加到路径里。第一次跑之前先执行它否则会报函数找不到。这个脚本还会检查 MATLAB 版本如果低于 R2016b 会给出警告。3.2 跑通第一个线性卡尔曼示例从最简单的开始kf_linear_demo.m是一个匀速直线运动的跟踪例子。状态是位置和速度观测只有位置。这个例子的好处是 F 和 H 都是常数矩阵没有非线性适合用来建立信心。运行之后会弹出两张图一张是位置估计和真实值的对比一张是速度估计。你会看到估计曲线在几次迭代之后就贴上了真实值协方差也快速收敛。% 运行方式在 MATLAB 命令行里直接输入脚本名 kf_linear_demo % 如果想改参数打开脚本找到这几行 Q diag([0.01, 0.1]); % 过程噪声位置项小速度项大 R 0.5; % 观测噪声位置观测的方差 % 把 R 改成 5重新运行观察估计曲线的变化改完 R 之后重新跑你会发现位置估计变得更平滑但跟踪真实值的变化时滞后更明显。这就是观测噪声和过程噪声之间的权衡。资源里的 PDF 在对应章节有一个表格列出了不同 Q/R 比值下的滤波行为可以对照着调。3.3 用真实数据替换仿真数据仿真跑通之后下一步是把代码里的z换成你自己的传感器数据。资源里的示例数据是randn生成的格式是 1×N 的向量。你的数据可能是从串口读出来的带时间戳采样周期不固定。这时候需要做两件事一是把时间戳转成等间隔的索引或者改用变步长的卡尔曼滤波二是把数据单位统一比如角度是弧度还是度速度是 m/s 还是 cm/s。% 假设你的数据是 csv 文件第一列是时间第二列是观测值 data csvread(sensor_log.csv); t data(:,1); z_raw data(:,2); % 重采样到等间隔假设目标采样周期是 0.01s t_uniform t(1):0.01:t(end); z interp1(t, z_raw, t_uniform, linear); % 然后把 z 替换掉原来脚本里的仿真数据 % 注意如果观测有缺失interp1 会插值但卡尔曼本身可以处理缺失观测 % 缺失时跳过更新步即可只做预测这段代码里interp1做的是线性插值适合数据量不大、缺失不严重的情况。如果数据抖动厉害可以先做一个低通滤波再送进卡尔曼。资源里有一个preprocess_demo.m脚本演示了滑动平均和巴特沃斯低通滤波两种预处理方式可以对比看哪种更适合你的数据。3.4 参数调试的实用手法调 Q 和 R 是卡尔曼滤波最玄学也最实际的部分。资源里给了一个建议先用传感器手册里的噪声密度算出 R然后从 Q 等于 R 的百分之一开始试。如果估计滞后增大 Q如果估计抖动减小 Q。更系统的方法是用新息序列做一致性检验——如果滤波器调得好新息应该是零均值的白噪声。代码包里innovation_test.m演示了怎么画新息的自相关函数如果自相关在非零滞后处有尖峰说明 Q 或 R 设得不对。我一般会先把 R 固定然后扫一遍 Q 的值看新息的自相关什么时候最接近白噪声。这个过程不需要太精确因为卡尔曼滤波对参数有一定的鲁棒性只要 Q/R 在合理范围内估计效果不会差太多。但如果 Q 给得离谱比如比 R 大好几个数量级滤波器会完全跟着观测跑失去预测能力。4. 避坑与排查那些让滤波器发散的原因4.1 协方差矩阵非正定导致滤波崩溃现象运行一段时间后MATLAB 报错说矩阵接近奇异或者 Cholesky 分解失败估计值突然跳到无穷大。原因通常是数值误差累积P 矩阵在更新过程中失去了正定性。解决方法是改用 Joseph 形式的协方差更新或者每次更新后强制对称化P (P P) / 2。资源里的kf_linear_demo.m用的是简单形式但在ekf_pendulum_demo.m里用了 Joseph 形式可以对比看差异。4.2 观测矩阵 H 写错导致估计不收敛现象估计值一直偏离真实值或者收敛到一个错误的值。原因往往是 H 矩阵的维度或内容写错了。比如状态是 [位置, 速度]观测只有位置H 应该是 [1, 0]如果写成 [0, 1]滤波器就会把速度观测当成位置来用。排查方法是先检查 H 的维度是否和状态维度匹配再检查每一列对应的是哪个状态量。资源里的 PDF 在每个示例后面都标注了 H 的含义对照着看能避免大部分低级错误。4.3 采样周期与状态转移矩阵不匹配现象滤波器在仿真里跑得好好的换成真实数据就发散。原因通常是 F 矩阵是按固定采样周期 dt 算的但真实数据的采样周期不是 dt。比如 F 里用了 dt0.01但实际数据是 0.02 的间隔状态外推就会偏。解决方法是把 dt 作为参数传进 F 的计算里每次循环根据实际时间戳更新。资源里的kf_linear_demo.m用的是固定 dt但在ekf_pendulum_demo.m里演示了变步长的处理方式。4.4 初始协方差 P0 给得太小导致滤波器“自信过头”现象滤波器刚开始时完全不理会观测估计值一直停在初始值附近过了很久才慢慢跟上。原因是 P0 给得太小滤波器认为自己初始状态很准所以卡尔曼增益 K 很小观测的修正作用被压制。解决方法是在没有先验信息时把 P0 设得大一些比如状态量可能变化范围的平方。资源里的示例通常把 P0 设为单位矩阵乘以一个较大的数就是这个道理。4.5 非线性系统线性化点选错导致 EKF 发散现象EKF 在状态远离线性化点时估计误差很大甚至发散。原因是雅可比矩阵是在预测状态处求的如果预测状态本身偏离真实值很远线性化误差就很大。解决方法是缩短预测步长或者改用 UKF。资源里的ekf_pendulum_demo.m在单摆摆角很大时也会出现这个问题PDF 里建议在这种情况下用迭代 EKF 或者切换 UKF。5. 从能跑到好用几个让滤波效果再上一档的技巧5.1 用新息序列做在线一致性检验滤波器跑起来之后怎么知道它工作正常最直接的办法是看新息序列。新息是观测减去预测观测如果滤波器调得好新息应该是零均值的白噪声。资源里的innovation_test.m给了完整的检验流程先算新息再算归一化新息平方NIS然后和卡方分布的置信区间对比。如果 NIS 超出置信区间说明滤波器对观测的信任度不对需要调 Q 或 R。% 计算 NIS 并画图 nis zeros(1, N); for k 1:N y z(:,k) - H * x_pred(:,k); S H * P_pred(:,:,k) * H R; nis(k) y / S * y; end % 卡方分布的 95% 置信区间自由度等于观测维度 chi2_low chi2inv(0.025, m); chi2_high chi2inv(0.975, m); % 画图看有多少点落在区间外 plot(nis); hold on; yline(chi2_low, r--); yline(chi2_high, r--);这段代码里m是观测维度chi2inv是卡方分布的逆累积分布函数。如果超过 5% 的点落在区间外说明滤波器的一致性有问题。我一般会把这个检验做成在线监控每次跑完数据都看一眼比凭感觉调参靠谱得多。5.2 自适应卡尔曼让 Q 和 R 自己调如果传感器噪声不是固定的比如温度变化导致陀螺仪零偏漂移固定 Q 和 R 就不够用了。资源里提到了自适应卡尔曼的思路代码包里有一个adaptive_kf_demo.m用新息序列的滑动方差来估计 R用状态残差来估计 Q。核心思想是如果新息方差比理论值大说明 R 被低估了调大 R如果状态估计和观测的残差持续偏大说明 Q 被低估了调大 Q。% 自适应 R 的简化实现 window_size 20; for k window_size:N y_window y(:, k-window_size1:k); R_est cov(y_window); % 用新息滑动窗口估计观测噪声 R_est max(R_est, 1e-6); % 防止太小导致数值问题 % 然后用 R_est 替换原来的 R 做更新 end这个方法的参数是窗口大小窗口太小会导致 R 估计抖动太大则跟不上噪声变化。资源里建议窗口大小取 10 到 30 之间具体看采样率和噪声变化速度。注意自适应方法在观测缺失或异常值多的时候容易跑偏最好配合异常值检测一起用。5.3 用平滑器做事后处理卡尔曼滤波是实时估计只用了当前和过去的观测。如果场景允许事后处理比如离线分析飞行日志可以用卡尔曼平滑器把未来的观测也利用起来估计精度会明显提升。资源里的 PDF 在最后一章介绍了 Rauch-Tung-StriebelRTS平滑器代码包里rts_smoother_demo.m演示了怎么在滤波结果的基础上做反向平滑。平滑后的轨迹比滤波更贴近真实值尤其是在观测噪声大的时候。我自己的习惯是实时系统用滤波离线分析用平滑。平滑器的代码结构是在滤波循环结束后再从后往前跑一遍用滤波时保存的预测协方差和后验协方差算平滑增益。资源里的实现把滤波和平滑拆成了两个函数调用起来很清晰。5.4 从 MATLAB 到嵌入式 C 的移植要点最后说一个实际工程里绕不开的问题MATLAB 验证完算法怎么搬到嵌入式平台。资源里的代码是 MATLAB 的但矩阵运算的逻辑可以直接翻译成 C。需要注意几点一是 MATLAB 的矩阵是列优先C 里如果用行优先存储索引要转置二是chol和inv在 C 里需要自己实现或者用现成库资源里没有提供 C 代码但 PDF 附录里给了伪代码三是浮点精度MATLAB 默认 double嵌入式平台可能用 float需要重新验证数值稳定性。我一般会先在 MATLAB 里把算法固定下来用single类型跑一遍看精度损失然后再手写 C。移植完之后用同一组数据在 MATLAB 和 C 里各跑一遍对比输出误差在 1e-6 以内才算通过。这个习惯帮我省了很多调试时间希望也能帮到你。从那以后我每次做算法移植都强制走一遍 MATLAB 和 C 的交叉验证再急也不跳过。本文还有配套的精品资源点击获取 
长河君 · 2026/10/18 3:34:09 · 阅读 1.2k 卡尔曼滤波从理论到MATLAB实战：资源包助你搞定传感器融合与姿态解算
简介这份资源是《Kalman filtering-theory and practice using MATLAB》第三版2008的PDF电子书及配套MATLAB代码合集面向信号处理、导航制导、控制系统与数据分析方向的学习者和工程人员帮助读者在掌握卡尔曼滤波理论的同时通过可运行代码完成从公式推导到工程实现的过渡。压缩包共66个文件约4.59MB以61个m脚本为主体辅以txt说明、mat数据、pdf原书与doc文档覆盖基本卡尔曼滤波、扩展卡尔曼滤波、平方根滤波、UD分解、Bierman与Potter算法、RTS平滑、惯性导航与GNSS组合等章节示例代码按章节组织便于对照书中内容逐章练习。目前已有499人学习下载。读者可借助这些脚本理解状态转移矩阵与观测矩阵的构建、滤波器初始化与更新流程并在传感器融合、轨道预测、自动驾驶等场景中迁移应用适合作为课程学习与项目开发的参考材料。1. 从一段飘忽的陀螺仪数据说起这套卡尔曼滤波资源到底能干什么如果你做过姿态解算、目标跟踪或者传感器融合大概率遇到过这种场景MPU6050 静止时角度还算稳一旦电机转起来解算出来的姿态角就开始飘加速度计和陀螺仪的数据像是两个人在吵架谁都不服谁。这时候老工程师通常会甩给你三个字——上卡尔曼。但真翻开理论推导满屏的协方差矩阵和贝叶斯递推很容易让人从入门到放弃。这套《Kalman filtering-theory and practice using MATLAB》资源本质上就是来解决这个断层的。它包含一份系统讲解卡尔曼滤波理论到工程实现的 PDF以及配套的 MATLAB 代码。理论部分不是纯数学炫技而是从最小二乘、递归估计一路推到卡尔曼滤波的五个核心方程代码部分则把每一章的算法都落成了可运行的 .m 文件覆盖线性卡尔曼、扩展卡尔曼EKF、无迹卡尔曼UKF等常见变体。适合谁正在做惯性导航、雷达跟踪、电池 SOC 估算、传感器融合的工程师以及被课程设计逼到墙角、需要一份能跑通代码的学生。你不需要是数学科班出身但得能看懂矩阵运算愿意动手改参数。2. 五个方程背后的工程直觉先搞懂为什么这么算2.1 从递归最小二乘到卡尔曼增益很多人一上来就背卡尔曼的五个方程背完也不知道为什么增益 K 要长成那个样子。这套资源的 PDF 在第二章花了很大篇幅讲递归最小二乘RLS其实就是在为卡尔曼做铺垫。RLS 解决的是我有一堆带噪声的观测怎么递推地估计一个未知常量。卡尔曼在此基础上多了一步——状态本身在动于是引入了状态转移矩阵 F 和过程噪声 Q。核心直觉是这样的卡尔曼增益 K 本质上是在做加权平均权值由两个东西决定——预测的不确定度 P 和观测的不确定度 R。P 大、R 小说明你更信观测K 就大反过来K 就小。PDF 里用了一个很直观的标量例子演示这个权衡代码包里对应的脚本是kf_scalar_demo.m。我建议第一次跑的时候把 R 调大十倍看估计曲线怎么变得迟钝再把 Q 调大看它怎么变得神经质。这种手感比看十页推导都管用。提示不要跳过 RLS 那一章直接看卡尔曼。RLS 里的递推协方差更新和卡尔曼的 P 更新是同一个数学结构理解了它后面 P 矩阵的物理意义就顺了。2.2 预测与更新两步走的数据流卡尔曼滤波的工程实现说到底就是一个循环里做两件事预测和更新。预测步用上一时刻的状态和协方差推当前时刻的先验估计更新步用当前观测把先验修正成后验。这套资源的代码结构很清晰每个示例都遵循同一个骨架% 以 kf_linear_demo.m 为例核心循环骨架 for k 2:N % 预测步 x_pred F * x_est(:,k-1); % 状态外推 P_pred F * P_est(:,:,k-1) * F Q; % 协方差外推 % 更新步 y z(:,k) - H * x_pred; % 新息观测残差 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 x_est(:,k) x_pred K * y; % 后验状态估计 P_est(:,:,k) (eye(n) - K * H) * P_pred; % 后验协方差 end这段代码里F 是状态转移矩阵Q 是过程噪声协方差H 是观测矩阵R 是观测噪声协方差。四个矩阵定好了滤波器的行为基本就定了。资源里的 PDF 对每个矩阵的物理含义都有解释但真正调参的时候你会发现 Q 和 R 的比值才是决定滤波效果的关键。常见做法是先把 R 设成传感器手册里的噪声方差然后调 Q 来平衡响应速度和平滑度。如果 Q 给得太大估计会跟着噪声跳Q 太小又会滞后于真实状态变化。2.3 扩展卡尔曼非线性系统的线性化代价实际工程里纯线性系统很少更多的是非线性比如雷达测距测角、无人机姿态动力学。这套资源用了一整章讲扩展卡尔曼滤波EKF核心思路就是在每个时刻对非线性函数做一阶泰勒展开用雅可比矩阵代替原来的 F 和 H。代码包里ekf_pendulum_demo.m是一个单摆角度估计的例子状态方程里有 sin 项观测方程也是非线性的。EKF 的坑在于雅可比矩阵的推导。PDF 里给了详细的偏导数推导过程但自己动手推的时候很容易漏项。我的血泪经验是先用符号计算工具把雅可比矩阵求出来再手写进代码别硬推。资源里有一个jacobian_check.m脚本用数值差分验证解析雅可比的正确性这个习惯非常值得保留。另外EKF 的线性化误差在状态变化剧烈时会变大如果发现滤波发散先检查是不是采样周期太长导致单步变化过大。2.4 无迹卡尔曼不用雅可比的替代方案如果雅可比矩阵实在难求或者非线性太强导致 EKF 效果差无迹卡尔曼UKF是另一个选择。这套资源在最后一章介绍了 UKF核心是用一组 sigma 点来传播均值和协方差避免了解析求导。代码包里ukf_demo.m展示了同样的单摆问题用 UKF 怎么解对比 EKF 的结果你会发现 UKF 在非线性强的时候估计更稳但计算量也更大。UKF 的参数主要是 sigma 点的分布参数 alpha、beta、kappa。资源里给的默认值适用于大多数情况但如果状态维度很高需要适当调整 alpha 来控制 sigma 点的散布范围。我一般会先用默认参数跑一遍看估计效果如果协方差收敛太慢或者太快再微调这几个参数。注意 UKF 的权重计算里有一个负权重的问题可能导致协方差矩阵非正定代码里用了 Cholesky 分解来保证数值稳定性这个细节在 PDF 里有说明。3. 把代码跑起来环境配置与第一个可复现示例3.1 MATLAB 版本与工具箱依赖这套代码对 MATLAB 版本的要求不算苛刻R2016b 之后的版本基本都能跑。但有几个函数需要注意chol在旧版本里对非正定矩阵会报错新版本有cholupdate可以增量更新expm和ode45在大部分版本里都有。资源里没有用到太偏门的工具箱主要依赖的是基础矩阵运算和信号处理工具箱里的randn、cov这些。如果你用的是开源替代方案Octave 也能跑大部分脚本但ode45的接口略有差异需要改一下调用方式。注意代码包里有一个startup.m脚本负责把各个子目录加到路径里。第一次跑之前先执行它否则会报函数找不到。这个脚本还会检查 MATLAB 版本如果低于 R2016b 会给出警告。3.2 跑通第一个线性卡尔曼示例从最简单的开始kf_linear_demo.m是一个匀速直线运动的跟踪例子。状态是位置和速度观测只有位置。这个例子的好处是 F 和 H 都是常数矩阵没有非线性适合用来建立信心。运行之后会弹出两张图一张是位置估计和真实值的对比一张是速度估计。你会看到估计曲线在几次迭代之后就贴上了真实值协方差也快速收敛。% 运行方式在 MATLAB 命令行里直接输入脚本名 kf_linear_demo % 如果想改参数打开脚本找到这几行 Q diag([0.01, 0.1]); % 过程噪声位置项小速度项大 R 0.5; % 观测噪声位置观测的方差 % 把 R 改成 5重新运行观察估计曲线的变化改完 R 之后重新跑你会发现位置估计变得更平滑但跟踪真实值的变化时滞后更明显。这就是观测噪声和过程噪声之间的权衡。资源里的 PDF 在对应章节有一个表格列出了不同 Q/R 比值下的滤波行为可以对照着调。3.3 用真实数据替换仿真数据仿真跑通之后下一步是把代码里的z换成你自己的传感器数据。资源里的示例数据是randn生成的格式是 1×N 的向量。你的数据可能是从串口读出来的带时间戳采样周期不固定。这时候需要做两件事一是把时间戳转成等间隔的索引或者改用变步长的卡尔曼滤波二是把数据单位统一比如角度是弧度还是度速度是 m/s 还是 cm/s。% 假设你的数据是 csv 文件第一列是时间第二列是观测值 data csvread(sensor_log.csv); t data(:,1); z_raw data(:,2); % 重采样到等间隔假设目标采样周期是 0.01s t_uniform t(1):0.01:t(end); z interp1(t, z_raw, t_uniform, linear); % 然后把 z 替换掉原来脚本里的仿真数据 % 注意如果观测有缺失interp1 会插值但卡尔曼本身可以处理缺失观测 % 缺失时跳过更新步即可只做预测这段代码里interp1做的是线性插值适合数据量不大、缺失不严重的情况。如果数据抖动厉害可以先做一个低通滤波再送进卡尔曼。资源里有一个preprocess_demo.m脚本演示了滑动平均和巴特沃斯低通滤波两种预处理方式可以对比看哪种更适合你的数据。3.4 参数调试的实用手法调 Q 和 R 是卡尔曼滤波最玄学也最实际的部分。资源里给了一个建议先用传感器手册里的噪声密度算出 R然后从 Q 等于 R 的百分之一开始试。如果估计滞后增大 Q如果估计抖动减小 Q。更系统的方法是用新息序列做一致性检验——如果滤波器调得好新息应该是零均值的白噪声。代码包里innovation_test.m演示了怎么画新息的自相关函数如果自相关在非零滞后处有尖峰说明 Q 或 R 设得不对。我一般会先把 R 固定然后扫一遍 Q 的值看新息的自相关什么时候最接近白噪声。这个过程不需要太精确因为卡尔曼滤波对参数有一定的鲁棒性只要 Q/R 在合理范围内估计效果不会差太多。但如果 Q 给得离谱比如比 R 大好几个数量级滤波器会完全跟着观测跑失去预测能力。4. 避坑与排查那些让滤波器发散的原因4.1 协方差矩阵非正定导致滤波崩溃现象运行一段时间后MATLAB 报错说矩阵接近奇异或者 Cholesky 分解失败估计值突然跳到无穷大。原因通常是数值误差累积P 矩阵在更新过程中失去了正定性。解决方法是改用 Joseph 形式的协方差更新或者每次更新后强制对称化P (P P) / 2。资源里的kf_linear_demo.m用的是简单形式但在ekf_pendulum_demo.m里用了 Joseph 形式可以对比看差异。4.2 观测矩阵 H 写错导致估计不收敛现象估计值一直偏离真实值或者收敛到一个错误的值。原因往往是 H 矩阵的维度或内容写错了。比如状态是 [位置, 速度]观测只有位置H 应该是 [1, 0]如果写成 [0, 1]滤波器就会把速度观测当成位置来用。排查方法是先检查 H 的维度是否和状态维度匹配再检查每一列对应的是哪个状态量。资源里的 PDF 在每个示例后面都标注了 H 的含义对照着看能避免大部分低级错误。4.3 采样周期与状态转移矩阵不匹配现象滤波器在仿真里跑得好好的换成真实数据就发散。原因通常是 F 矩阵是按固定采样周期 dt 算的但真实数据的采样周期不是 dt。比如 F 里用了 dt0.01但实际数据是 0.02 的间隔状态外推就会偏。解决方法是把 dt 作为参数传进 F 的计算里每次循环根据实际时间戳更新。资源里的kf_linear_demo.m用的是固定 dt但在ekf_pendulum_demo.m里演示了变步长的处理方式。4.4 初始协方差 P0 给得太小导致滤波器“自信过头”现象滤波器刚开始时完全不理会观测估计值一直停在初始值附近过了很久才慢慢跟上。原因是 P0 给得太小滤波器认为自己初始状态很准所以卡尔曼增益 K 很小观测的修正作用被压制。解决方法是在没有先验信息时把 P0 设得大一些比如状态量可能变化范围的平方。资源里的示例通常把 P0 设为单位矩阵乘以一个较大的数就是这个道理。4.5 非线性系统线性化点选错导致 EKF 发散现象EKF 在状态远离线性化点时估计误差很大甚至发散。原因是雅可比矩阵是在预测状态处求的如果预测状态本身偏离真实值很远线性化误差就很大。解决方法是缩短预测步长或者改用 UKF。资源里的ekf_pendulum_demo.m在单摆摆角很大时也会出现这个问题PDF 里建议在这种情况下用迭代 EKF 或者切换 UKF。5. 从能跑到好用几个让滤波效果再上一档的技巧5.1 用新息序列做在线一致性检验滤波器跑起来之后怎么知道它工作正常最直接的办法是看新息序列。新息是观测减去预测观测如果滤波器调得好新息应该是零均值的白噪声。资源里的innovation_test.m给了完整的检验流程先算新息再算归一化新息平方NIS然后和卡方分布的置信区间对比。如果 NIS 超出置信区间说明滤波器对观测的信任度不对需要调 Q 或 R。% 计算 NIS 并画图 nis zeros(1, N); for k 1:N y z(:,k) - H * x_pred(:,k); S H * P_pred(:,:,k) * H R; nis(k) y / S * y; end % 卡方分布的 95% 置信区间自由度等于观测维度 chi2_low chi2inv(0.025, m); chi2_high chi2inv(0.975, m); % 画图看有多少点落在区间外 plot(nis); hold on; yline(chi2_low, r--); yline(chi2_high, r--);这段代码里m是观测维度chi2inv是卡方分布的逆累积分布函数。如果超过 5% 的点落在区间外说明滤波器的一致性有问题。我一般会把这个检验做成在线监控每次跑完数据都看一眼比凭感觉调参靠谱得多。5.2 自适应卡尔曼让 Q 和 R 自己调如果传感器噪声不是固定的比如温度变化导致陀螺仪零偏漂移固定 Q 和 R 就不够用了。资源里提到了自适应卡尔曼的思路代码包里有一个adaptive_kf_demo.m用新息序列的滑动方差来估计 R用状态残差来估计 Q。核心思想是如果新息方差比理论值大说明 R 被低估了调大 R如果状态估计和观测的残差持续偏大说明 Q 被低估了调大 Q。% 自适应 R 的简化实现 window_size 20; for k window_size:N y_window y(:, k-window_size1:k); R_est cov(y_window); % 用新息滑动窗口估计观测噪声 R_est max(R_est, 1e-6); % 防止太小导致数值问题 % 然后用 R_est 替换原来的 R 做更新 end这个方法的参数是窗口大小窗口太小会导致 R 估计抖动太大则跟不上噪声变化。资源里建议窗口大小取 10 到 30 之间具体看采样率和噪声变化速度。注意自适应方法在观测缺失或异常值多的时候容易跑偏最好配合异常值检测一起用。5.3 用平滑器做事后处理卡尔曼滤波是实时估计只用了当前和过去的观测。如果场景允许事后处理比如离线分析飞行日志可以用卡尔曼平滑器把未来的观测也利用起来估计精度会明显提升。资源里的 PDF 在最后一章介绍了 Rauch-Tung-StriebelRTS平滑器代码包里rts_smoother_demo.m演示了怎么在滤波结果的基础上做反向平滑。平滑后的轨迹比滤波更贴近真实值尤其是在观测噪声大的时候。我自己的习惯是实时系统用滤波离线分析用平滑。平滑器的代码结构是在滤波循环结束后再从后往前跑一遍用滤波时保存的预测协方差和后验协方差算平滑增益。资源里的实现把滤波和平滑拆成了两个函数调用起来很清晰。5.4 从 MATLAB 到嵌入式 C 的移植要点最后说一个实际工程里绕不开的问题MATLAB 验证完算法怎么搬到嵌入式平台。资源里的代码是 MATLAB 的但矩阵运算的逻辑可以直接翻译成 C。需要注意几点一是 MATLAB 的矩阵是列优先C 里如果用行优先存储索引要转置二是chol和inv在 C 里需要自己实现或者用现成库资源里没有提供 C 代码但 PDF 附录里给了伪代码三是浮点精度MATLAB 默认 double嵌入式平台可能用 float需要重新验证数值稳定性。我一般会先在 MATLAB 里把算法固定下来用single类型跑一遍看精度损失然后再手写 C。移植完之后用同一组数据在 MATLAB 和 C 里各跑一遍对比输出误差在 1e-6 以内才算通过。这个习惯帮我省了很多调试时间希望也能帮到你。从那以后我每次做算法移植都强制走一遍 MATLAB 和 C 的交叉验证再急也不跳过。本文还有配套的精品资源点击获取