简介面向本硕测绘导航类课程教研学习利用EKF卡尔曼滤波实现GNSS/INS传感器融合的Matlab仿真项目可帮助理解惯性导航系统与卫星导航组合定位中的状态估计、误差修正等核心问题。压缩包共22个文件包含13个.m源码脚本、7张结果示意图、1份.mat仿真数据以及1份avi仿真操作录像整体仅4.52MB结构轻量、便于快速部署。其中源码覆盖EKF融合主程序及相关函数mat数据提供GNSS辅助INS的实测/模拟输入jpg展示各阶段定位与误差曲线avi录制了从启动到出图的全过程。已有610人学习下载适合本硕学生及科研人员对照操作视频复现实验并结合源码和图示深入理解EKF在组合导航中的实际应用。通过反复调试与录像对照可快速掌握惯性导航/GNSS融合建模流程提升传感器融合算法的工程实现能力。1. 单纯 GNSS 丢星后惯导位置为什么五分钟就偏出一公里在市区高架、隧道和地下车库连续跑过组合导航的人基本都见过这样的曲线开始十几秒位置还贴着 GNSS 轨迹一旦卫星信号断开惯导输出按“惯性外推”继续走陀螺零偏 1°/h、加速度计零偏影响下水平位置误差以二阶积分速度扩张几分钟就能漂出几十米甚至几百米。纯 INS 短时间可用长时间不可信所以工程上把 GNSS 作为“外部校正源”每隔一段时间对惯导误差状态做一次滤除更新。下面要拆的这套 MATLAB 仿真就是用 EKF扩展卡尔曼滤波把 INS 机械编排结果和 GNSS 量测融合成一套可复现的 GNSS-INS 惯性导航系统。压缩包里 main.m 是主入口func 目录下封装了状态转移、观测矩阵和滤波更新过程GNSSaidedINS_data.mat 里存好了预先模拟的惯导与 GNSS 数据还附带一段操作录像直接跟着跑就能看到融合前后的估计效果。对正在做课程设计、组合导航毕业课题或想验证 EKF 公式落地细节的人这比只看论文更容易理解。2. 状态空间建模是 EKF 融合的第一步15 维状态与误差传播矩阵2.1 姿态、速度、位置误差和外参乱成一团怎么办GNSS-INS 松耦合不想直接对位置、速度、姿态这些大数值做滤波因为惯导输出本身就是积分出来的均值不为零非线性强直接放进卡尔曼滤波并不合适。更实用的做法是把状态变量定义成“误差量”姿态误差、速度误差、位置误差再加上陀螺零偏和加速度计零偏。这样误差量在每次 GNSS 修正前都保持小量级线性近似更可靠。常见松耦合状态向量取 15 维依次排列如下表。索引状态含义单位1:3φ姿态误差平台失准角rad4:6δv速度误差m/s7:9δp位置误差m10:12ε陀螺零偏rad/s13:15∇加速度计零偏m/s²这里的“误差”是惯导解算值与真实值之间的偏差。把它们放成状态后系统的过程模型是“误差如何随时间演化”量测模型是“GNSS 能直接看到误差状态中的哪些分量”。2.2 F 矩阵的分块写法和离散化参数连续时间误差传播方程来自惯导误差微分方程简化后可以写成姿态误差φ̇ − (ω_in_n)× φ − C_b^n ε − 陀螺噪声项速度误差δv̇ (C_b^n f_b)× φ − (2ω_ie_n ω_en_n)× δv C_b^n ∇ 加速度计噪声项位置误差δṙ δv陀螺零偏ε̇ 0 零偏随机游走加速度计零偏∇̇ 0 零偏随机游走在 MATLAB 里这些分块会被拼成一个 15×15 的连续时间矩阵 F。实际建模中func目录里一般会有一个getF.m或类似函数返回带当前姿态矩阵C_b_n、比力f_b、角速度w_in_n的 F。% 连续时间状态转移矩阵 F维度 15x15 F zeros(15); % 姿态误差与姿态误差之间的耦合项 F(1:3, 1:3) -skew(w_in_n); % omega_in_n 的斜对称矩阵 % 陀螺零偏驱动姿态误差 F(1:3, 10:12) -C_b_n; % 姿态误差随零偏漂移 % 比力引起的速度误差项 F(4:6, 1:3) skew(C_b_n * f_b); % 由比力产生的姿态-速度耦合 % 科里奥利和地球自转项 F(4:6, 4:6) -(skew(2 * w_ie_n w_en_n)); % 加速度计零偏驱动速度误差 F(4:6, 13:15) C_b_n; % 位置误差由速度误差积分得到 F(7:9, 4:6) eye(3);代码里skew是构造斜对称阵的辅助函数需要自己在func里实现完成v×运算。C_b_n是从载体坐标系到导航坐标系的旋转矩阵f_b是加速度计测得的比力w_in_n是惯性角速度在导航系上的投影。常有人误把 F 的非零块填错位置导致姿态误差和位置误差耦合关系翻转后续滤波必发散。每一行对应一个误差微分方程检查时把公式与行列一一对齐即可。离散化时IMU 更新频率通常远高于误差状态变化速率常见做法是直接使用前向欧拉Fd eye(15) F * dt。严格一些可以用矩阵指数expm(F * dt)或 MATLAB 的c2d做零阶保持离散化。离散化方式精度适用场景前向欧拉eye F*dt一阶O(dt²)高频惯导误差变化平缓最常用后向欧拉一阶不适合直接用于状态转移少用更多出现在控制领域零阶保持 ZOHexpm(F*dt)精确低频采样或 dt 过大时推荐二阶截断折中需要兼顾计算量时使用对于 100Hz 以上的惯导数据前向欧拉已经足够。如果发现滤波结果对 dt 敏感先把离散化换成expm(F*dt)看是否改善再回头查模型参数。2.3 量测方程GNSS 只给位置和速度H 矩阵怎么搭GNSS 接收机输出的是当前天线相位中心的位置和速度对应状态向量中的位置误差 δp 和速度误差 δv。因此在松耦合量测模型里量测矩阵 H 是 6×15% GNSS 量测位置 速度 H zeros(6, 15); H(1:3, 7:9) eye(3); % 位置误差 H(4:6, 4:6) eye(3); % 速度误差实际工程中这里还有一个杆臂补偿问题GNSS 天线并不在 IMU 中心如果挂在车顶或飞机后机身姿态旋转时会产生和机动速度无关的等效位置偏差。简单处理是把杆臂误差折算到速度量测中再在更新前从量测值里扣掉。对陆地车辆杆臂量级小很多课程仿真会忽略但真正跑真实采集数据时这是最早需要检查的坑之一。3. 在 main.m 复现 EKF 的预测-更新循环3.1 预测把 INS 机械编排的结果拿来做状态外推EKF 的预测和 INS 的“递推”是两回事。INS 机械编排负责从角增量、速度增量算出位置、速度、姿态这是主解算。EKF 预测则是在误差状态上做线性外推和主解算并行运行。在 main.m 里通常每个 IMU 周期都执行一次机械编排再执行一次误差状态预测% 误差状态预测 x_pred Fd * x; % 状态外推15x1 P_pred Fd * P * Fd Q; % 协方差外推15x15这里Fd是上一章离散化后的状态转移矩阵x是误差状态估计P是误差状态协方差Q是过程噪声协方差。P_pred的更新幅度取决于系统噪声 QQ 越大协方差增长越快后续更新时卡尔曼增益越高GNSS 量测权重越大。所以 Q 不能随意拍脑袋它直接控制滤波器对惯导结果和 GNSS 量测的信任分配。3.2 更新用卡尔曼增益把 GNSS 量测修正到状态上GNSS 更新触发条件通常是“新量测已到达”在仿真数据里对应时间戳跨过某一个 GNSS 采样点。更新公式写出来很简单% 量测更新 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 innov z_meas - H * x_pred; % 位置/速度新息 x x_pred K * innov; % 状态修正 P (eye(15) - K * H) * P_pred; % 协方差修正S是量测预测结果的协方差维度 6×6。K是 15×6 的增益矩阵作用是把六个维度的 GNSS 新息投影到 15 维误差状态上。注意innov的第三个分量可能是纬度和经度需要先转成米制位置误差否则和状态单位对不上滤波会直接发散。修正之后是反馈校正。由于建模的是误差状态得到估计的误差后要立即把这些误差反馈给机械编排的位置、速度和姿态然后清零误差状态。这样能让误差始终约束在小量级符合 EKF 的局部线性化假设。常见做法是% 反馈校正后清零误差状态 pos pos - x(7:9); vel vel - x(4:6); C_b_n (eye(3) - skew(x(1:3))) * C_b_n; % 小角度近似 x(1:15) 0;3.3 滤波发散时的协方差对称化处理数值计算中P 的对称性会被浮点累积破坏。特别是在长时间运行时P可能变成非对称矩阵导致平方根滤波或某些正定性判断失败。main.m 里最好在每个更新周期后做一次强制对称化P 0.5 * (P P); % 强制对称如果想更稳定可以用 Joseph 形式计算后验协方差I_KH eye(15) - K * H; P I_KH * P_pred * I_KH K * R * K;Joseph 形式在数值上不容易变负定代价是多一次矩阵乘法。对课程设计来说非必需但如果你想在论文里提“数值健壮性”这是一个可以替换的细节。4. 传感器融合的时间同步与坐标系对齐4.1 100Hz IMU 与 10Hz GNSS 的 ZOH 零阶保持IMU 输出频率通常是 100Hz 或更高GNSS 输出频率常见 10Hz、5Hz。如果直接把最近一次 GNSS 量测挂在当前 IMU 时刻上做更新等价于默认量测时间相差不超过一个 GNSS 周期。多数仿真数据是理想对齐的但真实数据里 GNSS 时间戳和 IMU 时间戳可能来自不同时钟误差会影响新息的统计特性。最常用的策略是零阶保持ZOHGNSS 输出的位置速度在下一个有效量测到达之前保持不变到点后再切换。对应代码% 找到当前 IMU 时刻之前最近的 GNSS 量测 idx find(gnss_time imu_time(i), 1, last); z_meas [gnss_pos(idx,:); gnss_vel(idx,:)];如果两个传感器时间偏移较大ZOH 会引入锯齿状估计。更平滑的做法是线性插值% 在相邻 GNSS 历元之间线性插值 frac (imu_time(i) - gnss_time(idx)) / (gnss_time(idx1) - gnss_time(idx)); pos_intp gnss_pos(idx,:) frac * (gnss_pos(idx1,:) - gnss_pos(idx,:)); vel_intp gnss_vel(idx,:) frac * (gnss_vel(idx1,:) - gnss_vel(idx,:));线性插值适合匀速运动高动态环境下插值方向与载体运动方向的匹配很重要。对车辆ZOH 已经够用。对航空或无人机应该做时间戳对齐预处理而不是强行把一个时刻的量测当成另一个时刻的更新。4.2 松耦合融合中坐标系不一致的几个坑GNSS 通常输出经纬高而惯导误差状态中的位置误差必须在直角坐标系里表达。常见做法是把经纬高转成 NED 坐标或 ECEF 坐标再作为量测值传给滤波器。如果状态单位是米、量测是度滤波结果完全不收敛。在实际工程中GNSS 的位置基准是 WGS-84惯导的导航坐标系可能是北东地NED或东-北-天ENU。两者的旋转顺序不同速度量测也需要做对应旋转。下面这张表给出常见组合惯导坐标系常用位置误差形式对应的转换函数NED相对原点的北向、东向、地向位移lla2nedENU东向、北向、天向位移lla2enuECEFECEF 下的位置偏差lla2ecef在 MATLAB 里lla2ned需要地理坐标系工具箱或者可以自己用椭球公式实现。如果没有工具箱可以用flat2lla反向推导也可以用navigation类里的lla2ned。这里的资料包里可能没有工具箱依赖所以 main.m 里多半用的是简化局部切平面近似。4.3 数据落盘GNSSaidedINS_data.mat 的组织拿到压缩包后第一件事不是跑 main.m而是先加载数据看看变量结构md load(GNSSaidedINS_data.mat); fieldnames(md)不同工程的数据命名差别很大有的用imu_data和gnss_data有的用IMU和GNSS。从保存格式看通常会包含以下字段时间戳数组imu_time单位秒三轴角增量或角速度三轴速度增量或加速度GNSS 位置经纬高或米制GNSS 速度NED 或 ECEFGNSS 时间戳建议先打印第一列时间戳确认起始时间一致。常见问题是 IMU 从 0s 开始GNSS 从 0.2s 开始不经对齐直接进入滤波会丢掉开头几个更新周期导致最初一段协方差偏大。5. 从 func 到 main.m融合参数怎么装配5.1 噪声矩阵 Q 与 R 的数值经验Q 的工程物理意义是“误差状态在单位时间内的不确定性增长”。常见做法是把 IMU 原始噪声谱密度换算到状态方程中。直接给出参考值如下表实际以数据单位为准。状态参数含义典型值姿态误差陀螺角度随机游走1e-5 ~ 1e-4 rad²/s速度误差加速度计速度随机游走1e-3 ~ 1e-2 m²/s³位置误差位置随机游走1e-4 ~ 1e-2 m²/s陀螺零偏零偏随机游走1e-8 ~ 1e-6 rad²/s³加速度计零偏零偏随机游走1e-6 ~ 1e-4 m²/s⁵R 相对直观它就是 GNSS 量测的噪声协方差。位置量测标差乘以本身得到方差常见参考值R zeros(6); R(1:3,1:3) eye(3) * 1.0; % 位置误差方差 1 m² R(4:6,4:6) eye(3) * 0.04; % 速度误差方差 0.04 m²/s²在实际使用中GNSS 在遮挡环境下的误差远大于标称值。如果你发现滤波后的轨迹在开阔地带没问题、进隧道后位置仍然带着 GNSS 的跳跃说明 R 给得偏小应当适当放大 R 以降低 GNSS 在恶劣环境下的权重。5.2 初始协方差 P0 对收敛速度的影响P0是滤波开始时对误差状态估计不确定性的度量。如果设置太小滤波器会非常相信自己初始状态导致 GNSS 前几次更新作用很小设置太大前几个点会出现明显的修正跳变。一般做这样初始化P0 zeros(15); P0(1:3,1:3) eye(3) * (1e-3)^2; % 姿态误差初始约 0.001 rad P0(4:6,4:6) eye(3) * (0.1)^2; % 速度误差初始约 0.1 m/s P0(7:9,7:9) eye(3) * (10)^2; % 位置误差初始约 10 m P0(10:12,10:12) eye(3) * (1e-4)^2; % 陀螺零偏不确定性 P0(13:15,13:15) eye(3) * (1e-3)^2; % 加速度计零偏不确定性如果初始姿态误差真的很大例如 1° 以上小角度线性化就不成立EKF 直接更新会发散。刚才的 P0 需要配合预处理先用静止初始对准做好粗略姿态再启动 EKF。5.3 一段可直接替换参数的执行流程结合 main.m 的常见框架核心循环可以组织成下面的伪代码。这是一个“预测 更新 反馈”的标准结构复制到自己的工程里只需要替换数据结构。for i 1 : length(imu_time) u imu_data(i,:); % 当前 IMU 量测 [pos, vel, C_b_n] insMechanization(u, pos, vel, C_b_n, dt); % 误差状态预测 [F, Fd, Q] buildModel(C_b_n, f_b, dt); x_pred Fd * x; P_pred Fd * P * Fd Q; % 检查是否有新 GNSS 量测 if gnss_idx length(gnss_time) imu_time(i) gnss_time(gnss_idx) H buildH(); z_meas [gnss_pos(gnss_idx,:) gnss_vel(gnss_idx,:)]; R buildR(); S H * P_pred * H R; K P_pred * H / S; innov z_meas - H * x_pred; x x_pred K * innov; P (eye(15) - K * H) * P_pred; P 0.5 * (P P); % 反馈校正并清零误差状态 pos pos - x(7:9); vel vel - x(4:6); C_b_n (eye(3) - skew(x(1:3))) * C_b_n; x zeros(15,1); gnss_idx gnss_idx 1; end end这段流程里buildModel负责把当前姿态矩阵、比力、角速度填进连续时间 F 并完成离散化buildH返回观测矩阵buildR根据环境设定量测噪声。如果跑出来的轨迹在每次 GNSS 更新点上出现水平跳变检查反馈校正是否执行以及是否先修正位置、再清零状态。6. 跑通之后用新息序列验证滤波和第二组轨迹复现6.1 新息检验滤波收敛不代表滤波正确。最直接的观察对象是“新息序列”innov。理想情况下新息均值为零协方差数值与S H*P_pred*H R匹配。在 main.m 末尾可以这样统计% 预先保存每次更新的 innov 和 S innov_mean mean(innov_log, 2); innov_cov cov(innov_log); s_mean mean(S_log, 3); s_mean squeeze(s_mean); disp(diag(innov_cov) ./ diag(s_mean));比值接近 1 说明量测噪声和过程噪声设置基本一致远小于 1 说明 R 偏大远大于 1 说明模型误差主导。这个比值比看轨迹误差更早暴露问题。6.2 常见发散与异形使用这套资源最常遇到的问题集中在三处。第一量测单位不一致GNSS 给的是经纬高但 H 把位置误差按米对齐结果位置新息巨大更新后状态直接跳飞。第二时间戳没对齐GNSS 数据比 IMU 数据晚一个周期会导致每次更新都滞后轨迹不连续。第三初始 P0 设置过小滤波器过于相信初始姿态GNSS 前几次更新力度不足后续协方差收缩后也无法纠正。6.3 换个数据集要改哪几处仿真包只能验证自己的数据。换一组轨迹时至少检查四个位置数据文件中的时间戳起点与采样频率是否与dt一致GNSS 位置量测单位是米还是经纬度杆臂向量是否更新到 H 矩阵以及 R 中的位置方差是否需要改成该 GNSS 接收机的实际噪声水平。四者都对齐后main.m 几乎不用改逻辑只需替换路径和初始点。这套结构同样适用于把 EKF 换成无迹卡尔曼滤波把量测从松耦合换成 GNSS 原始伪距伪距率紧耦合只是建模文件需要另行处理。本文还有配套的精品资源点击获取