简介：这份MATLAB例程包面向学习卡尔曼滤波与自适应卡尔曼滤波的学生、工程师及科研人员，帮助其在噪声环境下完成动态系统状态估计的建模与仿真。包内共4个文件，以3个m脚本和1份pdf文档为主，脚本分别对应主程序、标准卡尔曼滤波与自适应卡尔曼滤波实现，pdf则提供一种基于自适应卡尔曼的数据滤波方法参考，压缩包约165KB，轻量便于快速上手。已有5382人学习下载，说明其在滤波算法入门与实践中具备一定认可度。读者可借助例程理解预测与更新两步迭代、状态转移与观测矩阵的设定、初始状态估计与协方差初始化，以及通过调整卡尔曼增益和噪声协方差应对系统参数时变、不确定性较大的场景，并可将思路迁移至传感器融合、目标跟踪与导航定位等任务中。
1. 卡尔曼滤波与自适应卡尔曼滤波 MATLAB 例程：从状态估计到参数自整定
做目标跟踪、组合导航或者传感器融合的同行，大概率都经历过这样的场景：IMU 和 GPS 的数据摆在面前，理论推导看着都懂，真到写代码时却不知道过程噪声 Q 和观测噪声 R 该填多少。填小了滤波器发散，填大了跟踪迟钝，调参调到怀疑人生。这份卡尔曼滤波与自适应卡尔曼滤波 MATLAB 例程，就是冲着这个痛点来的——它把标准卡尔曼滤波（KF）和自适应卡尔曼滤波（AKF）的完整实现打包在一起，包含状态预测、协方差更新、卡尔曼增益计算以及噪声协方差自适应调整等核心模块。适合正在做多传感器融合、导航定位、信号去噪的工程师和研究生，拿来就能跑，改改参数就能套到自己的项目里。
2. 标准卡尔曼滤波的 MATLAB 实现骨架
2.1 状态空间模型怎么建
卡尔曼滤波的核心就五条公式，但真正落地时第一步不是写公式，而是把状态空间模型建对。例程里通常采用匀速直线运动（CV）模型作为入门示例，状态向量取位置和速度：
x = [p; v] % 状态向量：位置、速度状态转移矩阵 F 和观测矩阵 H 根据采样周期 T 来确定。这里有个容易翻车的地方：很多人直接把连续时间的模型离散化时忘了 T 的平方项。以 CV 模型为例，正确的离散形式是：
% 状态转移矩阵 F（CV模型，采样周期 T） F = [1, T; 0, 1]; % 观测矩阵 H（假设只观测位置） H = [1, 0]; % 过程噪声协方差 Q（连续白噪声加速度模型） q = 0.1; % 加速度噪声功率谱密度 Q = q * [T^3/3, T^2/2; T^2/2, T]; % 观测噪声协方差 R R = 0.5; % 根据传感器精度设定这段代码里，T是采样周期，q是过程噪声强度，R是观测噪声方差。Q 矩阵的构造方式直接决定了滤波器对模型不确定性的容忍度。我一般会先把q设得偏小，跑一遍看残差是否白噪声化，再逐步调整。如果残差序列出现明显趋势，说明 Q 太小，滤波器跟不上真实动态。
2.2 预测与更新循环的代码落地
模型建好之后，核心就是一个循环：预测 → 计算增益 → 更新。例程里这部分通常封装成一个函数，输入是上一时刻的状态和协方差，输出是当前时刻的估计值。下面是一个典型的实现：
function [x_est, P_est] = kf_update(x_pred, P_pred, z, H, R) % 计算卡尔曼增益 S = H * P_pred * H' + R; % 新息协方差 K = P_pred * H' / S; % 卡尔曼增益 % 状态更新 innovation = z - H * x_pred; % 新息（观测残差） x_est = x_pred + K * innovation; % 协方差更新（Joseph形式，数值更稳定） I = eye(size(K, 1)); P_est = (I - K * H) * P_pred * (I - K * H)' + K * R * K'; end这里有几个参数需要说明。z是当前时刻的观测值，H是观测矩阵，R是观测噪声协方差。卡尔曼增益K的物理含义是：观测越可靠（R 小），增益越大，滤波器越信任观测；预测越可靠（P_pred 小），增益越小，滤波器越信任模型。协方差更新我用了 Joseph 形式而不是简单的(I-KH)P，因为前者在数值上更稳定，长时间递推不容易出现协方差矩阵非正定的问题——这个坑我在做长时间导航解算时踩过，后来养成习惯一律用 Joseph 形式。
预测步的代码同样关键：
% 预测步 x_pred = F * x_est; % 状态预测 P_pred = F * P_est * F' + Q; % 协方差预测F是状态转移矩阵，Q是过程噪声协方差。预测步的本质是把上一时刻的估计值通过模型外推到当前时刻，同时把不确定性也一并传播过去。如果Q设为零，协方差会越来越小，滤波器最终完全不信任观测，这就是所谓的“滤波器僵化”。
2.3 初始化的三个关键参数
很多人跑例程时直接跳过初始化，结果第一帧就发散。初始化的核心是三个量：初始状态x0、初始协方差P0、以及前面提到的Q和R。
% 初始化 x0 = [0; 0]; % 初始状态：位置0，速度0 P0 = diag([100, 100]); % 初始协方差：对初始状态很不确定P0设大一点没关系，滤波器会在前几帧快速收敛。但如果P0设得过大而R又很小，第一帧的卡尔曼增益会接近 1，滤波器几乎完全信任观测，可能导致状态跳变。常见做法是P0取观测噪声方差的 10 到 100 倍，给滤波器一个“快速学习”的起点。
3. 自适应卡尔曼滤波：让 Q 和 R 自己说话
3.1 为什么要自适应
标准 KF 最大的软肋是 Q 和 R 固定。实际系统里，传感器噪声可能随温度漂移，目标机动可能突然加剧，固定参数根本应付不来。自适应卡尔曼滤波（AKF）的思路就是让滤波器根据新息序列的统计特性在线调整 Q 或 R。例程里通常实现的是基于新息协方差匹配的 AKF，核心逻辑是：如果实际新息协方差大于理论值，说明模型不准，该增大 Q；反之则减小 Q。
3.2 新息协方差匹配的实现
理论新息协方差是S = H*P_pred*H' + R，而实际新息协方差可以用滑动窗口内的新息样本估计：
% 滑动窗口估计实际新息协方差 window_size = 20; innovation_buffer = zeros(1, window_size); % ... 每个时刻存入新息 ... if count >= window_size C_hat = (innovation_buffer * innovation_buffer') / window_size; % 调整 R：如果实际新息协方差大于理论值，增大 R R_adaptive = C_hat - H * P_pred * H'; R_adaptive = max(R_adaptive, R_min); % 防止 R 过小 end这段代码里，innovation_buffer存最近window_size个新息值，C_hat是实际新息协方差的估计。R_adaptive的计算逻辑是：实际新息协方差减去理论预测部分，剩下的就是观测噪声的贡献。R_min是下限保护，防止 R 被压到零导致滤波器发散。窗口大小一般取 10 到 50，太小估计不准，太大跟踪不上噪声变化。
3.3 自适应策略的两种路线
例程里通常包含两种自适应路线：一种是调 R，一种是调 Q。调 R 适合观测噪声变化明显的场景，比如视觉跟踪中光照突变导致测量精度下降；调 Q 适合目标机动性变化的场景，比如无人机从匀速巡航切换到急转弯。
% 路线一：自适应 R（观测噪声变化） R_adaptive = C_hat - H * P_pred * H'; R_adaptive = max(R_adaptive, R_min); % 路线二：自适应 Q（过程噪声变化） Q_adaptive = K * C_hat * K'; % 用增益反推等效过程噪声 Q_adaptive = min(Q_adaptive, Q_max);调 Q 的公式Q_adaptive = K * C_hat * K'来自新息与过程噪声的关系推导，直观理解是：卡尔曼增益越大，说明滤波器越依赖观测，此时如果新息协方差大，就归因于过程噪声增大。Q_max是上限保护，防止 Q 过大导致滤波器完全信任观测而忽略模型。
注意：自适应滤波不是万能的。如果新息序列本身不是零均值白噪声，协方差匹配的前提就不成立，这时候强行自适应反而会让滤波器震荡。跑例程时建议先画出新息序列的自相关函数，确认白噪声特性后再开自适应。
4. 避坑与排查：那些让滤波器发散的操作
4.1 协方差矩阵非正定
现象：跑着跑着 MATLAB 报错Cholesky decomposition failed或者协方差矩阵出现负特征值。
原因：协方差更新用了简单的(I-KH)P形式，长时间递推累积舍入误差；或者 Q 矩阵构造时参数为负。
解决：改用 Joseph 形式更新协方差，如 2.2 节代码所示；同时检查 Q 矩阵的构造，确保q > 0、T > 0。如果已经发散了，可以在每步更新后强制对称化：P = (P + P')/2。
4.2 滤波器“僵化”不跟踪
现象：观测值明显变化，但滤波输出几乎不动，新息序列持续偏大。
原因：Q 设得太小，协方差 P 收敛到极小值，卡尔曼增益趋近于零，滤波器完全不信任观测。
解决：增大 Q，或者给 P 设一个下限。我一般会在每步预测后检查trace(P)，如果小于某个阈值就手动注入一点过程噪声。另一个办法是定期重置 P 的对角线元素，但会牺牲平滑性。
4.3 自适应 R 导致震荡
现象：开启自适应后，滤波输出出现周期性抖动，R 的估计值来回跳。
原因：滑动窗口太小，新息协方差估计噪声大；或者 R_min 设得太小，R 被压到接近零后滤波器过度信任观测。
解决：增大窗口到 30 以上；给 R_adaptive 加低通滤波，比如R_adaptive = 0.9*R_old + 0.1*R_new；R_min 取观测噪声标称值的 0.1 倍左右。
4.4 观测矩阵维度不匹配
现象：MATLAB 报维度错误，Matrix dimensions must agree。
原因：H 矩阵的列数必须等于状态维数，行数等于观测维数。比如状态是 4 维（位置+速度各两轴），观测只有位置两维，H 应该是 2×4 而不是 4×4。
解决：写代码前先把状态向量和观测向量的维度在纸上列清楚，H 矩阵按[观测维数 × 状态维数]构造。例程里一般会有注释标明维度，改模型时务必同步改 H。
4.5 采样周期 T 与数据时间戳不一致
现象：跟踪滞后或超前，滤波输出与真实轨迹有明显相位差。
原因：F 和 Q 里的 T 用的是标称采样周期，但实际数据的时间戳间隔不均匀，比如传感器丢帧导致某两帧间隔变大。
解决：不要用固定 T，改为每步根据实际时间戳计算T_actual = t(k) - t(k-1)，动态构造 F 和 Q。这个改动很小，但对非均匀采样场景效果立竿见影。
5. 进阶技巧：从例程到工程的最后一公里
例程跑通只是起点，真正用到工程里还有几件事要做。第一件是参数标定：拿一段已知真值的数据，用网格搜索或者优化算法找 Q 和 R 的最优组合。我一般用新息序列的归一化一致性检验——如果滤波器参数正确，归一化新息平方（NIS）应该服从卡方分布。MATLAB 里可以这样验证：
% NIS 检验 NIS = innovation' / S * innovation; % 如果 NIS 的均值远大于观测维数，说明 R 偏小或 Q 偏大第二件是数值稳定性：长时间运行时，协方差矩阵的对称性和正定性会逐渐退化。除了 Joseph 形式，还可以用平方根滤波（SRKF）或者 UD 分解，例程里如果没带这些，可以自己加。第三件是多模型切换：如果系统存在多种工作模式，单一 AKF 可能不够，可以考虑交互式多模型（IMM）配合自适应，但复杂度会上去。
最后说一个我自己的习惯：每次拿到新的传感器数据，先跑一遍标准 KF，把新息序列画出来，看均值和自相关。如果新息均值明显偏离零，说明观测有系统偏差，这时候先做标定而不是急着开自适应。从那以后我每次调滤波器都强制走一遍新息白噪声检验，省下了大量反复调参的时间。希望帮到你。
本文还有配套的精品资源，点击获取
