简介这是一份卡尔曼滤波经典教材《Kalman filtering-theory and practice using MATLAB第3版》的PDF及配套MATLAB代码资源包面向信号处理、导航控制、传感器融合等方向的学习者与工程师用以从理论推导到编程实践系统掌握卡尔曼滤波。压缩包共66个文件以61个MATLAB脚本.m为主辅以原书PDF、数据文件.mat、说明文档.txt和Word说明.doc整体大小仅4.59MB按Chapter_1至Chapter_9分章组织并带有README等引导文件便于对照理论章节逐例运行。已有498人学习下载。内容包括随机过程建模、最小均方误差估计、动态系统模型与滤波器设计等核心理论代码覆盖基本卡尔曼滤波、扩展卡尔曼、无迹卡尔曼、平方根滤波等变体以及惯导组合与GNSS融合等算例。读者既可借助PDF夯实基础又能通过MATLAB脚本观察滤波收敛过程、修改初始参数并将状态转移与观测更新等模块迁移至实际项目。1. Kalman filtering—theory and practice using MATLAB这不是一本能跳着读的代码书很多第一次拿到《Kalman filtering: theory and practice using MATLAB》PDF的人都会把注意力放在最后的代码清单上恨不得复制粘贴就能让滤波器跑起来。但真正把代码跑通只是一瞬间的事后面几个星期的调试你会一直跟状态方程、观测矩阵和噪声协方差较劲。这本书的价值恰恰不在代码而在它用MATLAB把卡尔曼滤波的理论推演和工程实现串成了一条可实践路径。适合正在做传感器融合、目标跟踪、导航解算的开发者也适合被“卡尔曼滤波是个黑匣子”卡住的人。它能解决的核心问题只有一个让你在调Q和R时不再靠猜。2. 先把卡尔曼滤波的五个公式在MATLAB里摆对状态空间模型与代码骨架2.1 从线性高斯模型出发为什么这本书先讲状态空间而非直接讲滤波器卡尔曼滤波不是凭空冒出来的五个公式。它的前提是两条假设系统状态演化是线性的噪声是高斯白噪声。在这两个前提下状态的后验概率全程保持高斯分布因此只需要维护均值和协方差两个量。这个前提决定了你使用滤波器之前的第一件事不是写代码而是把物理过程写成状态空间模型。状态空间模型可以拆成两部分。状态方程描述“上一时刻状态如何变成这一时刻”观测方程描述“当前状态如何被传感器测量到”。用线性代数写就是x_k F x_{k-1} G u wz_k H x_k v其中F是状态转移矩阵H是量测矩阵G是输入矩阵u是外部输入w和v分别是过程噪声和量测噪声互不相关协方差矩阵对应Q和R。很多开发者跳过这一节直接看滤波递推公式结果就是把F/H填错导致滤波器跑出“自以为很准的错结果”。这本书里大量篇幅放在如何从物理模型写F和H。最常用的是恒定速度模型状态量取位置和速度x [pos; vel]在采样间隔dt不变的前提下F [1 dt; 0 1]。观测如果只报位置H [1 0]。这个模型看起来简单但它决定了你对“目标会不会加速”这件事的默认态度。匀速模型默认加速量为零有加速度时只能靠过程噪声Q吸收这也是后面所有调参矛盾的根源。为什么不能跳过“高斯假设”因为卡尔曼滤波的递推公式本质上是贝叶斯滤波在线性高斯情形下的闭式解。书里先推导这个闭式解再给MATLAB实现就是为了防止你看到公式就晕、看到代码就抄。理解了这一点你才知道如果系统有强非线性比如目标做转弯运动仅改Q是治标不治本应该考虑扩展卡尔曼滤波或无迹卡尔曼滤波。但那是后面的进阶内容初学阶段先把线性模型跑稳比什么都重要。2.2 最小可跑的MATLAB卡尔曼滤波代码预测与更新两个步骤一旦模型确定滤波的递推循环只有两步预测和更新。预测用F把状态外推一步同时把协方差也外推一步不确定性变大更新用观测z修正状态不确定性变小。下面是最小骨架我把两个步骤封装成一个函数方便复用。function [x, P, K] kf_step(x, P, z, F, H, Q, R) % 单步卡尔曼滤波预测 更新 % x: 状态向量例如 [位置; 速度] % P: 状态协方差矩阵 % z: 当前时刻量测向量 % F: 状态转移矩阵 % H: 量测矩阵 % Q: 过程噪声协方差 % R: 量测噪声协方差 % 返回值更新后的x, P和卡尔曼增益K可用于调试 % 1. 预测 x_pred F * x; % 状态预测这里省略外部输入项 P_pred F * P * F Q; % 协方差预测F*P*F保持对称 % 2. 新息及新息协方差 y z - H * x_pred; % 新息实际测量与预测测量的差 S H * P_pred * H R; % 新息协方差矩阵 % 3. 卡尔曼增益 K P_pred * H / S; % 用 / 而不是 inv(S)数值更稳定 % 4. 更新 x x_pred K * y; % 后验状态 预测 增益 * 新息 P (eye(size(P)) - K * H) * P_pred; % 后验协方差 end这段代码的逻辑很清楚新息y是观测进入滤波器的入口如果y大说明预测和观测偏离大K会相应变大状态向观测靠拢如果R很大导致S大K变小状态更依赖预测。P更新那一行在数值上不是最稳的直接用于长序列可能出问题我在后面的避坑章节会再给更稳的Joseph形式。配套主脚本可以这样写生成一个匀速运动目标加噪声观测再用卡尔曼滤波还原位置。% 匀速运动目标的卡尔曼滤波示例 clear; clc; close all; rng(1); % 固定随机种子便于复现 dt 0.1; % 采样时间单位秒 N 200; % 采样点数 F [1 dt; 0 1]; % 状态转移矩阵位置和速度 H [1 0]; % 量测矩阵只能测位置 Q [0.01 0; 0 0.01]; % 过程噪声单位时间内速度扰动方差 R 0.25; % 量测噪声方差对应标准差0.5 x [0; 5]; % 初始状态 [位置; 速度] P eye(2) * 10; % 初始协方差给大表示初始状态不确定 % 真实位置 x_true (0:N-1) * 5 * dt; z x_true sqrt(R) * randn(1, N); % 带噪量测 x_est zeros(2, N); for k 1:N [x, P, K] kf_step(x, P, z(k), F, H, Q, R); x_est(:, k) x; end % 画图对比 plot(z, r.); hold on; plot(x_true, g); plot(x_est(1, :), b); legend(量测, 真值, 卡尔曼滤波);主脚本里几个初值值得说明初始状态x用了真实速度5实际工程中不知道真实速度时可以给一个大概值P给大就能让滤波器在几步内收敛。dt越小F中速度对位置的影响越小滤波器对瞬时速度变化更敏感。Q和R的比例关系比绝对大小更重要通常先固定R再调Q这样你调参时变量少一个。2.3 参数说明Q、R、P0该怎么给初值初值直接影响滤波器的收敛速度。常见做法是P0给得比较大让滤波器在前几步相信观测R来自传感器标称精度比如毫米级雷达给R1e-6是数量级参考Q最难给因为没有直接传感器能测过程噪声通常靠实验试凑。参数含义给太小的后果给太大的后果常见初始策略Q过程噪声协方差滤波器过于相信模型观测更新弱存在滞后过分跟随观测噪声曲线粗糙先给R同数量级或更小然后逐步增大R量测噪声协方差滤波增益变大容易把噪声当真值滤波响应慢收敛变慢用传感器静态数据方差估算P0初始协方差收敛慢前期估计严重依赖初始状态前期波动大给比状态量平方值大一个量级的值这张表不是让你直接抄而是解释为什么书里每个例子都会给一组看起来“随手填”的数字。实际上这些参数背后与采样时间相关。比如Q里的量纲是状态量平方/秒如果你把采样时间从0.1秒改成1秒同一个物理系统的最优Q数值应该不一样。书里的章节代码通常固定dt你复制到自己的场景时第一件事就是把dt和Q、R一起检查不然就是给自己埋雷。我见过最容易被忽略的是P0。新手为了让P看起来“合理”把P0设成[0.1 0; 0 0.1]结果滤波器前几十步几乎不更新因为初始协方差太小K被压得极低。实际工程中P0应该表达“你对初始状态有多不确定”。静态启动时位置初值可能偏几米P0位置项至少给10速度项给1。如果K一直接近0说明滤波器已经几乎关闭对观测的响应如果K接近1说明它只信任单点观测不信任模型。这两种状态都不健康调参时先看K曲线比直接看输出曲线更直观。3. 把书中代码跑通PDF配套源码的目录结构与运行顺序3.1 常见的代码文件构成主脚本、函数、数据生成一本以“theory and practice”为名的书配套代码不会只有一个大脚本。常见的组织方式是按章节目录划分每一章下面有主脚本、被调用的子函数和一个负责生成模拟数据的脚本。你拿到PDF和书中代码后最忌讳的是用“运行”按钮把整个目录里所有脚本一次性跑完因为很多脚本之间有依赖顺序错了直接报undefined。我一般会先扫一眼文件列表找文件名里带“main”、“demo”、“example”字样的脚本它们通常是入口。然后看该脚本头部是否调用其他函数。很多脚本顶部会用addpath把子目录加进来如果你的MATLAB当前路径不在代码根目录就会报错。建议先cd到根目录再运行入口脚本。如果书里的例子是连续章节演进的比如第3章先建立模型第4章再用你可能会看到公共函数被放在一个公共目录下。这种结构下不要把自己新写的同名函数放在当前目录因为MATLAB路径顺序会优先选当前目录里的函数导致调用错版本。这个问题很隐蔽属于“看起来没问题结果总是对不上号”的一类浪费的时间远超你的预期。3.2 复现一个书中例子目标跟踪的最小完整流程假设我们要复现书里最经典的目标跟踪例子雷达每隔固定周期测量目标距离我们估计目标的位置和速度。这个例子的价值在于测量数据是仿真的你可以方便地比较滤波输出和真实轨迹。% 目标跟踪最小完整流程 clear; clc; close all; rng(42); % 固定随机种子不然每次轨迹不一样无法和书上图对照 dt 0.5; % 雷达采样周期 0.5 秒 N 150; % 总共150个采样点 v 12; % 目标匀速运动速度真实值 % 真实轨迹位置从0开始沿线运动 x_true v * dt * (0:N-1); % 观测距离噪声 z x_true 2 * randn(1, N); % 状态x[位置; 速度] F [1 dt; 0 1]; H [1 0]; % 过程噪声加速度扰动体现在速度上所以Q(2,2)稍大 Q [0.05 0; 0 0.2]; R 4; % 对应观测噪声标准差2取方差4 % 初始状态用第一帧观测速度用典型目标速度12 x [z(1); v]; P diag([100, 1]); % 位置不确定100m^2速度不确定1(m/s)^2 x_log zeros(2, N); for k 1:N [x, P] kf_step(x, P, z(k), F, H, Q, R); x_log(:, k) x; end % 画图 t dt * (0:N-1); plot(t, x_true, k-, LineWidth, 1.5); hold on; plot(t, z, r.); plot(t, x_log(1, :), b-); legend(真值, 观测, 滤波输出, Location, best);这段代码里有一个容易被忽略的设计初始状态里速度直接用了真实值12。这不是偷懒而是因为滤波初值对收敛速度的影响可以通过P0吸收先把P0设大前三步估计就会快速靠向观测。如果你把速度初始值设成0P0速度项保持较小那么前几十步位置估计会有一个明显的爬坡过程这正好可以观察P0的作用。参数说明R4来自观测噪声的标准差2开方后是方差。如果你分不清为什么观测噪声用方差4而不是标准差2说明R的度量需要重新理解卡尔曼滤波公式里高斯噪声用的是方差。Q我给对角线值分别对应位置过程和速度过程的位置扰动。实际过程噪声在不同维度间可能有耦合那时需要非对角阵但工程中多数先忽略耦合项跑出问题再回头查。3.3 遇到运行报错时先检查这三处把代码复制到自己项目里第一次运行有三类常见报错值得记下来。第一类是Matrix dimensions must agree。多半是F的维度与状态向量不匹配。比如你状态里放了三个量F却只写了2x2。解决方法是输出size(F), size(x)看两侧维度是否一致。第二类是Error using inv。有些初版代码会写K P_pred * H * inv(S)在S接近奇异时数值上很痛苦改成K P_pred * H / S即可。A / B等价于A * inv(B)但内部用高斯消元更稳定。第三类滤波曲线明显不对但不是报错。先看随机种子是否固定。书里的仿真例子如果不固定随机种子每次生成的观测噪声不同结果曲线对不上是正常的。如果种子固定后还是对不上再检查是否把z和x_true顺序弄混。观测数据是带噪的真值是不带噪的很多脚本里两者交替出现容易张冠李戴。这类问题不是语法错误而是数据使用错误报错信息不会提示你只能靠画图对比发现。4. 卡尔曼滤波 MATLAB 实战避坑5 个让结果发散或失真的常见问题4.1 现象状态估计震荡发散Q和R调不动有一次我在处理某目标跟踪模拟时位置估计曲线像锯齿跑几十步误差反而越来越大。Q和R无论怎么调都没有改善。后来一项项排查才发现观测矩阵H写错了我把H写成[0 1]以为雷达测的是速度实际上测距仪输出的是距离。传感器名字误导了我模型也跟着错。现象背后的原因不是Q/R不给力而是模型定义错误。卡尔曼滤波会把错误的观测矩阵当成真理然后“努力”拟合一个错误的相关关系。解决方法是先核对模型再调参数。核对完H之后我还做了残差均值检查新息y z - H*x_pred如果均值明显偏离零说明观测模型和实际不符。这个检查方法我在第5章还会展开。这条也是血泪经验凡是滤波器表现诡异先怀疑F和H再怀疑Q/R。很多人上来就调噪声参数等于在一个错模型上做优化白费力气。书上把F/H的推导放在前面不是排版习惯而是在强调建模优先级。4.2 现象滤波结果滞后位置总是慢半拍目标突然减速时估计轨迹还在按原来速度走明显落后。这个现象在匀速模型下非常常见。标准卡尔曼滤波只能处理线性高斯系统匀速模型默认速度不变目标减速就成了一种“模型误差”只能靠过程噪声Q吸收。解决方式有两种。第一种是把Q(2,2)调大让速度不确定性变大滤波器会更愿意接受观测数据。第二种是把状态扩展到加速度项从二维状态变成三维状态模型本身就允许加速度变化% 二阶模型位置、速度、加速度 dt 0.1; F [1 dt dt^2/2; 0 1 dt; 0 0 1]; H [1 0 0]; Q diag([0.1, 0.5, 1.0]); % 加速度过程噪声稍大这个模型能让滤波器更快跟踪机动目标代价是状态维度增加Q的维度也从2x2变成3x3调参难度明显上升。如果目标机动不频繁优先调大Q(2,2)不要一上来就加状态维度。否则滤波器可能对观测噪声更敏感输出曲线变得毛糙得不偿失。这本书在后面章节引入机动模型也是这个思路模型复杂度要和目标动态匹配。4.3 现象协方差矩阵变得不对称或非正定运行很长时间后P矩阵对角线出现负值或者P(1,2)和P(2,1)不相等然后滤波开始发散。原因在于递推中P (I - K*H)*P_pred的直接形式在计算机浮点运算下会积累不对称误差尤其是当系统状态维度高、运行步数长时这个问题会被放大。解决办法是改用Joseph形式。这个形式来自理论推导虽然看起来多算两个矩阵乘法但数值稳定性好很多% 更稳定的协方差更新Joseph形式 I eye(size(P)); P (I - K*H) * P_pred * (I - K*H) K * R * K;如果你懒得记Joseph形式也可以用P (P P) / 2做对称化但那只是表面修复无法解决非正定问题。遇到协方差阵奇怪的情况先检查每一步的P是否对称如果非对称问题基本出在更新公式。书中介绍Joseph形式时把它放到理论推导的末尾很多读者当作补充知识跳过实战中它才是防翻车的关键。4.4 现象代码在PDF里和提供版本不一致结果对不上很多人会照着PDF上的公式手写代码结果和配套代码运行出来的轨迹完全不同。甚至PDF里的符号在MATLAB代码里找不到对应变量。这类书出过多个版次代码更新过而PDF可能是旧版本变量名、状态顺序都可能变化。解决方法是以可运行的代码为准把PDF当作理论背景参考不要逐句对照。先运行配套代码确认输出图正常再回到PDF读对应的推导过程。如果你需要拿PDF里的公式作为自己代码的依据一定要检查状态向量里是先位置后速度还是先速度后位置。这个顺序差会导致H、F完全不一样。这类问题不应当算书的问题而是版本管理问题。我建议把PDF和代码放同一个目录并在代码文件名后加备注比如“chapter3_old_PDF”“chapter3_code_v2”。否则过几个月你自己都分不清哪个对应哪个。固定随机种子也很关键如果代码里没有rng你在不同机器上跑出的观测不同和书上的图对不上是正常的不是bug。4.5 现象离散时间与连续时间混淆模型矩阵算错状态方程写成连续时间微分方程然后直接套进离散卡尔曼滤波公式结果发散。这是理论到实践最常见的落差。卡尔曼滤波的递推公式是离散形式的F必须是采样间隔内的状态转移矩阵而不是连续系统矩阵A。模拟连续系统时正确的做法是用矩阵指数做离散化% 连续系统矩阵A - 离散状态转移矩阵F_d dt 0.1; A [0 1; 0 0]; % 连续模型位置导数速度速度导数0 F_d expm(A * dt); % expm是矩阵指数如果dt特别小用F_d I A*dt做线性近似也可以。但dt变大时近似误差会累积最终的滤波器会认为系统演化得比实际快或慢。另一种情况是采样时间不固定有些工程场景下传感器帧率抖动此时必须在每一帧重算F_d而很多代码里F是常量这也是结果失真的原因。书里离散化部分容易读得很快因为公式不复杂但真正出问题时你会发现错误的F会让整个卡尔曼增益的数值都错掉。建议在上机前用一次简单的状态预测测试给一个已知状态看F*x是否和物理直觉一致。这个步骤能筛掉一大半模型错误。5. 把书里的例子改造成自己的滤波器验证方法、调参习惯与一个实用技巧5.1 用蒙特卡洛模拟代替拍脑袋调参为了评估滤波器性能不能只看一次运行曲线。卡尔曼滤波输出和随机噪声强相关单次平滑并不代表整体性能。常见做法是固定模型和参数随机生成M次观测序列运行M次滤波计算每个时间点的均方根误差RMSE。M 200; rmse zeros(1, N); for rep 1:M z x_true sqrt(R) * randn(1, N); x x0; P P0; for k 1:N [x, P] kf_step(x, P, z(k), F, H, Q, R); rmse(k) rmse(k) (x(1) - x_true(k)).^2; end end rmse sqrt(rmse / M); plot(rmse);这个统计指标可以帮助判断Q/R比例比肉眼调曲线更可靠。注意每次循环要重置x和P否则上一次的结果会影响下一次。蒙特卡洛次数M在200左右稳定性就比较好了再大除了耗时对结论的影响很有限。如果你发现不同种子下误差方差极大说明滤波器对噪声敏感需要重点调Q/R而不是调初值。5.2 用残差序列验证滤波器的健康度残差即新息y z - H*x_pred。如果模型和噪声协方差设置正确残差的均值应接近零标准差应接近sqrt(diag(S))。保存残差并统计能直接看到模型有没有偏差。innov zeros(1, N); for k 1:N [x, P, K] kf_step(x, P, z(k), F, H, Q, R); % 这里用更新后的预测值计算残差更精确的做法是在函数内部输出 innov(k) z(k) - H * x; end innov_mean mean(innov); innov_std std(innov); if abs(innov_mean) 3 * innov_std / sqrt(N) disp(残差均值显著非零模型或观测存在偏差); end如果残差序列出现趋势性或自相关说明模型无法解释观测中的某种动态比如未建模加速度或者观测存在常值偏置。这个方法书里可能以理论形式出现但实战中这是最直接的“带病诊断”。某次模拟项目里P曲线看起来完全正常但残差均值一直偏大最后发现传感器存在常值偏置。这个偏置在输出曲线上很难发现只有残差统计能把它揪出来。5.3 一个实用技巧把单步更新封装成函数便于切换模型如果你同时在比较匀速模型、匀加速模型、甚至不同采样时间的模型不要复制三份滤波循环。把之前的kf_step用结构体打包模型参数循环代码保持不动。function [x, P, innov] kf_step_model(x, P, z, model) % model包含F,H,Q,R [x, P, ~, innov] kf_step(x, P, z, ... model.F, model.H, model.Q, model.R); end切换模型时只需要换model结构体循环代码不变能减少复制粘贴引入的错误。另外我习惯在调试函数内部临时输出K矩阵观察K是否收敛到某个稳定值。如果K一直跳说明Q/R比值有问题如果K很快趋于零说明滤波器已经不再相信观测。这两条都指向参数设置而不是算法本身。最后一次项目调试我把P更新写成直接形式跑了半小时后P矩阵非对称结果发散。从那以后我每次都保留残差记录和P对称性检查。这本书让我学会的不仅是公式更是“先怀疑模型再怀疑参数”的排查顺序。希望帮到你。本文还有配套的精品资源点击获取