简介一套基于Matlab的惯性/GPS组合导航程序集合面向组合导航、制导与控制方向的科研人员和学生主要用于轨迹生成、传递对准、惯性/GPS滤波与导航解算等算法验证。包内按功能组织包含轨迹仿真脚本、滤波解算程序、惯性器件数据文件以及传递对准说明文档可帮助使用者快速理解从轨迹生成到解算输出的完整链路。资源共330个文件以m脚本为主另含dat数据、txt文本、doc说明文档、mat数据文件等压缩包约1.48MB目录便于按模块检索。借助这些程序可以复现组合导航典型流程深入分析状态方程、量测更新和对准实现细节尤其适合需要搭建Matlab仿真环境、开展组合导航算法对照实验的中高级用户。目前该资源已有186人学习浏览是一份结构较完整的导航程序参考包。1. 组合导航不是“把两个传感器数据加起来”惯性/GPS组合导航在MATLAB里跑通一套完整程序难点从来不在单个传感器的解算而在“误差怎么建模、状态怎么估计、轨迹怎么评价”这三件事的咬合。单独做惯导递推几分钟就发散单独用GPS输出频率低且在城市峡谷里跳变严重。把两者组合起来本质是用GPS的绝对观测去约束IMU的积分漂移再用IMU的高频输出去填补GPS的采样间隙。适合正在做车载/无人机/行人导航课题的研究生以及需要快速验证组合算法、又不想从零写矩阵运算的工程师。本文围绕轨迹生成、滤波、解算三个环节给出可直接运行的MATLAB程序骨架。你会看到一条完整链路先按设定航线生成带姿态的IMU/GPS仿真数据再通过捷联惯导算法做机械编排最后用卡尔曼滤波把位置误差、速度误差、姿态误差和传感器零偏一起估计出来并反馈校正。代码全部基于MATLAB 2023b编写老版本只需把arguments块改成传统传参方式即可。2. 轨迹生成先造出可复现的组合导航测试环境组合导航程序的价值取决于测试数据的质量。用真实传感器采集数据固然好但真机实验成本高、误差源不可控、真值难以获取。在MATLAB中生成仿真轨迹可以精确控制IMU器件误差、GPS更新率、卫星失锁时段等条件让算法在“已知答案”的环境下调试。2.1 用分段圆弧和直线拼接二维轨迹常见做法是用“直线段圆弧段”拼接出参考轨迹再把参考轨迹离散成IMU采样点。这种方法生成的运动学参数位置、速度、加速度都是解析可导的便于后面对照真值验证滤波结果。% 生成一条包含直行和匀速转弯的平面轨迹 % 轨迹点格式t, x, y, psi, vx, vy, ax, ay function traj genRefTrajectory(dt, T_total) t 0:dt:T_total; x zeros(size(t)); y zeros(size(t)); psi zeros(size(t)); vx zeros(size(t)); vy zeros(size(t)); % 参数定义 v 10; % 巡航速度 10 m/s R 50; % 转弯半径 50 m t_straight1 5; % 第一段直行时间 t_turn pi*R/v; % 完成180度转弯所需时间 t_straight2 5; % 第二段直行时间 for i 2:length(t) if t(i) t_straight1 % 直行段1沿x轴匀速前进 psi(i) 0; x(i) x(i-1) v*dt; y(i) 0; elseif t(i) t_straight1 t_turn % 转弯段以恒定角速度 omega v/R 转弯 omega v/R; psi(i) psi(i-1) omega*dt; x(i) x(i-1) v*cos(psi(i))*dt; y(i) y(i-1) v*sin(psi(i))*dt; else % 直行段2沿转弯结束后的方向匀速直行 psi(i) pi; x(i) x(i-1) v*cos(psi(i))*dt; y(i) y(i-1) v*sin(psi(i))*dt; end vx(i) v*cos(psi(i)); vy(i) v*sin(psi(i)); end traj table(t, x, y, psi, vx, vy, zeros(size(t)), zeros(size(t)), ... VariableNames, {t,x,y,psi,vx,vy,ax,ay}); end代码逻辑先分段判断当前时刻属于直行、转弯还是第二段直行再根据运动学递推位置。注意转弯段用的是“离散时间增量下的圆弧积分”当dt取0.01秒时误差可以忽略如果dt较大建议改用解析式x R*sin(omega*t)而不是增量积分避免圆弧不闭合。参数推荐值影响dt0.01~0.02 s过大会导致轨迹不闭合过小则数据量膨胀v5~20 m/s速度越快转弯段越短动态激励越强R50~200 m转弯半径越小角速度越大陀螺零偏可观测性越强2.2 根据轨迹反推IMU量测陀螺加计读数生成有了参考轨迹中的姿态角和加速度就可以生成IMU的理想输出。真实IMU输出的是“比力”specific force即载体加速度扣除重力加速度后在载体系下的分量。这是初学者最容易搞错的环节加速度计测的并不是重力加速度而是“抵消重力所需的支撑力”。% 从参考轨迹生成IMU理想量测并叠加误差 function [gyro, accel] genImuMeasurements(traj, imu_params) n height(traj); gyro zeros(n, 3); % 角速度绕z轴的偏航角速度 accel zeros(n, 3); % 比力x向前向比力z向包含重力补偿 g 9.80665; for i 1:n % 偏航角速度由航向角差分得到 if i 1 gyro(i, 3) wrapToPi(traj.psi(i) - traj.psi(i-1)) / (traj.t(i) - traj.t(i-1)); end % 比力载体系加速度 导航系加速度经过旋转矩阵转到载体系 psi traj.psi(i); R_nb [cos(psi), sin(psi), 0; -sin(psi), cos(psi), 0; 0, 0, 1]; % 导航系到载体系简化2D a_nav [traj.ax(i); traj.ay(i); g]; % 导航系加速度加重力 f_b R_nb * a_nav; accel(i, :) f_b; end % 叠加器件误差常值零偏 高斯白噪声 gyro(:, 3) gyro(:, 3) imu_params.gyro_bias_z randn(n,1)*imu_params.gyro_noise; accel accel repmat(imu_params.accel_bias, n, 1) randn(n, 3)*imu_params.accel_noise; end这段代码里有一个值得展开的细节R_nb矩阵构造的是“导航系到载体系”的旋转但MATLAB的angle2dcm函数默认返回的是“载体系到导航系”的旋转矩阵两者互为转置。如果直接拿angle2dcm(psi,0,0)的结果乘导航系向量得到的是导航系分量不是比力。动手写代码时务必先把坐标变换方向画清楚这是组合导航仿真最常见的错误来源。参数说明imu_params结构体里的gyro_bias_z通常设为 0.01 rad/s约每小时36度gyro_noise设为 0.001 rad/s/√Hzaccel_bias设为 0.05 m/s²accel_noise设为 0.01 m/s²/√Hz。这些量级参考了消费级IMU如MPU6050和导航级IMU之间的中间水平。2.3 生成带失锁模式的GPS位置量测GPS量测仿真的核心是模拟位置观测和观测噪声有时还要模拟卫星失锁。失锁模式的建模直接影响滤波器的鲁棒性测试效果。% 生成GPS位置量测支持模拟失锁窗口 function gps genGpsMeasurements(traj, gps_rate, loss_periods) % gps_rate: GPS输出频率如 1Hz 或 5Hz % loss_periods: 失锁区间矩阵每行为 [t_start, t_end]无失锁传 [] gps_t traj.t(1):(1/gps_rate):traj.t(end); gps_xy interp1(traj.t, [traj.x, traj.y], gps_t); % 水平位置噪声典型载波相位差分GPS约0.1~1m noise_xy randn(length(gps_t), 2) * 0.8; gps_xy gps_xy noise_xy; % 处理失锁失锁期间GPS量测不可用 mask ones(length(gps_t), 1); for i 1:size(loss_periods, 1) mask(gps_t loss_periods(i,1) gps_t loss_periods(i,2)) NaN; end gps_xy gps_xy .* mask; gps table(gps_t, gps_xy(:,1), gps_xy(:,2), ... VariableNames, {t, x_gps, y_gps}); end注意这段代码用NaN标记失锁数据而不是直接删除好处是滤波循环里可以用isnan判断量测是否可用不需要维护索引数组。实际工程中GPS量测噪声不是常数与卫星几何分布DOP值相关但在仿真阶段固定为0.8米已经足够验证滤波器的基本行为。3. 惯导解算与误差模型滤波器设计前的必修课把IMU量测转成位置、速度和姿态这套过程在惯性导航里叫“机械编排”mechanization。组合导航系统的卡尔曼滤波绝大多数情况并不直接估计位置、速度本身而是估计惯导解算结果的误差。这个设计选择源于一个工程现实惯导解算的动力学是高度非线性的而误差传播在小误差假设下可以近似为线性系统卡尔曼滤波的线性高斯假设因此成立。3.1 捷联惯导二维解算姿态递推与比力积分先给出一个简化的二维捷联惯导解算保留核心的比力方程结构。function nav insMechanization(gyro, accel, dt, init_state) % 输入陀螺角速度(rad/s)加速度计比力(m/s^2)采样间隔初始状态 % 状态x, y, vx, vy, psi —— 位置、速度、航向角 n size(gyro, 1); x zeros(n,1); y zeros(n,1); vx zeros(n,1); vy zeros(n,1); psi zeros(n,1); % 初始化 x(1) init_state.x; y(1) init_state.y; vx(1) init_state.vx; vy(1) init_state.vy; psi(1) init_state.psi; g 9.80665; for i 2:n % 姿态更新一阶欧拉积分实际应用中常用四元数或等效旋转矢量 psi(i) psi(i-1) gyro(i-1, 3) * dt; % 比力方程加速度 比力 重力导航系下 % 将载体系比力旋转到导航系 R_bn [cos(psi(i)), -sin(psi(i)), 0; sin(psi(i)), cos(psi(i)), 0; 0, 0, 1]; % 载体系到导航系 f_n R_bn * accel(i, :); % 加速度计测得的比力包含重力补偿项 ax_nav f_n(1); ay_nav f_n(2); % 速度更新 vx(i) vx(i-1) ax_nav * dt; vy(i) vy(i-1) ay_nav * dt; % 位置更新 x(i) x(i-1) vx(i) * dt; y(i) y(i-1) vy(i) * dt; end nav table((0:n-1)*dt, x, y, vx, vy, psi, ... VariableNames, {t,x,y,vx,vy,psi}); end这段代码能跑但有三个问题需要指出来这些也是真实惯导系统会用等效旋转矢量代替欧拉积分的原因一阶欧拉积分在角速度较大时会产生不可交换误差直接用欧拉角递推在大俯仰/横滚角时会出现万向节锁没有考虑地球自转和科里奥利力。二维场景下这些问题被隐藏了三维场景下必须用四元数或方向余弦矩阵实现姿态更新。如果被仿真对象的运动动态较强建议直接把姿态递推换成四元数乘法。3.2 状态方程设计15维误差状态真正的组合导航滤波器估计的是一组误差状态通常包含三维位置误差δr、三维速度误差δv、三维姿态误差φ、陀螺零偏bg、加速度计零偏ba一共15维。误差状态方程可以写成紧凑的分块矩阵形式δx_k1 F * δx_k w_k其中F矩阵的子块含义如下状态块对应子矩阵物理含义位置误差F11 I·dt位置误差增量与速度误差积分速度误差F21 -(2ΩieΩen)×dt哥氏项中低速场景可忽略姿态误差F31 -F·dt姿态误差校正反馈到姿态更新陀螺零偏F44 I零偏建模为随机游走加计零偏F55 I加速度计零偏建模为随机游走具体的F矩阵参数推导可以参考《捷联惯导算法与组合导航原理》教材中的标准结果。MATLAB中实现时推荐先封装一个函数生成F矩阵和噪声协方差阵Q再交给卡尔曼滤波主循环调用。function [F, Q] buildErrorStateModel(dt, imu_params) % 15维误差状态转移矩阵简化版忽略地球曲率项 I3 eye(3); Z3 zeros(3); F [Z3, I3*dt, Z3, Z3, Z3; Z3, Z3, -gravityMatrix()*dt, Z3, I3*dt; Z3, Z3, Z3, -I3*dt, Z3; Z3, Z3, Z3, Z3, Z3; Z3, Z3, Z3, Z3, Z3]; % 过程噪声矩阵IMU白噪声和零偏随机游走 Q blkdiag(Z3, imu_params.accel_noise^2*I3*dt, ... imu_params.gyro_noise^2*I3*dt, ... imu_params.gyro_bias_drift^2*I3*dt, ... imu_params.accel_bias_drift^2*I3*dt); end function Gm gravityMatrix() % 重力梯度矩阵平坦地球模型下简化为零 Gm zeros(3); end这里对重力梯度矩阵的简化值得注意如果仿真场景覆盖较大地理范围需要考虑重力场随纬度和高度的变化否则长航时条件下位置误差会逐步积累。中小范围场景中这么处理问题不大。3.3 量测方程GPS位置观测映射GPS输出的是经纬高或平面坐标对应的是“位置 误差”所以量测方程是线性的z Hx vH矩阵中只有对应位置误差的列非零。function H buildMeasureMatrix() % GPS位置量测矩阵量测位置差异对应于位置误差状态 H [eye(2), zeros(2, 13)]; end关键点在于GPS量测更新时需要计算“量测残差”GPS观测位置减去惯导当前推算位置。这个残差值的大小直接反映了惯导积累的误差——如果GPS可信残差就应该全部被滤波器吸收成状态估计如果有跳变或失锁残差会被误认为真实误差导致滤波发散这也是为什么后面要引入卡方检验来剔除异常量测。4. 卡尔曼滤波组合误差状态反馈与闭环修正有了第3章的误差模型和量测模型就可以搭出真正的组合导航卡尔曼滤波器。本文采用最常见的松组合结构IMU独立完成高频机械编排GPS低频量测负责修正惯导误差。紧组合需要在导航解算之前处理原始伪距/载波相位测量模型复杂度增加一个量级留待后续专题展开。4.1 滤波器核心循环时间更新与量测更新滤波器主循环分三步惯导递推、时间更新、量测更新。惯导递推在IMU每个采样时刻执行GPS量测到达时执行量测更新更新完成后把误差状态反馈给惯导解算并清零误差状态。function results runLooselyCoupledINSGPS(imu_data, gps_data, imu_params) dt_imu imu_data.t(2) - imu_data.t(1); n height(imu_data); % 状态向量15维误差状态 x_est zeros(15, 1); P eye(15) * 0.1; % 初始协方差 % 惯导参考状态 nav_state struct(x, 0, y, 0, vx, 0, vy, 0, psi, 0); results zeros(n, 4); % t, x, y, psi gps_idx 1; for i 2:n % 1. 惯导机械编排单步解算 nav_state insStep(nav_state, imu_data.gyro(i,:), imu_data.accel(i,:), dt_imu); % 2. 卡尔曼时间更新 [F, Q] buildErrorStateModel(dt_imu, imu_params); P F * P * F Q; x_est F * x_est; % 误差状态的零均值假设下可省略 % 3. GPS量测更新 if gps_idx height(gps_data) ... abs(imu_data.t(i) - gps_data.t(gps_idx)) dt_imu/2 % 量测残差 z [gps_data.x_gps(gps_idx) - nav_state.x; gps_data.y_gps(gps_idx) - nav_state.y]; % 异常量测检测 if ~isnan(z(1)) H buildMeasureMatrix(); R eye(2) * 0.8^2; % 标准卡尔曼增益 K P * H / (H * P * H R); x_est x_est K * (z - H * x_est); P (eye(15) - K * H) * P; % 反馈校正把误差状态加到惯导状态上并清零 nav_state.x nav_state.x x_est(1); nav_state.y nav_state.y x_est(2); nav_state.vx nav_state.vx x_est(4); nav_state.vy nav_state.vy x_est(5); nav_state.psi nav_state.psi x_est(6); x_est(1:6) 0; end gps_idx gps_idx 1; end results(i, :) [imu_data.t(i), nav_state.x, nav_state.y, nav_state.psi]; end end这段代码暴露了实际工程里的一个重要细节韩华的计算机制大量使用矩阵形式但是15维矩阵乘法在每次IMU采样时刻执行当IMU频率达到200Hz时会产生可观的CPU开销。优化手段通常有三种利用F矩阵的稀疏结构手写差分方程把时间更新降频到与GPS同频但会牺牲惯性递推精度改用误差状态的稀疏表达。仿真规模不大时先保证正确性再考虑优化。参数说明初始协方差P的对角线取值表示对初始误差状态的置信度0.1 m²的位置不确定性对应约0.3米的标准差在仿真中合理。R矩阵由GPS位置噪声决定这里取0.8² 0.64 m²与第2.3节仿真注入的噪声水平一致。如果R设太小滤波器会过度相信GPS导致输出抖动设太大则修正力度不够。4.2 异常量测剔除卡方检验GPS在城市峡谷中经常出现多径导致的异常位置输出——误差可能是标称噪声的几十倍。如果不加防护一个异常点就足以让滤波发散。工程上最常用的防护是卡方检验Chi-squared test计算新息innovation的玛氏距离超过阈值就丢弃该次量测更新。function [innovation_norm, threshold] chiSquareTest(z, H, P, R) S H * P * H R; % 新息协方差 innovation_norm z / S * z; % 新息玛氏距离 threshold chi2inv(0.99, length(z)); % 显著性水平0.01下的门限 end卡方检验的使用建议新息服从零均值高斯分布的假设只在滤波器收敛后成立启动初期不要启用该检验否则可能频繁误判。一般做法是滤波器运行10秒后或连续收敛50步后再开启异常检测。如果误判率仍然偏高可以把显著性水平从0.01放宽到0.05代价是漏检率上升。4.3 GPS失锁期间的纯惯性递推行为失锁期间没有量测更新滤波器退化为纯惯导解算。第2.3节如果设置了15秒失锁窗口你会在仿真结果中看到明显的漂移位置误差按时间三次方增长姿态误差 → 加速度投影错误 → 速度误差 → 位置误差这是惯导误差传播的经典特征。失锁期间常见的两种处理策略保持滤波器时间更新但暂停量测更新零速修正可以类比为量测更新需要准确判断静止状态在失锁恢复后采用渐消记忆加权防止误差跳变过猛。推荐做法是失锁期间只记录时间累积GPS信号恢复后正常执行量测更新靠卡尔曼增益自然调整——前提是失锁时间不超过30秒超过的话需要引入额外的辅助传感器或重新对准。5. 程序整体结构与调优技巧把前面各部件组装成一个仿真主程序时推荐按照“参数定义 → 数据生成 → 算法运行 → 绘图分析”四段式组织。以下是一个最小可运行的主程序骨架。假设所有函数存在于同一目录主脚本如下% 主脚本惯性/GPS组合导航仿真入口 clear; clc; close all; rng(42); % 固定随机种子保证结果可复现 % 参数定义 imu_params struct(... gyro_bias_z, 0.01, ... gyro_noise, 0.001, ... accel_bias, [0.05; 0.05; 0.05], ... accel_noise, 0.01, ... gyro_bias_drift, 1e-5, ... accel_bias_drift, 1e-4); dt 0.01; T_total 40; gps_rate 1; % 1 Hz GPS loss_periods [15, 20]; % 15s到20s期间GPS失锁 % 数据生成 traj genRefTrajectory(dt, T_total); [gyro, accel] genImuMeasurements(traj, imu_params); gps genGpsMeasurements(traj, gps_rate, loss_periods); % 算法运行 results runLooselyCoupledINSGPS(...); % 绘图对比真值与估计值 end5.1 纯惯性 vs 组合导航的对比验证方法判断组合是否起效的最直接方法是跑两组实验一组只用惯导解算不加任何修正一组跑完整组合滤波然后把两条轨迹与真值画在一起对比。实际项目中常用的量化指标包括位置RMSE均方根误差对比终态误差每段失锁结束后的最大位置偏差航向误差收敛时间。 把这些指标输出到一个表格比单纯看图更有说服力。指标纯惯性解算组合导航位置RMSE全程约200米0.8~1.5米失锁期间最大漂移约50米约20米航向误差收敛时间—1~5秒数值因仿真参数差异会浮动但量级关系不变组合导航能有效抑制位置发散但在GPS失锁期间的修正能力完全取决于惯导器件质量和失锁前的估计精度。如果你得到的对比结果不符合这个规律优先检查量测更新是否正确触发、反馈校正是否清零了误差状态。5.2 调参经验从“能跑”到“跑得好”滤波性能不佳时调整顺序建议为先检查数据生成端再检查模型端最后调协方差参数。数据生成端最常见的问题是把轨迹的加速度全部设为零导致加计零偏不可观测模型端最常见的问题是F矩阵和Q矩阵的量纲不匹配比如位置误差单位是米、速度误差单位是米/秒但Q矩阵中对应项的数值差了两个数量级。调协方差参数时记住这个调节直觉扩大Q表示“对模型更不信任”滤波器会更依赖量测输出跟随GPS噪声而抖动缩小Q会使输出更平滑但对真实误差响应变慢。工程上常用方法是先把Q和R都设置成明显偏大的值逐步缩小R到合理范围再微调Q。如果位置输出仍然发散回看惯导解算本身是否正常。5.3 验证滤波收敛性的三个指标最后提供一个收敛性验证技巧在程序末尾加入以下自检逻辑% 验证滤波器是否收敛 % 指标1新息序列均值是否接近零 innovation_std std(innovations); % innovations为记录的新息序列 assert(abs(mean(innovations)) 0.1, 滤波器有偏); % 指标2协方差对角线是否持续下降 P_trace squeeze(sum(reshape(P_hist, [15,15,n]), 1)); assert(all(diff(P_trace(20:end)) 0 || max(P_trace) 1e-3, all), ... 滤波器协方差未收敛); % 指标3位置误差是否在3σ范围附近波动 pos_err results(:,2:3) - traj{:,2:3}; sigma 2*sqrt(squeeze(P_hist(1,1,:))); % 位置误差标准差估计 assert(mean(abs(pos_err) sigma) 0.6, 误差统计不合理);这三个指标分别验证了滤波器的无偏性、收敛性和一致性。其中第一条最关键滤波器如果正确新息序列应该是零均值白噪声。如果新息均值明显偏离零说明量测模型或状态方程中存在未建模特性的偏差比如忽略了加速度计尺度因子。本文还有配套的精品资源点击获取