联邦卡尔曼滤波融合IMU/GNSS/里程计的MATLAB仿真实现

做组合导航的朋友应该都有过这种经历手里同时拿到了 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_gP1 P_g / β1x2 x_gP2 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: IMUGNSS 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(k1) v_scalar(k) a_forward*dt; yaw_true(k1) 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(k1) vx_true(k) awx*dt; vy_true(k1) vy_true(k) awy*dt; px_true(k1) px_true(k) vx_true(k)*dt 0.5*awx*dt^2; py_true(k1) 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(k1) xg(1); py_fed(k1) xg(2); vx_fed(k1) xg(3); vy_fed(k1) xg(4); yaw_fed(k1) xg(5); gb_fed(k1) xg(6); abx_fed(k1) xg(7); aby_fed(k1) xg(8); % 记录集中式滤波结果 xc(5) wrapToPi_(xc(5)); px_cen(k1) xc(1); py_cen(k1) xc(2); vx_cen(k1) xc(3); vy_cen(k1) xc(4); yaw_cen(k1) 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; end4. 关键参数与实现细节解读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 的平衡逻辑信息分配系数不是随便拍的。β10.5、β20.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、轮速计的仿真数据或者实车数据建议先把这个例程跑通然后把传感器噪声参数换成你自己的数值再看看轨迹和误差曲线是否符合预期。多折腾几次组合导航里那些“为什么这么调”“为什么那个参数不能太大”的问题会慢慢变得非常清晰。