企业官网建设流程全解析

做组合导航的朋友应该都有过这种经历：手里同时拿到了 IMU、GNSS 和轮速计的原始数据，第一反应是先写一个大而全的集中式 EKF，把三路数据全塞进去。集中式 EKF 原理简单，调试也不麻烦，跑通仿真也快，可一旦放到工程项目里就会开始别扭：GNSS 偶尔跳一个野值，整个位置估计跟着抖一下；里程计某个时间戳没回来，速度通道瞬间被 IMU 积分噪声接管。联邦卡尔曼滤波（融合反馈模式）就是为这类场景出现的分布式滤波结构，它把 IMU 作为公共参考系统，GNSS 和里程计分别挂在不同的子滤波器上，最后由主滤波器把子滤波器的结果融合起来。

这篇文章给出一套可以直接复制到 MATLAB 空脚本运行的联邦卡尔曼滤波仿真例程，融合 IMU、GNSS、里程计三路数据，包含完整轨迹生成、传感器仿真、滤波器主循环和对比曲线。代码分成段落、注释齐全，运行后会得到轨迹对比图、误差曲线和偏差估计曲线，适合做组合导航入门、多传感器融合课程实验，以及智能小车、AGV、车辆导航项目预研。整个过程只要 MATLAB R2016b 以上版本，不需要额外工具箱，粘贴保存就能跑。

1. 为什么要写这个仿真例程

1.1 三个传感器，各有各的脾气

IMU、GNSS、里程计这三类传感器在导航里的角色很不一样，谁都不能完全替代谁。

IMU 输出频率高，一般到 100Hz、200Hz 甚至更高，短时间内的相对增量非常平滑，但它是积分型传感器。加速度计测的是比力，陀螺仪测的是角速度，你一旦对它做积分，偏差和噪声就会一起积分进状态里。几分钟不校正，位置可能漂出十几米甚至几十米。这是零偏和随机游走造成的，不是 IMU 本身不行，而是单靠惯性导航维持不了长时间精度。

GNSS 是绝对传感器，直接给经纬度或平面坐标，长期无累积误差。问题在于更新频率低，通常 1Hz 到 10Hz，而且容易受卫星期望、多路径、遮挡影响。城市峡谷里突然跳一个 5 米甚至 10 米的野值，并不是稀奇事。如果把 GNSS 直接当位置真值用，车辆轨迹会非常“跳”。

里程计输出的是车辆前向速度，频率一般 10Hz 到 50Hz，短时稳定、不受电磁干扰，这是它最大的优点。缺点是只能约束纵向速度，不能绝对定位，也不能纠正横向速度误差，单靠里程计时间长了还是会偏。所以这三类传感器天然互补：IMU 提供高频运动增量，GNSS 把绝对位置拉回来，里程计把速度通道约束住。难点不在于“要不要融合”，而在于怎么融合才不容易被某个传感器的故障带偏。

1.2 联邦滤波在这套组合里的优势

如果只做一次仿真，集中式 EKF 确实最省事。把所有量测方程写进一个大滤波器里，状态向量十几维，量测矩阵写长一些，跑出来的精度通常也相当好。但工程上我更推荐联邦滤波结构，理由很实际：

第一，传感器故障隔离。子滤波器之间是并联关系，GNSS 子滤波器被野值污染，主滤波器融合时可以通过协方差和残差做卡方检测，把它权重压下去；里程计子滤波器异常，也不会立刻污染 GNSS 通道。集中式滤波器里量测更新是全局共享的，某个传感器出问题，所有状态都会跟着遭殃。

第二，方便增减传感器。今天接 GNSS 和里程计，明天想加一个视觉里程计，集中式滤波器得重新推导整条量测矩阵。联邦滤波只要新增一个子滤波器，主滤波器融合时多一项 P 逆相加就行，结构扩展性非常友好。

第三，工程上可以做成“即插即用”节点。每个子滤波器相当于独立模块，IMU 预测由公共模块负责，GNSS 更新、里程计更新各自封装。这种模块化在团队协作和维护阶段特别有价值。

所以这个例程没有用最“简单”的集中式方案，而是选择联邦卡尔曼滤波，而且专门采用融合反馈模式。后面我会解释融合反馈模式和普通无反馈模式的差别，代码里也预留了开关，你可以一键切换对比。

2. 联邦卡尔曼滤波（融合反馈模式）核心原理

2.1 结构拆解：主滤波器加两个子滤波器

联邦卡尔曼滤波的结构可以理解成“中央调度加独立分支”。整个系统有一个主滤波器，两个子滤波器，底层公共参考是 IMU 的机械编排或运动学递推。

