简介这份资源是面向导航算法学习者与工程研发人员的GPS/INS组合导航仿真代码包围绕松耦合组合方式展开重点演示如何用卡尔曼滤波与Mahony AHRS姿态解算融合GPS定位信息与惯性传感器数据解决单一GPS在遮挡环境下精度下降、纯INS误差随时间累积的问题。压缩包共8个文件以7个.m脚本和1个.md说明文档为主整体约8KB脚本分别承担四元数运算、欧拉角转换、GPS/INS位置速度滤波、Mahony与Madgwick姿态更新及主程序导航解算等职责README则提供使用说明。已有260人学习。读者可借此理解传感器建模、数据融合接口与仿真流程掌握滤波器参数调节和结果输出方法为组合导航系统设计与优化提供可运行的参考范例。1. 拆开 GPS-INS-Integrated-Navigation-master一套能跑通松耦合的 MATLAB 组合导航代码如果你手头只有一段带噪声的加速度计和陀螺仪数据外加一串稀疏的 GPS 定位点想验证自己的融合算法到底能不能收敛这套GPS-INS-Integrated-Navigation-master.zip就是为这个场景准备的。它用 MATLAB 实现了 GPS/INS 松耦合组合导航核心文件包括main_NAV.m、kalman_GPS_INS_pv.m、madgwickAHRS.m、MahonyAHRSupdate.m、Quaternion_FromEulerAngle.m、quaternProd.m、quaternConj.m。整套代码不依赖任何工具箱纯脚本加函数适合做算法验证、课程设计、论文复现。它解决的不是产品级导航而是让你在桌面上把姿态解算、卡尔曼滤波、GPS 校正这三件事串起来看清每一步的输入输出。适合已经学过惯性导航基础、能看懂四元数乘法、想动手改参数看结果的人。下面按“资源是什么 → 怎么跑 → 坑在哪 → 怎么改”的顺序拆。2. 从四元数到卡尔曼这套代码的模块分工与数据流2.1 姿态解算层Mahony 与 Madgwick 两条路MahonyAHRSupdate.m和madgwickAHRS.m是两个独立的姿态更新函数输入都是陀螺仪角速度、加速度计比力、磁力计可选输出四元数。Mahony 走的是非线性互补滤波用加速度计修正陀螺仪积分漂移Madgwick 则是梯度下降加互补滤波计算量略大但动态响应更平滑。代码里两个都给了说明作者想让你对比。实际跑的时候main_NAV.m里默认调用哪一个取决于你注释掉哪一行。常见做法是先用 Mahony 跑通因为参数少主要调Kp和Ki两个增益。% MahonyAHRSupdate.m 核心片段 function [q] MahonyAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz, q, Kp, Ki) % 归一化加速度计和磁力计 norm_a sqrt(ax^2 ay^2 az^2); ax ax / norm_a; ay ay / norm_a; az az / norm_a; % 估计重力方向与测量重力方向叉积得到误差 halfvx q(2)*q(4) - q(1)*q(3); halfvy q(1)*q(2) q(3)*q(4); halfvz q(1)^2 - 0.5 q(4)^2; halfex (ay * halfvz - az * halfvy); halfey (az * halfvx - ax * halfvz); halfez (ax * halfvy - ay * halfvx); % PI 修正陀螺仪零偏 gx gx Kp * halfex; gy gy Kp * halfey; gz gz Kp * halfez; % 四元数积分 qDot 0.5 * quaternProd(q, [0 gx gy gz]); q q qDot * 0.01; % 0.01 为采样周期需与数据实际频率一致 q q / norm(q); end逻辑说明先归一化加速度计消除量纲影响再用当前四元数推算理论重力方向与实测重力方向做叉积得到姿态误差误差乘以Kp直接补偿陀螺仪Ki用于补偿陀螺仪零偏。参数Kp典型值 0.52.0Ki取 0.0010.01。Kp太大姿态会抖太小收敛慢。采样周期 0.01 对应 100Hz如果你的 IMU 是 200Hz这里必须改成 0.005否则四元数积分会翻车。2.2 组合导航层松耦合卡尔曼滤波怎么接kalman_GPS_INS_pv.m是位置-速度卡尔曼滤波器状态量是[px, py, pz, vx, vy, vz]观测量是 GPS 的位置和速度。INS 通过加速度计积分得到预测位置和速度GPS 到来时做量测更新。代码里没有做姿态与位置的联合滤波姿态只用于把机体加速度转到导航系所以是典型的松耦合。main_NAV.m负责读数据、循环调用姿态更新和卡尔曼更新、画图。% kalman_GPS_INS_pv.m 预测步核心 function [x_pred, P_pred] kalman_predict(x, P, acc_nav, dt, Q) % 状态转移矩阵 F F [1 0 0 dt 0 0; 0 1 0 0 dt 0; 0 0 1 0 0 dt; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; % 控制输入矩阵 B把导航系加速度映射到位置和速度 B [0.5*dt^2 0 0; 0 0.5*dt^2 0; 0 0 0.5*dt^2; dt 0 0; 0 dt 0; 0 0 dt]; x_pred F * x B * acc_nav; P_pred F * P * F Q; end逻辑说明dt是 IMU 采样周期acc_nav是去掉重力后的导航系加速度。Q是过程噪声协方差反映你对 INS 积分结果的信任程度。Q越大滤波器越相信 GPSQ越小越相信 INS 短期推算。常见做法是Q取对角阵位置项 1e-4速度项 1e-2具体看你的 IMU 噪声密度。量测更新部分用 GPS 位置和速度构造H矩阵计算卡尔曼增益更新状态和协方差。注意 GPS 速度不是所有模块都输出如果你的数据只有位置把H和R的对应行去掉即可。2.3 数据流与文件依赖关系main_NAV.m是入口先加载数据再初始化四元数和卡尔曼状态然后进入主循环。每个循环里读一帧 IMU → 调用MahonyAHRSupdate或madgwickAHRS更新姿态 → 用quaternConj和quaternProd把机体加速度转到导航系 → 调用kalman_GPS_INS_pv做预测 → 如果当前时刻有 GPS做量测更新 → 保存结果。Quaternion_FromEulerAngle.m用于初始化姿态如果你知道初始欧拉角用它转成四元数。quaternProd.m和quaternConj.m是四元数乘法和共轭姿态更新和坐标变换都依赖它们。整个数据流没有闭环控制纯开环仿真所以你可以随意替换数据源。3. 跑通 main_NAV.m数据准备、参数配置与结果验证3.1 数据格式与加载方式代码没有附带真实数据集main_NAV.m里通常用load读一个.mat文件变量名可能是imu_data和gps_data。IMU 数据一般包含时间戳、三轴陀螺仪rad/s、三轴加速度计m/s²。GPS 数据包含时间戳、经纬高或局部坐标、速度。如果你手头是neo-m8n gps模块输出的 NMEA 语句需要先转成局部坐标。常见做法是用geodetic2ned或自己写墨卡托投影把经纬高转成东北天坐标。注意 GPS 时间戳和 IMU 时间戳要对齐代码里一般用interp1插值到 IMU 时刻。% 数据加载与时间对齐示例 load(imu_data.mat); % 包含 t_imu, gyro, accel load(gps_data.mat); % 包含 t_gps, pos_gps, vel_gps % 把 GPS 位置插值到 IMU 时间轴 pos_gps_interp interp1(t_gps, pos_gps, t_imu, linear, extrap); vel_gps_interp interp1(t_gps, vel_gps, t_imu, linear, extrap); % 标记哪些 IMU 时刻有 GPS 观测 has_gps ~isnan(pos_gps_interp(:,1));逻辑说明interp1的extrap选项允许外推但 GPS 缺失段外推会引入虚假观测建议把外推部分置NaN在卡尔曼更新时用has_gps判断是否执行量测更新。参数t_imu和t_gps单位要一致通常用秒。如果你的 GPS 是 1HzIMU 是 100Hz插值后大部分时刻没有 GPS这是正常的滤波器靠 INS 递推。3.2 关键参数配置表参数含义典型值调整影响KpMahony 比例增益0.52.0越大姿态收敛快但噪声放大KiMahony 积分增益0.0010.01补偿陀螺零偏太大会缓慢振荡dtIMU 采样周期0.01s (100Hz)必须与实际数据频率一致Q_pos位置过程噪声1e-4越大越信 GPSQ_vel速度过程噪声1e-2越大越信 GPSR_posGPS 位置观测噪声110根据 GPS 精度设单位 m²R_velGPS 速度观测噪声0.11根据 GPS 速度精度设这些参数没有万能值我一般先让Q和R保持默认跑一遍看位置误差曲线如果 INS 发散太快就增大Q如果 GPS 跳变被过度平滑就减小R。注意R的单位要和状态量一致位置用 m²速度用 (m/s)²。3.3 运行与结果验证直接运行main_NAV.m正常会弹出三个图姿态角、位置、速度。姿态角看横滚和俯仰是否收敛到合理范围位置看组合结果是否比纯 INS 更贴近 GPS。验证方法把 GPS 量测更新关掉只跑 INS 积分位置会在几十秒内漂出几百米打开更新后位置应被拉回 GPS 附近。如果姿态角一直发散先检查加速度计符号和坐标系定义常见错误是加速度计正方向搞反导致重力估计错误。% 关闭 GPS 更新的对比测试 if has_gps(k) [x, P] kalman_update(x, P, pos_gps_interp(k,:), vel_gps_interp(k,:), R); end % 注释掉上面 if 块只保留预测观察纯 INS 漂移逻辑说明通过条件开关可以快速对比松耦合和纯 INS 的效果。kalman_update内部计算增益K P_pred * H / (H * P_pred * H R)然后更新x x_pred K * (z - H * x_pred)。如果R设得过大K趋近 0GPS 几乎不起作用结果和纯 INS 一样。4. 避坑与排查四元数、坐标系、时间同步的五个血泪经验4.1 现象姿态角在静止时缓慢旋转 → 原因陀螺仪零偏未补偿 → 解决增大Ki或先做静态零偏校准Mahony 的积分项就是干这个的但Ki太小时补偿速度跟不上零偏。我一般先让 IMU 静止采 30 秒求陀螺仪均值作为零偏在进入滤波器前减掉。如果代码里没有这一步Ki设到 0.01 以上但会引入低频振荡。注意静态校准要在恒温下做温度变化后零偏会变。4.2 现象位置在 GPS 更新时跳变 → 原因GPS 坐标系与 INS 导航系不一致 → 解决统一到东北天或北东地常见错误是 GPS 给的是经纬高直接当米用。必须先转局部坐标。另外注意 INS 导航系是东北天还是北东地kalman_GPS_INS_pv.m里状态量顺序要和 GPS 转换后的顺序一致。如果 GPS 是北东地INS 是东北天更新时 y 和 z 会反号位置曲线会出现锯齿。4.3 现象四元数模长逐渐偏离 1 → 原因积分后未归一化或dt错误 → 解决每步强制归一化核对dtMahonyAHRSupdate末尾有q q / norm(q)但如果你自己改了代码忘了加四元数会漂。另外dt写错会导致积分步长不对比如实际 200Hz 写成 0.01四元数旋转速度会慢一倍姿态跟踪不上。检查方法静止时四元数应保持常数旋转时模长始终为 1。4.4 现象卡尔曼滤波结果比纯 INS 还差 → 原因Q和R量级不匹配 → 解决先调R再调Q如果R设得太小GPS 噪声被完全信任位置会跟着 GPS 跳。如果Q设得太大预测步几乎不起作用滤波器退化成 GPS 平滑。我一般先把R设为 GPS 标称精度的平方比如 GPS 水平精度 2mR_pos设 4然后微调Q让曲线在 GPS 缺失段漂移可接受。4.5 现象quaternProd报维度错误 → 原因四元数行向量与列向量混用 → 解决统一为行向量或列向量代码里quaternProd(q, [0 gx gy gz])要求两个输入维度一致。如果你从别处拷来的四元数是列向量这里会报错。打开quaternProd.m看第一行如果是function ab quaternProd(a, b)检查a和b的size。统一用行向量最省事MATLAB 默认也是行向量。5. 进阶改法把 Mahony 换成 Madgwick再给卡尔曼加零偏状态5.1 切换姿态算法并对比动态响应main_NAV.m里把MahonyAHRSupdate替换成madgwickAHRS注意 Madgwick 的输入参数是Beta而不是Kp/Ki。Beta典型值 0.010.1越大越信加速度计越小越信陀螺仪。切换后跑同一段数据看横滚角在快速机动时的滞后。Mahony 在低频振动下更稳Madgwick 在加减速剧烈时收敛更快。我一般两个都跑取曲线重合度高的那个。% 切换为 Madgwick % [q] MahonyAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz, q, Kp, Ki); [q] madgwickAHRS(gx, gy, gz, ax, ay, az, mx, my, mz, q, Beta);逻辑说明Madgwick 内部用梯度下降求姿态误差梯度再与陀螺仪积分融合。Beta相当于梯度下降步长太大姿态会抖太小漂移抑制弱。建议从 0.03 开始试。5.2 给卡尔曼加陀螺仪零偏状态松耦合的kalman_GPS_INS_pv.m只估计位置和速度陀螺零偏靠 Mahony 的Ki补偿。如果你想更精确可以把状态扩到 9 维加入三轴陀螺零偏观测量不变。这样 GPS 位置误差会间接修正零偏。改法状态转移矩阵加零偏项零偏随机游走Q加零偏过程噪声。代码改动量不大但需要重新调Q。我试过在低精度 IMU 上扩维位置误差能降 20% 左右代价是收敛变慢。5.3 用真实 GPS 数据替换仿真数据如果你有neo-m8n gps模块的实测数据先解析 NMEA 的GPGGA和GPRMC提取经纬高、速度、时间。用geodetic2ned转局部坐标参考点取第一条 GPS 的经纬高。IMU 数据如果是gps 数据里常见的 MPU6050 输出注意量程和单位陀螺仪可能是 °/s要转 rad/s。时间同步用 GPS 的 PPS 或串口时间戳对齐。替换后先跑纯 INS 看漂移速率再开组合看修正效果。从那以后我每次拿到新数据都先画一遍原始加速度计和陀螺仪曲线确认没有截断和符号错误再进滤波器。希望帮到你。本文还有配套的精品资源点击获取