简介这是一款MATLAB实现的GPS/SINS组合导航位置组合程序面向组合导航、导航制导与控制领域的学习者与开发人员内附程序说明与数据输入文件可帮助快速理解松组合中位置观测信息融合的Kalman滤波流程。压缩包共5个文件以2个m脚本为核心包含主程序和子函数配套1个mat格式的数据输入文件、1份doc格式结果说明和1份txt格式程序说明整体约670KB小巧完整。目前已有1101人学习下载适合新手按照文档逐步复现也适合有经验者直接移植或改造。通过运行和研读代码可获得从数据准备、误差方程构建到滤波解算的完整实现思路并掌握GPS/SINS位置组合算法的仿真验证与结果分析方法为进一步开展组合导航研究打下基础。1. 组合导航程序包这套Matlab代码解决什么实际问题车载导航、无人机定位、船载测绘这类场景里GPS和SINS是天生互补的一对SINS能在一段时间内独立输出高频姿态和位置但误差会随时间发散GPS没有长期漂移却怕遮挡和动态干扰。把两者用卡尔曼滤波器融合就是组合导航的核心思路。你手上这套带程序说明和数据输入文件的matlab_gps_sins组合导航程序做的正是这件事读入IMU和GPS原始数据跑一遍松耦合组合解算输出融合后的轨迹、姿态和速度。对刚开始接触组合导航的工程师来说它最大的价值不只是算法本身而是把数据格式、说明文档、可运行代码串成了一条能改、能调、能验证的链路比从论文复现门槛低得多。2. 数据输入文件格式IMU、GPS与初始对准信息怎么读2.1 数据组织方式与字段定义拿到程序包第一步不是打开主函数而是先看数据文件的列定义。绝大多数类似程序会提供三到四个文本或Excel数据文件IMU原始数据、GPS观测数据、初始对准参数。IMU数据的标准列顺序通常是时间戳、三轴陀螺角速度(deg/s或rad/s)、三轴加速度计比力(m/s²)部分程序还会附一列温度或者标志位。GPS数据则常见为UTC时间、经度、纬度、高程、北向速度、东向速度、垂直速度有些还会包含定位状态标志和卫星数。在动手运行之前建议先把文件读进Matlab看一眼表头和数据段确认单位。一个非常容易踩的坑是角速度单位如果程序注释里写的是deg/s而数据文件实际是rad/s姿态解算在几十秒内就会出现肉眼可见的漂移。类似地加速度计比力单位也有g和m/s²两种写法读数据时乘以9.80665是常规操作。2.2 数据解析的Matlab实现常见做法是写一个独立的数据读取脚本把解析逻辑和主算法分开。这样换数据时只需要改文件路径和列索引不必动主程序。function imu_data load_imu_file(filename) % 读取IMU数据文件返回结构体 raw load(filename); % 假设列定义t, gx, gy, gz, ax, ay, az, status t raw(:,1); gyro raw(:,2:4) * deg2rad(1); % 如果原始单位是deg/s转成rad/s accel raw(:,5:7) * 9.80665; % 如果原始单位是g转成m/s^2 status raw(:,8); imu_data.t t; imu_data.gyro gyro; imu_data.accel accel; imu_data.status status; end这段代码做了三件关键的事给时间戳单独赋值方便后续插值、把陀螺单位统一到rad/s、把加速度单位统一到m/s²。之所以强调单位转换是因为后续的姿态更新、速度积分全部基于国际单位制任何一处不一致都会导致导航结果偏差。GPS数据的解析逻辑类似但要多处理一个时间基准问题。IMU的时间戳通常是相对开机时刻的秒数而GPS数据里往往是UTC时间。如果两者的时间起点不一致必须先做时间对齐否则组合滤波器的量测更新会直接把错误信息引入系统。2.2.1 GPS时间与IMU时间的对齐处理% 假设gps_time是GPS周内秒imu_time是相对开机时刻 % 先找到GPS第一条有效数据对应的imu_time基准 dtime interp1(gps_t, gps_time_utc, imu_time, linear, extrap);这里的原则是GPS数据频率通常只有1Hz到10HzIMU则可能是100Hz到400Hz组合导航主循环以IMU频率推进GPS量测到达时才做校正。时间对齐的意义在于GPS位置反映的是某个时刻的绝对位置如果用错时间戳做量测更新等效于给量测方程注入了一个随时间增长的误差。2.3 数据输入文件里的初始对准参数初始对准是组合导航里容易被轻视却影响极大的环节。数据包里通常会有一个初始化文件或脚本里面至少包含以下几项初始经度、初始纬度、初始高度、初始姿态角或者初始姿态矩阵、初始速度。这些值不是随便填的初始位置错1度GPS量测更新会把这个偏差当作观测误差反向校正速度造成滤波初期的剧烈震荡。% 初始对准参数示例结构 init_param.lat 39.9847; % 初始纬度, 度 init_param.lon 116.3184; % 初始经度, 度 init_param.alt 50.0; % 初始高度, 米 init_param.att [0; 0; 90] * deg2rad(1); % 初始姿态: 横滚/俯仰/航向 init_param.vel [0; 0; 0]; % 初始速度: 东北天方向, m/s如果数据文件里没有现成的初始姿态一般做法是利用静止状态的加速度计输出计算横滚和俯仰航向角则需要外部给定或在运动中完成对准。这套程序如果带了初始化脚本建议一定先跑通它再跑主组合程序因为滤波器状态初值里的误差协方差矩阵也需要在这里设置。3. 松耦合组合的实现SINS机械编排与卡尔曼滤波量测更新3.1 捷联惯导递推姿态、速度、位置的更新框架SINS的核心机械编排是三步递推姿态更新、速度更新、位置更新。姿态更新用的是陀螺输出的角增量或角速度速度更新用加速度计比力扣除科里奥利加速度和重力位置更新用速度积分。这套组合导航程序里机械编排一般被封装成单独的函数比如ins_mechanization内部不做GPS相关的任何操作。姿态更新推荐用等效旋转矢量法双子样补偿是常见配置。直接给一段常见的姿态更新核心逻辑框架function [Cbn, vn, pos] ins_mechanization(Cbn, vn, pos, gyro, accel, dt) % 输入: 上一时刻姿态矩阵/速度/位置, 当前时刻陀螺和加计输出 % 输出: 更新后的姿态矩阵/速度/位置 % 角增量计算假设陀螺输出为角速度 dtheta gyro * dt; % 双子样圆锥补偿简化形式 if size(gyro,1) 2 % 圆锥误差补偿量 alpha 1/12; dtheta_corr alpha * cross(gyro(1,:), gyro(2,:)); dtheta dtheta dtheta_corr; end % 构造等效旋转矢量并更新姿态矩阵 rotvec_norm norm(dtheta); if rotvec_norm 1e-12 Cbn_new Cbn * (eye(3) sin(rotvec_norm)/rotvec_norm * skew(dtheta) ... (1-cos(rotvec_norm))/rotvec_norm^2 * skew(dtheta)^2); else Cbn_new Cbn; end % 比力投影到导航系 fn Cbn_new * accel; % 扣除重力和科里奥利加速度简化为重力补偿 vn_new vn (fn [0;0;9.80665]) * dt; % 位置更新用平均速度 pos_new pos 0.5 * (vn vn_new) * dt; Cbn Cbn_new; vn vn_new; pos pos_new; end这段代码展示了机械编排的最小骨架。注意姿态更新用了旋转矢量转方向余弦矩阵的方式比直接积分角速度数值稳定性更好。速度更新里[0;0;9.80665]是把重力加速度加到天向分量上抵消重力影响——这里符号取决于坐标系定义东北天坐标系下重力加速度写作-g再加回等于补一个正项。如果你拿到的程序代码里这一项符号相反说明它用的可能是北东地坐标系看代码前一定要确认坐标系定义。3.2 15维误差状态与卡尔曼滤波量测方程松耦合组合最常用的状态量是15维包括3维姿态误差、3维速度误差、3维位置误差、3维陀螺漂移、3维加速度计零偏。滤波器的状态方程来自SINS误差传播模型量测方程则是GPS提供的速度、位置与SINS解算值之差。一个容易混淆的细节是这里滤波器估计的不是导航状态本身而是SINS解算结果的误差。所以每次量测更新后要拿估计出的误差去修正SINS输出的姿态、速度和位置修正完把误差状态清零。这套程序的流程如果是标准的松耦合结构主循环里必然有一段类似下面的修正逻辑。function [Cbn, vn, pos, P] kalman_update(Cbn, vn, pos, P, z, H, R) % z GPS观测 - SINS解算, H为量测矩阵, R为量测噪声矩阵 % 卡尔曼增益 K P * H / (H * P * H R); % 状态修正量 dx K * z; % 误差状态反馈修正SINS输出 % 姿态修正用姿态误差角构造旋转矩阵 phi dx(1:3); Cbn (eye(3) - skew(phi)) * Cbn; % 速度修正 vn vn - dx(4:6); % 位置修正 pos pos - dx(7:9); % 协方差更新 P (eye(15) - K * H) * P; end这段修正代码里姿态误差的反馈用的是小角度近似。实际工程中如果姿态误差超过几度小角度近似会带来明显偏差不过组合导航系统每次量测更新周期通常不超过1秒姿态误差在GPS校正下通常被限制在角分级水平这个近似是安全的。量测矩阵H在速度位置松耦合下的典型形式是6×15矩阵对应速度误差和位置误差的观测。这套程序的数据包里如果附了滤波参数文件R矩阵往往以对角阵形式写入具体数值和GPS接收机的定位精度指标直接相关。3.3 组合导航主循环的Matlab实现主循环的结构通常是时间推进、IMU数据插值或匹配、机械编排、判断是否有GPS量测到达、有则执行卡尔曼量测更新与反馈修正。下面是这种循环的骨架。% 主组合导航循环 for k 2:length(imu_time) dt imu_time(k) - imu_time(k-1); % SINS递推 [Cbn, vn, pos] ins_mechanization(Cbn, vn, pos, ... imu_gyro(k,:), imu_accel(k,:), dt); % 检查当前时刻是否有GPS量测 if gps_index length(gps_time) abs(imu_time(k) - gps_time(gps_index)) 0.005 % 构造量测: GPS位置速度与SINS之差 z_pos gps_pos(gps_index,:) - pos; z_vel gps_vel(gps_index,:) - vn; z [z_pos; z_vel]; % 量测更新 [Cbn, vn, pos, P] kalman_update(Cbn, vn, pos, P, z, H, R); gps_index gps_index 1; end % 记录结果 result.t(k) imu_time(k); result.pos(k,:) pos; result.vel(k,:) vn; result.att(k,:) dcm2euler(Cbn); end这段循环里有几个关键的工程决策点。时间匹配的阈值0.005秒意味着IMU时间戳和GPS时间戳差在半毫秒内才触发量测更新这个值可以根据IMU频率调整。如果IMU是200Hz一个周期是0.005秒所以这个阈值实质上是“当前IMU时刻恰好对应GPS整秒时刻”。如果GPS频率是10Hz也就是0.1秒间隔那么实际上每20个IMU周期才会触发一次量测更新。运行这段代码之前建议先检查GPS时间戳是否有小数抖动某些GPS模块输出的时间戳不是严格等间隔直接按固定阈值匹配会漏掉部分量测。4. 参数怎么设滤波初值、噪声矩阵与IMU误差预算4.1 状态协方差初值与过程噪声矩阵的拟定滤波器的参数设置决定了组合导航是平稳收敛还是快速发散。先看初始误差协方差矩阵P0它描述的是滤波起始时刻对SINS解算误差的信任程度。P0设得太大滤波初期GPS量测会以很大增益修正造成速度估计剧烈摆动设得太小滤波器会“自信”地忽略GPS信息收敛变慢。常见取值如下表状态量P0典型值依据姿态误差(0.1°)² 对角阵静止初始对准精度约0.1度速度误差(0.1 m/s)² 对角阵静止条件下速度约0位置误差(5 m)² 对角阵与GPS单点定位精度相当陀螺漂移(10 °/h)² 对角阵消费级MEMS陀螺零偏稳定性加计零偏(1 mg)² 对角阵消费级MEMS加速度计零偏过程噪声矩阵Q理论上应由陀螺和加计的随机游走系数推导。工程上更常见的做法是先给一个保守估计再通过跑数据观察滤波残差调整。这里给一个可用的初始Q设置逻辑把陀螺角度随机游走设为0.1°/√h加速度计速度随机游走设为0.01 m/s/√h然后按时间步长离散化填入Q矩阵。% 过程噪声矩阵离散化(简化版) arw 0.1 * deg2rad(1) / 60; % 陀螺角度随机游走, rad/sqrt(s) vrw 0.01 / 60; % 加计速度随机游走, m/s/sqrt(s) Q_gyro (arw^2) * eye(3) * dt; Q_accel (vrw^2) * eye(3) * dt; Q blkdiag(Q_gyro, Q_accel, zeros(9,9)); % 此处简化, 完整Q需含相关项注意这里的Q矩阵是简化形式把姿态、速度的过程噪声归因于陀螺和加计噪声位置和零偏部分的过程噪声暂时置零。实际程序中Q应该是一个15×15的矩阵并且还有陀螺漂移和加计零偏的一阶马尔可夫过程噪声。如果你的程序里Q矩阵是完整的重点关注漂移项的驱动噪声方差它控制着GPS信号长时间中断时SINS解算的置信度衰减速度。4.2 GPS量测噪声与数据更新率匹配量测噪声矩阵R的取值对应GPS接收机的位置和速度测量精度。消费级GPS单点定位水平约3到5米速度约0.1 m/s。R矩阵不是越大越稳而是要和Q匹配。一个常见问题是GPS位置噪声设得比实际小很多导致滤波增益过高GPS的随机噪声被当作真实位置偏移注入到导航结果中轨迹看起来会抖得厉害。另一个极端是R设得太大GPS量测几乎不起作用整个组合退化成纯SINS位置会随时间漂移。% 量测噪声矩阵设置示例 R_pos (5.0)^2 * eye(3); % 位置噪声方差, 5m R_vel (0.1)^2 * eye(3); % 速度噪声方差, 0.1m/s R blkdiag(R_pos, R_vel);更新率方面如果GPS频率是5Hz而IMU是100Hz量测更新的间隔是0.2秒。这个间隔内SINS的位置误差增量由Q传播如果Q设置偏小两次GPS校正之间滤波器会非常相信SINS一旦IMU有未建模误差量测更新时就会出现明显的修正跳变。观察滤波效果时关注每次GPS量测更新前后的位置残差残差如果在正负几米内随机摆动说明参数基本合理如果残差呈系统性偏置或持续同号说明存在杆臂、时间同步或坐标转换误差。4.3 杠杆臂与时间同步的两个隐蔽坑杠杆臂是指GPS天线相位中心与IMU测量中心之间的空间偏移。车辆上GPS天线装在车顶IMU装在车身内部两者可能相差一米以上。这个偏移如果在量测方程里不补偿会被滤波器的位置误差完全吸收表现为车辆转弯时组合导航输出的轨迹向外侧偏出。% 杠杆臂补偿在构造z之前执行 % 将GPS天线位置转换到IMU中心 % lever_arm_n Cbn * lever_arm_b; % pos_imu gps_pos - lever_arm_n;时间同步问题隐蔽性更强。GPS输出时刻是接收机内部的测量时刻但在串口传输和协议解析中会产生几十到几百毫秒的延迟这个延迟对应车辆在高速公路上几十米的位移。要验证时间同步是否准确可以让车辆做直线加减速观察GPS速度与SINS速度的残差是否与加速度相关。如果减速时残差出现正向尖峰加速时出现负向尖峰基本可以断定存在固定时间延迟需要在数据预处理阶段做平移补偿。5. 验证与调试三个快速检查手段拿到这套程序并调好参数之后强烈建议先做静基座测试再上动态数据。静基座测试是指把IMU静止放置在桌面上同时保证GPS天线有良好信号然后以组合导航模式运行程序。理想情况下组合后的位置输出应该稳定在一个小范围内速度接近零姿态角保持恒定。如果静基座下位置发散或速度出现周期振荡说明滤波器的Q和R没匹配好此时调参数的效果最直观也最容易收敛。具体操作是运行程序后把result.pos减去初始位置画一条三轴位置误差曲线观察十分钟内的漂移量。第二个快速检查是故障注入模拟GPS信号丢失。常见做法是把数据里的GPS量测从中间某一段全部置为无效观察组合导航在GPS缺失期间的位置漂移速度。MEMS IMU的频率和精度决定能撑多久消费级IMU可能在几十秒内漂移几米到十几米导航级IMU则能维持几分钟的高精度。这个测试可以验证滤波器的Q矩阵是否正确——如果GPS中断后位置发散速度与理论推算差异过大通常是Q矩阵中陀螺漂移的驱动噪声设置有问题。第三个技巧是利用Allan方差做IMU误差预分析再回头设参数。虽然这个数据包不一定自带Allan方差工具但可以在Matlab里简单实现取一段静止IMU数据按不同积分时间计算角速度均方根画出对数坐标下的Allan方差曲线读取零偏不稳定性和随机游走系数用这些实测值替换滤波器里的陀螺和加计噪声参数远比拍脑袋设置靠谱。具体的代码思路是用循环累加均值再求方差20行左右就能完成网上这类Matlab实现很多可以直接套用到这个程序包的数据文件上验证IMU数据质量和参数是否自洽。本文还有配套的精品资源点击获取