在这个例程里，状态向量取 8 维：

• 位置：东向 x、北向 y，单位 m；
• 速度：x 向速度 vx、y 向速度 vy，单位 m/s；
• 航向角 yaw，单位 rad；
• 陀螺零偏 gyro_bias，单位 rad/s；
• 加速度计零偏 acc_bias_x、acc_bias_y，单位 m/s²。

子滤波器 1 接收 GNSS 位置量测，子滤波器 2 接收里程计速度量测。两者都用 IMU 数据进行预测。主滤波器不直接接传感器量测，它只负责把两个子滤波器的估计结果按照协方差加权融合成一个全局估计。

这种结构的好处是局部性。子滤波器 1 只关心 GNSS 能不能压住位置漂移，子滤波器 2 只关心里程计能不能压住速度漂移。两个子滤波器互不干扰，即使其中一个因为传感器故障发散，主滤波器依然能利用另一个子滤波器的信息维持基本定位。

2.2 信息分配系数怎么理解

联邦滤波里有一个核心概念叫信息分配系数 β。一个简单的配置是 β1 + β2 = 1，两个子滤波器各分一半。也可以根据实际传感器可靠性调整，例如 GNSS 在开阔环境下很可靠，就可以给 β1 分配 0.7，里程计分到 0.3。

信息分配在实现上的体现主要有两处：

• 子滤波器预测时，过程噪声协方差放大为 Q / β_i；
• 融合反馈重置时，子滤波器协方差设置为 P_g / β_i。

为什么要这么做？因为子滤波器本质上只拥有全局信息的一部分。如果把全部信息都复制给每个子滤波器，融合时就会重复计算，相当于同一个信息被用了两遍，协方差会被严重低估。信息分配就是给子滤波器“分资粮”，让它们各自只负责一部分信息量，融合时才不会虚高。

在这个例程里，β1 和 β2 都取 0.5，代表 GNSS 和里程计的信任程度基本持平。如果你想模拟“GNSS 链路更可靠”的情况，改成 0.7 和 0.3，就能明显看到位置误差曲线的形态发生变化。

2.3 主滤波器的融合公式和反馈重置

两个子滤波器各自输出一组状态估计 x1、协方差 P1 和状态估计 x2、协方差 P2。主滤波器融合时使用信息加权：

P_g = (P1⁻¹ + P2⁻¹)⁻¹

x_g = P_g (P1⁻¹ x1 + P2⁻¹ x2)

这个公式相当于在信息域做加权平均，谁的协方差小，谁的信息矩阵大，谁的权重就高。

融合反馈模式下，得到全局估计 x_g、P_g 后，再把它回灌到子滤波器：

x1 = x_g，P1 = P_g / β1

x2 = x_g，P2 = P_g / β2

这样做的效果是：子滤波器每拆解完一步传感器更新，又被全局结果“拉”回到同一参考点，下一轮预测和更新就从更准确的起点出发。整个系统不会因为子滤波器各自漂移而越跑越远。

代码里use_feedback这个布尔变量就是干这个的。设成 true 是融合反馈模式，设成 false 就是无反馈模式。两种模式对比着看，你会更清楚反馈在联邦滤波中的作用。

2.4 融合反馈和无反馈在工程上的取舍

无反馈模式下，两个子滤波器各自从初始状态出发独立递推，除非遇到自己的量测，否则不会被另一个滤波器影响。好处是故障隔离最彻底，一个子滤波器坏了，另一个完全不知道；坏处是子滤波器长时间得不到外部修正，误差会慢慢积累，全局融合结果的长期稳定性会差一些。

融合反馈模式下，主滤波器每步融合完就把全局结果写回子滤波器。好处是子滤波器始终被全局信息校正，整体精度更高，特别是在长时间运行场景下；代价是故障隔离能力有所下降。一旦某个传感器的坏值被融合进主滤波器，反馈会把它同时传染给另一个子滤波器。

工程上怎么选？我的经验是：传感器质量稳定、链路可靠的场景用融合反馈模式追求精度；传感器质量参差不齐、经常发生野值和断连的场景，要么用无反馈模式配合故障检测，要么在反馈之前增加严格的新息卡方检验，把坏量测先挡住。

3. 完整 MATLAB 仿真例程

3.1 代码结构一览

整套代码按照“先造真值、再生产传感器数据、最后跑滤波器”的顺序组织。这样做的好处是可以精确知道每一条航迹的真值，方便后期算误差和画对比图。

