简介一套基于MATLAB的GPS/SINS组合导航程序源码包面向新手及有一定经验的开发人员主要解决组合导航仿真中算法实现、数据输入与结果分析等问题适合惯性导航与卫星导航组合研究场景也可为教学演示和毕业设计提供参考。压缩包共包含5个文件以2个m源码文件为核心配合mat数据输入文件、doc结果说明文档与txt程序说明整体约670KB结构紧凑便于快速部署。其中m文件为算法实现主体mat文件提供数据输入doc与txt则用于说明原理和运行步骤。当前已有1101人学习下载源码由达摩老生亲测校正包含位置组合主程序、卡尔曼滤波实现与可运行demo并附有数据处理流程和结果文档方便对照理解GPS/SINS紧组合或松组合的代码逻辑。读者可借此快速搭建仿真环境、复用核心算法并依据说明文档排查常见运行问题做好参数调整与结果验证。1. GPS与SINS组合导航为什么要写成MATLAB程序最先把GPS和SINS数据同时放进MATLAB的人往往第一反应是画两条轨迹然后做平均期待“组合”后更接近真值。实际情况恰恰相反纯惯导解在几十秒钟后就开始发散GPS噪声却还在几米量级两者直接平均只会让轨迹既保留GPS的高频抖动又保留惯导的低频漂移。组合导航要解决的不是“数据合并”而是“误差估计”。这套以matlab_gps_sins命名的组合导航程序做的是松耦合loosely coupled融合SINS负责高频机械编排GPS以较低频率提供位置、速度观测卡尔曼滤波器估计姿态误差、速度误差、位置误差以及陀螺和加计的零偏再把估值反馈到惯导解里。对刚入手组合导航的工程师来说这套程序的价值在于三点结构完整、附程序说明、数据输入文件字段明确可以直接用真实采集或仿真数据把整个流程跑通而不是只看到一段卡尔曼更新公式。2. matlab_gps_sins组合导航程序的目录结构与数据输入文件2.1 程序包里三类文件源码、说明与数据输入解压这个程序包后典型的目录结构长这样常见做法是分为四个部分matlab_gps_sins/ ├── src/ │ ├── imu_mech.m % SINS机械编排 │ ├── kalman_update.m % 卡尔曼量测更新 │ ├── align_initial.m % 初始对准 │ └── att_quat.m % 四元数与欧拉角互转 ├── data/ │ ├── imu_gps_data.csv % IMU/GPS原始数据 │ ├── ref_trajectory.mat % 参考轨迹用于误差评估 │ └── input_config.m % 数据输入文件采样率、初值、噪声配置 ├── docs/ │ └── 程序说明.md └── main_gps_sins.m % 主入口主入口通常只做三件事读配置、循环处理数据、保存结果。源码目录里不推荐放数据文件尤其是几十MB的csv否则MATLAB每次打开工程都会扫描一遍工程一多就卡。2.2 用MATLAB把路径和数据文件一次性挂进来很多人拿到程序第一件事是双击main_gps_sins.m然后按F5结果报“找不到函数”。原因不是代码错了而是MATLAB当前路径不在src上。正确打开方式是先用addpath把源码目录挂进工作区再运行主脚本% main_gps_sins.m 顶部 clc; clear; close all; projectRoot fileparts(mfilename(fullpath)); addpath(fullfile(projectRoot, src)); addpath(fullfile(projectRoot, data)); cfg input_config(); % 从数据输入文件读取参数这里的fileparts(mfilename(fullpath))拿到的是主脚本所在绝对路径不依赖你当前在哪个目录启动MATLAB。路径挂好之后读取数据输入文件直接用相对路径即可我一般不会在脚本里写死盘符路径不然换一台机器全部要改。2.3 数据输入文件的字段约定与采样率表数据输入文件是整个程序能否跑对的根基。imu_gps_data.csv里常见字段如下字段单位说明t_imusIMU时间戳相对启动时刻gyro_x / gyro_y / gyro_zdeg/s三轴角速度体坐标系acc_x / acc_y / acc_zm/s²三轴比力体坐标系t_gpssGPS时间戳与IMU同一时间基准lat / lon / altdeg / deg / mWGS84经纬高vel_n / vel_e / vel_dm/sGPS速度北东地NED坐标系程序说明里通常会强调一条约定所有时间戳必须单调递增不允许出现回跳。GPS接收机如果输出的是UTC字符串例如“2025-04-10 03:12:48.000”主脚本里会先把字符串转成datenum再统一减掉首个时间戳换算成相对秒。这里会用到datetime转string这类常规操作但数据文件里最好已经处理成数值型秒数少一道出错环节。采样率匹配也要在进滤波器之前确认。常见组合是IMU 100Hz、GPS 1Hz也就是100个IMU周期做一次量测更新。如果发现GPS数据缺帧最简单的处理是在主循环里判断“本条GPS时间等于或晚于当前惯导时间”时才执行量测更新而不是用插值硬凑。3. 数据输入到导航解时间对齐与坐标转换3.1 时间戳统一GPS周内秒、UTC字符串与datenum转换数据输入文件里的时间戳如果来自真实GPS模块大概率是“周内秒”或“UTC日期时间”。直接用原始值做差会导致结果差出一整周或者出现负时间。我一般会在主脚本最前面加一段时间解析% 假设csv中已有列gps_week, gps_tow gpstime gps_week * 604800 gps_tow; % 单位秒 t0 gpstime(1); t_gps gpstime - t0; % 如果原始数据是UTC字符串转datenum再统一 % dt datetime(time_str, InputFormat, yyyy-MM-dd HH:mm:ss.SSS); % t_gps seconds(dt - dt(1));这段代码的逻辑是先把GPS时间归一到相对秒避免主循环里出现10位数级别的大数。MATLAB里大数做减法会损失精度尤其是时间戳到了毫秒级时double的有效位数只有15~16位直接拿“2025年4月10日的datenum”去减微秒级差异根本分辨不出来。所以先减掉首值再进后续计算是标准做法。特别注意GPS周数翻转week number rollover问题某些模块在特定时刻会把周计数回退到0造成时间戳突然跳变。重放数据前先判断周数与上一行的差值是否异常若出现负跳变就修正避免时间轴发生不可逆的错位。3.2 将经纬高转换为ENU局部坐标做松组合SINS机械编排输出通常是经纬高或ECEF坐标而卡尔曼量测更新时用“GPS位置减去惯导推算位置”得到观测残差。两者直接相减会有一个问题纬度差0.0001度在赤道和中纬度对应的米数不同数值不稳定。常见做法是取一个本地参考点把经纬高都转到北东地NED或东北天ENU局部坐标系里做差。% geodetic2enu_local.m短距离WGS84近似转换 function [xEast, yNorth, zUp] geodetic2enu_local(... lat, lon, alt, lat0, lon0, alt0) a 6378137.0; e2 0.00669437999014; N a / sqrt(1 - e2 * sin(lat0)^2); dx (lon - lon0) * deg2rad(1) * N * cos(lat0); dy (lat - lat0) * deg2rad(1) * (N * (1 - e2) / (1 - e2 * sin(lat0)^2)^1.5); dz alt - alt0; xEast dx; yNorth dy; zUp dz; end这个函数用卯酉圈曲率半径做了一阶近似10公里范围内精度足够松耦合使用。若数据覆盖上百公里就必须用完整的大地测量正反算或MATLAB Mapping Toolbox里的geodetic2enu。顺带一提如果要把组合结果转到惯性系做后分析还有ecef2eci等现成接口但那只用于事后评估不会出现在导航主循环里。3.3 原始观测量进入滤波器前必须做的三个检查数据输入文件里的GPS观测量不能直接信任进滤波器前至少要过三道检查否则一个野值就能把整个估计拉偏检查卫星定位状态很多模块输出“单点定位”和“RTK浮点解”两种模式精度完全不同。R矩阵要根据定位状态切换RTK固定解给0.3m量测噪声单点定位给5m。检查速度量测是否可用部分接收机速度由载波多普勒解算质量比位置差分好得多但速度无效时通常是0或满量程必须在主循环里判断。检查高程可信度城市峡谷里高度误差经常超过10米如果滤波器R矩阵里高度项与水平项取相同值位置整体会被抬高。这里的第三点最容易被忽略。我一般会把GPS高度量测噪声单独设大例如水平位置R取1m高度R取10m效果立竿见影。4. 在MATLAB里实现松耦合组合15维状态量与卡尔曼滤波4.1 15维状态量与系统矩阵F的搭建这套组合导航程序的核心状态量是15维姿态误差3维、速度误差3维、位置误差3维、陀螺零偏3维、加计零偏3维。前9维是导航误差参数后6维是惯性器件误差参数全部由滤波器在线估计。机械编排在imu_mech.m里单独完成滤波器不直接解算姿态它只估计误差。% x [phi(3); dv(3); dp(3); eb(3); db(3)] % 姿态误差 速度误差 位置误差 陀螺零偏 加计零偏 F zeros(15, 15); % 姿态误差方程phi_dot - cross(omega_in_n, phi) delta_omega F(1:3, 1:3) -skew(omega_in_n); % 地球自转与载体运动耦合 % 速度误差方程dv_dot f^n x phi C_b^n * db ... F(4:6, 1:3) skew(f_n); % 比力对姿态误差的耦合 F(4:6, 13:15) Cbn; % 加计零偏到速度误差 % 位置误差方程dp_dot dv F(7:9, 4:6) eye(3); % 陀螺零偏/加计零偏建模为一阶随机游走导数为0 F(10:12, 10:12) zeros(3); F(13:15, 13:15) zeros(3);姿态误差方程里skew(omega_in_n)是把地球自转和载体在地球表面运动带来的角速度耦合写成了反对称矩阵。若数据采集时间只有几分钟这一项的数值很小但对长时间跑数据来说缺少这一项会导致航向漂移估计完全错误。速度误差方程里的Cbn矩阵是从惯导姿态得到的旋转矩阵它把加计零偏从体坐标系投影到导航坐标系这一步直接决定零偏估计的可观测性。4.2 量测更新与状态反馈GPS位置、速度怎么进入滤波器松耦合的量测方程比较简单GPS给出位置和速度惯导也给出位置和速度两者之差就是观测残差。程序设计里H矩阵只有6行对应GPS的3个位置量测和3个速度量测。% 提取GPS量测已转ENU z_pos [gps_x_enu; gps_y_enu; gps_z_enu] - [pos_enu(1); pos_enu(2); pos_enu(3)]; z_vel [gps_vel_n; gps_vel_e; gps_vel_d] - [vel_ned(1); vel_ned(2); vel_ned(3)]; z [z_pos; z_vel]; % H矩阵量测直接对应位置误差(7:9)和速度误差(4:6) H zeros(6, 15); H(1:3, 7:9) eye(3); H(4:6, 4:6) eye(3); [x, P] kalman_update(x, P, H, R_gps, z); % 状态反馈把估计出来的误差修正回惯导解 [roll, pitch, yaw] att_quat(quat, euler); roll roll - x(1); pitch pitch - x(2); yaw yaw - x(3); vel_ned vel_ned - x(4:6); pos_enu pos_enu - x(7:9); % 反馈后把误差状态清零 x(1:9) 0;卡尔曼更新里用右除P*H/S而不是inv(S)数值上好很多15维矩阵做逆本来没什么压力但工程习惯如此。反馈后清零不是可有可无如果不清零下一时刻这些误差状态会被再次积分等于把已经修正过的误差又加回系统滤波会震荡甚至发散。程序说明里若没有强调这点工程中很容易踩坑。4.3 初始P0、Q和R怎么给组合导航参数表数据输入文件里一般会带一份初始配置三个矩阵的设定直接决定滤波行为矩阵含义典型初值调整方向P0状态初始不确定度姿态(0.1°)²速度(0.1m/s)²位置(1m)²零偏按经验值偏大时前几秒修正剧烈偏小时收敛慢Q过程噪声陀螺白噪声 0.01°/√h加计速度随机游走 0.05m/s/√hQ过大滤波跟随噪声轨迹毛糙R量测噪声位置1~3m速度0.1~0.3m/s按GPS模块实测1σ取不拍脑袋Q矩阵的离散化是新手最容易绕晕的地方。连续域过程噪声要乘以采样间隔再进离散卡尔曼公式很多简化版本直接拿连续域数值填进去结果Q偏大或偏小滤波出来的轨迹要么过度平滑要么发毛。这里建议先按手册里陀螺角的随机游走参数推算再根据静态测试的新息均值微调。GPS位置R取多少最好用一段静止数据算实际标准差而不是抄模块标称精度。5. 组合解如何验证、可信参数怎么调5.1 三种验证思路静态测试、轨迹回放与新息检查拿到程序跑起来只是第一步验证才是判断这套组合导航程序可不可信的关键。最简单的验证是静态测试把IMU放在桌上静止20分钟纯惯导位置会随时间漂移出几十米甚至上百米开启GPS量测后组合解应被约束在1~3米内。若位置还在缓慢漂移优先怀疑GPS时间戳与IMU时间戳没有对齐而不是滤波器写错了。轨迹回放验证适合有参考轨迹的数据。数据输入文件里常附ref_trajectory.mat里面存了真值或高精度后处理轨迹直接计算RMS位置误差与速度误差。如果RMS误差呈周期性脉冲大概率是GPS更新频率与滤波器执行频率不匹配或者是出现了野值但没被剔除。第三种验证看新息序列这也是调参时最值得盯的指标。正常的新息应该在零附近波动且近似白噪声如果新息均值明显偏离零说明状态模型或量测模型有系统性偏差常见原因包括杆臂误差、GPS天线相位中心与IMU中心不重合。5.2 参数调整与常见坑不要同时放大Q和R调参时最容易犯的错误是看到轨迹发散就同时放大Q和R试图让滤波器既信任模型又信任量测。卡尔曼增益本质上取决于Q与R的比值而非绝对大小同时放大两者对滤波输出几乎没有影响只会让数值误差变大。正确做法是固定R根据新息方差调整Q新息过大说明模型不可信适当增大Q新息过小则说明模型太乐观可以减小Q。打印调试信息时用datetime转string给日志文件加时间戳是实用的细节。计算结果建议存成v7格式而不是v7.3v7.3的HDF5结构虽然能被Python的h5py直接读取但MATLAB旧版本打开可能不兼容v7格式用scipy.io.loadmat即可离线分析。工程上我通常这样收尾把每轮调参后的P矩阵对角元打印出来盯住陀螺零偏估计值——它收敛到一个物理上合理的量级例如0.01deg/h左右比位置曲线看起来平滑更能证明系统工作正常。本文还有配套的精品资源点击获取