简介这份资源是卡尔曼滤波经典教材《Kalman filtering-theory and practice using MATLAB》的配套资料包面向信号处理、导航制导、控制系统与数据分析方向的学习者和工程师帮助读者在掌握随机过程、最小均方误差估计与动态系统建模等理论的同时借助MATLAB把滤波算法真正跑起来。压缩包共66个文件约4.59MB以61个m脚本为主体辅以pdf原书、mat数据、doc与txt说明文档覆盖从基本卡尔曼滤波到扩展卡尔曼滤波、平方根滤波、UD分解与平滑算法的实现。代码按章节组织涉及状态转移与观测矩阵设置、滤波器初始化、更新迭代及后处理等关键环节并延伸到航天器轨道预测、GNSS/INS组合导航、传感器融合等应用场景。已有499人学习下载适合希望边读理论边动手复现、把滤波方法迁移到实际项目中的读者。1. 卡尔曼滤波为什么一堆带噪声的数据能算出真值第一次看到卡尔曼滤波的公式时多数人的反应是——这堆矩阵递推到底在干什么五个方程翻来覆去协方差矩阵更新来更新去看起来像某种数值玄学。但如果你把一组带噪声的传感器数据丢进去跑一遍出来的估计曲线平滑得几乎贴合真实轨迹就会意识到它背后有一套非常清晰的概率逻辑。这份资源的价值在于它没有停在“公式是什么”的层面而是把卡尔曼滤波从理论推导到 MATLAB 实现完整串了起来。PDF 讲原理配套代码让你能直接跑、直接改、直接看每一步中间变量的变化。对于已经会用kalman函数但说不清“为什么协方差会收敛”“为什么增益会越来越小”的人来说这套材料能把你从调包侠拉到能自己推导、自己实现的位置。卡尔曼滤波的核心问题只有一个如何在系统模型不完全准确、观测数据带有噪声的条件下递推地估计出系统状态的最优值。搞懂了这个“最优”是怎么定义的、递推是怎么做到的后面扩展卡尔曼、无迹卡尔曼都是在这个骨架上做文章。2. 从贝叶斯递推到卡尔曼增益五个方程到底在算什么2.1 状态空间模型先把问题写成矩阵形式卡尔曼滤波不是凭空冒出来的它建立在一个前提上系统可以用线性状态空间模型描述。所谓状态空间就是把系统的“内部状态”和“外部观测”分开建模。状态方程描述状态如何随时间演化观测方程描述状态如何映射到你能测量到的量。% 离散线性系统状态空间模型 % x(k) A * x(k-1) B * u(k) w(k) w ~ N(0, Q) % z(k) H * x(k) v(k) v ~ N(0, R) A [1, 0.1; 0, 1]; % 状态转移矩阵位置速度模型 B [0.005; 0.1]; % 控制输入矩阵 H [1, 0]; % 观测矩阵只测位置 Q diag([0.001, 0.01]); % 过程噪声协方差 R 0.5; % 观测噪声协方差这段代码定义了一个经典的位置-速度模型。A矩阵的含义是下一时刻的位置 当前位置 0.1 × 当前速度下一时刻的速度 当前速度。H矩阵说明你只能观测到位置速度是隐含状态。Q和R分别描述过程噪声和观测噪声的强度它们的比值直接决定了滤波器“信模型”还是“信观测”。这里有一个容易被忽略的设计动机为什么要求噪声是高斯的因为在高斯假设下贝叶斯递推的每一步都保持高斯分布均值和协方差就能完全描述状态的不确定性。一旦噪声非高斯这套闭式递推就不成立了才需要粒子滤波之类的数值方法。卡尔曼滤波的“最优”是在最小均方误差意义下的最优而这个最优性依赖于高斯假设。2.2 预测与更新卡尔曼滤波的两个节拍卡尔曼滤波的递推分两步预测和更新。预测是根据上一步的估计和系统模型推算当前时刻状态的先验估计更新是用当前时刻的观测数据修正先验估计得到后验估计。% 预测步 x_pred A * x_est; % 先验状态估计 P_pred A * P_est * A Q; % 先验误差协方差 % 更新步 K P_pred * H / (H * P_pred * H R); % 卡尔曼增益 x_est x_pred K * (z - H * x_pred); % 后验状态估计 P_est (eye(2) - K * H) * P_pred; % 后验误差协方差预测步的物理含义很直观用系统模型往前推一步同时把过程噪声Q加进不确定性里。更新步是卡尔曼滤波的精髓所在。K是卡尔曼增益它决定了你多大程度上相信观测。当P_pred很大先验不确定性高或R很小观测很准时K趋近于让估计直接跳到观测值反过来K趋近于零估计几乎不变。P_est (I - K*H) * P_pred这个更新公式有一个数值上的坑当P_pred接近奇异时直接这样算可能失去对称正定性。工程上常用 Joseph 形式来替代这一点在配套代码里通常会有体现。2.3 协方差矩阵的演化为什么增益会收敛很多人第一次跑卡尔曼滤波时会注意到一个现象K在几步之后就不再变化了。这不是 bug而是滤波器达到了稳态。当P_pred和P_est收敛到固定值时K自然也就固定了。% 观察卡尔曼增益的收敛过程 N 50; K_history zeros(1, N); P eye(2); for k 1:N P_pred A * P * A Q; K P_pred * H / (H * P_pred * H R); P (eye(2) - K * H) * P_pred; K_history(k) K(1); end plot(K_history);这段代码把增益的收敛过程画出来你会看到它单调递减到一个稳态值。这个稳态值只取决于A、H、Q、R与初始状态和初始协方差无关。这意味着在系统模型和噪声统计特性不变的情况下滤波器会“记住”一个最优的信任分配比例。理解这一点很重要它解释了为什么卡尔曼滤波可以在线运行而计算量可控——稳态之后增益可以离线算好在线只需要做向量乘法和加法。3. MATLAB 实现里的矩阵运算从公式到可运行代码的鸿沟3.1 向量化递推为什么不能逐元素写循环理论公式写起来干净但落到 MATLAB 代码里第一个要面对的问题就是矩阵维度和运算顺序。卡尔曼滤波的五个方程全是矩阵运算如果按标量思维逐元素写循环代码会又长又慢而且容易在转置和求逆上翻车。function [x_est, P_est] kalman_step(x_prev, P_prev, z, A, H, Q, R) % 预测 x_pred A * x_prev; P_pred A * P_prev * A Q; % 更新 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 innovation z - H * x_pred; % 新息 x_est x_pred K * innovation; P_est (eye(size(P_pred)) - K * H) * P_pred; end这个函数把单步卡尔曼滤波封装成可复用的模块。注意K P_pred * H / S用的是右除等价于P_pred * H * inv(S)但数值上更稳定。S是新息协方差它衡量的是观测预测误差的方差。innovation是新息即实际观测与预测观测之间的差。如果系统模型准确、噪声统计正确新息应该是一个零均值白噪声序列。这一点是后面做一致性检验的基础。3.2 过程噪声与观测噪声的调参Q 和 R 怎么定Q和R的设定是卡尔曼滤波工程应用中最考验经验的地方。理论上它们分别是过程噪声和观测噪声的协方差但实际系统中这两个量往往无法直接测量只能通过试凑或自适应方法确定。% 调参实验不同 Q/R 比值对滤波效果的影响 Q_ratio [0.01, 0.1, 1, 10, 100]; for i 1:length(Q_ratio) Q_test Q_ratio(i) * Q; % 运行滤波并记录 RMSE rmse(i) run_filter(Q_test, R); endQ增大意味着你认为系统模型不够可信滤波器会更依赖观测跟踪速度快但噪声抑制弱。R增大则相反滤波器更相信模型输出平滑但可能跟不上真实状态的变化。常见的实现思路是用新息序列做自适应估计如果新息持续偏大说明模型或噪声假设有问题可以据此调整Q或R。配套代码里通常会有这类调参实验的脚本跑一遍比看十遍公式管用。3.3 数值稳定性协方差矩阵为什么不能随便算卡尔曼滤波在理论上很优美但直接按公式实现时浮点运算的舍入误差可能让协方差矩阵失去对称正定性导致滤波器发散。这不是理论缺陷而是数值实现的问题。% 不稳定的写法 P_est (eye(n) - K * H) * P_pred; % 更稳定的 Joseph 形式 P_est (eye(n) - K * H) * P_pred * (eye(n) - K * H) K * R * K;Joseph 形式在代数上等价于标准形式但在数值上能保证P_est始终对称正定。原因是它把K*R*K这一项显式加回去补偿了舍入误差。另一个常用技巧是平方根滤波直接对协方差的 Cholesky 因子做递推从根本上避免正定性丢失。这些细节在理论推导里通常一笔带过但在实际代码里是决定滤波器能不能长时间稳定运行的关键。4. 避坑与排查卡尔曼滤波跑起来不对时先查什么4.1 滤波器发散估计值越来越偏离真实值现象滤波运行一段时间后估计误差不但没有收敛反而越来越大协方差矩阵的对角元素持续增长。原因最常见的原因是系统模型与实际系统不匹配或者Q设得太小导致滤波器过度信任模型。另一个可能原因是数值舍入误差累积导致协方差矩阵失去正定性。解决先检查A和H是否准确描述了系统动态和观测关系。然后适当增大Q让滤波器更依赖观测。如果怀疑数值问题改用 Joseph 形式更新协方差或者直接上平方根滤波。4.2 增益不收敛K 一直在震荡现象卡尔曼增益在每一步之间来回跳动没有趋于稳态的趋势。原因通常是因为Q或R设置得使系统在每一步都有较大的新息滤波器不断在“信模型”和“信观测”之间切换。也可能是观测数据中存在异常值或非高斯噪声。解决检查观测数据是否有离群点必要时加入新息卡方检验做异常值剔除。确认Q和R的物理量纲是否匹配量纲不一致会导致增益计算出现数量级错误。4.3 初始协方差 P0 设为零导致滤波器“僵死”现象滤波器启动后估计值几乎不动完全跟随初始猜测对观测数据几乎没有响应。原因P0设为零意味着你认为初始状态完全准确先验不确定性为零。这导致卡尔曼增益在第一步就趋近于零滤波器不再信任任何观测。解决P0应该反映你对初始状态的真实不确定程度。如果完全不知道初始状态设一个较大的对角矩阵比如P0 100 * eye(n)。宁可设大也不要设小设大了滤波器会快速收敛设小了滤波器会拒绝观测。4.4 观测矩阵 H 维度写反导致结果完全错误现象代码能跑通不报错但滤波结果完全不对甚至维度不匹配的警告被忽略后继续运行。原因MATLAB 的矩阵乘法在维度不匹配时会直接报错但如果H的维度恰好能乘但语义错误比如把1×2写成了2×1代码可能不报错但结果完全错误。解决在函数入口加维度断言比如assert(size(H,2) length(x_prev))。养成习惯每次修改A、H后先跑一遍维度检查再跑数据。4.5 过程噪声 Q 非对角导致协方差矩阵不正定现象运行若干步后 MATLAB 报错“矩阵接近奇异”或“Cholesky 分解失败”。原因Q如果不是半正定矩阵P_pred A*P*A Q可能失去正定性。常见于手动构造Q时填入了不合理的非对角元素。解决确保Q是对称半正定矩阵。最简单的做法是用Q L * L构造其中L是任意矩阵这样Q自动半正定。如果必须手动指定用eig(Q)检查特征值是否非负。5. 从线性到非线性扩展卡尔曼的取舍与这套代码的延伸价值卡尔曼滤波的五个方程建立在线性假设上但真实系统很少是线性的。这份资源在讲完线性卡尔曼之后通常会延伸到扩展卡尔曼滤波EKF。EKF 的核心思路很简单用一阶泰勒展开把非线性函数局部线性化然后套用线性卡尔曼的框架。但这里有一个根本性的取舍——线性化误差。% EKF 的预测步用雅可比矩阵替代线性矩阵 F jacobian(f, x_est); % 状态转移函数的雅可比 x_pred f(x_est); P_pred F * P_est * F Q; % 更新步观测函数的雅可比 H_jac jacobian(h, x_pred); K P_pred * H_jac / (H_jac * P_pred * H_jac R);EKF 的问题在于雅可比矩阵只在当前估计点附近有效如果非线性强或者估计误差大线性化误差会直接破坏滤波器的收敛性。更麻烦的是EKF 需要你手动推导雅可比矩阵对于复杂系统来说推导过程本身就是血泪经验。无迹卡尔曼滤波UKF用 sigma 点采样替代线性化避免了雅可比推导但计算量更大。粒子滤波则完全放弃高斯假设用一组加权粒子表示任意分布代价是计算复杂度随状态维度指数增长。这套 MATLAB 代码的价值在于它把从线性卡尔曼到 EKF 的过渡做成了可运行的对比实验。你可以用同一组数据分别跑线性卡尔曼和 EKF观察线性化误差在什么条件下开始显著影响估计精度。这种“跑出来看到”的理解方式比只看推导公式要扎实得多。我自己的习惯是每次拿到一个新的滤波问题先用线性卡尔曼跑一遍基线记录新息序列的统计特性。如果新息均值明显偏离零或者自相关函数不衰减说明模型有问题这时候再考虑上 EKF 或 UKF。这套代码里的模块化设计让这种对比实验变得很容易——替换A、H为雅可比函数其余框架不动。希望这份材料能帮你把卡尔曼滤波从“调包能跑”变成“知道每一步在干什么、为什么这么干”。本文还有配套的精品资源点击获取