简介基于变分贝叶斯推断的自适应卡尔曼滤波MATLAB实现是一份融合变分推断与卡尔曼滤波技术、面向非线性动态系统参数自学习的算法资源适合具备数学与编程基础的科研人员、工程师及高校相关专业师生在目标追踪、精密导航、自动控制等场景中应用。包内共17个文件以10个m源码文件为主涵盖核心滤波实现、迭代更新与参数估计等模块另有论文docx、说明txt、示意jpeg及备份文件压缩包仅218KB便于快速下载与查阅。目前已有43人学习下载。该资源系统呈现了变分贝叶斯自适应卡尔曼滤波的理论基础与计算步骤通过非线性建模、参数学习策略和自适应调整机制的设计思路配合可直接运行的MATLAB程序读者可对照实际输出深入掌握算法结构并据此改进自身滤波方案的估计精度与鲁棒性能。资源来源于网络分享供学习交流。1. 变分贝叶斯推断的自适应卡尔曼滤波当量测噪声不再是常数基于变分贝叶斯推断的自适应卡尔曼滤波VB-AKF解决一件具体事量测噪声协方差不是常数时标准卡尔曼滤波会逐渐失去判断力。组合导航里 GPS 信号穿过楼群、目标跟踪里传感器被遮挡R 的真实值瞬间放大几十倍滤波器却还攥着标定好的固定 R状态估计越来越偏甚至发散。变分贝叶斯推断把 R 当作隐变量每个时刻用状态与噪声交替更新的方式逼出 R 的近似后验分布让滤波器自己学会何时信量测、何时信模型。本文按建模、MATLAB 实现、参数整定、工程验证的顺序展开代码在 R2023b 上跑通。适合正在做组合导航、目标跟踪被时变噪声折磨过的工程师。2. 从标准卡尔曼滤波到 VB-AKF噪声协方差为什么要在线估计2.1 标准卡尔曼滤波的假设条件恒定噪声是最大的软肋标准卡尔曼滤波的模型框架大家都很熟x_k F x_{k-1} w_k, w_k ~ N(0, Q) y_k H x_k v_k, v_k ~ N(0, R)它隐含了两个硬性前提过程噪声协方差 Q 和量测噪声协方差 R 都已知且恒定。卡尔曼增益的计算式是 K P⁻Hᵀ(H P⁻Hᵀ R)⁻¹增益的大小完全由模型预测的不确定度 P⁻ 和量测噪声 R 之间的比值决定。只要 R 给得准滤波器的状态估计就是最小方差意义下的最优。问题出在 R 一旦偏离真实值。假设真实 R 从 1 跳到 100而滤波器还拿着 R 1 在算增益K 会被算得偏大滤波器对量测的信任程度远超实际应给的水平。量测噪声放大 10 倍之后估计结果直接把这些噪声吃进状态里位置曲线会明显变毛糙。更隐蔽的是 P 矩阵还在持续收缩滤波器表现得越来越“自信”但其实这种自信是虚的。它的误差协方差输出严重低于真实误差水平下游如果拿这个 P 去做故障检测或者组合导航的信息融合会给出完全错误的门限判断。这个现象在组合导航里尤其致命。SINS/GPS 松组合里量测是位置和速度之差GPS 的噪声受卫星几何分布、多径、城市峡谷遮挡影响噪声方差很容易在一个时间段内变化一个数量级。标准卡尔曼滤波没有能力识别这种变化要么把噪声当小量处理导致估计毛糙要么把噪声当大量处理导致动态响应迟钝。2.2 变分贝叶斯推断的建模思路把 R 当作隐变量来逼近变分贝叶斯推断的思路是换一个视角看问题既然 R 不确定那就别把它当成已知常量而是把它和状态 x_k 一起都当作待估计的变量。真正的目标是求联合后验 p(x_k, R_k | y_{1:k})这个联合分布在卡尔曼滤波框架里没有解析解所以变分贝叶斯用了一个近似手段——把它拆成两个独立分布的乘积p(x_k, R_k | y_{1:k}) ≈ q(x_k) q(R_k)q(x_k) 取高斯分布q(R_k) 取逆 Wishart 分布。选逆 Wishart 不是随手抓来的它是高斯似然函数的共轭先验意味着后验形式能保持在同一个分布族里迭代更新的时候只需要更新分布参数不需要重新推导整个概率密度。而且逆 Wishart 分布天然定义在半正定矩阵空间上采样出来的 R 永远是正定的这比后面要对比的 Sage-Husa 方法省了一大堆正定性维护的麻烦。逆 Wishart 分布有两个参数尺度矩阵 V 和自由度 v它的均值是 V / (v - d - 1)其中 d 是量测维数前提是 v d 1。看到这个均值公式就能理解代码里为什么 V 和 v 的更新那么关键——它们直接决定了当前时刻对 R 的点估计是多少。变分推断在这里做的就是最小化 KL 散度对指数族分布来说最后落成的更新规则就是匹配一阶矩和二阶矩也就是把 V 用新息的外积加上状态协方差投影项去刷新v 每次加 1。2.3 VB-AKF 的迭代更新公式与遗忘因子的作用VB-AKF 的每个滤波周期分两段走。先是预测段状态均值和协方差用标准的 F、Q 递推同时噪声先验也要做时间更新这里的核心是遗忘因子 ρV_pred ρ * V_{k-1} v_pred ρ * (v_{k-1} - d - 1) d 1ρ 的取值范围是 0 到 1它控制了噪声先验对历史信息的记忆长度。ρ 越接近 1记忆越长R 的估计越平滑但响应越慢ρ 越小旧信息衰减越快R 能更快跟上突变但估计方差变大。工程上常说“ρ 对应约 1/(1-ρ) 步有效记忆”ρ 0.95 时大约是 20 步ρ 0.98 时就到 50 步了。进入更新段之后要做 N 次变分迭代。每次迭代里先拿当前对 R 的估计去跑一遍标准卡尔曼量测更新得到状态均值和协方差的刷新值再用新息 ε y - Hx 和状态协方差 P 去更新 V 和 v得到新一轮的 R 估计。这两个步骤交替进行N 次之后状态和噪声的估计互相“咬合”在一起逼近联合后验的近似解。迭代次数 N 决定了计算量和估计精度之间的权衡这个参数在本章的参数整定部分会展开讲。3. MATLAB 实现 VB-AKF主循环代码、遗忘因子与迭代次数整定3.1 VB-AKF 完整主循环状态更新与噪声估计交替进行下面这个算例是匀速直线运动模型量测只测位置第 501 步起量测噪声方差从 1 突变到 100。完整的测试脚本如下%% VB-AKF 完整算例匀速直线运动 量测噪声突变 % 在 R2023b 上验证通过只需要基础 MATLAB 环境无额外工具箱 clear; close all; clc; %% 1. 系统模型参数 dt 0.1; F [1 dt; 0 1]; % 状态转移矩阵状态 [位置; 速度] H [1 0]; % 量测矩阵只测位置 dim_x 2; % 状态维数 dim_y 1; % 量测维数 q 0.01; % 过程噪声强度 Q q * [dt^3/3, dt^2/2; dt^2/2, dt]; % 连续白噪声离散化 %% 2. 生成真值与量测 N 1000; x_true zeros(dim_x, N); y zeros(dim_y, N); R_true ones(1, N); R_true(501:end) 100; % 第501步起量测噪声放大100倍 Lq chol(Q, lower); % 用Cholesky分解生成相关过程噪声 x_true(:, 1) [0; 10]; for k 2:N x_true(:, k) F * x_true(:, k-1) Lq * randn(dim_x, 1); end for k 1:N y(:, k) H * x_true(:, k) sqrt(R_true(k)) * randn(dim_y, 1); end %% 3. VB-AKF 参数与初始化 rho 0.96; % 遗忘因子典型取值 0.9~0.98 N_iter 8; % 变分迭代次数 V 1; % 逆Wishart尺度矩阵1维量测时是标量 v dim_y 2; % 自由度必须大于 dim_y1 m [y(1,1); 9]; % 初始状态估计位置用第一帧量测 P diag([10, 10]); % 初始状态协方差不要给太小 x_vb zeros(dim_x, N); R_hat zeros(1, N); %% 4. VB-AKF 主循环 for k 2:N % ---- 4.1 状态预测 ---- m_pred F * m; P_pred F * P * F Q; % ---- 4.2 噪声先验的时间更新遗忘因子衰减 ---- V_pred rho * V; v_pred rho * (v - dim_y - 1) dim_y 1; % ---- 4.3 变分迭代状态与噪声交替更新 ---- m_k m_pred; P_k P_pred; R_k V_pred / (v_pred - dim_y - 1); % 逆Wishart分布均值 for i 1:N_iter % 用当前R估计做标准卡尔曼量测更新 K P_pred * H / (H * P_pred * H R_k); m_k m_pred K * (y(:, k) - H * m_pred); P_k (eye(dim_x) - K * H) * P_pred; % 用当前状态估计刷新噪声分布参数 innov y(:, k) - H * m_k; V V_pred innov * innov H * P_k * H; v v_pred 1; R_k V / (v - dim_y - 1); end % ---- 4.4 递推到下一时刻 ---- m m_k; P P_k; x_vb(:, k) m; R_hat(k) R_k; end这段代码的逻辑分四块。第一块是状态预测和标准卡尔曼滤波完全一样核心参数是 Q它决定了滤波器对模型本身的信任度第二块是噪声先验的时间更新这里 V 和 v 都按遗忘因子 ρ 做衰减v 的公式里那个- dim_y - 1不能省否则自由度会漂移导致逆 Wishart 分布均值公式失效第三块是变分迭代的核心里面交替执行量测更新和噪声参数更新注意这里P_k用的是(eye(dim_x) - K*H) * P_pred这种简化形式数值上不如 Joseph 形式稳定后面避坑章节会专门讲第四块把收敛后的状态和噪声估计存下来递推。3.2 三个必须整定的参数遗忘因子、迭代次数与初始协方差VB-AKF 比标准卡尔曼滤波多出来的参数就三个遗忘因子 ρ、迭代次数 N_iter、逆 Wishart 先验的 V0 和 v0。调这几个参数有点玄学但本质上它们各自对应一个明确的物理含义。参数符号典型取值作用与调节方向遗忘因子ρ0.900.98控制噪声先验的记忆长度ρ 越大 R 估计越平滑但响应越慢噪声突变场景取小值变分迭代次数N_iter510每个时刻状态与噪声的交替更新轮数噪声突变剧烈时取大值自由度初值v0dim_y 2逆 Wishart 分布的形状参数必须大于 dim_y 1否则均值公式不成立尺度矩阵初值V0(v0 - dim_y - 1) * R0R0 取你对起始量测噪声量级的估计决定滤波开始的 R 先验位置初始状态协方差P0按物理量级给给太小滤波器过度自信给太大前几百步震荡明显ρ 是最容易调出问题的一个。调得太接近 1比如 0.995R 的估计会非常平滑但真实噪声突变之后要一两百步才能追上去这段时间里滤波器一直用偏小的 R 跑估计质量下降调得太小比如 0.5R 的估计会被每一帧新息牵着走方差很大严重时滤波直接发散。我做测试的习惯是先用 0.95 跑一遍看 R_hat 曲线的响应速度和毛糙程度再往两个方向微调。N_iter 和 ρ 之间有耦合关系。ρ 比较小的时候噪声先验的方差本身就大每一轮迭代对噪声参数的修正量也大迭代到第 4、5 轮基本就收敛了ρ 接近 1 的时候先验很“硬”需要更多轮迭代才能让噪声参数动起来。所以如果 ρ 取 0.98N_iter 建议给到 10 以上否则每个时刻的噪声更新不充分等效于削弱了自适应能力。3.3 用匀速直线运动模型跑通第一个算例与固定 R 的标准 KF 对比光跑 VB-AKF 看不出它的价值必须拿固定 R 的标准卡尔曼滤波做对照。对照脚本如下%% 5. 标准KF对比R 固定为 1不随真实噪声变化 x_kf zeros(dim_x, N); P_kf diag([10, 10]); m_kf [y(1,1); 9]; R_fix 1; for k 2:N m_pred F * m_kf; P_pred F * P_kf * F Q; K P_pred * H / (H * P_pred * H R_fix); m_kf m_pred K * (y(:, k) - H * m_pred); P_kf (eye(dim_x) - K * H) * P_pred; x_kf(:, k) m_kf; end %% 6. 位置误差对比 rmse_vb sqrt(mean((x_vb(1,:) - x_true(1,:)).^2)); rmse_kf sqrt(mean((x_kf(1,:) - x_true(1,:)).^2)); fprintf(VB-AKF 位置RMSE: %.4f\n, rmse_vb); fprintf(固定R 位置RMSE: %.4f\n, rmse_kf);在这个算例里前 500 步两个滤波器表现接近位置 RMSE 都在 0.3 左右因为真实 R 就是 1固定 R 的滤波器没有吃亏。第 501 步开始分化固定 R 的滤波器位置 RMSE 迅速涨到前段的将近十倍而且 P 矩阵还在继续缩小输出的误差协方差和真实误差严重不符VB-AKF 的 R_hat 大约在 60 到 100 步之内从 1 爬到接近 100之后位置 RMSE 回落到略高于前段的水平。这就是自适应带来的本质差异——它不追求任何单帧做得多准而是保证滤波器在噪声环境变化之后还能维持对自己误差的诚实估计。4. VB-AKF 避坑指南发散、非正定与噪声滞后怎么排查4.1 现象滤波发散估计曲线直接飞出天际原因遗忘因子与迭代次数不匹配这是 VB-AKF 最常见的翻车现场。R_hat 曲线剧烈抖动状态估计跟着一起发散位置误差随时间增大而不是收敛。排查顺序先看 ρ 是不是小于 0.8ρ 太小的时候每一帧新息对 V 和 v 的冲击太大逆 Wishart 分布被单帧噪声主导R 的估计完全失去平滑性再看 N_iterρ 小的时候如果 N_iter 也小噪声参数在每一帧里只迭代一两轮就递推下去误差在时间维度上累积。解决方法是让 ρ 和 N_iter 朝相反方向配合。ρ 取小值追求快速响应的时候 N_iter 至少取 810给噪声参数足够的收敛空间ρ 取大值追求平滑的时候 N_iter 可以降到 56节省计算量。另一个可行方案是给 R_hat 加一个限幅约束在物理合理的区间内比如 GPS 位置噪声方差不可能小于 0.01 m²也不可能大于 10⁴ m²超过就截断。4.2 现象R 估计出现负值或非正定原因矩阵对称性丢失与数值误差如果是标量量测R 不会算出负值因为 V 始终是非负的。但量测维数升到 2 以上V 矩阵更新公式里H * P_k * H如果因为 P_k 不对称而引入非对称误差几轮迭代之后 V 的特征值可能变负R 估计就失去正定性。P_k 不对称的主要来源是(eye(dim_x) - K*H) * P_pred这种简化更新形式在数值上不保对称每一步引入的微小非对称误差会累积。解决方法是换 Joseph 形式的协方差更新或者每次迭代后强制对称% 用Joseph形式替代简化形式数值稳定性更好 I_KH eye(dim_x) - K * H; P_k I_KH * P_pred * I_KH K * R_k * K; % 或者在每一轮结束做一次对称化兜底 P_k (P_k P_k) / 2;注意 Joseph 形式里需要显式用到当前 R_k这部分运算量比简化形式大但换来的是 V 矩阵长期迭代不坏。高维量测场景下我一般两个都做用 Joseph 形式更新再在 V 更新前置一次对称化成本可以接受。4.3 现象噪声突变后估计滞后几百步原因先验记忆太长R_true 从 1 跳到 100R_hat 花了 200 多步才爬上去这段时间滤波器一直用偏小的 R状态估计质量明显下降。滞后来源于两处一是 ρ 太接近 1比如 0.99有效记忆 100 步旧的小噪声信息把新的大噪声信息“稀释”了二是变分迭代里每一轮对 R 的修正量本身有限N_iter 不够的话每个时刻最多动一点点。解决办法是缩短记忆和加大迭代双管齐下。把 ρ 降到 0.930.95 之间同时 N_iter 加到 10。如果还是嫌慢可以在检测到新息能量突变时临时把 ρ 调小一点等 R_hat 稳定后再恢复这是一种简易的变遗忘因子策略。实现上记住ρ 的调节要基于新息序列的滑动方差不要用单帧新息否则会被偶然的大噪声误触发。4.4 现象量测维数升高后单步耗时成倍增长VB-AKF 每次迭代里要做一次矩阵求逆量测维数 d 决定这个求逆的代价。d 1 时毫无压力d 6 时每个时刻 N_iter 次 6×6 求逆如果仿真步长很短、长时间跑累积耗时很明显。另外逆 Wishart 分布的 v 必须大于 d 1维数升高后 v0 和 V0 的初始设置也需要同步调整v0 给得太小会导致均值公式失真。如果量测噪声各维度独立一个常见做法是让 R 保持对角形式把 V 也限制为对角矩阵每次只更新对角线元素。这样一次 d 维矩阵求逆退化成 d 次标量除法计算量从 O(d³) 降到 O(d)对嵌入式平台很友好。代价是放弃了量测噪声通道间相关的估计能力需要根据实际传感器特性权衡。4.5 现象初始 P0 设置不当导致前段滤波震荡明显P0 给得太小滤波器一开始就认定自己的初始状态很准前几十帧的增益被压得很低量测信息进不来状态要很久才“咬住”真实轨迹P0 给得太大前段增益过大每一帧量测噪声都被直接吃进来曲线毛糙。更隐蔽的是初始 P0 还会影响 V 的更新前段 P_k 偏大H * P_k * H这一项给 V 注入的偏大增量会让 R_hat 在启动阶段就偏高后面要花额外时间回落。解决方法是按物理量级给 P0。位置初始不确定度给到量测噪声方差的 510 倍速度给到系统物理可达范围的四分之一到一半别用eye(dim_x)这种拍脑袋的默认值。启动阶段如果 R_hat 波动太大可以加大 N_iter 压制或者给 R_hat 加一阶低通但低通的截止频率要低于噪声本身的快变频率否则就抵消了自适应能力。5. 把 VB-AKF 用到组合导航矩阵量测噪声与蒙特卡洛验证5.1 应用场景改造量测噪声协方差从标量变成矩阵前面算例是 1 维量测R 是个标量。真实组合导航里量测往往是位置三维加速度三维R 是 6×6 矩阵VB-AKF 的公式不需要改动变的只是 V 和 v 的维数与初值。逆 Wishart 分布的均值公式 R V / (v - d - 1) 对矩阵同样成立V 设为 d×d 对称正定矩阵。% 量测维数 dim_y 6 时的参数设置位置速度松组合 dim_y 6; R_nominal diag([1, 1, 1, 0.1, 0.1, 0.1]); % 根据传感器手册给量级 v0 dim_y 2; % 自由度大于 dim_y1 V0 (v0 - dim_y - 1) * R_nominal; % 尺度矩阵保证初始均值等于 R_nominal % 主循环里的噪声更新不需要改 % V V_pred innov * innov H * P_k * H; % v v_pred 1; % R V / (v - dim_y - 1);这里最容易被忽略的是 v0 的量纲含义。v0 直接等于 dim_y 2 意味着先验非常“软”R 几乎完全由量测数据驱动这适合噪声变化剧烈的场景如果想让先验更“硬”一些可以给 v0 更大的值比如 dim_y 10但这会拖慢自适应响应。工程上建议先用软先验跑通再根据 R_hat 的平滑度决定是否收紧。5.2 蒙特卡洛实验RMSE 与 NEES 作为一致性指标单次仿真的 RMSE 说服力有限因为它只反映了一个噪声种子下的行为。标准做法是跑 M 50100 次蒙特卡洛每次换随机种子把所有次数下的结果一起统计。RMSE 反映精度但精度高不等于滤波器健康——真正重要的是滤波器输出的 P 和真实误差是否一致这个用 NEES 检验。% NEES归一化估计误差平方一致性检查 M 50; nees zeros(M, N); for mc 1:M % ... 每次重新生成量测并跑一遍 VB-AKF ... for k 1:N err x_est(:, k) - x_true(:, k); nees(mc, k) err / P_out(:, :, k) * err; % 用滤波器自己输出的 P end end nees_mean mean(nees(:)); % 理想情况下 NEES 等于状态维数 dim_x % 明显大于 dim_x滤波器过度自信P 给小了 % 明显小于 dim_x滤波器过于保守P 给大了NEES 的判定标准是卡方分布对 dim_x 295% 置信区间大约落在 1.5 到 2.6 之间超出这个区间就说明 P 和真实误差不匹配。VB-AKF 的一个额外好处是它在噪声变化后依然能保持 NEES 在合理区间内而固定 R 的标准 KF 在 R 突变后 NEES 会急剧升高因为它输出的 P 还在按旧噪声收缩。5.3 与 Sage-Husa 自适应滤波对比什么时候选 VB-AKF做量测噪声自适应很多人第一个想到的是 Sage-Husa 方法它用新息序列的滑动统计量来估计 R公式是 R ≈ (1/M)Σεεᵀ - HPHᵀ。这个方法实现简单、计算量小但有两个先天缺陷一是滑动窗口 M 的选取很尴尬M 太小噪声估计抖动大M 太大跟不上突变二是减法项 HPHᵀ 一旦大于新息协方差的滑动均值R 就失去正定性需要额外加投影或限幅。对比维度Sage-HusaVB-AKF估计原理新息序列滑动统计变分贝叶斯近似迭代求解记忆控制窗口长度 M遗忘因子 ρ正定性保障可能丢失需额外处理逆 Wishart 结构天然保证对噪声突变的响应与 M 强相关难兼顾ρ 和 N_iter 配合响应更快计算开销低高每时刻 N_iter 次矩阵求逆适合场景噪声慢变、计算资源紧张噪声快变、量测维数中等我的选择标准很简单量测噪声是渐变比如温度漂移、老化而且计算资源紧张的场景Sage-Husa 够用量测噪声会突变城市峡谷、遮挡、传感器切换并且下游还需要可信 P 矩阵的场景选 VB-AKF。后者的迭代机制让状态估计和噪声估计互相印证本质上是一种“诚实估计”——它知道自己噪声变了也会让输出协方差如实反映这一点。6. 提升 VB-AKF 收敛速度的技巧给变分迭代一个“热启动”VB-AKF 默认的迭代初值是用噪声先验的均值V_pred / (v_pred - dim_y - 1)这个值在噪声突变时会离真实 R 很远前几轮迭代全花在“从先验往真实值爬”的路上。常见做法的优化点是如果 R 是慢变的上一时刻收敛出来的 R_hat(k-1) 通常比先验均值离真实值更近直接拿它当本轮迭代起点可以省掉一半以上的迭代轮数。% 热启动慢变噪声场景用上一时刻收敛值做迭代初值 if k 2 R_k V_pred / (v_pred - dim_y - 1); % 首帧用先验均值 else R_k R_hat(k - 1); % 后续帧用上一时刻收敛值 end % 配合提前终止条件进一步省计算量 for i 1:N_iter R_prev R_k; % ... 状态更新、噪声更新 ... if i 2 abs(R_k - R_prev) 1e-4 % 连续两轮R几乎不动就停 break; end end在我自己的算例里这个改动把 N_iter 从 8 降到 4 还能保持同样的 R 估计精度单步耗时可观地下降。注意这个技巧在噪声突变剧烈时不适用——突变后的第一帧R_hat(k-1) 和真实 R 差得很远热启动反而慢这时应该退回先验均值做初值。实现里可以加一个判断当新息的能量超过滑动平均的若干倍时判定发生了突变强制用先验均值重新冷启动。这几年我调滤波器的习惯是任何参数都要能说出它对应的物理含义而不是让它躺在代码里当黑匣子。VB-AKF 的 ρ、N_iter、v0 三个参数各有各的作用边界理解了它们在做什么调起来就不需要靠运气。希望帮到你。本文还有配套的精品资源点击获取