基于变分贝叶斯推断的自适应卡尔曼滤波：量测噪声在线估计与工程实践

简介：基于变分贝叶斯推断的自适应卡尔曼滤波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_y+1 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。调这几个参数有点玄学，但本质上它们各自对应一个明确的物理含义。

ρ 是最容易调出问题的一个。调得太接近 1，比如 0.995，R 的估计会非常平滑，但真实噪声突变之后要一两百步才能追上去，这段时间里滤波器一直用偏小的 R 跑，估计质量下降；调得太小，比如 0.5，R 的估计会被每一帧新息牵着走，方差很大，严重时滤波直接发散。我做测试的习惯是先用 0.95 跑一遍，看 R_hat 曲线的响应速度和毛糙程度，再往两个方向微调。

N_iter 和 ρ 之间有耦合关系。ρ 比较小的时候，噪声先验的方差本身就大，每一轮迭代对噪声参数的修正量也大，迭代到第 4、5 轮基本就收敛了；ρ 接近 1 的时候，先验很“硬”，需要更多轮迭代才能让噪声参数动起来。所以如果 ρ 取 0.98，N_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 至少取 8～10，给噪声参数足够的收敛空间；ρ 取大值（追求平滑）的时候 N_iter 可以降到 5～6，节省计算量。另一个可行方案是给 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 跳到 100，R_hat 花了 200 多步才爬上去，这段时间滤波器一直用偏小的 R，状态估计质量明显下降。滞后来源于两处：一是 ρ 太接近 1，比如 0.99，有效记忆 100 步，旧的小噪声信息把新的大噪声信息“稀释”了；二是变分迭代里每一轮对 R 的修正量本身有限，N_iter 不够的话每个时刻最多动一点点。

解决办法是缩短记忆和加大迭代双管齐下。把 ρ 降到 0.93～0.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。位置初始不确定度给到量测噪声方差的 5～10 倍，速度给到系统物理可达范围的四分之一到一半，别用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_y+1 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 = 50～100 次蒙特卡洛，每次换随机种子，把所有次数下的结果一起统计。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 = 2，95% 置信区间大约落在 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-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 三个参数各有各的作用边界，理解了它们在做什么，调起来就不需要靠运气。希望帮到你。

本文还有配套的精品资源，点击获取