第一部分是参数定义，包含仿真时长、IMU 采样率、传感器噪声标准差、信息分配系数。第二部分是真实轨迹生成，车辆先直线加速、再左转、右转、最后减速，基本覆盖了陆地车辆常见的运动状态。第三部分根据真实轨迹反向生成 IMU 数据、GNSS 位置观测和里程计速度观测。第四部分是滤波器主体，包括两个子滤波器和一个集中式对比滤波器。最后一部分是结果统计与绘图。

代码中同时实现了一个普通的集中式 EKF，作为联邦滤波的对比基准。集中式滤波器同时使用 GNSS 和里程计量测，理论上是信息最充分的参考，联邦滤波的结果和它做对比，能清楚看到分布式结构带来的精度差异。

3.2 仿真数据生成：先有真值，再反推观测

构建仿真例程最容易犯的错误是“边生成边滤波”。那样做的问题在于，滤波器估计状态会影响观测生成，误差分析就不干净了。这里采用的标准做法是先离线生成一组真值轨迹，然后基于真值模拟传感器输出。

真实轨迹生成时，车辆运动按体坐标系描述。体坐标系前向轴与车头方向一致，横向轴与侧向一致。加速度计测量的是体坐标系下的前向加速度和横向加速度，其中横向加速度包含了转弯时的向心加速度，等于速度乘以转向角速度。这一部分理解了，后面 IMU 数据生成才不会乱。

GPS 观测每隔 1 秒生成一组位置，加入 1 米标准差的高斯噪声。里程计观测每隔 0.1 秒生成一个前向速度，加入 0.2 米每秒标准差的高斯噪声。IMU 数据按 100Hz 生成，并额外叠加了零偏和白噪声。这样一组仿真数据下来，传感器频率差异、噪声差异、系统偏差全都有了，比纯理想数据有参考价值得多。

3.3 主循环：预测、量测更新、融合反馈

主循环是整段代码的灵魂。每来一帧 IMU 数据，先对两个子滤波器和集中式滤波器做预测；然后判断当前时刻是否有 GNSS 观测，有就给子滤波器 1 和集中式滤波器做位置更新；再判断是否有里程计观测，有就给子滤波器 2 和集中式滤波器做速度更新；最后做联邦融合，按需反馈重置子滤波器。

预测函数使用数值雅可比矩阵。原因我后面会详细说，这里先记住：数值雅可比虽然比解析式多花一点计算时间，但能最大程度避免公式推导错误。量测更新部分，GNSS 是线性位置更新，H 矩阵只有两个非零元素；里程计是非线性的速度模值更新，H 矩阵需要根据当前速度方向实时计算。

3.4 完整脚本（可直接复制运行）

下面是完整的 MATLAB 脚本，复制到空脚本文件中，保存后直接运行即可。建议保持默认仿真参数先跑一遍，再手动修改use_feedback、beta1、sigma_gnss等参数做对比实验。

