简介这是一套基于Matlab的全球定位系统与惯性导航系统组合导航仿真程序面向组合导航算法初学者、导航专业学生及无人机、车辆定位开发者。资源重点解决卫星信号在城区林荫道或受电磁干扰时容易丢失的问题通过卫星定位与惯性推算互补在信号失锁期间仍能连续输出速度与姿态信息。压缩包仅六百七十千字节共五个文件两个脚本用于主流程与卡尔曼滤波融合一个数据文件提供惯导与卫星量测输入一份程序说明梳理运行步骤一份结果文档展示误差曲线与精度分析。已有超过一千七百人下载学习适合希望掌握松组合或紧组合实现细节并快速验证的读者。程序覆盖初始化、数据融合、实时更新与误差修正等环节配合示例数据与文档可降低复现难度帮助理解组合导航从理论到工程落地的完整流程。1. GPSINS组合导航为什么这两个传感器必须绑定搞过定位的人大多经历过这种时刻城市高架桥下GPS信号来回跳变纯卫星定位画出一堆假轨迹进了隧道更是直接连星都丢了。惯性导航INS的短期精度很好更新率又高可它靠积分加速度和角速度推算出位置跑得越久漂移越大。GPSINS组合导航就是把这两者绑在一起干活GPS负责长期不漂移INS负责短期不丢星中间用卡尔曼滤波把两者缝起来。这篇笔记拆的是一套可直接运行的GPSINS组合导航代码包包含仿真数据生成、15维EKF融合主循环、配套文档和几个我踩过的坑适合想快速上手组合导航的初学者也适合已经在调定位算法的同学拿来当参照系。2. 原理选型从误差状态到15维卡尔曼滤波设计2.1 为什么GPS和INS必须绑在一起GPS是典型的绝对定位技术靠卫星信号测量距离位置误差不会随时间累积长期精度好。但它更新率低普遍在10Hz左右城市峡谷里多路径严重信号还会整段丢失。INS是相对定位技术陀螺和加速度计量测的是角速度和比力能在100到400Hz的高频率下递推姿态、速度和位置短时间内精度很高。但它有两个核心误差源陀螺零偏、加计零偏和测量噪声。这些误差经过两次积分后会放大成明显漂移两分钟偏出去几米很正常。我常用一个比喻来理解这套系统GPS是一个偶尔告诉你“你在哪”但会撒谎的同事INS是一个勤快但方向感很差的助手卡尔曼滤波则是那个结合两边说法判断真实位置的决策者。这里的“撒谎”对应GPS的多路径、周跳和丢星而“方向感差”对应INS的零偏和时间积分漂移。组合导航要解决的核心问题有两个第一在GPS不可用或劣化时继续输出一个不发散的导航解第二在GPS可用时同步估计出INS的零偏等误差参数让这个“勤快助手”越来越靠谱。两者互为校准缺一不可。2.2 松耦合和紧耦合怎么选组合导航按耦合深度可以分松耦合、紧耦合和超紧耦合三类。松耦合最简单GPS接收机自己完成定位输出经纬度和速度组合滤波器把它们当作量测直接使用。这种结构清晰GPS和INS两个系统相对独立便于分开调试。缺点也很明显GPS输出要求至少四颗以上有效卫星当可见星很少时GPS解算精度会大幅打折而这种“打折”在松耦合里很难被上层及时感知。紧耦合则不再假设GPS先完成定位而是把单颗卫星的伪距、载波相位、多普勒等原始观测直接送进滤波器。即使只有三颗卫星滤波器依然能用部分约束去修正INS误差鲁棒性更好。代价是状态维度和调参工作量显著增加还要处理电离层、对流层等误差模型。我的选型习惯是第一次移植代码验证算法成本先上松耦合产品化或者测试车队在城市环境里跑再往紧耦合过渡。这份资源默认实现的是松耦合因为它是理解整个融合流程最快的方式。紧耦合的实现难度和排错成本对新手来说不太友好先把松耦合的原理吃透再考虑升级是更务实的路径。2.3 误差状态和15维状态量组合导航的状态向量通常不直接用位置、速度、姿态的具体数值而用“误差状态”。原因很实际惯导积分出的名义轨迹和真实轨迹往往只差“一小点”比如姿态误差0.01°速度误差0.1m/s位置误差几米。误差量近似线性滤波器模型可以保持近似线性数值稳定性更好不容易出现奇异和跳变。最常见的误差状态是15维姿态误差3维位置误差3维速度误差3维陀螺零偏3维加计零偏3维。不同代码包的排列顺序不一样有的把位置放在姿态前面有的把速度放最后这都没关系关键是状态转移矩阵F和观测矩阵H要和排列顺序严格对应。我拿到一份新代码第一件事就是画一张“维度序号表”把每个维度对应哪个物理量标出来否则改参数时很容易搞乱矩阵拼接。# 状态向量维度约定0姿态误差(3)3位置误差(3)6速度误差(3) # 9陀螺零偏(3)12加计零偏(3) STATE_DIM 15 IDX_ATT [0, 1, 2] IDX_POS [3, 4, 5] IDX_VEL [6, 7, 8] IDX_GYRO_BIAS [9, 10, 11] IDX_ACC_BIAS [12, 13, 14]这个索引表不是摆设F矩阵和H矩阵的拼接顺序都以它为准。比如GPS观测到的是位置残差那观测矩阵H就要取IDX_POS对应的那几列其余列置零。这个细节最容易出错后面避坑章节还会专门展开。2.4 EKF还是UKF组合导航的非线性主要来自姿态更新和坐标变换。扩展卡尔曼滤波EKF的做法是在当前估计点做一阶线性化计算量小实现成熟是业内最常用的框架。无迹卡尔曼滤波UKF不强求雅可比矩阵对强非线性模型精度更高但15维状态要生成31个Sigma点计算开销比EKF高一个量级在嵌入式实时环境里负担不小。这份资源默认使用EKF核心是两个矩阵的更新预测阶段用离散化状态转移矩阵F和过程噪声矩阵Q量测更新阶段用观测矩阵H和量测噪声R。代码里有疑问时优先检查H矩阵的行列数是否和量测数据对齐比如松耦合量测是三维位置加三维速度H矩阵就是6×15。还有一个选型细节值得注意。EKF需要提供过程噪声和量测噪声这两个矩阵的比值决定了滤波器的性格放Q大会让滤波器认为状态快速变化、更依赖GPS观测结果带高频噪声放R大会让滤波器更相信INS递推轨迹平滑但GPS真值修正时反应迟缓。这套资源的默认配置偏向适中平滑跑完对照误差图再微调。调试时我一般先固定Q只调R等方向对了再反过来调Q这样能减少变量定位问题更快。# 初始化Q矩阵和R矩阵的常见做法 Q np.eye(STATE_DIM) * 1e-4 Q[IDX_ATT, IDX_ATT] 1e-5 # 姿态噪声较小 Q[IDX_GYRO_BIAS, IDX_GYRO_BIAS] 1e-6 # 零偏随机游走 R np.eye(6) * 0.1 R[3:6, 3:6] 0.01 # 速度量测噪声小于位置这段初始化的逻辑是Q的物理含义是“状态模型没描述清楚的部分”比如载体振动、零偏缓慢变化通常给得比传感器白噪声大一个量级R的物理含义是“观测本身的可信程度”GPS位置噪声天然比速度噪声大所以位置方向的R设为0.1速度方向设为0.01。这两个矩阵是调试组合导航最核心的旋钮没有之一。3. 跑通组合导航代码仿真数据、融合主循环与结果解读3.1 套件目录与运行环境这份资源包拿到手不是一堆散乱文件目录结构很清晰方便按步骤复现。我打开了第一眼感觉它的组织方式比较合理四块内容各司其职。目录结构如下路径作用config.yaml轨迹和噪声参数配置文件data/仿真数据输出目录src/generate_data.py生成参考轨迹、IMU、GNSS观测src/fusion.pyEKF融合主循环src/plot_results.py误差绘图脚本docs/算法说明文档运行环境只需要基础科学计算库Python 3.10以上都可以。依赖安装和跑通流程就三步pip install numpy scipy matplotlib pyyaml python src/generate_data.py python src/fusion.py顺序执行后终端会把融合轨迹文件和误差分析文件输出到data目录。如果你只想先看一眼效果再深入代码那先跑这两个脚本就够了。代码里有两个容易被忽略但很关键的参数gnss_update_rate控制GPS更新率默认10Hzsim_time控制仿真时长默认180秒。改这两个参数就能快速做长航时和高更新率的不同测试。3.2 生成仿真数据轨迹、IMU与GNSS观测组合导航的调试不能只用真机数据因为真机数据的真值很难获得。仿真生成器可以先定义一条参考轨迹再根据轨迹反推IMU理论输出最后按GPS接收机的特性叠加观测误差。这样每一步的误差都可控你才能确认滤波器有没有把误差正确抑制掉。参考轨迹通常由若干段组成直线加速、匀速转弯、爬升、减速。这样能让IMU的加速度变化覆盖不同频段也方便检查融合后的位置误差是否和机动强度相关。参考轨迹生成要注意量纲位置用米姿态用弧度千万不要用度否则后续旋转矩阵拼接时反复换算容易出隐蔽错误。IMU仿真的核心代码片段是这样def simulate_imu(traj_points, dt, gyro_bias, acc_bias, gyro_noise, acc_noise): 从参考轨迹生成角速度和比力观测 traj_points: 参考轨迹点位置、姿态 dt: IMU采样间隔 # 从姿态序列差分得到参考角速度叠加零偏和白噪声 gyro traj_angular_velocity(traj_points, dt) gyro_bias gyro np.random.normal(0, gyro_noise, gyro.shape) # 从轨迹差分得到线加速度旋转到载体坐标系再叠加噪声 acc traj_linear_velocity_diff(traj_points, dt) acc rotate_vector(acc, traj_points.attitude) acc_bias acc np.random.normal(0, acc_noise, acc.shape) return gyro, acc参数说明traj_angular_velocity是从参考姿态序列微分得到角速度gyro_bias是固定的常值零偏gyro_noise是白噪声标准差。真实的陀螺噪声其实是有色的低频分量明显但这里先用白噪声近似已经足够验证滤波算法等跑通了再引入Allan方差建模也不迟。GNSS观测模型更直接在参考位置上叠加高斯噪声# 每 0.1 秒输出一次位置和速度位置误差设为高斯白噪声 gnss_pos ref_pos np.random.normal(0, pos_noise_std, (n, 3)) gnss_vel ref_vel np.random.normal(0, vel_noise_std, (n, 3))这段代码没有模拟多路径和丢星但它给出了一个干净基准方便先验证滤波器本身没有把数据源带坏。等你确认滤波器实现正确了再往数据生成器里加丢星、加跳变测试容错逻辑也不迟。3.3 融合主循环预测、量测更新与矩阵拼接融合主循环是整个资源包里最值得反复读的部分。IMU数据到达一次就做一次预测GNSS数据到达一次则触发量测更新。这种非等间隔触发结构贴近真实传感器的工作方式比固定步长仿真有参考价值得多。核心代码在fusion.py中实现主循环逻辑如下# fusion.py 主循环核心 for i in range(imu_n): dt imu_time[i] - imu_time[i - 1] # 1. 用IMU观测递推名义轨迹 predict_nominal(gyro[i], acc[i], dt) # 2. 预测误差状态协方差 F state_transition_matrix(dt) P F P F.T Q # 3. 如果当前时刻有GNSS观测做量测更新 if gnss_sync_idx gnss_n and abs(imu_time[i] - gnss_time[gnss_sync_idx]) 1e-3: z gnss_obs[gnss_sync_idx] y z - H x_err S H P H.T R K P H.T np.linalg.inv(S) x_err x_err K y P (np.eye(STATE_DIM) - K H) P gnss_sync_idx 1逻辑说明第2行先用IMU的高频采样更新姿态、位置等名义值这个更新率约200Hz。第7行是核心每次预测都要把协方差矩阵往前推这个矩阵表达的是“当前误差有多大”的置信度。第11到20行属于量测更新把GPS观测转为ENU坐标后计算新息y通过卡尔曼增益K修正误差状态再更新协方差P。这个循环里最容易出问题的变量是dt。如果IMU时间戳单位混了dt会变成极小或极大值P矩阵会瞬时发散。我见过有人把微秒和毫秒混在一起融合结果直接飞掉排查了整整两天。时间单位统一是第一步建议在读取数据后就强制转换到秒。观测矩阵H的写法也需要留意。误差状态序列是姿态误差3、位置误差3、速度误差3、零偏6而GPS观测只对应位置和速度所以H矩阵取前6行零偏列全部置零。很多调试者在这里把H写错导致滤波器完全没有量测更新效果可GPS数据明明是通的这类问题最难排查。3.4 看懂输出误差曲线与定位指标运行完fusion.py会生成两个文件fused_trajectory.csv保存融合轨迹error_analysis.csv保存融合和纯惯导各自的定位误差。读这两个文件时我一般看三个指标RMSE是均方根误差能综合反映偏差和抖动是横向对比滤波器参数最常用的单值指标。最大误差看最坏情况如果最大误差发生在转弯时往往和机动加速度有关。误差分布直方图则用来观察是否近似正态分布如果偏态严重说明有个固定偏差没被滤掉比如杆臂未补偿或者坐标转换存在常数偏差。可视化对比只需要一条命令python src/plot_results.py这个脚本会输出三张图全局轨迹、融合误差曲线、纯INS误差曲线。判断标准很简单融合误差的漂移斜率应远小于纯INS。如果融合结果反而比纯INS还差问题几乎可以锁定在坐标系转换或H矩阵不用去调滤波增益。4. 避坑时间同步、坐标系和协方差初值的四个翻车点4.1 时间戳没对齐融合结果出现周期性锯齿现象跑完仿真融合轨迹和GPS轨迹叠加在一起在每秒左右位置出现微小锯齿而且误差周期和GPS更新率一致。这是时间戳不对齐最典型的外观视觉上像轨道边缘长了毛刺。原因GNSS接收机输出位置的时间戳是观测时刻但经过接收机内部处理、串口传输、上位机解析后数据进入融合主循环会晚几十到几百毫秒。如果把没对齐的GPS观测直接当当前时刻的位置去修正INS滤波器会把一个过去的观测量当成现在的导致周期性的修正过冲。解决先在源头用PPS秒脉冲做硬件对齐没有硬件条件时软件上做线性插值。代码里可以给GNSS数据做一个延迟补偿。常见做法是先估算固定延迟比如发现GNSS数据恒定晚100ms直接把量测时间戳整体后移100ms再做最近邻配对。跑仿真时也可以通过交叉相关计算两个序列的延迟量。4.2 坐标系混用ECEF当ENU用现象融合轨迹整体绕原点旋转或向某个方向漂移误差曲线的形状不是发散而是存在一个稳定的斜率方向和GPS残差一致。原因GPS模块输出的是经纬高或ECEF坐标而INS递推通常在ENU或NED导航坐标系下进行。如果直接把ECEF位置当作ENU位置输入误差量级会非常大卡尔曼滤波的最佳估计也就跟着跑偏。这个问题在视觉上很隐蔽因为轨迹形状看起来合理只是整体偏了。解决写一个显式的坐标转换函数并在跑数据前做一个自检在静态点上分别求ENU坐标和ECEF坐标做一次往返转换确认回读误差接近零。我还会用一个已知长度的静态基线去检验如果转换后的距离和真实距离差超过毫米级说明坐标参数或椭球模型用错了。4.3 初始协方差给太大启动瞬间状态跳变现象滤波器启动前五秒融合位置波动幅度明显大于后面几百秒的平均水平有的轨迹甚至直接偏出去一截再缓慢拉回。原因初始协方差矩阵P0描述的是对初始误差的置信程度。P给得过大意味着滤波器认为初始状态完全不可信它就会更大胆地相信前几个GPS观测量造成修正过冲。P给得过小又会让滤波器过于自信等价于拉不住误差。解决P0不要拍脑袋。参考器件手册的经验值位置误差方差给1米的平方量级速度误差方差给0.1米每秒的平方姿态误差方差给0.01度的平方。陀螺零偏方差可以参考静止数据方差加计零偏同理。调试时把P0记录下来用同一个轨迹做A/B对照只改P0其他不变就能快速看出它有多大影响。4.4 GPS跳变时滤波器跟着跳新息没有设防现象正常路段轨迹平滑但在某个位置比如桥下或建筑物旁融合轨迹突然多出一个和GPS跳变形状一样的毛刺之后又逐渐拉回。原因GPS在多路径环境下可能出现短时异常观测比如伪距反射导致位置突然偏出几米。卡尔曼滤波本身不会拒绝这种异常观测反而会把它当成真实观测去修正状态于是融合结果被带偏。解决加新息卡方检验。新息就是量测值和预测值的差正常情况下它应该满足零均值高斯分布异常时某个分量会突然变大。设置一个门限超过门限就跳过本次量测更新或降低权重from scipy.stats import chi2 # 新息卡方门限判断 gate chi2.ppf(0.95, dflen(z)) innovation_sq y np.linalg.solve(S, y) if innovation_sq gate: # 通过检验正常执行量测更新 x_err x_err K y else: # 认为GNSS观测异常跳过本次更新 pass门限的df要和观测维度对应。松耦合6维观测就用df6一旦用错维度所有观测都可能被误杀。卡方门限的选择会影响滤波器对异常观测的敏感度一般先按95%分位用现场再根据实际误杀率微调。5. 落地标定零偏、杆臂和PPS时间同步怎么做5.1 IMU零偏标定静止采集和旋转采集陀螺零偏是INS漂移的主要来源姿态误差积分后最终会变成几十上百米的位置误差。零偏标定有两种常见方案。静止采集最简单把IMU固定在水平台上不动采集两到三分钟求角速度和加速度平均值。静态均值法只能估计零偏的常值部分随机游走等慢变部分需要Allan方差分析才能分离。如果只是跑通这套仿真资源静止采集完全够用如果要接真实传感器建议再做一次温度循环标定。旋转采集更进一步让IMU绕不同轴旋转理论上能分离零偏和比例因子还能发现安装误差耦合。实际项目里我会在开始采集前静止一分钟跑完再做一次零偏复核。如果复核值和初始值差很多说明导航解算过程已经漂移了前面那段数据不能信。需要注意滤波框架里的在线零偏估计和离线标定并不冲突。P矩阵的最后6维对应陀螺和加计零偏初始值可以用离线标定结果填收敛后Q矩阵里再留一点点随机游走让它能继续跟踪温度变化引起的零偏漂移。5.2 杆臂标定IMU中心到GPS天线相位中心杆臂是IMU中心与GPS天线相位中心之间的三维位置差。车辆转弯时IMU感受到的角速度和GPS天线感受到的加速度不一样如果不补偿这个长度差高速转弯时会出现明显的位置超前或扫描式误差。车顶GPS天线通常比IMU高几十厘米、偏几十厘米转弯越急误差越大。杆臂可以用三维标注工具测量或者用普通钢尺从一个已知参考点量到另一个点在车体坐标系下分别量出x、y、z三个方向的偏移填入config文件。验证办法是让车绕圆形路线持续行驶观察融合估计和RTK参考轨迹的横向误差是否随转弯方向变化。如果转弯方向一致时误差同向变大大概率是杆臂符号写反了或某个方向量错了。5.3 PPS秒脉冲从硬件对齐到软件插值GPS接收机会在上电后输出PPS秒脉冲每个脉冲和UTC秒边界对齐。把PPS接到单片机或工控机的GPIO记录脉冲到达时刻再用中断时间戳修正IMU采样时间和GPS消息时间这是组合导航产品的标准做法。没有硬件条件时也可以用软件后处理插值# 将低采样GNSS观测插值到IMU时间轴 gnss_aligned np.interp(imu_time, gnss_time, gnss_pos)np.interp只能做一维插值三维位置需要逐维调用。如果GNSS时间戳有明显的固定延迟插值前先做常数偏移修正效果会好很多。注意这个方案是拿软件时序赌硬件时序如果系统调度抖动大插值结果依然有残余误差真正的高精度应用仍然绕不开PPS。5.4 参数表零偏、更新率与观测噪声的合理初值参数初值的合理范围取决于传感器层级和运动场景。以常见MEMS IMU为例陀螺零偏一般在1度每小时到100度每小时之间加计零偏从1mg到几十mg不等。IMU仿真代码可以随便填但实车数据必须反映真实器件水平否则在线估计的零偏值和真实值差太多融合反而更差。GPS位置噪声可以给0.1米到2米的量级1σ速度噪声0.01到0.1米每秒左右。值得注意的是这里的“噪声参数”本质是滤波器对数据源信任程度的一种表达不完全是传感器手噪声。没有建模的时间同步残留、多路径、载体弹性形变都会吃掉一部分噪声预算所以最终参数必然要在现场微调一次。参数建议范围备注陀螺零偏1-100 deg/h离线标定仿真可直接填加计零偏1-50 mg高精度器件更小GPS位置噪声0.1-2 m1σRTK模式可达0.02 mGPS速度噪声0.01-0.1 m/s取决于接收机和模式P0位置协方差(1 m)^2与真实初始偏差匹配卡尔曼更新率GPS 1-20 HzIMU 50-400 Hz从实际采集频率来6. 验证精度的一个实用技巧先退化GPS再恢复组合导航真正要验证的不是“GPS信号好的时候精度多高”而是“GPS失效时INS能顶多久恢复后多久能拉回”。我一般会给仿真脚本加一个退化场景前60秒GPS正常工作60到90秒把GPS噪声放大100倍并减少输出模拟信号丢失90秒以后恢复正常。具体做法是在config里加两个字段gnss_failure_start: 60 gnss_failure_end: 90数据生成脚本会在失效区间跳过GNSS观测生成或加大噪声。然后对比三条曲线纯INS轨迹、融合轨迹、GPS观测。评价要点有三个失锁期间融合轨迹的漂移斜率是否和初始零偏正相关恢复后三次有效GPS观测内滤波结果能否明显回到真值附近恢复瞬间有没有过冲或振荡有就说明P被推得太大了。做完之后我会格式化出一张对比表记录失锁30秒、60秒、120秒三种时长下的最大漂移和恢复时间以后换传感器型号、换硬件板卡都重跑一遍。组合导航玄学不少但“先退化GPS再恢复”是能快速暴露问题的最小测试值得每次都做。从那以后我每次跑组合导航都强制走一遍这个失锁恢复流程先让GPS健康跑30秒再故意屏蔽90秒最后看融合误差能不能在短时间内回到正常水平。这份资源包的配套仿真和数据集已经按这个思路搭好了拿到手就能直接复现。希望帮到你。本文还有配套的精品资源点击获取