%% 联邦卡尔曼滤波(融合反馈模式)融合IMU/GNSS/里程计 仿真例程 % 适用版本：MATLAB R2016b及以上 % 使用方法：复制到空脚本中，保存后直接运行 clear; clc; close all; rng(2024); %% 1. 参数定义 dt = 0.01; % IMU采样间隔 100Hz T_end = 100; % 仿真时长(s) t = 0:dt:T_end; % 时间轴 N = numel(t); yaw0 = 0.4; % 初始航向角 rad v0 = 5.0; % 初始速度 m/s % IMU误差/噪声参数 gb_true = 0.01; % 陀螺零偏 rad/s ab_true = [0.05; -0.05]; % 加速度计零偏 m/s^2 sigma_acc = 0.2; % 加速度计白噪声标准差 m/s^2 sigma_gyro = deg2rad(0.3); % 陀螺白噪声标准差 rad/s % 量测噪声参数 sigma_gnss = 1.0; % GNSS位置噪声标准差 m sigma_odom = 0.2; % 里程计速度噪声标准差 m/s % 联邦滤波信息分配系数 beta1 = 0.5; % 子滤波器1: IMU+GNSS beta2 = 0.5; % 子滤波器2: IMU+里程计 use_feedback = true; % true=融合反馈模式, false=无反馈模式 %% 2. 生成真实运动轨迹 px_true = zeros(1,N); py_true = zeros(1,N); vx_true = zeros(1,N); vy_true = zeros(1,N); yaw_true = zeros(1,N); v_scalar = zeros(1,N); a_forward_profile = zeros(1,N); yaw_rate_true = zeros(1,N); px_true(1) = 0; py_true(1) = 0; vx_true(1) = v0*cos(yaw0); vy_true(1) = v0*sin(yaw0); yaw_true(1) = yaw0; v_scalar(1) = v0; for k = 1:N-1 tt = k*dt; if tt < 20 a_forward = 0.2; yr = 0; % 直线加速 elseif tt < 50 a_forward = 0; yr = 0.1; % 左转 elseif tt < 80 a_forward = 0; yr = -0.1; % 右转 else a_forward = -0.2; yr = 0; % 直线减速 end a_forward_profile(k) = a_forward; yaw_rate_true(k) = yr; v_scalar(k+1) = v_scalar(k) + a_forward*dt; yaw_true(k+1) = yaw_true(k) + yr*dt; a_lat = v_scalar(k) * yr; % 体坐标系横向向心加速度 awx = cos(yaw_true(k))*a_forward - sin(yaw_true(k))*a_lat; awy = sin(yaw_true(k))*a_forward + cos(yaw_true(k))*a_lat; vx_true(k+1) = vx_true(k) + awx*dt; vy_true(k+1) = vy_true(k) + awy*dt; px_true(k+1) = px_true(k) + vx_true(k)*dt + 0.5*awx*dt^2; py_true(k+1) = py_true(k) + vy_true(k)*dt + 0.5*awy*dt^2; end a_forward_profile(N) = a_forward_profile(N-1); yaw_rate_true(N) = yaw_rate_true(N-1); %% 3. 生成IMU/GNSS/里程计测量值 imu_acc_body = zeros(2,N); imu_yawrate = zeros(1,N); gnss_count = 0; odom_count = 0; gnss_meas = zeros(2, floor(N/100)); gnss_idx = zeros(1, floor(N/100)); odom_meas = zeros(1, floor(N/10)); odom_idx = zeros(1, floor(N/10)); for k = 1:N a_body = [a_forward_profile(k); v_scalar(k)*yaw_rate_true(k)]; imu_acc_body(:,k) = a_body + ab_true + sigma_acc*randn(2,1); imu_yawrate(k) = yaw_rate_true(k) + gb_true + sigma_gyro*randn; if mod(k,100) == 0 gnss_count = gnss_count + 1; gnss_idx(gnss_count) = k; gnss_meas(:,gnss_count) = [px_true(k); py_true(k)] + sigma_gnss*randn(2,1); end if mod(k,10) == 0 odom_count = odom_count + 1; odom_idx(odom_count) = k; odom_meas(odom_count) = v_scalar(k) + sigma_odom*randn; end end %% 4. 滤波器初始化 x0 = zeros(8,1); x0(1) = px_true(1) + 2; % 故意给初始位置误差 x0(2) = py_true(1) - 2; x0(3) = vx_true(1); x0(4) = vy_true(1); x0(5) = yaw_true(1); x0(6) = 0; % 陀螺零偏初始估计 x0(7) = 0; % 加速度计零偏初始估计 x0(8) = 0; P0 = diag([2, 2, 0.5, 0.5, deg2rad(2), deg2rad(0.2), 0.1, 0.1].^2); x1 = x0; P1 = P0; x2 = x0; P2 = P0; xc = x0; Pc = P0; % 过程噪声协方差 sigma_a = 0.2; % 加速度白噪声 sigma_w = deg2rad(0.3); % 角速度白噪声 sigma_gb_rw = 1e-4; % 陀螺零偏随机游走 sigma_ab_rw = 1e-4; % 加速度计零偏随机游走 Q = diag([... (0.5*sigma_a*dt^2)^2, ... (0.5*sigma_a*dt^2)^2, ... (sigma_a*dt)^2, ... (sigma_a*dt)^2, ... (sigma_w*dt)^2, ... (sigma_gb_rw*sqrt(dt))^2, ... (sigma_ab_rw*sqrt(dt))^2, ... (sigma_ab_rw*sqrt(dt))^2]); R_gnss = sigma_gnss^2 * eye(2); R_odom = sigma_odom^2; %% 5. 主滤波循环 px_fed = zeros(1,N); py_fed = zeros(1,N); vx_fed = zeros(1,N); vy_fed = zeros(1,N); yaw_fed = zeros(1,N); gb_fed = zeros(1,N); abx_fed = zeros(1,N); aby_fed = zeros(1,N); px_cen = zeros(1,N); py_cen = zeros(1,N); vx_cen = zeros(1,N); vy_cen = zeros(1,N); yaw_cen = zeros(1,N); px_fed(1) = x0(1); py_fed(1) = x0(2); vx_fed(1) = x0(3); vy_fed(1) = x0(4); yaw_fed(1) = x0(5); px_cen(1) = x0(1); py_cen(1) = x0(2); vx_cen(1) = x0(3); vy_cen(1) = x0(4); yaw_cen(1) = x0(5); gnss_ptr = 1; odom_ptr = 1; for k = 1:N-1 u = [imu_acc_body(1,k); imu_acc_body(2,k); imu_yawrate(k)]; % 预测：两个子滤波器 + 集中式滤波器 [x1, P1] = ekf_predict(x1, P1, u, dt, Q/beta1); [x2, P2] = ekf_predict(x2, P2, u, dt, Q/beta2); [xc, Pc] = ekf_predict(xc, Pc, u, dt, Q); % GNSS更新：子滤波器1 + 集中式 if gnss_ptr <= gnss_count && gnss_idx(gnss_ptr) == k z = gnss_meas(:, gnss_ptr); [x1, P1] = ekf_update_gnss(x1, P1, z, R_gnss); [xc, Pc] = ekf_update_gnss(xc, Pc, z, R_gnss); gnss_ptr = gnss_ptr + 1; end % 里程计更新：子滤波器2 + 集中式 if odom_ptr <= odom_count && odom_idx(odom_ptr) == k z = odom_meas(odom_ptr); [x2, P2] = ekf_update_odom(x2, P2, z, R_odom); [xc, Pc] = ekf_update_odom(xc, Pc, z, R_odom); odom_ptr = odom_ptr + 1; end % 联邦融合 invP1 = inv(P1); invP2 = inv(P2); Pg = inv(invP1 + invP2); xg = Pg * (invP1*x1 + invP2*x2); xg(5) = wrapToPi_(xg(5)); if use_feedback % 融合反馈模式：全局解回灌给两个子滤波器 x1 = xg; P1 = Pg/beta1; x2 = xg; P2 = Pg/beta2; end % 记录联邦滤波结果 px_fed(k+1) = xg(1); py_fed(k+1) = xg(2); vx_fed(k+1) = xg(3); vy_fed(k+1) = xg(4); yaw_fed(k+1) = xg(5); gb_fed(k+1) = xg(6); abx_fed(k+1) = xg(7); aby_fed(k+1) = xg(8); % 记录集中式滤波结果 xc(5) = wrapToPi_(xc(5)); px_cen(k+1) = xc(1); py_cen(k+1) = xc(2); vx_cen(k+1) = xc(3); vy_cen(k+1) = xc(4); yaw_cen(k+1) = xc(5); end %% 6. 误差统计与绘图 pos_err_fed = sqrt((px_fed - px_true).^2 + (py_fed - py_true).^2); pos_err_cen = sqrt((px_cen - px_true).^2 + (py_cen - py_true).^2); rms_pos_fed = sqrt(mean(pos_err_fed.^2)); rms_pos_cen = sqrt(mean(pos_err_cen.^2)); fprintf('===== 仿真结果 =====\n'); fprintf('联邦卡尔曼滤波(融合反馈) 位置RMSE: %.3f m\n', rms_pos_fed); fprintf('集中式EKF 位置RMSE: %.3f m\n', rms_pos_cen); figure('Name','轨迹对比'); plot(px_true, py_true, 'k-', 'LineWidth', 1.5); hold on; plot(gnss_meas(1,:), gnss_meas(2,:), 'g.', 'MarkerSize', 4); plot(px_cen, py_cen, 'b--', 'LineWidth', 1.2); plot(px_fed, py_fed, 'r-.', 'LineWidth', 1.5); legend('真值','GNSS观测','集中式EKF','联邦滤波(融合反馈)','Location','best'); axis equal; grid on; xlabel('东向位置 (m)'); ylabel('北向位置 (m)'); title('联邦卡尔曼滤波融合IMU/GNSS/里程计 轨迹对比'); figure('Name','位置误差'); plot(t, pos_err_fed, 'r-', 'LineWidth', 1.5); hold on; plot(t, pos_err_cen, 'b--', 'LineWidth', 1.2); legend('联邦滤波(融合反馈)','集中式EKF'); xlabel('时间 (s)'); ylabel('位置误差 (m)'); title('位置误差曲线对比'); grid on; yaw_err_fed = wrapToPi_(yaw_fed - yaw_true); yaw_err_cen = wrapToPi_(yaw_cen - yaw_true); figure('Name','航向误差与速度误差'); subplot(2,1,1); plot(t, rad2deg(yaw_err_fed), 'r-', 'LineWidth', 1.2); hold on; plot(t, rad2deg(yaw_err_cen), 'b--', 'LineWidth', 1.2); legend('联邦滤波(融合反馈)','集中式EKF'); ylabel('航向误差 (deg)'); grid on; title('航向误差曲线'); vel_err_fed = sqrt((vx_fed - vx_true).^2 + (vy_fed - vy_true).^2); vel_err_cen = sqrt((vx_cen - vx_true).^2 + (vy_cen - vy_true).^2); subplot(2,1,2); plot(t, vel_err_fed, 'r-', 'LineWidth', 1.2); hold on; plot(t, vel_err_cen, 'b--', 'LineWidth', 1.2); legend('联邦滤波(融合反馈)','集中式EKF'); xlabel('时间 (s)'); ylabel('速度误差 (m/s)'); grid on; title('速度误差曲线'); figure('Name','偏差估计'); subplot(3,1,1); plot(t, gb_fed, 'r-', 'LineWidth', 1.2); hold on; plot(t, gb_true*ones(1,N), 'k--'); xlabel('时间 (s)'); ylabel('陀螺零偏 (rad/s)'); grid on; legend('估计值','真值'); title('陀螺零偏估计'); subplot(3,1,2); plot(t, abx_fed, 'r-', 'LineWidth', 1.2); hold on; plot(t, ab_true(1)*ones(1,N), 'k--'); xlabel('时间 (s)'); ylabel('acc零偏x (m/s^2)'); grid on; legend('估计值','真值'); title('加速度计零偏x估计'); subplot(3,1,3); plot(t, aby_fed, 'r-', 'LineWidth', 1.2); hold on; plot(t, ab_true(2)*ones(1,N), 'k--'); xlabel('时间 (s)'); ylabel('acc零偏y (m/s^2)'); grid on; legend('估计值','真值'); title('加速度计零偏y估计'); %% 局部函数 function xn = state_transition(x, u, dt) % 状态转移函数：使用IMU测量作为控制输入 yaw = x(5); a_body = [u(1) - x(7); u(2) - x(8)]; R = [cos(yaw) -sin(yaw); sin(yaw) cos(yaw)]; a_world = R * a_body; w_true = u(3) - x(6); xn = x; xn(1) = x(1) + x(3)*dt + 0.5*a_world(1)*dt^2; xn(2) = x(2) + x(4)*dt + 0.5*a_world(2)*dt^2; xn(3) = x(3) + a_world(1)*dt; xn(4) = x(4) + a_world(2)*dt; xn(5) = x(5) + w_true*dt; % 零偏状态保持原值，靠过程噪声驱动 end function F = numerical_jacobian(f, x, u, dt) % 数值雅可比矩阵：中心差分 n = numel(x); F = zeros(n, n); h = 1e-6; for i = 1:n xp = x; xp(i) = x(i) + h; xm = x; xm(i) = x(i) - h; fp = f(xp, u, dt); fm = f(xm, u, dt); F(:, i) = (fp - fm) / (2*h); end end function [x, P] = ekf_predict(x, P, u, dt, Q) % EKF预测 f = @state_transition; F = numerical_jacobian(f, x, u, dt); x = f(x, u, dt); x(5) = wrapToPi_(x(5)); P = F * P * F' + Q; end function [x, P] = ekf_update_gnss(x, P, z, R) % GNSS位置量测更新 H = zeros(2, 8); H(1,1) = 1; H(2,2) = 1; y = z - H*x; S = H*P*H' + R; K = P*H'/S; x = x + K*y; P = (eye(8) - K*H)*P; P = 0.5*(P + P'); end function [x, P] = ekf_update_odom(x, P, z, R) % 里程计速度量测更新 vx = x(3); vy = x(4); v = sqrt(vx^2 + vy^2); if v < 1e-6 v = 1e-6; end H = zeros(1, 8); H(3) = vx/v; H(4) = vy/v; h = v; y = z - h; S = H*P*H' + R; K = P*H'/S; x = x + K*y; P = (eye(8) - K*H)*P; P = 0.5*(P + P'); end function y = wrapToPi_(y) % 角度归一化到 [-pi, pi) y = mod(y + pi, 2*pi) - pi; end

4. 关键参数与实现细节解读

4.1 为什么用数值雅可比矩阵而不是解析公式

EKF 预测时需要用状态转移矩阵 F 把协方差递推过去。这个例程的状态转移函数里，航向角要影响加速度从体坐标系到导航坐标系的旋转，所以 F 矩阵不是简单常系数，而是和 yaw、速度、零偏都相关的时变矩阵。推导解析雅可比不是不行，但非常容易在某一行突然写错一个负号，检查起来又费时间。

代码里直接用中心差分法算数值雅可比。核心逻辑是对每个状态维度分别加一个小步长和减一个小步长，然后代入状态转移函数求差分。步长取 1e-6，对米、米每秒、弧度这些单位都足够小，不会带来明显截断误差，也不容易受浮点噪声影响。

数值雅可比在仿真阶段完全够用。虽然每个 IMU 时刻要多算 16 次状态转移，但在 100Hz、100 秒的例程里，总时间也就几秒到十几秒。实际工程如果对实时性要求极高，可以先用这套仿真验证算法结构，再在部署阶段把关键雅可比矩阵解析化，两个阶段可以分开处理。

4.2 Q 矩阵里的 dt 和 sqrt(dt) 是怎么来的

过程噪声协方差 Q 是卡尔曼滤波最容易拍脑袋的地方。我见过很多初学者直接给 Q 设一个对角常数，比如水平位置噪声 0.1、速度噪声 0.01，然后滤波器要么收敛很慢，要么干脆发散。正确的做法是把传感器白噪声和随机游走分开处理。

加速度计白噪声量为 sigma_a，单位是 m/s²。在一个 IMU 周期 dt 内，它对速度的影响是 sigma_a * dt，对位置的影响是 0.5 * sigma_a * dt²，所以 Q 里对应速度项是 (sigma_a * dt)²，对应位置项是 (0.5 * sigma_a * dt²)²。陀螺白噪声对航向角的影响是 sigma_w * dt，所以航向过程噪声项是 (sigma_w * dt)²。

零偏一般不按白噪声建模，而是按随机游走建模。随机游走每一步的方差增量正比于时间间隔，也就是 sigma_rw² * dt，所以协方差对角项写 (sigma_rw * sqrt(dt))²。这就是为什么代码里出现了一堆 sqrt(dt)。

这里有一个容易被忽略的量级问题。dt 只有 0.01 秒，位置过程噪声大约是 10⁻⁵ 量级，速度过程噪声是 10⁻³ 量级。如果你把这些项强行设成 0.1 甚至 1，系统会认为 IMU 输入完全不可信，滤波器会过度依赖低频量测，位置会在两次量测之间明显抖动。反过来设成 0，滤波器又会对 IMU 过于自信，最终被零偏带偏。

4.3 β 取 0.5/0.5 的平衡逻辑

信息分配系数不是随便拍的。β1=0.5、β2=0.5 的意思是 GNSS 和里程计在当前例程里的信息量贡献大致相当，最终给主滤波器提供的权重一样大。

但要注意，这里的“一样大”不代表实际精度贡献一样大。GNSS 是绝对位置，每 1 秒一个点，噪声 1 米；里程计是相对速度，每 0.1 秒一个点，噪声 0.2 米每秒。两者的量纲不同，没法直接比。β 分配的是“信息预算”，而不是量测数量。如果你希望系统更信任 GNSS，可以把 β1 提高，比如 0.7，同时把 β2 降到 0.3。这样子滤波器 2 的过程噪声会被放得更大，它对全局解的权重自然下降。

实际调试时，我的做法是先等权重跑一版，看两个子滤波器各自的新息序列和协方差。新息总是偏大的那一路，说明真实噪声比模型设置的大，应该降低它的信息分配权重，或者把对应 R 调大。调整 β 和调整 R 在效果上很相似，但 β 影响的是预测阶段的信息量，R 影响的是量测更新阶段的信任度，两者结合使用会更灵活。

4.4 反馈开关到底怎么影响估计

代码里use_feedback只用一行 if 控制是否执行协方差回灌。很多人第一次跑完，只盯着位置误差曲线，觉得反馈模式和无反馈模式差距不大，就容易忽略它在子滤波器层面的意义。

开启反馈时，每步融合后子滤波器 1 和子滤波器 2 的状态都被拉到全局值，下一轮预测从同一个起点开始。这样两个子滤波器的状态差异不会积累，协方差也始终维持在合理范围。关闭反馈时，子滤波器各自积累误差，尤其是 GNSS 更新不够频繁的子滤波器 2，速度误差会在两次量测之间慢慢增长，融合结果主要靠协方差加权来平衡。

如果你想做传感器故障注入实验，建议先跑无反馈模式。无反馈模式下可以人为在某个时间段去掉 GNSS 观测，观察子滤波器 1 是否继续漂移，以及主滤波器融合结果被拖累多少。开关反馈模式，故障影响会被反馈回灌体现得更快，也更难定位是哪个传感器出了问题。

5. 运行中常见问题与排查技巧

5.1 位置误差曲线发散，越跑越远

如果跑出来误差不是收敛而是持续增长，第一优先检查过程噪声 Q 是否设置过小。滤波器对 IMU 过于自信时，GNSS 更新的修正作用会被压得很小，位置误差就会一路漂移。可以把 Q 整体放大一个数量级，看误差曲线是否回到稳定波动状态。

第二个常见原因是初始协方差 P0 和真实初始误差不匹配。例程里故意给初始位置加了 2 米误差，如果 P0 对应位置项设得太小，滤波器会认为初始位置很准，之后改正的速度就会很慢。正确做法是 P0 的每个对角项大致反映你对初始状态的置信区间，宁可稍微放大，也不要让滤波器“自以为是”。

第三个原因是状态转移函数里的 IMU 零偏符号搞反。这个例程中陀螺零偏的定义是估计的yaw_rate = 测量值 - 零偏，加速度计零偏的定义是真实比力 = 测量值 - 零偏。如果你之前习惯写成加号，请特别注意。

5.2 航向角跳变导致估计突然崩掉

角度是卡尔曼滤波里最容易翻车的地方。当航向角在 π 和 -π 附近变化时，直接用差值计算会产生 2π 的跳变，滤波器会把这个跳变当成巨大新息，导致状态突变。

例程里每次预测、更新、融合后都调用了wrapToPi_做角度归一化，就是为了避免这个问题。如果你基于这个例程增加航向相关量测或者修改状态转移，请务必保留这个处理。另一个角度坑是计算atan2或atan后没有把结果归一化，凡是用角度做状态的情况，统一养成wrapToPi的习惯，能省掉很多排查时间。

5.3 联邦滤波和集中式滤波结果差得比较多

如果联邦滤波位置 RMSE 明显劣于集中式 EKF，不要立刻怀疑公式错，先检查信息分配和协方差是否匹配。

首先确认 β1 + β2 是否等于 1。如果两个子滤波器都用了完整 Q，没有进行信息分权，融合时相当于重复使用了公共信息，协方差会被低估，误差却不会变好。其次确认预测时Q/beta1、Q/beta2是否真的带进了局部滤波器。最后看一眼反馈模式是否开启，如果关闭反馈且子滤波器长时间不更新，它们的状态会漂得很远，加权融合结果自然不如集中式。

从原理上讲，联邦滤波在理想配置下可以达到和集中式滤波接近的精度。如果差得太大，问题基本出在信息分配和协方差缩放上。

5.4 MATLAB 版本和脚本粘贴报错

这段代码用到了脚本局部函数，从 MATLAB R2016b 开始支持。如果你用的是更老的版本，运行时会在局部函数定义处报语法错误，解决办法是把所有局部函数复制到一个单独的 function 文件中，或者升级 MATLAB 版本。

wrapToPi_是我自己写的局部函数，没有依赖 Mapping Toolbox，所以基础版 MATLAB 也能跑。如果你在粘贴过程中遇到中文注释乱码，通常是因为编辑器编码问题，把脚本另存为 UTF-8 并重新打开即可。还要注意不要从网页复制时带入行号或者换行符，最好先粘贴到纯文本编辑器里过一遍，再粘进 MATLAB 编辑器。

6. 工程上的一点体会

这个例程我前前后后改过好几版。最开始也是写集中式 EKF，一版跑完感觉“挺顺的”。后来拿真实采集的 IMU 和轮速数据一测，发现集中式滤波器对野值几乎没有任何防御能力。GNSS 在桥下跳了一下，位置轨迹直接多出一个尖角，还把航向角一起带偏。改成联邦滤波后，虽然代码量多了，但每个传感器通道都是独立模块，故障隔离和参数调试都清爽了很多。

融合反馈模式特别适合长时间持续运行的系统。智能小车、AGV 或者园区无人车，跑一整天的时候，子滤波器如果长期不反馈，局部协方差会慢慢变得不合理，主滤波器融合时的权重分配也会失真。加了反馈之后，子滤波器始终被全局结果纠正，整体稳定性明显提升。代价是故障隔离变弱，所以工程上真正落地时，我会在反馈之前加一层新息卡方检验，先把野值挡在子滤波器外面。

如果你手上正好有 IMU、GNSS、轮速计的仿真数据或者实车数据，建议先把这个例程跑通，然后把传感器噪声参数换成你自己的数值，再看看轨迹和误差曲线是否符合预期。多折腾几次，组合导航里那些“为什么这么调”“为什么那个参数不能太大”的问题，会慢慢变得非常清晰。